Replace adapter ResolveConstraintFrames with direct function call (mo…

…veit#2975) The ResolveConstraintFrames adapter was only changing the planning request used within the planning pipeline, while the subsequent trajectory validation was still using the original, unresolved request, causing at best a warning and at worst acceptance of paths not satisfying the path constraints. We make the adapter obsolete and just call the resolver function once in the beginning of the planning pipeline.
ubi-agni · Oct 31, 2024 · f1a6071 · f1a6071
1 parent 0a93db3
commit f1a6071
Show file tree

Hide file tree

Showing 5 changed files with 12 additions and 11 deletions.
diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp
@@ -38,6 +38,7 @@
 #include <moveit/robot_state/conversions.h>
 #include <moveit/collision_detection/collision_tools.h>
 #include <moveit/trajectory_processing/trajectory_tools.h>
+#include <moveit/kinematic_constraints/utils.h>
 #include <moveit_msgs/DisplayTrajectory.h>
 #include <visualization_msgs/MarkerArray.h>
 #include <boost/tokenizer.hpp>
@@ -216,7 +217,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla
 }
 
 bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene,
-                                                       const planning_interface::MotionPlanRequest& req,
+                                                       const planning_interface::MotionPlanRequest& request,
                                                        planning_interface::MotionPlanResponse& res,
                                                        std::vector<std::size_t>& adapter_added_state_index) const
 {
@@ -225,7 +226,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla
 
   // broadcast the request we are about to work on, if needed
   if (publish_received_requests_)
-    received_request_publisher_.publish(req);
+    received_request_publisher_.publish(request);
   adapter_added_state_index.clear();
 
   if (!planner_instance_)
@@ -236,6 +237,12 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla
     return false;
   }
 
+  // resolve constraint frames
+  planning_interface::MotionPlanRequest req = request;
+  kinematic_constraints::resolveConstraintFrames(planning_scene->getCurrentState(), req.path_constraints);
+  for (moveit_msgs::Constraints& constraint : req.goal_constraints)
+    kinematic_constraints::resolveConstraintFrames(planning_scene->getCurrentState(), constraint);
+
   bool solved = false;
   try
   {

diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp
@@ -46,6 +46,7 @@ class ResolveConstraintFrames : public planning_request_adapter::PlanningRequest
 public:
   ResolveConstraintFrames() : planning_request_adapter::PlanningRequestAdapter()
   {
+    ROS_WARN("The planning adapter 'ResolveConstraintFrames' is obsolete and can be removed from your config.");
   }
 
   void initialize(const ros::NodeHandle& /*nh*/) override
@@ -61,12 +62,7 @@ class ResolveConstraintFrames : public planning_request_adapter::PlanningRequest
                     const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
                     std::vector<std::size_t>& /*added_path_index*/) const override
   {
-    ROS_DEBUG("Running '%s'", getDescription().c_str());
-    planning_interface::MotionPlanRequest modified = req;
-    kinematic_constraints::resolveConstraintFrames(planning_scene->getCurrentState(), modified.path_constraints);
-    for (moveit_msgs::Constraints& constraint : modified.goal_constraints)
-      kinematic_constraints::resolveConstraintFrames(planning_scene->getCurrentState(), constraint);
-    return planner(planning_scene, modified, res);
+    return planner(planning_scene, req, res);
   }
 };
 

diff --git a/moveit_ros/planning/planning_request_adapters_plugin_description.xml b/moveit_ros/planning/planning_request_adapters_plugin_description.xml
@@ -27,7 +27,7 @@
 
   <class name="default_planner_request_adapters/ResolveConstraintFrames" type="default_planner_request_adapters::ResolveConstraintFrames" base_class_type="planning_request_adapter::PlanningRequestAdapter">
     <description>
-      Resolves constraints that are defined in collision objects or subframes to robot links, because the former are not known to the planner.
+      Obsolete. Resolved constraints that are defined w.r.t. collision objects or subframes.
     </description>
   </class>
 

diff --git a/..._assistant/templates/moveit_config_pkg_template/launch/chomp_planning_pipeline.launch.xml b/..._assistant/templates/moveit_config_pkg_template/launch/chomp_planning_pipeline.launch.xml
@@ -5,7 +5,6 @@
   <arg name="planning_adapters"
        default="default_planner_request_adapters/LimitMaxCartesianLinkSpeed
                 default_planner_request_adapters/AddTimeParameterization
-                default_planner_request_adapters/ResolveConstraintFrames
                 default_planner_request_adapters/FixWorkspaceBounds
                 default_planner_request_adapters/FixStartStateBounds
                 default_planner_request_adapters/FixStartStateCollision

diff --git a/...p_assistant/templates/moveit_config_pkg_template/launch/ompl_planning_pipeline.launch.xml b/...p_assistant/templates/moveit_config_pkg_template/launch/ompl_planning_pipeline.launch.xml
@@ -4,7 +4,6 @@
   <arg name="planning_adapters"
        default="default_planner_request_adapters/LimitMaxCartesianLinkSpeed
                 default_planner_request_adapters/AddTimeParameterization
-                default_planner_request_adapters/ResolveConstraintFrames
                 default_planner_request_adapters/FixWorkspaceBounds
                 default_planner_request_adapters/FixStartStateBounds
                 default_planner_request_adapters/FixStartStateCollision