37 #include "ompl/extensions/morse/MorseSimpleSetup.h"
38 #include "ompl/extensions/morse/MorseControlSpace.h"
39 #include "ompl/extensions/morse/MorseProjection.h"
40 #include "ompl/extensions/morse/MorseStatePropagator.h"
41 #include "ompl/extensions/morse/MorseStateValidityChecker.h"
42 #include "ompl/extensions/morse/MorseTerminationCondition.h"
49 si_->setPropagationStepSize(
env_->stepSize_);
50 si_->setMinMaxControlDuration(
env_->minControlSteps_,
env_->maxControlSteps_);
73 if (!si_->getStateValidityChecker())
75 OMPL_INFORM(
"Using default state validity checker for MORSE");
79 if (!space->hasDefaultProjection())
81 OMPL_INFORM(
"Registering MorseProjection as default projection evaluator for MORSE");
84 if (pdef_->getStartStateCount() == 0)
86 OMPL_INFORM(
"Using the initial state of MORSE as the starting state for the planner");
87 pdef_->addStartState(getCurrentState());
101 if (haveSolutionPath())
102 playPath(pdef_->getSolutionPath());
125 double d = si_->getPropagationStepSize();
129 getEnvironment()->worldStep(d);
138 Control *c = si_->allocControl();
154 si_->propagate(s0, control, steps, s1);
157 p->
getControls().push_back(si_->cloneControl(control));
164 Control *c = si_->allocControl();
State propagation with MORSE. Only forward propagation is possible.
base::ScopedState< base::MorseStateSpace > getCurrentState(void) const
Get the current MORSE state (read parameters from MORSE bodies)
Control * getControl(unsigned int index)
Get the control located at index along the path. This is the control that gets applied to the state l...
State space representing MORSE states.
double getControlDuration(unsigned int index) const
Get the duration of the control at index, which gets applied to the state at index.
Definition of a scoped state.
Definition of an abstract control.
A boost shared pointer wrapper for ompl::base::StateSpace.
const base::MorseEnvironmentPtr env_
Pointer to the environment representing the MORSE simulation.
Create the set of classes typically needed to solve a control problem.
T * as(const unsigned int index) const
Cast a component of this instance to a desired type.
MorseSimpleSetup(const base::MorseEnvironmentPtr &env)
The control space is assumed to be MorseControlSpace. The state space is assumed to be MorseStateSpac...
void playSolutionPath(void) const
Call playPath() on the solution path, if one is available.
This class implements a generic projection for the MorseStateSpace, namely, the subspace representing...
Definition of a control path.
base::PlannerStatus solve(void)
Run the planner until solution is found or user shuts down MORSE.
base::State * getState(unsigned int index)
Get the state located at index along the path.
std::size_t getStateCount(void) const
Get the number of states (way-points) that make up this path.
virtual base::PlannerStatus solve(double time=1.0)
Run the planner for a specified amount of time (default is 1 second)
std::vector< base::State * > & getStates(void)
Get the states that make up the path (as a reference, so it can be modified, hence the function is no...
The simplest state validity checker: all states are valid if they are within bounds.
base::State * getState(unsigned int index)
Get the state located at index along the path.
base::PathPtr simulateControl(const double *control, unsigned int steps) const
Simulate the MORSE environment forward for steps simulation steps, using the control control...
A boost shared pointer wrapper for ompl::base::StateValidityChecker.
const T * as(void) const
Cast this instance to a desired type.
A boost shared pointer wrapper for ompl::control::ControlSpace.
std::vector< Control * > & getControls(void)
Get the controls that make up the path (as a reference, so it can be modified, hence the function is ...
virtual void setup(void)
This method will create the necessary classes for planning. The solve() method will call this functio...
StateType * get(void)
Returns a pointer to the contained state.
void setup(void)
This method will create the necessary classes for planning. The solve() method will call this functio...
A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.
A class to store the exit status of Planner::solve()
void playPath(const base::PathPtr &path) const
Set the MORSE world to the states that are contained in a given path, sequentially.
The definition of a control in Rn
Definition of an abstract state.
std::size_t getControlCount(void) const
Get the number of controls applied along this path. This should be equal to getStateCount() - 1 unles...
SpaceInformationPtr si_
The created space information.
The exception type for ompl.
const T * as(void) const
Cast this instance to a desired type.
Definition of a geometric path.
base::PathPtr simulate(unsigned int steps) const
Simulate the MORSE environment forward for steps simulation steps, using the null control (ompl::cont...
std::vector< double > & getControlDurations(void)
Get the control durations used along the path (as a reference, so it can be modified, hence the function is not const)
Representation of controls applied in MORSE environments. This is an array of double values...
A boost shared pointer wrapper for ompl::base::MorseEnvironment.
A boost shared pointer wrapper for ompl::base::Path.
void setCurrentState(const base::ScopedState<> &state)
Set the current MORSE state (set parameters for MORSE bodies)
This class represents a termination condition for the planner that only terminates if the user shuts ...
double * values
An array of length n, representing the value of the control.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.