Update planning request adapters

moveit_configs_utils/default_configs/chomp_planning.yaml

@@ -4,9 +4,9 @@ jiggle_fraction: 0.05
 # The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline.
 request_adapters:
   - default_planning_request_adapters/ResolveConstraintFrames
-  - default_planning_request_adapters/FixWorkspaceBounds
-  - default_planning_request_adapters/FixStartStateBounds
-  - default_planning_request_adapters/FixStartStateCollision
+  - default_planning_request_adapters/ValidateWorkspaceBounds
+  - default_planning_request_adapters/CheckStartStateBounds
+  - default_planning_request_adapters/CheckStartStateCollision
 response_adapters:
   - default_planning_response_adapters/AddTimeOptimalParameterization
 ridge_factor: 0.01

moveit_configs_utils/default_configs/ompl_planning.yaml

@@ -4,8 +4,8 @@ jiggle_fraction: 0.05
 # The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline.
 request_adapters:
   - default_planning_request_adapters/ResolveConstraintFrames
-  - default_planning_request_adapters/FixWorkspaceBounds
-  - default_planning_request_adapters/FixStartStateBounds
-  - default_planning_request_adapters/FixStartStateCollision
+  - default_planning_request_adapters/ValidateWorkspaceBounds
+  - default_planning_request_adapters/CheckStartStateBounds
+  - default_planning_request_adapters/CheckStartStateCollision
 response_adapters:
   - default_planning_response_adapters/AddTimeOptimalParameterization

moveit_configs_utils/default_configs/stomp_planning.yaml

@@ -2,9 +2,9 @@ planning_plugin: stomp_moveit/StompPlanner
 # The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline.
 request_adapters:
   - default_planning_request_adapters/ResolveConstraintFrames
-  - default_planning_request_adapters/FixWorkspaceBounds
-  - default_planning_request_adapters/FixStartStateBounds
-  - default_planning_request_adapters/FixStartStateCollision
+  - default_planning_request_adapters/ValidateWorkspaceBounds
+  - default_planning_request_adapters/CheckStartStateBounds
+  - default_planning_request_adapters/CheckStartStateCollision
 response_adapters:
   - default_planning_response_adapters/AddTimeOptimalParameterization
 

moveit_planners/test_configs/prbt_moveit_config/launch/demo.launch.py

@@ -87,7 +87,7 @@ def generate_launch_description():
         },
         "ompl": {
             "planning_plugin": "ompl_interface/OMPLPlanner",
-            "request_adapters": """default_planning_request_adapters/AddTimeOptimalParameterization default_planning_request_adapters/FixWorkspaceBounds default_planning_request_adapters/FixStartStateBounds default_planning_request_adapters/FixStartStateCollision default_planning_request_adapters/FixStartStatePathConstraints""",
+            "request_adapters": """default_planning_request_adapters/AddTimeOptimalParameterization default_planning_request_adapters/ValidateWorkspaceBounds default_planning_request_adapters/CheckStartStateBounds default_planning_request_adapters/CheckStartStateCollision default_planning_request_adapters/FixStartStatePathConstraints""",
             "start_state_max_bounds_error": 0.1,
         },
     }

moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py

@@ -71,7 +71,7 @@ def generate_common_hybrid_launch_description():
     planning_pipelines_config = {
         "ompl": {
             "planning_plugin": "ompl_interface/OMPLPlanner",
-            "request_adapters": """default_planning_request_adapters/AddTimeOptimalParameterization default_planning_request_adapters/FixWorkspaceBounds default_planning_request_adapters/FixStartStateBounds default_planning_request_adapters/FixStartStateCollision""",
+            "request_adapters": """default_planning_request_adapters/AddTimeOptimalParameterization default_planning_request_adapters/ValidateWorkspaceBounds default_planning_request_adapters/CheckStartStateBounds default_planning_request_adapters/CheckStartStateCollision""",
             "start_state_max_bounds_error": 0.1,
         }
     }

moveit_ros/planning/default_request_adapters_plugin_description.xml

@@ -1,18 +1,18 @@
 <library path="moveit_default_planning_request_adapter_plugins">
 
-  <class name="default_planning_request_adapters/FixStartStateBounds" type="default_planning_request_adapters::FixStartStateBounds" base_class_type="planning_interface::PlanningRequestAdapter">
+  <class name="default_planning_request_adapters/CheckStartStateBounds" type="default_planning_request_adapters::CheckStartStateBounds" base_class_type="planning_interface::PlanningRequestAdapter">
     <description>
       Fixes the start state to be within the joint limits specified in the URDF.
     </description>
   </class>
 
