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 ControlManifoldPtr &manifold) : SimpleSetup(manifold)
00042 {
00043 if (!dynamic_cast<ODEControlManifold*>(manifold.get()))
00044 throw Exception("ODE Control Manifold needed for ODE Simple Setup");
00045 useEnvParams();
00046 }
00047
00048 ompl::control::ODESimpleSetup::ODESimpleSetup(const base::StateManifoldPtr &manifold) :
00049 SimpleSetup(ControlManifoldPtr(new ODEControlManifold(manifold)))
00050 {
00051 useEnvParams();
00052 }
00053
00054 ompl::control::ODESimpleSetup::ODESimpleSetup(const ODEEnvironmentPtr &env) :
00055 SimpleSetup(ControlManifoldPtr(new ODEControlManifold(base::StateManifoldPtr(new ODEStateManifold(env)))))
00056 {
00057 useEnvParams();
00058 }
00059
00060 void ompl::control::ODESimpleSetup::useEnvParams(void)
00061 {
00062 si_->setPropagationStepSize(getStateManifold()->as<ODEStateManifold>()->getEnvironment()->stepSize_);
00063 si_->setMinMaxControlDuration(getStateManifold()->as<ODEStateManifold>()->getEnvironment()->minControlSteps_,
00064 getStateManifold()->as<ODEStateManifold>()->getEnvironment()->maxControlSteps_);
00065 }
00066
00067 ompl::base::ScopedState<ompl::control::ODEStateManifold> ompl::control::ODESimpleSetup::getCurrentState(void) const
00068 {
00069 base::ScopedState<ODEStateManifold> current(getStateManifold());
00070 getStateManifold()->as<ODEStateManifold>()->readState(current.get());
00071 return current;
00072 }
00073
00074 void ompl::control::ODESimpleSetup::setCurrentState(const base::State *state)
00075 {
00076 getStateManifold()->as<ODEStateManifold>()->writeState(state);
00077 }
00078
00079 void ompl::control::ODESimpleSetup::setCurrentState(const base::ScopedState<> &state)
00080 {
00081 getStateManifold()->as<ODEStateManifold>()->writeState(state.get());
00082 }
00083
00084 void ompl::control::ODESimpleSetup::setup(void)
00085 {
00086 if (!si_->getStateValidityChecker())
00087 {
00088 msg_.inform("Using default state validity checker for ODE");
00089 si_->setStateValidityChecker(base::StateValidityCheckerPtr(new ODEStateValidityChecker(si_)));
00090 }
00091 if (pdef_->getStartStateCount() == 0)
00092 {
00093 msg_.inform("Using the initial state of ODE as the starting state for the planner");
00094 pdef_->addStartState(getCurrentState());
00095 }
00096 SimpleSetup::setup();
00097 }
00098
00099 void ompl::control::ODESimpleSetup::playSolutionPath(double timeFactor) const
00100 {
00101 if (haveSolutionPath())
00102 playPath(getGoal()->getSolutionPath(), timeFactor);
00103 }
00104
00105 void ompl::control::ODESimpleSetup::playPath(const base::PathPtr &path, double timeFactor) const
00106 {
00107 bool ctl = false;
00108 if (dynamic_cast<PathControl*>(path.get()))
00109 ctl = true;
00110 else
00111 if (!dynamic_cast<geometric::PathGeometric*>(path.get()))
00112 throw Exception("Unknown type of path");
00113
00114 const geometric::PathGeometric &pg = ctl ?
00115 static_cast<PathControl*>(path.get())->asGeometric() : *static_cast<geometric::PathGeometric*>(path.get());
00116
00117 if (!pg.states.empty())
00118 {
00119 msg_.debug("Playing through %u states (%0.3f seconds)", (unsigned int)pg.states.size(),
00120 timeFactor * si_->getPropagationStepSize() * (double)(pg.states.size() - 1));
00121 time::duration d = time::seconds(timeFactor * si_->getPropagationStepSize());
00122 getStateManifold()->as<ODEStateManifold>()->writeState(pg.states[0]);
00123 for (unsigned int i = 1 ; i < pg.states.size() ; ++i)
00124 {
00125 boost::this_thread::sleep(d);
00126 getStateManifold()->as<ODEStateManifold>()->writeState(pg.states[i]);
00127 }
00128 }
00129 }
00130
00131 ompl::base::PathPtr ompl::control::ODESimpleSetup::simulateControl(const double* control, unsigned int steps) const
00132 {
00133 Control *c = si_->allocControl();
00134 memcpy(c->as<ODEControlManifold::ControlType>()->values, control, sizeof(double) * getControlManifold()->getDimension());
00135 base::PathPtr path = simulateControl(c, steps);
00136 si_->freeControl(c);
00137 return path;
00138 }
00139
00140 ompl::base::PathPtr ompl::control::ODESimpleSetup::simulateControl(const Control* control, unsigned int steps) const
00141 {
00142 PathControl *p(new PathControl(si_));
00143
00144 base::State *s0 = si_->allocState();
00145 getStateManifold()->as<ODEStateManifold>()->readState(s0);
00146 p->states.push_back(s0);
00147
00148 base::State *s1 = si_->allocState();
00149 si_->propagate(s0, control, steps, s1);
00150 p->states.push_back(s1);
00151
00152 p->controls.push_back(si_->cloneControl(control));
00153 p->controlDurations.push_back(steps);
00154 return base::PathPtr(p);
00155 }
00156
00157 ompl::base::PathPtr ompl::control::ODESimpleSetup::simulate(unsigned int steps) const
00158 {
00159 Control *c = si_->allocControl();
00160 si_->nullControl(c);
00161 base::PathPtr path = simulateControl(c, steps);
00162 si_->freeControl(c);
00163 return path;
00164 }