OpenDESimpleSetup.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #include "ompl/extensions/opende/OpenDESimpleSetup.h"
38 #include "ompl/util/Exception.h"
39 #include <boost/thread.hpp>
40 
42 {
43  if (!dynamic_cast<OpenDEControlSpace*>(space.get()))
44  throw Exception("OpenDE Control Space needed for OpenDE Simple Setup");
45  useEnvParams();
46 }
47 
50 {
51  useEnvParams();
52 }
53 
55  SimpleSetup(ControlSpacePtr(new OpenDEControlSpace(base::StateSpacePtr(new OpenDEStateSpace(env)))))
56 {
57  useEnvParams();
58 }
59 
60 void ompl::control::OpenDESimpleSetup::useEnvParams()
61 {
62  si_->setPropagationStepSize(getStateSpace()->as<OpenDEStateSpace>()->getEnvironment()->stepSize_);
63  si_->setMinMaxControlDuration(getStateSpace()->as<OpenDEStateSpace>()->getEnvironment()->minControlSteps_,
64  getStateSpace()->as<OpenDEStateSpace>()->getEnvironment()->maxControlSteps_);
65  si_->setStatePropagator(StatePropagatorPtr(new OpenDEStatePropagator(si_)));
66 }
67 
69 {
71  getStateSpace()->as<OpenDEStateSpace>()->readState(current.get());
72  return current;
73 }
74 
76 {
77  getStateSpace()->as<OpenDEStateSpace>()->writeState(state);
78 }
79 
81 {
82  getStateSpace()->as<OpenDEStateSpace>()->writeState(state.get());
83 }
84 
86 {
87  if (!si_->getStateValidityChecker())
88  {
89  OMPL_INFORM("Using default state validity checker for OpenDE");
90  si_->setStateValidityChecker(base::StateValidityCheckerPtr(new OpenDEStateValidityChecker(si_)));
91  }
92  if (pdef_->getStartStateCount() == 0)
93  {
94  OMPL_INFORM("Using the initial state of OpenDE as the starting state for the planner");
95  pdef_->addStartState(getCurrentState());
96  }
98 }
99 
101 {
102  if (haveSolutionPath())
103  playPath(pdef_->getSolutionPath(), timeFactor);
104 }
105 
106 void ompl::control::OpenDESimpleSetup::playPath(const base::PathPtr &path, double timeFactor) const
107 {
108  bool ctl = false;
109  if (dynamic_cast<PathControl*>(path.get()))
110  ctl = true;
111  else
112  if (!dynamic_cast<geometric::PathGeometric*>(path.get()))
113  throw Exception("Unknown type of path");
114 
115  const geometric::PathGeometric &pg = ctl ?
116  static_cast<PathControl*>(path.get())->asGeometric() : *static_cast<geometric::PathGeometric*>(path.get());
117 
118  if (pg.getStateCount() > 0)
119  {
120  OMPL_DEBUG("Playing through %u states (%0.3f seconds)", (unsigned int)pg.getStateCount(),
121  timeFactor * si_->getPropagationStepSize() * (double)(pg.getStateCount() - 1));
122  time::duration d = time::seconds(timeFactor * si_->getPropagationStepSize());
123  getStateSpace()->as<OpenDEStateSpace>()->writeState(pg.getState(0));
124  for (unsigned int i = 1 ; i < pg.getStateCount() ; ++i)
125  {
126  boost::this_thread::sleep(d);
127  getStateSpace()->as<OpenDEStateSpace>()->writeState(pg.getState(i));
128  }
129  }
130 }
131 
132 ompl::base::PathPtr ompl::control::OpenDESimpleSetup::simulateControl(const double *control, unsigned int steps) const
133 {
134  Control *c = si_->allocControl();
135  memcpy(c->as<OpenDEControlSpace::ControlType>()->values, control, sizeof(double) * getControlSpace()->getDimension());
136  base::PathPtr path = simulateControl(c, steps);
137  si_->freeControl(c);
138  return path;
139 }
140 
142 {
143  PathControl *p = new PathControl(si_);
144 
145  base::State *s0 = si_->allocState();
146  getStateSpace()->as<OpenDEStateSpace>()->readState(s0);
147  p->getStates().push_back(s0);
148 
149  base::State *s1 = si_->allocState();
150  si_->propagate(s0, control, steps, s1);
151  p->getStates().push_back(s1);
152 
153  p->getControls().push_back(si_->cloneControl(control));
154  p->getControlDurations().push_back(steps);
155  return base::PathPtr(p);
156 }
157 
159 {
160  Control *c = si_->allocControl();
161  si_->nullControl(c);
162  base::PathPtr path = simulateControl(c, steps);
163  si_->freeControl(c);
164  return path;
165 }
base::ScopedState< OpenDEStateSpace > getCurrentState() const
Get the current OpenDE state (read parameters from OpenDE bodies)
std::vector< base::State * > & getStates()
Get the states that make up the path (as a reference, so it can be modified, hence the function is no...
Definition: PathControl.h:129
const T * as() const
Cast this instance to a desired type.
Definition: Control.h:72
base::ProblemDefinitionPtr pdef_
The created problem definition.
Definition: SimpleSetup.h:278
Definition of a scoped state.
Definition: ScopedState.h:56
Definition of an abstract control.
Definition: Control.h:48
A boost shared pointer wrapper for ompl::base::StateSpace.
boost::posix_time::time_duration duration
Representation of a time duration.
Definition: Time.h:53
bool haveSolutionPath() const
Return true if a solution path is available (previous call to solve() was successful). The solution may be approximate.
Definition: SimpleSetup.h:139
Create the set of classes typically needed to solve a control problem.
Definition: SimpleSetup.h:64
virtual void setup()
This method will create the necessary classes for planning. The solve() method will call this functio...
std::vector< Control * > & getControls()
Get the controls that make up the path (as a reference, so it can be modified, hence the function is ...
Definition: PathControl.h:135
The simplest state validity checker: all states are valid.
Definition of a control path.
Definition: PathControl.h:60
base::State * getState(unsigned int index)
Get the state located at index along the path.
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
virtual void setup()
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: SimpleSetup.cpp:59
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.
A boost shared pointer wrapper for ompl::control::ControlSpace.
StateType * get()
Returns a pointer to the contained state.
Definition: ScopedState.h:396
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:62
base::PathPtr simulate(unsigned int steps) const
Simulate the OpenDE environment forward for steps simulation steps, using the null control (ompl::con...
const OpenDEEnvironmentPtr & getEnvironment() const
Get the OpenDE environment associated to the state and control spaces.
State space representing OpenDE states.
base::PathPtr simulateControl(const double *control, unsigned int steps) const
Simulate the OpenDE environment forward for steps simulation steps, using the control control...
const ControlSpacePtr & getControlSpace() const
Get the current instance of the control space.
Definition: SimpleSetup.h:99
OpenDESimpleSetup(const ControlSpacePtr &space)
Constructor needs the control space needed for planning.
Definition of an abstract state.
Definition: State.h:50
SpaceInformationPtr si_
The created space information.
Definition: SimpleSetup.h:275
The exception type for ompl.
Definition: Exception.h:47
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
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...
State propagation with OpenDE. Only forward propagation is possible.
const base::StateSpacePtr & getStateSpace() const
Get the current instance of the state space.
Definition: SimpleSetup.h:93
std::vector< double > & getControlDurations()
Get the control durations used along the path (as a reference, so it can be modified, hence the function is not const)
Definition: PathControl.h:141
Definition of a geometric path.
Definition: PathGeometric.h:60
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.
Definition: Console.h:68