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
54{
55 base::ScopedState<base::MorseStateSpace> current(getStateSpace());
56 getStateSpace()->as<base::MorseStateSpace>()->readState(current.get());
57 return current;
58}
59
61{
62 getStateSpace()->as<base::MorseStateSpace>()->writeState(state);
63}
64
66{
67 getStateSpace()->as<base::MorseStateSpace>()->writeState(state.get());
68}
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
92{
93 setup();
94 // terminate if user exits MORSE
96}
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();
138 memcpy(c->as<MorseControlSpace::ControlType>()->values, control,
139 sizeof(double) * getControlSpace()->getDimension());
140 base::PathPtr path = simulateControl(c, steps);
141 si_->freeControl(c);
142 return path;
143}
144
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
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
T * as(const unsigned int index) const
Cast a component of this instance to a desired type.
Definition: StateSpace.h:590
State space representing MORSE states.
This class represents a termination condition for the planner that only terminates if the user shuts ...
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: ScopedState.h:393
Definition of an abstract state.
Definition: State.h:50
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
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 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.
Definition: PathControl.h:182
base::State * getState(unsigned int index)
Get the state located at index along the path.
Definition: PathControl.h:156
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...
Definition: PathControl.h:169
std::size_t getControlCount() const
Get the number of controls applied along this path. This should be equal to getStateCount() - 1 unles...
Definition: PathControl.h:195
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: SimpleSetup.cpp:54
virtual base::PlannerStatus solve(double time=1.0)
Run the planner for a specified amount of time (default is 1 second)
Definition: SimpleSetup.cpp:88
SpaceInformationPtr si_
The created space information.
Definition: SimpleSetup.h:276
Definition of a geometric path.
Definition: PathGeometric.h:66
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
STL namespace.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49