All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
ODESimpleSetup.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2010, Rice University
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Rice University nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
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 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator