Loading...
Searching...
No Matches
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/ode/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
60void 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
106void 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
131ompl::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}
The exception type for ompl.
Definition Exception.h:47
A shared pointer wrapper for ompl::base::Path.
Definition of a scoped state.
Definition ScopedState.h:57
StateType * get()
Returns a pointer to the contained state.
Definition of an abstract state.
Definition State.h:50
const T * as() const
Cast this instance to a desired type.
Definition State.h:66
A shared pointer wrapper for ompl::control::ControlSpace.
Definition of an abstract control.
Definition Control.h:48
Representation of controls applied in OpenDE environments. This is an array of double values.
A shared pointer wrapper for ompl::control::OpenDEEnvironment.
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....
base::PathPtr simulateControl(const double *control, unsigned int steps) const
Simulate the OpenDE environment forward for steps simulation steps, using the control control....
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...
base::ScopedState< OpenDEStateSpace > getCurrentState() const
Get the current OpenDE state (read parameters from OpenDE bodies)
void playSolutionPath(double timeFactor=1.0) const
Call playPath() on the solution path, if one is available.
OpenDESimpleSetup(const ControlSpacePtr &space)
Constructor needs the control space needed for planning.
void setCurrentState(const base::ScopedState<> &state)
Set the current OpenDE state (set parameters for OpenDE bodies)
State space representing OpenDE states.
Definition of a control path.
Definition PathControl.h:61
double * values
An array of length n, representing the value of the control.
Create the set of classes typically needed to solve a control problem.
Definition SimpleSetup.h:63
virtual void setup()
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition of a geometric path.
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
base::State * getState(unsigned int index)
Get the state located at index along the path.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition Console.h:68
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition Console.h:70
std::chrono::system_clock::duration duration
Representation of a time duration.
Definition Time.h:55
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition Time.h:64
STL namespace.