moveit2
The MoveIt Motion Planning Framework for ROS 2.
Public Member Functions | List of all members
ompl_interface::ConstrainedPlanningStateValidityChecker Class Reference

A StateValidityChecker that can handle states of type ompl::base::ConstraintStateSpace::StateType. More...

#include <state_validity_checker.h>

Inheritance diagram for ompl_interface::ConstrainedPlanningStateValidityChecker:
Inheritance graph
[legend]
Collaboration diagram for ompl_interface::ConstrainedPlanningStateValidityChecker:
Collaboration graph
[legend]

Public Member Functions

 ConstrainedPlanningStateValidityChecker (const ModelBasedPlanningContext *planning_context)
 
bool isValid (const ompl::base::State *wrapped_state, bool verbose) const override
 Check validity for states of type ompl::base::ConstrainedStateSpace. More...
 
bool isValid (const ompl::base::State *wrapped_state, double &dist, bool verbose) const override
 
bool isValid (const ompl::base::State *state) const override
 
bool isValid (const ompl::base::State *state, double &dist) const override
 
bool isValid (const ompl::base::State *state, double &dist, ompl::base::State *, bool &) const override
 
virtual bool isValid (const ompl::base::State *state, bool verbose) const
 
virtual bool isValid (const ompl::base::State *state, double &dist, bool verbose) const
 
- Public Member Functions inherited from ompl_interface::StateValidityChecker
 StateValidityChecker (const ModelBasedPlanningContext *planning_context)
 
bool isValid (const ompl::base::State *state) const override
 
bool isValid (const ompl::base::State *state, double &dist) const override
 
bool isValid (const ompl::base::State *state, double &dist, ompl::base::State *, bool &) const override
 
virtual double cost (const ompl::base::State *state) const
 
double clearance (const ompl::base::State *state) const override
 
void setVerbose (bool flag)
 

Additional Inherited Members

- Protected Attributes inherited from ompl_interface::StateValidityChecker
const ModelBasedPlanningContextplanning_context_
 
std::string group_name_
 
TSStateStorage tss_
 
collision_detection::CollisionRequest collision_request_simple_
 
collision_detection::CollisionRequest collision_request_with_distance_
 
collision_detection::CollisionRequest collision_request_simple_verbose_
 
collision_detection::CollisionRequest collision_request_with_distance_verbose_
 
collision_detection::CollisionRequest collision_request_with_cost_
 
bool verbose_
 

Detailed Description

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.

Definition at line 120 of file state_validity_checker.h.

Constructor & Destructor Documentation

◆ ConstrainedPlanningStateValidityChecker()

ompl_interface::ConstrainedPlanningStateValidityChecker::ConstrainedPlanningStateValidityChecker ( const ModelBasedPlanningContext planning_context)
inline

Definition at line 126 of file state_validity_checker.h.

Member Function Documentation

◆ isValid() [1/7]

bool ompl_interface::StateValidityChecker::isValid
inlineoverride

Definition at line 65 of file state_validity_checker.h.

◆ isValid() [2/7]

bool ompl_interface::StateValidityChecker::isValid

Definition at line 81 of file state_validity_checker.cpp.

◆ isValid() [3/7]

bool ompl_interface::StateValidityChecker::isValid
inlineoverride

Definition at line 70 of file state_validity_checker.h.

◆ isValid() [4/7]

bool ompl_interface::StateValidityChecker::isValid

Definition at line 82 of file state_validity_checker.cpp.

◆ isValid() [5/7]

bool ompl_interface::StateValidityChecker::isValid
inlineoverride

Definition at line 75 of file state_validity_checker.h.

◆ isValid() [6/7]

bool ompl_interface::ConstrainedPlanningStateValidityChecker::isValid ( const ompl::base::State *  wrapped_state,
bool  verbose 
) const
overridevirtual

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);

Reimplemented from ompl_interface::StateValidityChecker.

Definition at line 214 of file state_validity_checker.cpp.

Here is the call graph for this function:

◆ isValid() [7/7]

bool ompl_interface::ConstrainedPlanningStateValidityChecker::isValid ( const ompl::base::State *  wrapped_state,
double &  dist,
bool  verbose 
) const
overridevirtual

Reimplemented from ompl_interface::StateValidityChecker.

Definition at line 268 of file state_validity_checker.cpp.

Here is the call graph for this function:

The documentation for this class was generated from the following files: