go to init poses

kroshu · Oct 30, 2023 · 5d073c7 · 5d073c7
1 parent 633bfb3
commit 5d073c7
Show file tree

Hide file tree

Showing 3 changed files with 31 additions and 2 deletions.
diff --git a/kuka_driver_examples/eci_demo/include/moveit_example.h b/kuka_driver_examples/eci_demo/include/moveit_example.h
@@ -107,12 +107,28 @@ class MoveitExample : public rclcpp::Node
     }
   }
 
+    moveit_msgs::msg::RobotTrajectory::SharedPtr planToPosition(
+    const std::vector<double>& joint_pos)
+  {
+    move_group_interface_->setJointValueTarget(joint_pos);
+
+    moveit::planning_interface::MoveGroupInterface::Plan plan;
+    RCLCPP_INFO(LOGGER, "Sending planning request");
+    if (!move_group_interface_->plan(plan)) {
+      RCLCPP_INFO(LOGGER, "Planning failed");
+      return nullptr;
+    } else {
+      RCLCPP_INFO(LOGGER, "Planning successful");
+      return std::make_shared<moveit_msgs::msg::RobotTrajectory>(plan.trajectory_);
+    }
+  }
+
   moveit_msgs::msg::RobotTrajectory::SharedPtr planToPointUntilSuccess(
     const Eigen::Isometry3d & pose,
     const std::string & planning_pipeline = "pilz_industrial_motion_planner",
     const std::string & planner_id = "PTP")
   {
-    // Create planning request using pilz industrial motion planner
+    // Create planning request using given motion planner
     move_group_interface_->setPlanningPipelineId(planning_pipeline);
     move_group_interface_->setPlannerId(planner_id);
     move_group_interface_->setPoseTarget(pose);

diff --git a/kuka_driver_examples/eci_demo/src/MoveitCollisionAvoidanceExample.cpp b/kuka_driver_examples/eci_demo/src/MoveitCollisionAvoidanceExample.cpp
@@ -37,6 +37,13 @@ int main(int argc, char * argv[])
   // Add robot platform
   example_node->addRobotPlatform();
 
+  // Go to correct position for the example
+  auto init_trajectory = example_node->planToPosition(
+    std::vector<double>{0.3587, 0.3055, -1.3867, 0.0, -0.4896, -0.3587});
+  if (init_trajectory != nullptr) {
+    example_node->moveGroupInterface()->execute(*init_trajectory);
+  }
+
   // Add collision object
   example_node->addCollisionBox(
     geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0.125).y(0.15).z(0.5),

diff --git a/kuka_driver_examples/eci_demo/src/MoveitConstrainedPlanningExample.cpp b/kuka_driver_examples/eci_demo/src/MoveitConstrainedPlanningExample.cpp
@@ -36,7 +36,13 @@ int main(int argc, char * argv[])
 
   // Add robot platform
   example_node->addRobotPlatform();
-// geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0.125).y(0.15).z(0.5);
+
+  // Go to correct position for the example
+  auto init_trajectory = example_node->planToPosition(
+    std::vector<double>{0.0017, -2.096, 1.514, 0.0012, -0.9888, -0.0029});
+  if (init_trajectory != nullptr) {
+    example_node->moveGroupInterface()->execute(*init_trajectory);
+  }
 
   // Add collision object
   example_node->addCollisionBox(