Loading...
Searching...
No Matches
MorseSimpleSetup.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/* Authors: Ioan Sucan, Caleb Voss */
36
37#include "ompl/extensions/morse/MorseSimpleSetup.h"
38#include "ompl/extensions/morse/MorseControlSpace.h"
39#include "ompl/extensions/morse/MorseProjection.h"
40#include "ompl/extensions/morse/MorseStatePropagator.h"
41#include "ompl/extensions/morse/MorseStateValidityChecker.h"
42#include "ompl/extensions/morse/MorseTerminationCondition.h"
43#include "ompl/util/Console.h"
44
45ompl::control::MorseSimpleSetup::MorseSimpleSetup(const base::MorseEnvironmentPtr &env)
46 : SimpleSetup(std::make_shared<MorseControlSpace>(std::make_shared<base::MorseStateSpace>(env))), env_(env)
47{
48 si_->setPropagationStepSize(env_->stepSize_);
49 si_->setMinMaxControlDuration(env_->minControlSteps_, env_->maxControlSteps_);
50 si_->setStatePropagator(std::make_shared<MorseStatePropagator>(si_));
51}
52
59
64
69
71{
72 if (!si_->getStateValidityChecker())
73 {
74 OMPL_INFORM("Using default state validity checker for MORSE");
75 si_->setStateValidityChecker(std::make_shared<base::MorseStateValidityChecker>(si_));
76 }
77 base::StateSpacePtr space = si_->getStateSpace();
78 if (!space->hasDefaultProjection())
79 {
80 OMPL_INFORM("Registering MorseProjection as default projection evaluator for MORSE");
81 space->registerDefaultProjection(std::make_shared<base::MorseProjection>(space));
82 }
83 if (pdef_->getStartStateCount() == 0)
84 {
85 OMPL_INFORM("Using the initial state of MORSE as the starting state for the planner");
86 pdef_->addStartState(getCurrentState());
87 }
89}
90
97
99{
100 if (haveSolutionPath())
101 playPath(pdef_->getSolutionPath());
102}
103
104void ompl::control::MorseSimpleSetup::playPath(const base::PathPtr &path) const
105{
106 PathControl *pc = dynamic_cast<PathControl *>(path.get());
107 if (pc)
108 {
109 unsigned int i;
110 base::State *result = si_->allocState();
111 for (i = 0; i < pc->getControlCount(); i++)
112 {
113 si_->getStatePropagator()->propagate(pc->getState(i), pc->getControl(i), pc->getControlDuration(i), result);
114 }
115 getStateSpace()->as<base::MorseStateSpace>()->writeState(pc->getState(i));
116 }
117 else
118 {
119 geometric::PathGeometric *pg = dynamic_cast<geometric::PathGeometric *>(path.get());
120 if (!pg)
121 throw Exception("Unknown type of path");
122 if (pg->getStateCount() > 0)
123 {
124 double d = si_->getPropagationStepSize();
125 getStateSpace()->as<base::MorseStateSpace>()->writeState(pg->getState(0));
126 for (unsigned int i = 1; i < pg->getStateCount(); ++i)
127 {
128 getEnvironment()->worldStep(d);
129 getStateSpace()->as<base::MorseStateSpace>()->writeState(pg->getState(i));
130 }
131 }
132 }
133}
134
135ompl::base::PathPtr ompl::control::MorseSimpleSetup::simulateControl(const double *control, unsigned int steps) const
136{
137 Control *c = si_->allocControl();
139 sizeof(double) * getControlSpace()->getDimension());
140 base::PathPtr path = simulateControl(c, steps);
141 si_->freeControl(c);
142 return path;
143}
144
145ompl::base::PathPtr ompl::control::MorseSimpleSetup::simulateControl(const Control *control, unsigned int steps) const
146{
147 auto p(std::make_shared<PathControl>(si_));
148
149 base::State *s0 = si_->allocState();
150 getStateSpace()->as<base::MorseStateSpace>()->readState(s0);
151 p->getStates().push_back(s0);
152
153 base::State *s1 = si_->allocState();
154 si_->propagate(s0, control, steps, s1);
155 p->getStates().push_back(s1);
156
157 p->getControls().push_back(si_->cloneControl(control));
158 p->getControlDurations().push_back(steps);
159 return p;
160}
161
162ompl::base::PathPtr ompl::control::MorseSimpleSetup::simulate(unsigned int steps) const
163{
164 Control *c = si_->allocControl();
165 si_->nullControl(c);
166 base::PathPtr path = simulateControl(c, steps);
167 si_->freeControl(c);
168 return path;
169}
The exception type for ompl.
Definition Exception.h:47
State space representing MORSE states.
This class represents a termination condition for the planner that only terminates if the user shuts ...
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
Definition of an abstract control.
Definition Control.h:48
Representation of controls applied in MORSE environments. This is an array of double values.
base::PlannerStatus solve()
Run the planner until solution is found or user shuts down MORSE.
base::ScopedState< base::MorseStateSpace > getCurrentState() const
Get the current MORSE state (read parameters from MORSE bodies)
void setCurrentState(const base::ScopedState<> &state)
Set the current MORSE state (set parameters for MORSE bodies)
const base::MorseEnvironmentPtr & getEnvironment() const
Get the MORSE environment associated with this setup.
const base::MorseEnvironmentPtr env_
Pointer to the environment representing the MORSE simulation.
void setup() override
This method will create the necessary classes for planning. The solve() method will call this functio...
MorseSimpleSetup(const base::MorseEnvironmentPtr &env)
The control space is assumed to be MorseControlSpace. The state space is assumed to be MorseStateSpac...
base::PathPtr simulate(unsigned int steps) const
Simulate the MORSE environment forward for steps simulation steps, using the null control (ompl::cont...
void playSolutionPath() const
Call playPath() on the solution path, if one is available.
void playPath(const base::PathPtr &path) const
Set the MORSE world to the states that are contained in a given path, sequentially.
base::PathPtr simulateControl(const double *control, unsigned int steps) const
Simulate the MORSE environment forward for steps simulation steps, using the control control....
Definition of a control path.
Definition PathControl.h:61
double getControlDuration(unsigned int index) const
Get the duration of the control at index, which gets applied to the state at index.
base::State * getState(unsigned int index)
Get the state located at index along the path.
Control * getControl(unsigned int index)
Get the control located at index along the path. This is the control that gets applied to the state l...
std::size_t getControlCount() const
Get the number of controls applied along this path. This should be equal to getStateCount() - 1 unles...
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.
virtual base::PlannerStatus solve(double time=1.0)
Run the planner for a specified amount of time (default is 1 second)
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
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition Control.h:45
STL namespace.
A class to store the exit status of Planner::solve()