Add blending interface to examples

examples/iiqka_moveit_example/CMakeLists.txt

@@ -36,11 +36,18 @@ ament_target_dependencies(moveit_depalletizing_example
   moveit_visual_tools
 )
 
+add_executable(blending_example src/blending_example.cpp)
+ament_target_dependencies(blending_example
+  moveit_ros_planning_interface
+  moveit_visual_tools
+)
+
 install(TARGETS
   moveit_basic_planners_example
   moveit_collision_avoidance_example
   moveit_constrained_planning_example
   moveit_depalletizing_example
+  blending_example
   EXPORT export_${PROJECT_NAME}
   DESTINATION lib/${PROJECT_NAME}
 )

examples/iiqka_moveit_example/include/iiqka_moveit_example/moveit_example.hpp

@@ -21,13 +21,27 @@
 #include <string>
 #include <vector>
 
+#include "communication_helpers/service_tools.hpp"
 #include "geometry_msgs/msg/vector3.hpp"
 #include "moveit/move_group_interface/move_group_interface.h"
 #include "moveit/planning_scene_interface/planning_scene_interface.h"
 #include "moveit_msgs/msg/collision_object.hpp"
+#include "moveit_msgs/msg/motion_sequence_item.hpp"
+#include "moveit_msgs/srv/get_motion_sequence.hpp"
 #include "moveit_visual_tools/moveit_visual_tools.h"
 #include "rclcpp/rclcpp.hpp"
 
+struct MotionSegment
+{
+  MotionSegment(const Eigen::Isometry3d & pose, const std::string & planner_id, double blend_radius)
+  : pose(pose), planner_id(planner_id), blend_radius(blend_radius)
+  {
+  }
+  Eigen::Isometry3d pose;
+  std::string planner_id;  // One of the supported planners of the pilz pipeline
+  double blend_radius;
+};
+
 class MoveitExample : public rclcpp::Node
 {
 public:
@@ -51,6 +65,10 @@ class MoveitExample : public rclcpp::Node
 
     move_group_interface_->setMaxVelocityScalingFactor(0.1);
     move_group_interface_->setMaxAccelerationScalingFactor(0.1);
+
+    motion_sequence_client_ =
+      this->create_client<moveit_msgs::srv::GetMotionSequence>("plan_sequence_path");
+    motion_sequence_request_ = std::make_shared<moveit_msgs::srv::GetMotionSequence::Request>();
   }
 
   moveit_msgs::msg::RobotTrajectory::SharedPtr drawCircle()
@@ -339,12 +357,46 @@ class MoveitExample : public rclcpp::Node
     return move_group_interface_;
   }
 
+  moveit_msgs::msg::RobotTrajectory::SharedPtr blend(
+    const std::vector<MotionSegment> & motion_sequence)
+  {
+    motion_sequence_request_->request.items.clear();
+    for (size_t i = 0; i < motion_sequence.size(); ++i)
+    {
+      moveit_msgs::msg::MotionPlanRequest blending_request;
+      move_group_interface_->setPlanningPipelineId("pilz_industrial_motion_planner");
+      move_group_interface_->setPlannerId(motion_sequence[i].planner_id);
+      move_group_interface_->setPoseTarget(motion_sequence[i].pose);
+
+      move_group_interface_->constructMotionPlanRequest(blending_request);
+      moveit_msgs::msg::MotionSequenceItem motion_segment;
+      motion_segment.req = blending_request;
+      // Disable blend radius for last segment
+      motion_segment.blend_radius =
+        (i == motion_sequence.size() - 1) ? 0 : motion_sequence[i].blend_radius;
+      motion_sequence_request_->request.items.push_back(motion_segment);
+    }
+
+    auto response = kuka_drivers_core::sendRequest<moveit_msgs::srv::GetMotionSequence::Response>(
+      motion_sequence_client_, motion_sequence_request_, 0, 3000);
+    if (!response || response->response.planned_trajectories.size() == 0)
+    {
+      RCLCPP_ERROR(LOGGER, "Planning of blended trajectory failed");
+      return nullptr;
+    }
+    return std::make_shared<moveit_msgs::msg::RobotTrajectory>(
+      response->response.planned_trajectories[0]);
+  }
+
 protected:
   std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_interface_;
   rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr planning_scene_diff_publisher_;
   std::shared_ptr<moveit_visual_tools::MoveItVisualTools> moveit_visual_tools_;
   const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_basic_plan");
   const std::string PLANNING_GROUP = "manipulator";
+
+  rclcpp::Client<moveit_msgs::srv::GetMotionSequence>::SharedPtr motion_sequence_client_;
+  moveit_msgs::srv::GetMotionSequence::Request::SharedPtr motion_sequence_request_;
 };
 
 #endif  // IIQKA_MOVEIT_EXAMPLE__MOVEIT_EXAMPLE_HPP_

examples/iiqka_moveit_example/src/blending_example.cpp

@@ -0,0 +1,83 @@
+// Copyright 2024 Áron Svastits
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <math.h>
+
+#include <memory>
+
+#include "iiqka_moveit_example/moveit_example.hpp"
+
+
+// Workaround for trajectories not strictly increasing timstamps
+void shiftRobotTrajectory(moveit_msgs::msg::RobotTrajectory& trajectory) {
+    int time_shift_ns = 0;
+
+    for (size_t index = 0; index < trajectory.joint_trajectory.points.size(); ++index) {
+        if (index == 0) {
+            continue;
+        }
+
+        trajectory.joint_trajectory.points[index].time_from_start.nanosec += time_shift_ns;
+
+        if (trajectory.joint_trajectory.points[index-1].time_from_start == trajectory.joint_trajectory.points[index].time_from_start) {
+            trajectory.joint_trajectory.points[index].time_from_start.nanosec += static_cast<int>(1e8);
+            time_shift_ns += static_cast<int>(1e8);
+        }
+    }
+}
+
+
+int main(int argc, char * argv[])
+{
+  // Setup
+  // Initialize ROS and create the Node
+  rclcpp::init(argc, argv);
+  auto const example_node = std::make_shared<MoveitExample>();
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(example_node);
+  std::thread([&executor]() { executor.spin(); }).detach();
+
+  example_node->initialize();
+  example_node->addBreakPoint();
+
+  // Add robot platform
+  example_node->addRobotPlatform();
+
+  // Define goals
+  auto standing_pose =
+    Eigen::Isometry3d(Eigen::Translation3d(0.1, 0, 0.8) * Eigen::Quaterniond::Identity());
+  auto cart_goal =
+    Eigen::Isometry3d(Eigen::Translation3d(0.2, -0.15, 0.6) * Eigen::Quaterniond::Identity());
+  auto cart_goal2 =
+    Eigen::Isometry3d(Eigen::Translation3d(0.4, -0.15, 0.55) * Eigen::Quaterniond::Identity());
+
+  std::vector<MotionSegment> motion_sequence;
+  motion_sequence.emplace_back(standing_pose, "PTP", 0.01);
+  motion_sequence.emplace_back(cart_goal, "LIN", 0.05);
+  motion_sequence.emplace_back(cart_goal2, "LIN", 0.0);
+
+  auto planned_trajectory = example_node->blend(motion_sequence);
+
+  if (planned_trajectory != nullptr)
+  {
+    shiftRobotTrajectory(*planned_trajectory);
+    example_node->drawTrajectory(*planned_trajectory);
+    example_node->addBreakPoint();
+    example_node->moveGroupInterface()->execute(*planned_trajectory);
+  }
+
+  // Shutdown ROS
+  rclcpp::shutdown();
+  return 0;
+}