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 
46  SimpleSetup(ControlSpacePtr(new MorseControlSpace(base::StateSpacePtr(new base::MorseStateSpace(env))))),
47  env_(env)
48 {
49  si_->setPropagationStepSize(env_->stepSize_);
50  si_->setMinMaxControlDuration(env_->minControlSteps_, env_->maxControlSteps_);
51  si_->setStatePropagator(StatePropagatorPtr(new MorseStatePropagator(si_)));
52 }
53 
55 {
57  getStateSpace()->as<base::MorseStateSpace>()->readState(current.get());
58  return current;
59 }
60 
62 {
63  getStateSpace()->as<base::MorseStateSpace>()->writeState(state);
64 }
65 
67 {
68  getStateSpace()->as<base::MorseStateSpace>()->writeState(state.get());
69 }
70 
72 {
73  if (!si_->getStateValidityChecker())
74  {
75  OMPL_INFORM("Using default state validity checker for MORSE");
77  }
78  base::StateSpacePtr space = si_->getStateSpace();
79  if (!space->hasDefaultProjection())
80  {
81  OMPL_INFORM("Registering MorseProjection as default projection evaluator for MORSE");
82  space->registerDefaultProjection(base::ProjectionEvaluatorPtr(new base::MorseProjection(space)));
83  }
84  if (pdef_->getStartStateCount() == 0)
85  {
86  OMPL_INFORM("Using the initial state of MORSE as the starting state for the planner");
87  pdef_->addStartState(getCurrentState());
88  }
90 }
91 
93 {
94  setup();
95  // terminate if user exits MORSE
97 }
98 
100 {
101  if (haveSolutionPath())
102  playPath(pdef_->getSolutionPath());
103 }
104 
106 {
107  PathControl *pc = dynamic_cast<PathControl*>(path.get());
108  if (pc)
109  {
110  unsigned int i;
111  base::State *result = si_->allocState();
112  for (i = 0; i < pc->getControlCount(); i++)
113  {
114  si_->getStatePropagator()->propagate(pc->getState(i), pc->getControl(i), pc->getControlDuration(i), result);
115  }
116  getStateSpace()->as<base::MorseStateSpace>()->writeState(pc->getState(i));
117  }
118  else
119  {
120  geometric::PathGeometric *pg = dynamic_cast<geometric::PathGeometric*>(path.get());
121  if (!pg)
122  throw Exception("Unknown type of path");
123  if (pg->getStateCount() > 0)
124  {
125  double d = si_->getPropagationStepSize();
126  getStateSpace()->as<base::MorseStateSpace>()->writeState(pg->getState(0));
127  for (unsigned int i = 1 ; i < pg->getStateCount() ; ++i)
128  {
129  getEnvironment()->worldStep(d);
130  getStateSpace()->as<base::MorseStateSpace>()->writeState(pg->getState(i));
131  }
132  }
133  }
134 }
135 
136 ompl::base::PathPtr ompl::control::MorseSimpleSetup::simulateControl(const double *control, unsigned int steps) const
137 {
138  Control *c = si_->allocControl();
139  memcpy(c->as<MorseControlSpace::ControlType>()->values, control, sizeof(double) * getControlSpace()->getDimension());
140  base::PathPtr path = simulateControl(c, steps);
141  si_->freeControl(c);
142  return path;
143 }
144 
146 {
147  PathControl *p = new 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 base::PathPtr(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 }
State propagation with MORSE. Only forward propagation is possible.
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:159
State space representing MORSE states.
std::vector< base::State * > & getStates()
Get the states that make up the path (as a reference, so it can be modified, hence the function is no...
Definition: PathControl.h:129
const T * as() const
Cast this instance to a desired type.
Definition: Control.h:72
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:171
base::ProblemDefinitionPtr pdef_
The created problem definition.
Definition: SimpleSetup.h:278
Definition of a scoped state.
Definition: ScopedState.h:56
Definition of an abstract control.
Definition: Control.h:48
A boost shared pointer wrapper for ompl::base::StateSpace.
const base::MorseEnvironmentPtr env_
Pointer to the environment representing the MORSE simulation.
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:183
bool haveSolutionPath() const
Return true if a solution path is available (previous call to solve() was successful). The solution may be approximate.
Definition: SimpleSetup.h:139
Create the set of classes typically needed to solve a control problem.
Definition: SimpleSetup.h:64
std::vector< Control * > & getControls()
Get the controls that make up the path (as a reference, so it can be modified, hence the function is ...
Definition: PathControl.h:135
MorseSimpleSetup(const base::MorseEnvironmentPtr &env)
The control space is assumed to be MorseControlSpace. The state space is assumed to be MorseStateSpac...
base::PlannerStatus solve()
Run the planner until solution is found or user shuts down MORSE.
This class implements a generic projection for the MorseStateSpace, namely, the subspace representing...
Definition of a control path.
Definition: PathControl.h:60
base::State * getState(unsigned int index)
Get the state located at index along the path.
void playSolutionPath() const
Call playPath() on the solution path, if one is available.
virtual base::PlannerStatus solve(double time=1.0)
Run the planner for a specified amount of time (default is 1 second)
Definition: SimpleSetup.cpp:92
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
virtual void setup()
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: SimpleSetup.cpp:59
The simplest state validity checker: all states are valid if they are within bounds.
base::State * getState(unsigned int index)
Get the state located at index along the path.
Definition: PathControl.h:147
base::PathPtr simulateControl(const double *control, unsigned int steps) const
Simulate the MORSE environment forward for steps simulation steps, using the control control...
A boost shared pointer wrapper for ompl::base::StateValidityChecker.
A boost shared pointer wrapper for ompl::control::ControlSpace.
StateType * get()
Returns a pointer to the contained state.
Definition: ScopedState.h:396
const ControlSpacePtr & getControlSpace() const
Get the current instance of the control space.
Definition: SimpleSetup.h:99
base::ScopedState< base::MorseStateSpace > getCurrentState() const
Get the current MORSE state (read parameters from MORSE bodies)
A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
void playPath(const base::PathPtr &path) const
Set the MORSE world to the states that are contained in a given path, sequentially.
Definition of an abstract state.
Definition: State.h:50
void setup()
This method will create the necessary classes for planning. The solve() method will call this functio...
SpaceInformationPtr si_
The created space information.
Definition: SimpleSetup.h:275
The exception type for ompl.
Definition: Exception.h:47
const base::MorseEnvironmentPtr & getEnvironment() const
Get the MORSE environment associated with this setup.
const base::StateSpacePtr & getStateSpace() const
Get the current instance of the state space.
Definition: SimpleSetup.h:93
std::vector< double > & getControlDurations()
Get the control durations used along the path (as a reference, so it can be modified, hence the function is not const)
Definition: PathControl.h:141
Definition of a geometric path.
Definition: PathGeometric.h:60
base::PathPtr simulate(unsigned int steps) const
Simulate the MORSE environment forward for steps simulation steps, using the null control (ompl::cont...
Representation of controls applied in MORSE environments. This is an array of double values...
A boost shared pointer wrapper for ompl::base::MorseEnvironment.
A boost shared pointer wrapper for ompl::base::Path.
void setCurrentState(const base::ScopedState<> &state)
Set the current MORSE state (set parameters for MORSE bodies)
This class represents a termination condition for the planner that only terminates if the user shuts ...
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