(ompl interface) add new state validity checker for ompl constrained planning

Author: JeroenDM

moveit_planners/ompl/ompl_interface/CMakeLists.txt

@@ -54,4 +54,8 @@ if(CATKIN_ENABLE_TESTING)
   catkin_add_gtest(test_constrained_planning_state_space test/test_constrained_planning_state_space.cpp)
   target_link_libraries(test_constrained_planning_state_space ${MOVEIT_LIB_NAME} ${OMPL_LIBRARIES} ${catkin_LIBRARIES})
   set_target_properties(test_constrained_planning_state_space PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}")
+
+  catkin_add_gtest(test_constrained_state_validity_checker test/test_constrained_state_validity_checker.cpp)
+  target_link_libraries(test_constrained_state_validity_checker ${MOVEIT_LIB_NAME} ${OMPL_LIBRARIES} ${catkin_LIBRARIES})
+  set_target_properties(test_constrained_state_validity_checker PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}")
 endif()

moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h

@@ -32,7 +32,7 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
 
-/* Author: Ioan Sucan */
+/* Authors: Ioan Sucan, Jeroen De Maeyer */
 
 #pragma once
 
@@ -61,8 +61,8 @@ class StateValidityChecker : public ompl::base::StateValidityChecker
     return isValid(state, dist, verbose_);
   }
 
-  bool isValid(const ompl::base::State* state, bool verbose) const;
-  bool isValid(const ompl::base::State* state, double& dist, bool verbose) const;
+  virtual bool isValid(const ompl::base::State* state, bool verbose) const;
+  virtual bool isValid(const ompl::base::State* state, double& dist, bool verbose) const;
 
   virtual double cost(const ompl::base::State* state) const;
   double clearance(const ompl::base::State* state) const override;
@@ -81,4 +81,60 @@ class StateValidityChecker : public ompl::base::StateValidityChecker
   collision_detection::CollisionRequest collision_request_with_cost_;
   bool verbose_;
 };
+
+/** \brief A StateValidityChecker that can handle states of type `ompl::base::ConstraintStateSpace::StateType`.
+ *
+ * We cannot not just cast the states and pass them to the isValid version of the parent class, because inside the
+ * state-validity checker, the line:
+ *
+ *     planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, state);
+ *
+ * requires the state type to be of the constrained state space, while:
+ *
+ *     state->as<ModelBasedStateSpace::StateType>()->isValidityKnown()
+ *
+ * requires accessing the underlying state by calling `getState()` on the constrained state space state.
+ * Therefore this class implements specific versions of the isValid methods.
+ *
+ * We still check the path constraints, because not all states sampled by the constrained state space
+ * satisfy the constraints unfortunately. This is a complicated issue. For more details see:
+ * https://github.com/ros-planning/moveit/issues/2092#issuecomment-669911722.
+ **/
+class ConstrainedPlanningStateValidityChecker : public StateValidityChecker
+{
+public:
+  using StateValidityChecker::isValid;
+
+public:
+  ConstrainedPlanningStateValidityChecker(const ModelBasedPlanningContext* planning_context)
+    : StateValidityChecker(planning_context)
+  {
+  }
+
+  /** \brief Check validity for states of type ompl::base::ConstrainedStateSpace
+   *
+   * This state type is special in that it "wraps" around a normal state,
+   * which can be accessed by the getState() method. In this class we assume that this state,
+   * is of type `ompl_interface::ConstrainedPlanningStateSpace`, which inherits from
+   * `ompl_interface::ModelBasedStateSpace`.
+   *
+   * (For the actual implementation of this, look at the ompl::base::WrapperStateSpace.)
+   *
+   * Code sample that can be used to check all the assumptions:
+   *
+   *    #include <moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space.h>
+   *    #include <ompl/base/ConstrainedSpaceInformation.h>
+   *
+   *    // the code below should be pasted at the top of the isValid method
+   *    auto css = dynamic_cast<const ompl::base::ConstrainedStateSpace::StateType*>(wrapped_state);
+   *    assert(css != nullptr);
+   *    auto cpss = dynamic_cast<const ConstrainedPlanningStateSpace*>(planning_context_->getOMPLStateSpace().get());
+   *    assert(cpss != nullptr);
+   *    auto cssi = dynamic_cast<const ompl::base::ConstrainedSpaceInformation*>(si_);
+   *    assert(cssi != nullptr);
+   *
+   **/
+  bool isValid(const ompl::base::State* wrapped_state, bool verbose) const override;
+  bool isValid(const ompl::base::State* wrapped_state, double& dist, bool verbose) const override;
+};
 }  // namespace ompl_interface

moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp

@@ -32,13 +32,15 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
 
-/* Author: Ioan Sucan */
+/* Authors: Ioan Sucan, Jeroen De Maeyer */
 
 #include <moveit/ompl_interface/detail/state_validity_checker.h>
 #include <moveit/ompl_interface/model_based_planning_context.h>
 #include <moveit/profiler/profiler.h>
 #include <ros/ros.h>
 