-  <class name="default_planning_request_adapters/FixWorkspaceBounds" type="default_planning_request_adapters::FixWorkspaceBounds" base_class_type="planning_interface::PlanningRequestAdapter">
+  <class name="default_planning_request_adapters/ValidateWorkspaceBounds" type="default_planning_request_adapters::ValidateWorkspaceBounds" base_class_type="planning_interface::PlanningRequestAdapter">
     <description>
       Specify a default workspace for planning if none is specified in the planning request.
     </description>
   </class>
 
-  <class name="default_planning_request_adapters/FixStartStateCollision" type="default_planning_request_adapters::FixStartStateCollision" base_class_type="planning_interface::PlanningRequestAdapter">
+  <class name="default_planning_request_adapters/CheckStartStateCollision" type="default_planning_request_adapters::CheckStartStateCollision" base_class_type="planning_interface::PlanningRequestAdapter">
     <description>
       If start state is in collision this adapter attempts to sample a new collision-free configuration near a specified configuration (in collision) by perturbing the joint values by a small amount.
     </description>

moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt

@@ -2,17 +2,16 @@ generate_parameter_library(default_request_adapter_parameters res/default_reques
 
 add_library(moveit_default_planning_request_adapter_plugins SHARED
   src/check_for_stacked_constraints.cpp
-  src/fix_start_state_bounds.cpp
-  src/fix_start_state_collision.cpp
-  src/fix_workspace_bounds.cpp
+  src/check_start_state_bounds.cpp
+  src/check_start_state_collision.cpp
+  src/validate_workspace_bounds.cpp
   src/resolve_constraint_frames.cpp
 )
 
 target_link_libraries(moveit_default_planning_request_adapter_plugins default_request_adapter_parameters)
 
 set_target_properties(moveit_default_planning_request_adapter_plugins PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 ament_target_dependencies(moveit_default_planning_request_adapter_plugins
-  Boost
   moveit_core
   rclcpp
   pluginlib

moveit_ros/planning/planning_request_adapter_plugins/res/default_request_adapter_params.yaml

@@ -1,30 +1,12 @@
 # This files contains the parameters for all of MoveIt's default PlanRequestAdapters
 default_request_adapter_parameters:
-  start_state_max_dt: {
-    type: double,
-    description: "FixStartStateCollision/FixStartStateBounds: Maximum temporal distance of the fixed start state from the original state.",
-    default_value: 0.5,
-  }
-  jiggle_fraction: {
-    type: double,
-    description: "FixStartStateCollision: Joint value perturbation as a percentage of the total range of motion for the joint.",
-    default_value: 0.02,
-  }
-  max_sampling_attempts: {
-    type: int,
-    description: "FixStartStateCollision: Maximum number of attempts to re-sample a valid start state.",
-    default_value: 100,
-    validation: {
-        gt_eq<>: [ 0 ],
-    }
-  }
-  start_state_max_bounds_error: {
-    type: double,
-    description: "FixStartStateBounds: Maximum allowable error outside joint bounds for the starting configuration.",
-    default_value: 0.05,
+  fix_start_state: {
+    type: bool,
+    description: "CheckStartStateBounds: If set true and the start state is out of bounds, the MotionPlanRequests start will be updated to a start state with enforced bounds for revolute or continuous joints.",
+    default_value: false,
   }
   default_workspace_bounds: {
     type: double,
-    description: "FixWorkspaceBounds: Default workspace bounds representing a cube around the robot's origin whose edge length this parameter defines.",
+    description: "ValidateWorkspaceBounds: Default workspace bounds representing a cube around the robot's origin whose edge length this parameter defines.",
     default_value: 10.0,
   }

moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp → moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp

@@ -33,18 +33,17 @@
  *********************************************************************/
 
 /* Author: Ioan Sucan
- * Desc: Plan request adapter to fix start state bounds adapter fixes the start state to be within the joint limits
+ * Desc: The CheckStartStateBounds adapter validates if the start state is within the joint limits
  * specified in the URDF. The need for this adapter arises in situations where the joint limits for the physical robot
  * are not properly configured. The robot may then end up in a configuration where one or more of its joints is slightly
  * outside its joint limits. In this case, the motion planner is unable to plan since it will think that the starting
- * state is outside joint limits. The “FixStartStateBounds” planning request adapter will “fix” the start state by
+ * state is outside joint limits. The “CheckStartStateBounds” planning request adapter can “fix” the start state by
  * moving it to the joint limit. However, this is obviously not the right solution every time - e.g. where the joint is
  * really outside its joint limits by a large amount. A parameter for the adapter specifies how much the joint can be
  * outside its limits for it to be “fixable”.
  */
 
 #include <moveit/planning_interface/planning_request_adapter.h>
-#include <boost/math/constants/constants.hpp>
 #include <moveit/trajectory_processing/trajectory_tools.h>
 #include <moveit/robot_state/conversions.h>
 #include <class_loader/class_loader.hpp>
@@ -59,11 +58,11 @@
 namespace default_planning_request_adapters
 {
 
-/** @brief Fix start state bounds adapter fixes the start state to be within the joint limits specified in the URDF. */
-class FixStartStateBounds : public planning_interface::PlanningRequestAdapter
+/** @brief The CheckStartStateBounds adapter validates if the start state is within the joint limits specified in the URDF. */
+class CheckStartStateBounds : public planning_interface::PlanningRequestAdapter
 {
 public:
-  FixStartStateBounds() : logger_(moveit::makeChildLogger("fix_start_state_bounds"))
+  CheckStartStateBounds() : logger_(moveit::makeChildLogger("check_start_state_bounds"))
   {
   }
 
@@ -74,119 +73,114 @@ class FixStartStateBounds : public planning_interface::PlanningRequestAdapter
 
   std::string getDescription() const override
   {
-    return "Fix Start State Bounds";
+    return std::string("CheckStartStateBounds");
   }
 
   bool adapt(const planning_scene::PlanningSceneConstPtr& planning_scene,
              planning_interface::MotionPlanRequest& req) const override
   {
     RCLCPP_DEBUG(logger_, "Running '%s'", getDescription().c_str());
 
-    // get the specified start state
+    // Get the specified start state
     moveit::core::RobotState start_state = planning_scene->getCurrentState();
     moveit::core::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state);
 
+    // Get joint models
     const std::vector<const moveit::core::JointModel*>& jmodels =
         planning_scene->getRobotModel()->hasJointModelGroup(req.group_name) ?
             planning_scene->getRobotModel()->getJointModelGroup(req.group_name)->getJointModels() :
             planning_scene->getRobotModel()->getJointModels();
 
-    bool change_req = false;
-    for (const moveit::core::JointModel* jm : jmodels)
+    // Read parameters
+    const auto params = param_listener_->get_params();
+
+    bool changed_req = false;
+    for (const moveit::core::JointModel* jmodel : jmodels)
     {
       // Check if we have a revolute, continuous joint. If we do, then we only need to make sure
-      // it is within de model's declared bounds (usually -Pi, Pi), since the values wrap around.
+      // it is within the model's declared bounds (usually -Pi, Pi), since the values wrap around.
       // It is possible that the encoder maintains values outside the range [-Pi, Pi], to inform
       // how many times the joint was wrapped. Because of this, we remember the offsets for continuous
       // joints, and we un-do them when the plan comes from the planner
-
-      if (jm->getType() == moveit::core::JointModel::REVOLUTE)
+      switch (jmodel->getType())
       {
-        if (static_cast<const moveit::core::RevoluteJointModel*>(jm)->isContinuous())
+        case moveit::core::JointModel::REVOLUTE:
         {
-          double initial = start_state.getJointPositions(jm)[0];
-          start_state.enforceBounds(jm);
-          double after = start_state.getJointPositions(jm)[0];
-          if (fabs(initial - after) > std::numeric_limits<double>::epsilon())
-            change_req = true;
+          if (static_cast<const moveit::core::RevoluteJointModel*>(jmodel)->isContinuous())
+          {
+            double initial = start_state.getJointPositions(jmodel)[0];
+            start_state.enforceBounds(jmodel);
+            double after = start_state.getJointPositions(jmodel)[0];
+            if (fabs(initial - after) > std::numeric_limits<double>::epsilon())
+            {
+              changed_req = true;
+            }
+          }
+          break;
         }
-      }
-      else
         // Normalize yaw; no offset needs to be remembered
-        if (jm->getType() == moveit::core::JointModel::PLANAR)
+        case moveit::core::JointModel::PLANAR:
         {
-          const double* p = start_state.getJointPositions(jm);
+          const double* p = start_state.getJointPositions(jmodel);
           double copy[3] = { p[0], p[1], p[2] };
-          if (static_cast<const moveit::core::PlanarJointModel*>(jm)->normalizeRotation(copy))
+          if (static_cast<const moveit::core::PlanarJointModel*>(jmodel)->normalizeRotation(copy))
           {
-            start_state.setJointPositions(jm, copy);
-            change_req = true;
+            start_state.setJointPositions(jmodel, copy);
+            changed_req = true;
           }
+          break;
         }
-        else
+        case moveit::core::JointModel::FLOATING:
+        {
           // Normalize quaternions
-          if (jm->getType() == moveit::core::JointModel::FLOATING)
+          const double* p = start_state.getJointPositions(jmodel);
+          double copy[7] = { p[0], p[1], p[2], p[3], p[4], p[5], p[6] };
+          if (static_cast<const moveit::core::FloatingJointModel*>(jmodel)->normalizeRotation(copy))
           {
-            const double* p = start_state.getJointPositions(jm);
-            double copy[7] = { p[0], p[1], p[2], p[3], p[4], p[5], p[6] };
-            if (static_cast<const moveit::core::FloatingJointModel*>(jm)->normalizeRotation(copy))
-            {
-              start_state.setJointPositions(jm, copy);
-              change_req = true;
-            }
+            start_state.setJointPositions(jmodel, copy);
+            changed_req = true;
           }
-    }
-
-    // Read parameters
-    const auto params = param_listener_->get_params();
-
-    // pointer to a prefix state we could possibly add, if we detect we have to make changes
-    moveit::core::RobotStatePtr prefix_state;
-    for (const moveit::core::JointModel* jmodel : jmodels)
-    {
-      if (!start_state.satisfiesBounds(jmodel))
-      {
-        if (start_state.satisfiesBounds(jmodel, params.start_state_max_bounds_error))
-        {
-          if (!prefix_state)
-            prefix_state = std::make_shared<moveit::core::RobotState>(start_state);
-          start_state.enforceBounds(jmodel);
-          change_req = true;
-          RCLCPP_INFO(logger_, "Starting state is just outside bounds (joint '%s'). Assuming within bounds.",
-                      jmodel->getName().c_str());
+          break;
         }
-        else
+        default:
         {
-          std::stringstream joint_values;
-          std::stringstream joint_bounds_low;
-          std::stringstream joint_bounds_hi;
-          const double* p = start_state.getJointPositions(jmodel);
-          for (std::size_t k = 0; k < jmodel->getVariableCount(); ++k)
-            joint_values << p[k] << ' ';
-          const moveit::core::JointModel::Bounds& b = jmodel->getVariableBounds();
-          for (const moveit::core::VariableBounds& variable_bounds : b)
+          if (!start_state.satisfiesBounds(jmodel))
           {
-            joint_bounds_low << variable_bounds.min_position_ << ' ';
-            joint_bounds_hi << variable_bounds.max_position_ << ' ';
+              std::stringstream joint_values;
+              std::stringstream joint_bounds_low;
+              std::stringstream joint_bounds_hi;
+              const double* p = start_state.getJointPositions(jmodel);
+              for (std::size_t k = 0; k < jmodel->getVariableCount(); ++k)
+              {
+                joint_values << p[k] << ' ';
+              }
+              const moveit::core::JointModel::Bounds& b = jmodel->getVariableBounds();
+              for (const moveit::core::VariableBounds& variable_bounds : b)
+              {
+                joint_bounds_low << variable_bounds.min_position_ << ' ';
+                joint_bounds_hi << variable_bounds.max_position_ << ' ';
+              }
+              RCLCPP_ERROR(
+                  logger_,
+                  "Joint '%s' from the starting state is outside bounds by: [%s] should be in "
+                  "the range [%s], [%s].",
+                  jmodel->getName().c_str(), joint_values.str().c_str(), joint_bounds_low.str().c_str(),
+                  joint_bounds_hi.str().c_str());
+            }
           }
-          RCLCPP_WARN(logger_,
-                      "Joint '%s' from the starting state is outside bounds by a significant margin: [%s] should be in "
-                      "the range [%s], [%s] but the error above the 'start_state_max_bounds_error' parameter "
-                      "(currently set to %f)",
-                      jmodel->getName().c_str(), joint_values.str().c_str(), joint_bounds_low.str().c_str(),
-                      joint_bounds_hi.str().c_str(), params.start_state_max_bounds_error);
         }
       }
     }
 
-    // if we made any changes, use them
-    if (change_req)
+    // If we made any changes, consider using them them
+    if (params.fix_start_state && changed_req)
     {
       RCLCPP_WARN(logger_, "Changing start state.");
       moveit::core::robotStateToRobotStateMsg(start_state, req.start_state);
+      return true;
     }
 
-    return true;
+    return !changed_req; /* If the request did not get updated the start state is within bounds */
   }
 
 private:
@@ -195,5 +189,5 @@ class FixStartStateBounds : public planning_interface::PlanningRequestAdapter
 };
 }  // namespace default_planning_request_adapters
 
-CLASS_LOADER_REGISTER_CLASS(default_planning_request_adapters::FixStartStateBounds,
+CLASS_LOADER_REGISTER_CLASS(default_planning_request_adapters::CheckStartStateBounds,
                             planning_interface::PlanningRequestAdapter)