00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "ompl/extensions/ode/ODESimpleSetup.h"
00038 #include "ompl/util/Exception.h"
00039 #include <boost/thread.hpp>
00040
00041 ompl::control::ODESimpleSetup::ODESimpleSetup(const ControlSpacePtr &space) : SimpleSetup(space)
00042 {
00043 if (!dynamic_cast<ODEControlSpace*>(space.get()))
00044 throw Exception("ODE Control Space needed for ODE Simple Setup");
00045 useEnvParams();
00046 }
00047
00048 ompl::control::ODESimpleSetup::ODESimpleSetup(const base::StateSpacePtr &space) :
00049 SimpleSetup(ControlSpacePtr(new ODEControlSpace(space)))
00050 {
00051 useEnvParams();
00052 }
00053
00054 ompl::control::ODESimpleSetup::ODESimpleSetup(const ODEEnvironmentPtr &env) :
00055 SimpleSetup(ControlSpacePtr(new ODEControlSpace(base::StateSpacePtr(new ODEStateSpace(env)))))
00056 {
00057 useEnvParams();
00058 }
00059
00060 void ompl::control::ODESimpleSetup::useEnvParams(void)
00061 {
00062 si_->setPropagationStepSize(getStateSpace()->as<ODEStateSpace>()->getEnvironment()->stepSize_);
00063 si_->setMinMaxControlDuration(getStateSpace()->as<ODEStateSpace>()->getEnvironment()->minControlSteps_,
00064 getStateSpace()->as<ODEStateSpace>()->getEnvironment()->maxControlSteps_);
00065 si_->setStatePropagator(StatePropagatorPtr(new ODEStatePropagator(si_)));
00066 }
00067
00068 ompl::base::ScopedState<ompl::control::ODEStateSpace> ompl::control::ODESimpleSetup::getCurrentState(void) const
00069 {
00070 base::ScopedState<ODEStateSpace> current(getStateSpace());
00071 getStateSpace()->as<ODEStateSpace>()->readState(current.get());
00072 return current;
00073 }
00074
00075 void ompl::control::ODESimpleSetup::setCurrentState(const base::State *state)
00076 {
00077 getStateSpace()->as<ODEStateSpace>()->writeState(state);
00078 }
00079
00080 void ompl::control::ODESimpleSetup::setCurrentState(const base::ScopedState<> &state)
00081 {
00082 getStateSpace()->as<ODEStateSpace>()->writeState(state.get());
00083 }
00084
00085 void ompl::control::ODESimpleSetup::setup(void)
00086 {
00087 if (!si_->getStateValidityChecker())
00088 {
00089 msg_.inform("Using default state validity checker for ODE");
00090 si_->setStateValidityChecker(base::StateValidityCheckerPtr(new ODEStateValidityChecker(si_)));
00091 }
00092 if (pdef_->getStartStateCount() == 0)
00093 {
00094 msg_.inform("Using the initial state of ODE as the starting state for the planner");
00095 pdef_->addStartState(getCurrentState());
00096 }
00097 SimpleSetup::setup();
00098 }
00099
00100 void ompl::control::ODESimpleSetup::playSolutionPath(double timeFactor) const
00101 {
00102 if (haveSolutionPath())
00103 playPath(getGoal()->getSolutionPath(), timeFactor);
00104 }
00105
00106 void ompl::control::ODESimpleSetup::playPath(const base::PathPtr &path, double timeFactor) const
00107 {
00108 bool ctl = false;
00109 if (dynamic_cast<PathControl*>(path.get()))
00110 ctl = true;
00111 else
00112 if (!dynamic_cast<geometric::PathGeometric*>(path.get()))
00113 throw Exception("Unknown type of path");
00114
00115 const geometric::PathGeometric &pg = ctl ?
00116 static_cast<PathControl*>(path.get())->asGeometric() : *static_cast<geometric::PathGeometric*>(path.get());
00117
00118 if (!pg.states.empty())
00119 {
00120 msg_.debug("Playing through %u states (%0.3f seconds)", (unsigned int)pg.states.size(),
00121 timeFactor * si_->getPropagationStepSize() * (double)(pg.states.size() - 1));
00122 time::duration d = time::seconds(timeFactor * si_->getPropagationStepSize());
00123 getStateSpace()->as<ODEStateSpace>()->writeState(pg.states[0]);
00124 for (unsigned int i = 1 ; i < pg.states.size() ; ++i)
00125 {
00126 boost::this_thread::sleep(d);
00127 getStateSpace()->as<ODEStateSpace>()->writeState(pg.states[i]);
00128 }
00129 }
00130 }
00131
00132 ompl::base::PathPtr ompl::control::ODESimpleSetup::simulateControl(const double* control, unsigned int steps) const
00133 {
00134 Control *c = si_->allocControl();
00135 memcpy(c->as<ODEControlSpace::ControlType>()->values, control, sizeof(double) * getControlSpace()->getDimension());
00136 base::PathPtr path = simulateControl(c, steps);
00137 si_->freeControl(c);
00138 return path;
00139 }
00140
00141 ompl::base::PathPtr ompl::control::ODESimpleSetup::simulateControl(const Control* control, unsigned int steps) const
00142 {
00143 PathControl *p(new PathControl(si_));
00144
00145 base::State *s0 = si_->allocState();
00146 getStateSpace()->as<ODEStateSpace>()->readState(s0);
00147 p->states.push_back(s0);
00148
00149 base::State *s1 = si_->allocState();
00150 si_->propagate(s0, control, steps, s1);
00151 p->states.push_back(s1);
00152
00153 p->controls.push_back(si_->cloneControl(control));
00154 p->controlDurations.push_back(steps);
00155 return base::PathPtr(p);
00156 }
00157
00158 ompl::base::PathPtr ompl::control::ODESimpleSetup::simulate(unsigned int steps) const
00159 {
00160 Control *c = si_->allocControl();
00161 si_->nullControl(c);
00162 base::PathPtr path = simulateControl(c, steps);
00163 si_->freeControl(c);
00164 return path;
00165 }