All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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(void)
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 {
70  base::ScopedState<OpenDEStateSpace> current(getStateSpace());
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 }
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
Create the set of classes typically needed to solve a control problem.
Definition: SimpleSetup.h:63
The simplest state validity checker: all states are valid.
Definition of a control path.
Definition: PathControl.h:54
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...
Definition: PathControl.h:120
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.
Definition: State.h:74
A boost shared pointer wrapper for ompl::control::ControlSpace.
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...
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 ...
Definition: PathControl.h:126
virtual void setup(void)
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: SimpleSetup.cpp:64
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.
Definition: ScopedState.h:396
OpenDESimpleSetup(const ControlSpacePtr &space)
Constructor needs the control space needed for planning.
Definition of an abstract state.
Definition: State.h:50
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...
const T * as(void) const
Cast this instance to a desired type.
Definition: Control.h:72
State propagation with OpenDE. Only forward propagation is possible.
Definition of a geometric path.
Definition: PathGeometric.h:55
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)
Definition: PathControl.h:132
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.
Definition: Console.h:68