[FIX] removes link_ee

moveit · Oct 8, 2024 · db0fe36 · db0fe36
1 parent fb4fcb8
commit db0fe36
Show file tree

Hide file tree

Showing 3 changed files with 12 additions and 10 deletions.
diff --git a/include/ktopt_interface/ktopt_planning_context.hpp b/include/ktopt_interface/ktopt_planning_context.hpp
@@ -83,7 +83,7 @@ class KTOptPlanningContext : public planning_interface::PlanningContext
   void setRobotDescription(std::string robot_description);
   void transcribePlanningScene(const planning_scene::PlanningScene& planning_scene);
   void addPathPositionConstraints(KinematicTrajectoryOptimization& trajopt, const MultibodyPlant<double>& plant,
-                                  const Frame<double>& link_ee_frame, Context<double>& plant_context);
+                                  Context<double>& plant_context);
 
 private:
   const ktopt_interface::Params params_;

diff --git a/res/ktopt_moveit_parameters.yaml b/res/ktopt_moveit_parameters.yaml
@@ -4,11 +4,6 @@ ktopt_interface:
     description: "Robot description to be loaded by the internal Drake MultibodyPlant",
     default_value: "package://drake_models/franka_description/urdf/panda_arm_hand.urdf",
   }
-  link_ee: {
-    type: string,
-    description: "Name of the end-effector frame as per urdf/srdf",
-    default_value: "panda_hand"
-  }
   num_iterations: {
     type: int,
     description: "Number of iterations for the Drake mathematical program solver.",

diff --git a/src/ktopt_planning_context.cpp b/src/ktopt_planning_context.cpp
@@ -52,8 +52,8 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res)
   RCLCPP_INFO_STREAM(getLogger(), "Planning for group: " << getGroupName());
   const int num_positions = plant.num_positions();
   const int num_velocities = plant.num_velocities();
-  const auto link_ee = params_.link_ee;
-  const auto& link_ee_frame = plant.GetFrameByName(link_ee);
+  // const auto link_ee = params_.link_ee;
+  // const auto& link_ee_frame = plant.GetFrameByName(link_ee);
 
   // extract position and velocity bounds
   std::vector<double> lower_position_bounds;
@@ -180,7 +180,7 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res)
   trajopt.AddDurationConstraint(0.5, 5);
 
   // process path_constraints
-  addPathPositionConstraints(trajopt, plant, link_ee_frame, plant_context);
+  addPathPositionConstraints(trajopt, plant, plant_context);
 
   // solve the program
   auto result = Solve(prog);
@@ -237,7 +237,6 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res)
 
 void KTOptPlanningContext::addPathPositionConstraints(KinematicTrajectoryOptimization& trajopt,
                                                       const MultibodyPlant<double>& plant,
-                                                      const Frame<double>& link_ee_frame,
                                                       Context<double>& plant_context)
 {
   // retrieve the motion planning request
@@ -255,6 +254,14 @@ void KTOptPlanningContext::addPathPositionConstraints(KinematicTrajectoryOptimiz
       // Extract dimensions of the bounding box from
       // constraint_region.primitives Assuming it is a box
       // (shape_msgs::SolidPrimitive::BOX) and has dimensions in [x, y, z]
+      const auto link_ee = position_constraint.link_name;
+      if (!plant.HasBodyNamed(link_ee))
+      {
+        RCLCPP_ERROR(getLogger(), "The link specified in the PositionConstraint message does not exist in the Drake "
+                                  "plant, please check your URDF");
+        continue;
+      }
+      const auto& link_ee_frame = plant.GetFrameByName(link_ee);
       const auto& primitive = position_constraint.constraint_region.primitives[0];
       if (primitive.type != shape_msgs::msg::SolidPrimitive::BOX)
       {