MorseEnvironment.h
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: Caleb Voss */
36 
37 #ifndef OMPL_EXTENSION_MORSE_ENVIRONMENT_
38 #define OMPL_EXTENSION_MORSE_ENVIRONMENT_
39 
40 #include "ompl/config.h"
41 #if OMPL_EXTENSION_MORSE == 0
42 #error MORSE extension not built
43 #endif
44 
45 #include "ompl/base/State.h"
46 #include "ompl/util/ClassForward.h"
47 
48 #include <mutex>
49 
50 #include <limits>
51 #include <vector>
52 
53 namespace ompl
54 {
55  namespace base
56  {
58 
59  OMPL_CLASS_FORWARD(MorseEnvironment);
61 
66  class MorseEnvironment
67  {
68  public:
70  const unsigned int controlDim_;
71 
73  const std::vector<double> controlBounds_;
74 
76  const unsigned int rigidBodies_;
77 
79  std::vector<double> positionBounds_;
80 
82  std::vector<double> linvelBounds_;
83 
85  std::vector<double> angvelBounds_;
86 
88  double stepSize_;
89 
91  unsigned int minControlSteps_;
92 
94  unsigned int maxControlSteps_;
95 
97  bool simRunning_;
98 
100  mutable std::mutex mutex_;
101 
102  MorseEnvironment(const unsigned int controlDim, const std::vector<double> &controlBounds,
103  const unsigned int rigidBodies, const std::vector<double> &positionBounds,
104  const std::vector<double> &linvelBounds, const std::vector<double> &angvelBounds,
105  const double stepSize, const unsigned int minControlSteps,
106  const unsigned int maxControlSteps)
107  : controlDim_(controlDim)
108  , controlBounds_(controlBounds)
109  , rigidBodies_(rigidBodies)
110  , positionBounds_(positionBounds)
111  , linvelBounds_(linvelBounds)
112  , angvelBounds_(angvelBounds)
113  , stepSize_(stepSize)
114  , minControlSteps_(minControlSteps)
115  , maxControlSteps_(maxControlSteps)
116  , simRunning_(true)
117  {
118  // Replace infinite bounds with very large bounds, so, e.g., sampling can still work
119  for (auto &bound : positionBounds_)
120  {
121  if (bound == std::numeric_limits<double>::infinity())
122  bound = std::numeric_limits<double>::max() / 2;
123  else if (bound == -std::numeric_limits<double>::infinity())
124  bound = -std::numeric_limits<double>::max() / 2;
125  }
126  for (auto &bound : linvelBounds_)
127  {
128  if (bound == std::numeric_limits<double>::infinity())
129  bound = std::numeric_limits<double>::max() / 2;
130  else if (bound == -std::numeric_limits<double>::infinity())
131  bound = -std::numeric_limits<double>::max() / 2;
132  }
133  for (auto &bound : angvelBounds_)
134  {
135  if (bound == std::numeric_limits<double>::infinity())
136  bound = std::numeric_limits<double>::max() / 2;
137  else if (bound == -std::numeric_limits<double>::infinity())
138  bound = -std::numeric_limits<double>::max() / 2;
139  }
140  }
141 
143  {
144  }
145 
147  void getControlBounds(std::vector<double> &lower, std::vector<double> &upper) const;
148 
149  // These functions require interprocess communication and are left to be implemented in Python
150 
152  virtual void readState(State *state) = 0;
153 
155  virtual void writeState(const State *state) = 0;
156 
158  virtual void applyControl(const std::vector<double> &control) = 0;
159 
161  virtual void worldStep(const double dur) = 0;
162  };
163  }
164 }
165 
166 #endif
std::vector< double > angvelBounds_
Upper and lower bounds on angular velocity in each spatial dimension.
virtual void writeState(const State *state)=0
Overwrite the internal state of the simulation.
void getControlBounds(std::vector< double > &lower, std::vector< double > &upper) const
Get the control bounds – the bounding box in which to sample controls.
const unsigned int rigidBodies_
The number of rigid bodies in the simulation.
Definition of an abstract state.
Definition: State.h:113
const unsigned int controlDim_
The dimension of the control space for this simulation.
This class contains the MORSE constructs OMPL needs to know about when planning.
std::mutex mutex_
Lock to use when performing simulations in the world.
bool simRunning_
Indicates whether the simulation has been shut down externally.
virtual void worldStep(const double dur)=0
Proceed with the simulation for the given number of seconds.
virtual void readState(State *state)=0
Query the internal state of the simulation.
unsigned int maxControlSteps_
The maximum number of times a control is applied in sequence.
std::vector< double > positionBounds_
Upper and lower bounds on position in each spatial dimension.
double stepSize_
The simulation step size.
const std::vector< double > controlBounds_
Upper and lower bounds for each control dimension.
std::vector< double > linvelBounds_
Upper and lower bounds on linear velocity in each spatial dimension.
unsigned int minControlSteps_
The minimum number of times a control is applied in sequence.
virtual void applyControl(const std::vector< double > &control)=0
Configure simulation to proceed under a new control.
Main namespace. Contains everything in this library.