ompl::app::SE3MultiRigidBodyPlanning Class Reference
Wrapper for ompl::app::RigidBodyPlanning that plans for multiple rigid bodies in SE3. More...
#include <omplapp/apps/SE3MultiRigidBodyPlanning.h>
Inheritance diagram for ompl::app::SE3MultiRigidBodyPlanning:
Public Member Functions | |
SE3MultiRigidBodyPlanning (unsigned int n) | |
Constructs an instance of multiple rigid bodies for 3D geometric planning. n is the number of independent bodies in SE(3) | |
base::ScopedState | getDefaultStartState () const override |
Constructs the default start state where all robots begin at their geometric center. If robots are all using the same mesh, this state is not likely to be valid. | |
void | inferEnvironmentBounds () override |
void | inferProblemDefinitionBounds () override |
bool | isSelfCollisionEnabled () const override |
base::ScopedState | getFullStateFromGeometricComponent (const base::ScopedState<> &state) const override |
virtual const base::StateSpacePtr & | getGeometricComponentStateSpace (unsigned int index) const |
Returns the state space corresponding for the indexth rigid body. | |
const base::StateSpacePtr & | getGeometricComponentStateSpace () const override |
unsigned int | getRobotCount () const override |
Public Member Functions inherited from ompl::app::AppBase< AppType::GEOMETRIC > | |
AppBase (const typename AppTypeSelector< T >::SpaceType &space, MotionModel model) | |
AppType | getAppType () |
AppType | getAppType () |
const std::string & | getName () |
virtual bool | isSelfCollisionEnabled () const=0 |
virtual base::ScopedState | getDefaultStartState () const=0 |
virtual base::ScopedState | getGeometricComponentState (const base::ScopedState<> &state, unsigned int index) const |
virtual const base::StateSpacePtr & | getGeometricComponentStateSpace () const=0 |
virtual unsigned int | getRobotCount () const=0 |
GeometricStateExtractor | getGeometricStateExtractor () const |
void | setup () override |
This method will create the necessary classes for planning. The solve() method will call this function automatically. | |
void | setOptimizationObjectiveAndThreshold (const std::string &objective, double threshold) |
Convenience function for the omplapp GUI. The objective can be one of: "length", "max min clearance", or "mechanical work". | |
control::DecompositionPtr | allocDecomposition () |
Public Member Functions inherited from ompl::geometric::SimpleSetup | |
SimpleSetup (const base::SpaceInformationPtr &si) | |
Constructor needs the state space used for planning. | |
SimpleSetup (const base::StateSpacePtr &space) | |
Constructor needs the state space used for planning. | |
const base::SpaceInformationPtr & | getSpaceInformation () const |
Get the current instance of the space information. | |
const base::ProblemDefinitionPtr & | getProblemDefinition () const |
Get the current instance of the problem definition. | |
base::ProblemDefinitionPtr & | getProblemDefinition () |
Get the current instance of the problem definition. | |
const base::StateSpacePtr & | getStateSpace () const |
Get the current instance of the state space. | |
const base::StateValidityCheckerPtr & | getStateValidityChecker () const |
Get the current instance of the state validity checker. | |
const base::GoalPtr & | getGoal () const |
Get the current goal definition. | |
const base::PlannerPtr & | getPlanner () const |
Get the current planner. | |
const base::PlannerAllocator & | getPlannerAllocator () const |
Get the planner allocator. | |
const PathSimplifierPtr & | getPathSimplifier () const |
Get the path simplifier. | |
PathSimplifierPtr & | getPathSimplifier () |
Get the path simplifier. | |
const base::OptimizationObjectivePtr & | getOptimizationObjective () const |
Get the optimization objective to use. | |
bool | haveExactSolutionPath () const |
Return true if a solution path is available (previous call to solve() was successful) and the solution is exact (not approximate) | |
bool | haveSolutionPath () const |
Return true if a solution path is available (previous call to solve() was successful). The solution may be approximate. | |
const std::string | getSolutionPlannerName () const |
Get the best solution's planer name. Throw an exception if no solution is available. | |
PathGeometric & | getSolutionPath () const |
Get the solution path. Throw an exception if no solution is available. | |
void | getPlannerData (base::PlannerData &pd) const |
Get information about the exploration data structure the motion planner used. | |
void | setStateValidityChecker (const base::StateValidityCheckerPtr &svc) |
Set the state validity checker to use. | |
void | setStateValidityChecker (const base::StateValidityCheckerFn &svc) |
Set the state validity checker to use. | |
void | setOptimizationObjective (const base::OptimizationObjectivePtr &optimizationObjective) |
Set the optimization objective to use. | |
void | setStartAndGoalStates (const base::ScopedState<> &start, const base::ScopedState<> &goal, double threshold=std::numeric_limits< double >::epsilon()) |
Set the start and goal states to use. | |
void | addStartState (const base::ScopedState<> &state) |
Add a starting state for planning. This call is not needed if setStartAndGoalStates() has been called. | |
void | clearStartStates () |
Clear the currently set starting states. | |
void | setStartState (const base::ScopedState<> &state) |
Clear the currently set starting states and add state as the starting state. | |
void | setGoalState (const base::ScopedState<> &goal, double threshold=std::numeric_limits< double >::epsilon()) |
A simple form of setGoal(). The goal will be an instance of ompl::base::GoalState. | |
void | setGoal (const base::GoalPtr &goal) |
Set the goal for planning. This call is not needed if setStartAndGoalStates() has been called. | |
void | setPlanner (const base::PlannerPtr &planner) |
Set the planner to use. If the planner is not set, an attempt is made to use the planner allocator. If no planner allocator is available either, a default planner is set. | |
void | setPlannerAllocator (const base::PlannerAllocator &pa) |
Set the planner allocator to use. This is only used if no planner has been set. This is optional – a default planner will be used if no planner is otherwise specified. | |
virtual base::PlannerStatus | solve (double time=1.0) |
Run the planner for up to a specified amount of time (default is 1 second) | |
virtual base::PlannerStatus | solve (const base::PlannerTerminationCondition &ptc) |
Run the planner until ptc becomes true (at most) | |
base::PlannerStatus | getLastPlannerStatus () const |
Return the status of the last planning attempt. | |
double | getLastPlanComputationTime () const |
Get the amount of time (in seconds) spent during the last planning step. | |
double | getLastSimplificationTime () const |
Get the amount of time (in seconds) spend during the last path simplification step. | |
void | simplifySolution (double duration=0.0) |
Attempt to simplify the current solution path. Spent at most duration seconds in the simplification process. If duration is 0 (the default), a default simplification procedure is executed. | |
void | simplifySolution (const base::PlannerTerminationCondition &ptc) |
Attempt to simplify the current solution path. Stop computation when ptc becomes true at the latest. | |
virtual void | clear () |
Clear all planning data. This only includes data generated by motion plan computation. Planner settings, start & goal states are not affected. | |
virtual void | print (std::ostream &out=std::cout) const |
Print information about the current setup. | |
Public Member Functions inherited from ompl::app::RigidBodyGeometry | |
RigidBodyGeometry (MotionModel mtype, CollisionChecker ctype) | |
Constructor expects a state space that can represent a rigid body. More... | |
RigidBodyGeometry (MotionModel mtype) | |
Constructor expects a state space that can represent a rigid body. More... | |
MotionModel | getMotionModel () const |
CollisionChecker | getCollisionCheckerType () const |
bool | hasEnvironment () const |
bool | hasRobot () const |
unsigned int | getLoadedRobotCount () const |
aiVector3D | getRobotCenter (unsigned int robotIndex) const |
Get the robot's center (average of all the vertices of all its parts) | |
virtual bool | setEnvironmentMesh (const std::string &env) |
This function specifies the name of the CAD file representing the environment (env). Returns 1 on success, 0 on failure. | |
virtual bool | addEnvironmentMesh (const std::string &env) |
This function specifies the name of the CAD file representing a part of the environment (env). Returns 1 on success, 0 on failure. | |
virtual bool | setRobotMesh (const std::string &robot) |
This function specifies the name of the CAD file representing the robot (robot). Returns 1 on success, 0 on failure. | |
virtual bool | addRobotMesh (const std::string &robot) |
This function specifies the name of the CAD file representing a part of the robot (robot). Returns 1 on success, 0 on failure. | |
virtual void | setStateValidityCheckerType (CollisionChecker ctype) |
Change the type of collision checking for the rigid body. | |
const base::StateValidityCheckerPtr & | allocStateValidityChecker (const base::SpaceInformationPtr &si, const GeometricStateExtractor &se, bool selfCollision) |
Allocate default state validity checker using FCL. | |
const GeometrySpecification & | getGeometrySpecification () const |
void | setBoundsFactor (double factor) |
The bounds of the environment are inferred based on the axis-aligned bounding box for the objects in the environment. The inferred size is multiplied by factor. By default factor = 1,. | |
double | getBoundsFactor () const |
Get the data set by setBoundsFactor() | |
void | setBoundsAddition (double add) |
The bounds of the environment are inferred based on the axis-aligned bounding box for the objects in the environment. add is added to the inferred size. By default add = 0,. | |
double | getBoundsAddition () const |
Get the data set by setBoundsAddition() | |
base::RealVectorBounds | inferEnvironmentBounds () const |
Given the representation of an environment, infer its bounds. The bounds will be 2-dimensional when planning in 2D and 3-dimensional when planning in 3D. | |
void | setMeshPath (const std::vector< boost::filesystem::path > &path) |
set path to search for mesh files | |
Protected Member Functions | |
const base::State * | getGeometricComponentStateInternal (const base::State *state, unsigned int index) const override |
Returns the state corresponding to the indexth rigid body in the compound state. | |
Protected Member Functions inherited from ompl::app::RigidBodyGeometry | |
boost::filesystem::path | findMeshFile (const std::string &fname) |
return absolute path to mesh file if it exists and an empty path otherwise | |
void | computeGeometrySpecification () |
Protected Attributes | |
unsigned int | n_ |
The number of independent rigid bodies to plan for. | |
Protected Attributes inherited from ompl::app::AppBase< AppType::GEOMETRIC > | |
std::string | name_ |
Protected Attributes inherited from ompl::geometric::SimpleSetup | |
base::SpaceInformationPtr | si_ |
The created space information. | |
base::ProblemDefinitionPtr | pdef_ |
The created problem definition. | |
base::PlannerPtr | planner_ |
The maintained planner instance. | |
base::PlannerAllocator | pa_ |
The optional planner allocator. | |
PathSimplifierPtr | psk_ |
The instance of the path simplifier. | |
bool | configured_ |
Flag indicating whether the classes needed for planning are set up. | |
double | planTime_ |
The amount of time the last planning step took. | |
double | simplifyTime_ |
The amount of time the last path simplification step took. | |
base::PlannerStatus | lastStatus_ |
The status of the last planning request. | |
Protected Attributes inherited from ompl::app::RigidBodyGeometry | |
MotionModel | mtype_ |
double | factor_ |
The factor to multiply inferred environment bounds by (default 1) | |
double | add_ |
The value to add to inferred environment bounds (default 0) | |
std::vector< std::shared_ptr< Assimp::Importer > > | importerEnv_ |
Instance of assimp importer used to load environment. | |
std::vector< std::shared_ptr< Assimp::Importer > > | importerRobot_ |
Instance of assimp importer used to load robot. | |
GeometrySpecification | geom_ |
Object containing mesh data for robot and environment. | |
base::StateValidityCheckerPtr | validitySvc_ |
Instance of the state validity checker for collision checking. | |
CollisionChecker | ctype_ |
Value containing the type of collision checking to use. | |
std::vector< boost::filesystem::path > | meshPath_ {OMPLAPP_RESOURCE_DIR} |
Paths to search for mesh files if mesh file names do not correspond to absolute paths. | |
Detailed Description
Wrapper for ompl::app::RigidBodyPlanning that plans for multiple rigid bodies in SE3.
Definition at line 39 of file SE3MultiRigidBodyPlanning.h.
The documentation for this class was generated from the following files:
- omplapp/apps/SE3MultiRigidBodyPlanning.h
- omplapp/apps/SE3MultiRigidBodyPlanning.cpp