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 <thread>
40 
42 {
43  if (dynamic_cast<OpenDEControlSpace *>(space.get()) == nullptr)
44  throw Exception("OpenDE Control Space needed for OpenDE Simple Setup");
45  useEnvParams();
46 }
47 
49  : SimpleSetup(std::make_shared<OpenDEControlSpace>(space))
50 {
51  useEnvParams();
52 }
53 
55  : SimpleSetup(std::make_shared<OpenDEControlSpace>(std::make_shared<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(std::make_shared<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(std::make_shared<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()) != nullptr)
110  ctl = true;
111  else if (dynamic_cast<geometric::PathGeometric *>(path.get()) == nullptr)
112  throw Exception("Unknown type of path");
113 
114  const geometric::PathGeometric &pg = ctl ? static_cast<PathControl *>(path.get())->asGeometric() :
115  *static_cast<geometric::PathGeometric *>(path.get());
116 
117  if (pg.getStateCount() > 0)
118  {
119  OMPL_DEBUG("Playing through %u states (%0.3f seconds)", (unsigned int)pg.getStateCount(),
120  timeFactor * si_->getPropagationStepSize() * (double)(pg.getStateCount() - 1));
121  time::duration d = time::seconds(timeFactor * si_->getPropagationStepSize());
122  getStateSpace()->as<OpenDEStateSpace>()->writeState(pg.getState(0));
123  for (unsigned int i = 1; i < pg.getStateCount(); ++i)
124  {
125  std::this_thread::sleep_for(d);
126  getStateSpace()->as<OpenDEStateSpace>()->writeState(pg.getState(i));
127  }
128  }
129 }
130 
131 ompl::base::PathPtr ompl::control::OpenDESimpleSetup::simulateControl(const double *control, unsigned int steps) const
132 {
133  Control *c = si_->allocControl();
134  memcpy(c->as<OpenDEControlSpace::ControlType>()->values, control,
135  sizeof(double) * getControlSpace()->getDimension());
136  base::PathPtr path = simulateControl(c, steps);
137  si_->freeControl(c);
138  return path;
139 }
140 
142 {
143  auto p(std::make_shared<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 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)
base::PathPtr simulate(unsigned int steps) const
Simulate the OpenDE environment forward for steps simulation steps, using the null control (ompl::con...
void setup() override
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition of a scoped state.
Definition: ScopedState.h:56
Definition of an abstract control.
Definition: Control.h:47
A shared pointer wrapper for ompl::base::StateSpace.
base::PathPtr simulateControl(const double *control, unsigned int steps) const
Simulate the OpenDE environment forward for steps simulation steps, using the control control...
Create the set of classes typically needed to solve a control problem.
Definition: SimpleSetup.h:63
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
STL namespace.
Definition of a control path.
Definition: PathControl.h:60
base::State * getState(unsigned int index)
Get the state located at index along the path.
virtual void setup()
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: SimpleSetup.cpp:59
A shared pointer wrapper for ompl::control::ControlSpace.
StateType * get()
Returns a pointer to the contained state.
Definition: ScopedState.h:394
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:76
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
State space representing OpenDE states.
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.
std::chrono::system_clock::duration duration
Representation of a time duration.
Definition: Time.h:67
OpenDESimpleSetup(const ControlSpacePtr &space)
Constructor needs the control space needed for planning.
Definition of an abstract state.
Definition: State.h:49
The exception type for ompl.
Definition: Exception.h:46
#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...
void playSolutionPath(double timeFactor=1.0) const
Call playPath() on the solution path, if one is available.
Definition of a geometric path.
Definition: PathGeometric.h:60
A shared pointer wrapper for ompl::base::Path.
A 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