#include <model_based_planning_context.h>
Public Member Functions | |
bool | benchmark (double timeout, unsigned int count, const std::string &filename="") |
virtual void | clear () |
virtual void | configure () |
void | convertPath (const og::PathGeometric &pg, robot_trajectory::RobotTrajectory &traj) const |
const robot_state::RobotState & | getCompleteInitialRobotState () const |
const constraint_samplers::ConstraintSamplerManagerPtr & | getConstraintSamplerManager () |
const robot_model::JointModelGroup * | getJointModelGroup () const |
double | getLastPlanTime () const |
double | getLastSimplifyTime () const |
unsigned int | getMaximumGoalSamples () const |
unsigned int | getMaximumGoalSamplingAttempts () const |
unsigned int | getMaximumPlanningThreads () const |
double | getMaximumSolutionSegmentLength () const |
unsigned int | getMaximumStateSamplingAttempts () const |
unsigned int | getMinimumWaypointCount () const |
const ot::Benchmark & | getOMPLBenchmark () const |
ot::Benchmark & | getOMPLBenchmark () |
const og::SimpleSetupPtr & | getOMPLSimpleSetup () const |
og::SimpleSetupPtr & | getOMPLSimpleSetup () |
const ModelBasedStateSpacePtr & | getOMPLStateSpace () const |
const kinematic_constraints::KinematicConstraintSetPtr & | getPathConstraints () const |
const robot_model::RobotModelConstPtr & | getRobotModel () const |
bool | getSolutionPath (robot_trajectory::RobotTrajectory &traj) const |
const ModelBasedPlanningContextSpecification & | getSpecification () const |
const std::map< std::string, std::string > & | getSpecificationConfig () const |
void | interpolateSolution () |
ModelBasedPlanningContext (const std::string &name, const ModelBasedPlanningContextSpecification &spec) | |
void | setCompleteInitialState (const robot_state::RobotState &complete_initial_robot_state) |
void | setConstraintSamplerManager (const constraint_samplers::ConstraintSamplerManagerPtr &csm) |
void | setConstraintsApproximations (const ConstraintsLibraryConstPtr &constraints_library) |
bool | setGoalConstraints (const std::vector< moveit_msgs::Constraints > &goal_constraints, const moveit_msgs::Constraints &path_constraints, moveit_msgs::MoveItErrorCodes *error) |
void | setMaximumGoalSamples (unsigned int max_goal_samples) |
void | setMaximumGoalSamplingAttempts (unsigned int max_goal_sampling_attempts) |
void | setMaximumPlanningThreads (unsigned int max_planning_threads) |
void | setMaximumSolutionSegmentLength (double mssl) |
void | setMaximumStateSamplingAttempts (unsigned int max_state_sampling_attempts) |
void | setMinimumWaypointCount (unsigned int mwc) |
Get the minimum number of waypoints along the solution path. | |
bool | setPathConstraints (const moveit_msgs::Constraints &path_constraints, moveit_msgs::MoveItErrorCodes *error) |
void | setPlanningVolume (const moveit_msgs::WorkspaceParameters &wparams) |
void | setProjectionEvaluator (const std::string &peval) |
void | setSpecificationConfig (const std::map< std::string, std::string > &config) |
void | setVerboseStateValidityChecks (bool flag) |
void | simplifySolution (double timeout) |
bool | simplifySolutions () const |
void | simplifySolutions (bool flag) |
virtual bool | solve (planning_interface::MotionPlanResponse &res) |
virtual bool | solve (planning_interface::MotionPlanDetailedResponse &res) |
bool | solve (double timeout, unsigned int count) |
virtual bool | terminate () |
bool | useStateValidityCache () const |
void | useStateValidityCache (bool flag) |
virtual | ~ModelBasedPlanningContext () |
Protected Member Functions | |
virtual ob::StateSamplerPtr | allocPathConstrainedSampler (const ompl::base::StateSpace *ss) const |
virtual ob::GoalPtr | constructGoal () |
virtual ob::ProjectionEvaluatorPtr | getProjectionEvaluator (const std::string &peval) const |
void | postSolve () |
void | preSolve () |
void | registerTerminationCondition (const ob::PlannerTerminationCondition &ptc) |
void | startSampling () |
void | stopSampling () |
void | unregisterTerminationCondition () |
virtual void | useConfig () |
Protected Attributes | |
robot_state::RobotState | complete_initial_robot_state_ |
std::vector < kinematic_constraints::KinematicConstraintSetPtr > | goal_constraints_ |
double | last_plan_time_ |
the time spent computing the last plan | |
double | last_simplify_time_ |
the time spent simplifying the last plan | |
unsigned int | max_goal_samples_ |
maximum number of valid states to store in the goal region for any planning request (when such sampling is possible) | |
unsigned int | max_goal_sampling_attempts_ |
maximum number of attempts to be made at sampling a goal states | |
unsigned int | max_planning_threads_ |
when planning in parallel, this is the maximum number of threads to use at one time | |
double | max_solution_segment_length_ |
the maximum length that is allowed for segments that make up the motion plan; by default this is 1% from the extent of the space | |
unsigned int | max_state_sampling_attempts_ |
maximum number of attempts to be made at sampling a state when attempting to find valid states that satisfy some set of constraints | |
unsigned int | minimum_waypoint_count_ |
the minimum number of points to include on the solution path (interpolation is used to reach this number, if needed) | |
ot::Benchmark | ompl_benchmark_ |
the OMPL tool for benchmarking planners | |
ot::ParallelPlan | ompl_parallel_plan_ |
tool used to compute multiple plans in parallel; this uses the problem definition maintained by ompl_simple_setup_ | |
og::SimpleSetupPtr | ompl_simple_setup_ |
the OMPL planning context; this contains the problem definition and the planner used | |
kinematic_constraints::KinematicConstraintSetPtr | path_constraints_ |
moveit_msgs::Constraints | path_constraints_msg_ |
const ob::PlannerTerminationCondition * | ptc_ |
boost::mutex | ptc_lock_ |
bool | simplify_solutions_ |
std::vector< int > | space_signature_ |
ModelBasedPlanningContextSpecification | spec_ |
bool | use_state_validity_cache_ |
Definition at line 78 of file model_based_planning_context.h.
ompl_interface::ModelBasedPlanningContext::ModelBasedPlanningContext | ( | const std::string & | name, |
const ModelBasedPlanningContextSpecification & | spec | ||
) |
Definition at line 54 of file model_based_planning_context.cpp.
virtual ompl_interface::ModelBasedPlanningContext::~ModelBasedPlanningContext | ( | ) | [inline, virtual] |
Definition at line 84 of file model_based_planning_context.h.
ompl::base::StateSamplerPtr ompl_interface::ModelBasedPlanningContext::allocPathConstrainedSampler | ( | const ompl::base::StateSpace * | ss | ) | const [protected, virtual] |
Definition at line 135 of file model_based_planning_context.cpp.
bool ompl_interface::ModelBasedPlanningContext::benchmark | ( | double | timeout, |
unsigned int | count, | ||
const std::string & | filename = "" |
||
) |
Definition at line 382 of file model_based_planning_context.cpp.
void ompl_interface::ModelBasedPlanningContext::clear | ( | void | ) | [virtual] |
Implements planning_interface::PlanningContext.
Definition at line 329 of file model_based_planning_context.cpp.
void ompl_interface::ModelBasedPlanningContext::configure | ( | ) | [virtual] |
Definition at line 179 of file model_based_planning_context.cpp.
ompl::base::GoalPtr ompl_interface::ModelBasedPlanningContext::constructGoal | ( | ) | [protected, virtual] |
Definition at line 299 of file model_based_planning_context.cpp.
void ompl_interface::ModelBasedPlanningContext::convertPath | ( | const og::PathGeometric & | pg, |
robot_trajectory::RobotTrajectory & | traj | ||
) | const |
Definition at line 274 of file model_based_planning_context.cpp.
const robot_state::RobotState& ompl_interface::ModelBasedPlanningContext::getCompleteInitialRobotState | ( | ) | const [inline] |
Definition at line 119 of file model_based_planning_context.h.
const constraint_samplers::ConstraintSamplerManagerPtr& ompl_interface::ModelBasedPlanningContext::getConstraintSamplerManager | ( | ) | [inline] |
Definition at line 225 of file model_based_planning_context.h.
const robot_model::JointModelGroup* ompl_interface::ModelBasedPlanningContext::getJointModelGroup | ( | ) | const [inline] |
Definition at line 114 of file model_based_planning_context.h.
double ompl_interface::ModelBasedPlanningContext::getLastPlanTime | ( | ) | const [inline] |
Definition at line 288 of file model_based_planning_context.h.
double ompl_interface::ModelBasedPlanningContext::getLastSimplifyTime | ( | ) | const [inline] |
Definition at line 294 of file model_based_planning_context.h.
unsigned int ompl_interface::ModelBasedPlanningContext::getMaximumGoalSamples | ( | ) | const [inline] |
Definition at line 179 of file model_based_planning_context.h.
unsigned int ompl_interface::ModelBasedPlanningContext::getMaximumGoalSamplingAttempts | ( | ) | const [inline] |
Definition at line 167 of file model_based_planning_context.h.
unsigned int ompl_interface::ModelBasedPlanningContext::getMaximumPlanningThreads | ( | ) | const [inline] |
Definition at line 191 of file model_based_planning_context.h.
double ompl_interface::ModelBasedPlanningContext::getMaximumSolutionSegmentLength | ( | ) | const [inline] |
Definition at line 203 of file model_based_planning_context.h.
unsigned int ompl_interface::ModelBasedPlanningContext::getMaximumStateSamplingAttempts | ( | ) | const [inline] |
Definition at line 155 of file model_based_planning_context.h.
unsigned int ompl_interface::ModelBasedPlanningContext::getMinimumWaypointCount | ( | ) | const [inline] |
Definition at line 214 of file model_based_planning_context.h.
const ot::Benchmark& ompl_interface::ModelBasedPlanningContext::getOMPLBenchmark | ( | ) | const [inline] |
Definition at line 139 of file model_based_planning_context.h.
ot::Benchmark& ompl_interface::ModelBasedPlanningContext::getOMPLBenchmark | ( | ) | [inline] |
Definition at line 144 of file model_based_planning_context.h.
const og::SimpleSetupPtr& ompl_interface::ModelBasedPlanningContext::getOMPLSimpleSetup | ( | ) | const [inline] |
Definition at line 129 of file model_based_planning_context.h.
og::SimpleSetupPtr& ompl_interface::ModelBasedPlanningContext::getOMPLSimpleSetup | ( | ) | [inline] |
Definition at line 134 of file model_based_planning_context.h.
const ModelBasedStateSpacePtr& ompl_interface::ModelBasedPlanningContext::getOMPLStateSpace | ( | ) | const [inline] |
Definition at line 124 of file model_based_planning_context.h.
const kinematic_constraints::KinematicConstraintSetPtr& ompl_interface::ModelBasedPlanningContext::getPathConstraints | ( | ) | const [inline] |
Definition at line 149 of file model_based_planning_context.h.
ompl::base::ProjectionEvaluatorPtr ompl_interface::ModelBasedPlanningContext::getProjectionEvaluator | ( | const std::string & | peval | ) | const [protected, virtual] |
Definition at line 89 of file model_based_planning_context.cpp.
const robot_model::RobotModelConstPtr& ompl_interface::ModelBasedPlanningContext::getRobotModel | ( | ) | const [inline] |
Definition at line 109 of file model_based_planning_context.h.
bool ompl_interface::ModelBasedPlanningContext::getSolutionPath | ( | robot_trajectory::RobotTrajectory & | traj | ) | const |
Definition at line 284 of file model_based_planning_context.cpp.
const ModelBasedPlanningContextSpecification& ompl_interface::ModelBasedPlanningContext::getSpecification | ( | ) | const [inline] |
Definition at line 94 of file model_based_planning_context.h.
const std::map<std::string, std::string>& ompl_interface::ModelBasedPlanningContext::getSpecificationConfig | ( | ) | const [inline] |
Definition at line 99 of file model_based_planning_context.h.
Definition at line 265 of file model_based_planning_context.cpp.
void ompl_interface::ModelBasedPlanningContext::postSolve | ( | ) | [protected] |
Definition at line 430 of file model_based_planning_context.cpp.
void ompl_interface::ModelBasedPlanningContext::preSolve | ( | ) | [protected] |
Definition at line 419 of file model_based_planning_context.cpp.
void ompl_interface::ModelBasedPlanningContext::registerTerminationCondition | ( | const ob::PlannerTerminationCondition & | ptc | ) | [protected] |
Definition at line 593 of file model_based_planning_context.cpp.
void ompl_interface::ModelBasedPlanningContext::setCompleteInitialState | ( | const robot_state::RobotState & | complete_initial_robot_state | ) |
Definition at line 324 of file model_based_planning_context.cpp.
void ompl_interface::ModelBasedPlanningContext::setConstraintSamplerManager | ( | const constraint_samplers::ConstraintSamplerManagerPtr & | csm | ) | [inline] |
Definition at line 230 of file model_based_planning_context.h.
void ompl_interface::ModelBasedPlanningContext::setConstraintsApproximations | ( | const ConstraintsLibraryConstPtr & | constraints_library | ) | [inline] |
Definition at line 249 of file model_based_planning_context.h.
bool ompl_interface::ModelBasedPlanningContext::setGoalConstraints | ( | const std::vector< moveit_msgs::Constraints > & | goal_constraints, |
const moveit_msgs::Constraints & | path_constraints, | ||
moveit_msgs::MoveItErrorCodes * | error | ||
) |
Definition at line 351 of file model_based_planning_context.cpp.
void ompl_interface::ModelBasedPlanningContext::setMaximumGoalSamples | ( | unsigned int | max_goal_samples | ) | [inline] |
Definition at line 185 of file model_based_planning_context.h.
void ompl_interface::ModelBasedPlanningContext::setMaximumGoalSamplingAttempts | ( | unsigned int | max_goal_sampling_attempts | ) | [inline] |
Definition at line 173 of file model_based_planning_context.h.
void ompl_interface::ModelBasedPlanningContext::setMaximumPlanningThreads | ( | unsigned int | max_planning_threads | ) | [inline] |
Definition at line 197 of file model_based_planning_context.h.
void ompl_interface::ModelBasedPlanningContext::setMaximumSolutionSegmentLength | ( | double | mssl | ) | [inline] |
Definition at line 209 of file model_based_planning_context.h.
void ompl_interface::ModelBasedPlanningContext::setMaximumStateSamplingAttempts | ( | unsigned int | max_state_sampling_attempts | ) | [inline] |
Definition at line 161 of file model_based_planning_context.h.
void ompl_interface::ModelBasedPlanningContext::setMinimumWaypointCount | ( | unsigned int | mwc | ) | [inline] |
Get the minimum number of waypoints along the solution path.
Definition at line 220 of file model_based_planning_context.h.
bool ompl_interface::ModelBasedPlanningContext::setPathConstraints | ( | const moveit_msgs::Constraints & | path_constraints, |
moveit_msgs::MoveItErrorCodes * | error | ||
) |
Definition at line 340 of file model_based_planning_context.cpp.
void ompl_interface::ModelBasedPlanningContext::setPlanningVolume | ( | const moveit_msgs::WorkspaceParameters & | wparams | ) |
Definition at line 244 of file model_based_planning_context.cpp.
void ompl_interface::ModelBasedPlanningContext::setProjectionEvaluator | ( | const std::string & | peval | ) |
Definition at line 77 of file model_based_planning_context.cpp.
void ompl_interface::ModelBasedPlanningContext::setSpecificationConfig | ( | const std::map< std::string, std::string > & | config | ) | [inline] |
Definition at line 104 of file model_based_planning_context.h.
void ompl_interface::ModelBasedPlanningContext::setVerboseStateValidityChecks | ( | bool | flag | ) |
Definition at line 293 of file model_based_planning_context.cpp.
void ompl_interface::ModelBasedPlanningContext::simplifySolution | ( | double | timeout | ) |
Definition at line 259 of file model_based_planning_context.cpp.
bool ompl_interface::ModelBasedPlanningContext::simplifySolutions | ( | ) | const [inline] |
Definition at line 264 of file model_based_planning_context.h.
void ompl_interface::ModelBasedPlanningContext::simplifySolutions | ( | bool | flag | ) | [inline] |
Definition at line 269 of file model_based_planning_context.h.
bool ompl_interface::ModelBasedPlanningContext::solve | ( | planning_interface::MotionPlanResponse & | res | ) | [virtual] |
Implements planning_interface::PlanningContext.
Definition at line 441 of file model_based_planning_context.cpp.
bool ompl_interface::ModelBasedPlanningContext::solve | ( | planning_interface::MotionPlanDetailedResponse & | res | ) | [virtual] |
Implements planning_interface::PlanningContext.
Definition at line 470 of file model_based_planning_context.cpp.
bool ompl_interface::ModelBasedPlanningContext::solve | ( | double | timeout, |
unsigned int | count | ||
) |
Definition at line 516 of file model_based_planning_context.cpp.
void ompl_interface::ModelBasedPlanningContext::startSampling | ( | ) | [protected] |
Definition at line 399 of file model_based_planning_context.cpp.
void ompl_interface::ModelBasedPlanningContext::stopSampling | ( | ) | [protected] |
Definition at line 409 of file model_based_planning_context.cpp.
bool ompl_interface::ModelBasedPlanningContext::terminate | ( | ) | [virtual] |
Implements planning_interface::PlanningContext.
Definition at line 605 of file model_based_planning_context.cpp.
void ompl_interface::ModelBasedPlanningContext::unregisterTerminationCondition | ( | ) | [protected] |
Definition at line 599 of file model_based_planning_context.cpp.
void ompl_interface::ModelBasedPlanningContext::useConfig | ( | ) | [protected, virtual] |
Definition at line 202 of file model_based_planning_context.cpp.
bool ompl_interface::ModelBasedPlanningContext::useStateValidityCache | ( | ) | const [inline] |
Definition at line 254 of file model_based_planning_context.h.
void ompl_interface::ModelBasedPlanningContext::useStateValidityCache | ( | bool | flag | ) | [inline] |
Definition at line 259 of file model_based_planning_context.h.
robot_state::RobotState ompl_interface::ModelBasedPlanningContext::complete_initial_robot_state_ [protected] |
Definition at line 331 of file model_based_planning_context.h.
std::vector<kinematic_constraints::KinematicConstraintSetPtr> ompl_interface::ModelBasedPlanningContext::goal_constraints_ [protected] |
Definition at line 346 of file model_based_planning_context.h.
double ompl_interface::ModelBasedPlanningContext::last_plan_time_ [protected] |
the time spent computing the last plan
Definition at line 352 of file model_based_planning_context.h.
double ompl_interface::ModelBasedPlanningContext::last_simplify_time_ [protected] |
the time spent simplifying the last plan
Definition at line 355 of file model_based_planning_context.h.
unsigned int ompl_interface::ModelBasedPlanningContext::max_goal_samples_ [protected] |
maximum number of valid states to store in the goal region for any planning request (when such sampling is possible)
Definition at line 358 of file model_based_planning_context.h.
unsigned int ompl_interface::ModelBasedPlanningContext::max_goal_sampling_attempts_ [protected] |
maximum number of attempts to be made at sampling a goal states
Definition at line 364 of file model_based_planning_context.h.
unsigned int ompl_interface::ModelBasedPlanningContext::max_planning_threads_ [protected] |
when planning in parallel, this is the maximum number of threads to use at one time
Definition at line 367 of file model_based_planning_context.h.
double ompl_interface::ModelBasedPlanningContext::max_solution_segment_length_ [protected] |
the maximum length that is allowed for segments that make up the motion plan; by default this is 1% from the extent of the space
Definition at line 370 of file model_based_planning_context.h.
unsigned int ompl_interface::ModelBasedPlanningContext::max_state_sampling_attempts_ [protected] |
maximum number of attempts to be made at sampling a state when attempting to find valid states that satisfy some set of constraints
Definition at line 361 of file model_based_planning_context.h.
unsigned int ompl_interface::ModelBasedPlanningContext::minimum_waypoint_count_ [protected] |
the minimum number of points to include on the solution path (interpolation is used to reach this number, if needed)
Definition at line 373 of file model_based_planning_context.h.
ot::Benchmark ompl_interface::ModelBasedPlanningContext::ompl_benchmark_ [protected] |
the OMPL tool for benchmarking planners
Definition at line 337 of file model_based_planning_context.h.
ot::ParallelPlan ompl_interface::ModelBasedPlanningContext::ompl_parallel_plan_ [protected] |
tool used to compute multiple plans in parallel; this uses the problem definition maintained by ompl_simple_setup_
Definition at line 340 of file model_based_planning_context.h.
og::SimpleSetupPtr ompl_interface::ModelBasedPlanningContext::ompl_simple_setup_ [protected] |
the OMPL planning context; this contains the problem definition and the planner used
Definition at line 334 of file model_based_planning_context.h.
kinematic_constraints::KinematicConstraintSetPtr ompl_interface::ModelBasedPlanningContext::path_constraints_ [protected] |
Definition at line 344 of file model_based_planning_context.h.
moveit_msgs::Constraints ompl_interface::ModelBasedPlanningContext::path_constraints_msg_ [protected] |
Definition at line 345 of file model_based_planning_context.h.
const ob::PlannerTerminationCondition* ompl_interface::ModelBasedPlanningContext::ptc_ [protected] |
Definition at line 348 of file model_based_planning_context.h.
boost::mutex ompl_interface::ModelBasedPlanningContext::ptc_lock_ [protected] |
Definition at line 349 of file model_based_planning_context.h.
bool ompl_interface::ModelBasedPlanningContext::simplify_solutions_ [protected] |
Definition at line 377 of file model_based_planning_context.h.
std::vector<int> ompl_interface::ModelBasedPlanningContext::space_signature_ [protected] |
Definition at line 342 of file model_based_planning_context.h.
Definition at line 329 of file model_based_planning_context.h.
bool ompl_interface::ModelBasedPlanningContext::use_state_validity_cache_ [protected] |
Definition at line 375 of file model_based_planning_context.h.