37 #include "ompl/extensions/opende/OpenDESimpleSetup.h"
38 #include "ompl/util/Exception.h"
39 #include <boost/thread.hpp>
43 if (!dynamic_cast<OpenDEControlSpace*>(space.get()))
44 throw Exception(
"OpenDE Control Space needed for OpenDE Simple Setup");
60 void ompl::control::OpenDESimpleSetup::useEnvParams(
void)
62 si_->setPropagationStepSize(getStateSpace()->as<OpenDEStateSpace>()->getEnvironment()->stepSize_);
63 si_->setMinMaxControlDuration(getStateSpace()->as<OpenDEStateSpace>()->getEnvironment()->minControlSteps_,
64 getStateSpace()->as<OpenDEStateSpace>()->getEnvironment()->maxControlSteps_);
87 if (!si_->getStateValidityChecker())
89 OMPL_INFORM(
"Using default state validity checker for OpenDE");
92 if (pdef_->getStartStateCount() == 0)
94 OMPL_INFORM(
"Using the initial state of OpenDE as the starting state for the planner");
95 pdef_->addStartState(getCurrentState());
102 if (haveSolutionPath())
103 playPath(pdef_->getSolutionPath(), timeFactor);
109 if (dynamic_cast<PathControl*>(path.get()))
112 if (!dynamic_cast<geometric::PathGeometric*>(path.get()))
121 timeFactor * si_->getPropagationStepSize() * (double)(pg.
getStateCount() - 1));
126 boost::this_thread::sleep(d);
134 Control *c = si_->allocControl();
150 si_->propagate(s0, control, steps, s1);
153 p->
getControls().push_back(si_->cloneControl(control));
160 Control *c = si_->allocControl();
Definition of a scoped state.
Definition of an abstract control.
A boost shared pointer wrapper for ompl::base::StateSpace.
boost::posix_time::time_duration duration
Representation of a time duration.
Create the set of classes typically needed to solve a control problem.
The simplest state validity checker: all states are valid.
Definition of a control path.
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.
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...
void playSolutionPath(double timeFactor=1.0) const
Call playPath() on the solution path, if one is available.
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.
duration seconds(double sec)
Return the time duration representing a given number of seconds.
base::PathPtr simulate(unsigned int steps) const
Simulate the OpenDE environment forward for steps simulation steps, using the null control (ompl::con...
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...
State space representing OpenDE states.
virtual void setup(void)
This method will create the necessary classes for planning. The solve() method will call this functio...
base::PathPtr simulateControl(const double *control, unsigned int steps) const
Simulate the OpenDE environment forward for steps simulation steps, using the control control...
StateType * get(void)
Returns a pointer to the contained state.
The definition of a control in Rn
OpenDESimpleSetup(const ControlSpacePtr &space)
Constructor needs the control space needed for planning.
Definition of an abstract state.
The exception type for ompl.
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
void setCurrentState(const base::ScopedState<> &state)
Set the current OpenDE state (set parameters for OpenDE bodies)
Representation of controls applied in OpenDE environments. This is an array of double values...
const T * as(void) const
Cast this instance to a desired type.
State propagation with OpenDE. Only forward propagation is possible.
Definition of a geometric path.
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)
base::ScopedState< OpenDEStateSpace > getCurrentState(void) const
Get the current OpenDE state (read parameters from OpenDE bodies)
void playPath(const base::PathPtr &path, double timeFactor=1.0) const
Set the OpenDE world to the states that are contained in a given path, sequentially. Using timeFactor, the speed at which this sequence is iterated through is altered.
A boost shared pointer wrapper for ompl::base::Path.
A boost shared pointer wrapper for ompl::control::OpenDEEnvironment.
double * values
An array of length n, representing the value of the control.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.