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
74
76{
77 getStateSpace()->as<OpenDEStateSpace>()->writeState(state);
78}
79
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();
135 sizeof(double) * getControlSpace()->getDimension());
136 base::PathPtr path = simulateControl(c, steps);
137 si_->freeControl(c);
138 return path;
139}
140
141ompl::base::PathPtr ompl::control::OpenDESimpleSetup::simulateControl(const Control *control, unsigned int steps) const
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
158ompl::base::PathPtr ompl::control::OpenDESimpleSetup::simulate(unsigned int steps) const
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
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
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
geometric::PathGeometric asGeometric() const
Convert this path into a geometric path (interpolation is performed and then states are copied)
double * values
An array of length n, representing the value of the control.
base::ProblemDefinitionPtr pdef_
The created problem definition.
bool haveSolutionPath() const
Return true if a solution path is available (previous call to solve() was successful)....
virtual void setup()
This method will create the necessary classes for planning. The solve() method will call this functio...
SimpleSetup(const SpaceInformationPtr &si)
Constructor needs the control space used for planning.
const base::StateSpacePtr & getStateSpace() const
Get the current instance of the state space.
Definition SimpleSetup.h:92
const ControlSpacePtr & getControlSpace() const
Get the current instance of the control space.
Definition SimpleSetup.h:98
SpaceInformationPtr si_
The created space information.
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
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition Control.h:45
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.