+#include <ompl/base/spaces/constraint/ConstrainedStateSpace.h>
+
 namespace ompl_interface
 {
 constexpr char LOGNAME[] = "state_validity_checker";
@@ -194,4 +196,112 @@ double ompl_interface::StateValidityChecker::clearance(const ompl::base::State*
   collision_detection::CollisionResult res;
   planning_context_->getPlanningScene()->checkCollision(collision_request_with_distance_, res, *robot_state);
   return res.collision ? 0.0 : (res.distance < 0.0 ? std::numeric_limits<double>::infinity() : res.distance);
+}
+
+/*******************************************
+ * Constrained Planning StateValidityChecker
+ * *****************************************/
+
+bool ompl_interface::ConstrainedPlanningStateValidityChecker::isValid(const ompl::base::State* wrapped_state,
+                                                                      bool verbose) const
+{
+  // Unwrap the state from a ConstrainedStateSpace::StateType
+  auto state = wrapped_state->as<ompl::base::ConstrainedStateSpace::StateType>()->getState();
+
+  // Use cached validity if it is available
+  if (state->as<ModelBasedStateSpace::StateType>()->isValidityKnown())
+    return state->as<ModelBasedStateSpace::StateType>()->isMarkedValid();
+
+  // do not use the unwrapped state here, as satisfiesBounds expects a state of type ConstrainedStateSpace::StateType
+  if (!si_->satisfiesBounds(wrapped_state))
+  {
+    if (verbose)
+      ROS_INFO_NAMED(LOGNAME, "State outside bounds");
+    const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid();
+    return false;
+  }
+
+  moveit::core::RobotState* robot_state = tss_.getStateStorage();
+  // do not use the unwrapped state here, as copyToRobotState expects a state of type ConstrainedStateSpace::StateType
+  planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, wrapped_state);
+
+  // check path constraints
+  const kinematic_constraints::KinematicConstraintSetPtr& kset = planning_context_->getPathConstraints();
+  if (kset && !kset->decide(*robot_state, verbose).satisfied)
+  {
+    const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid();
+    return false;
+  }
+
+  // check feasibility
+  if (!planning_context_->getPlanningScene()->isStateFeasible(*robot_state, verbose))
+  {
+    const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid();
+    return false;
+  }
+
+  // check collision avoidance
+  collision_detection::CollisionResult res;
+  planning_context_->getPlanningScene()->checkCollision(
+      verbose ? collision_request_simple_verbose_ : collision_request_simple_, res, *robot_state);
+  if (!res.collision)
+  {
+    const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markValid();
+  }
+  else
+  {
+    const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid();
+  }
+  return !res.collision;
+}
+
+bool ompl_interface::ConstrainedPlanningStateValidityChecker::isValid(const ompl::base::State* wrapped_state,
+                                                                      double& dist, bool verbose) const
+{
+  // Unwrap the state from a ConstrainedStateSpace::StateType
+  auto state = wrapped_state->as<ompl::base::ConstrainedStateSpace::StateType>()->getState();
+
+  // Use cached validity and distance if they are available
+  if (state->as<ModelBasedStateSpace::StateType>()->isValidityKnown() &&
+      state->as<ModelBasedStateSpace::StateType>()->isGoalDistanceKnown())
+  {
+    dist = state->as<ModelBasedStateSpace::StateType>()->distance;
+    return state->as<ModelBasedStateSpace::StateType>()->isMarkedValid();
+  }
+
+  // do not use the unwrapped state here, as satisfiesBounds expects a state of type ConstrainedStateSpace::StateType
+  if (!si_->satisfiesBounds(wrapped_state))
+  {
+    if (verbose)
+      ROS_INFO_NAMED(LOGNAME, "State outside bounds");
+    const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid(0.0);
+    return false;
+  }
+
+  moveit::core::RobotState* robot_state = tss_.getStateStorage();
+
+  // do not use the unwrapped state here, as copyToRobotState expects a state of type ConstrainedStateSpace::StateType
+  planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, wrapped_state);
+
+  // check path constraints
+  const kinematic_constraints::KinematicConstraintSetPtr& kset = planning_context_->getPathConstraints();
+  if (kset && !kset->decide(*robot_state, verbose).satisfied)
+  {
+    const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid();
+    return false;
+  }
+
+  // check feasibility
+  if (!planning_context_->getPlanningScene()->isStateFeasible(*robot_state, verbose))
+  {
+    dist = 0.0;
+    return false;
+  }
+
+  // check collision avoidance
+  collision_detection::CollisionResult res;
+  planning_context_->getPlanningScene()->checkCollision(
+      verbose ? collision_request_with_distance_verbose_ : collision_request_with_distance_, res, *robot_state);
+  dist = res.distance;
+  return !res.collision;
 }