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 "boost/thread/mutex.hpp"
49 
50 #include <limits>
51 #include <vector>
52 
53 namespace ompl
54 {
55  namespace base
56  {
57 
59 
60  OMPL_CLASS_FORWARD(MorseEnvironment);
62 
68  {
69  public:
70 
72  const unsigned int controlDim_;
73 
75  const std::vector<double> controlBounds_;
76 
78  const unsigned int rigidBodies_;
79 
81  std::vector<double> positionBounds_;
82 
84  std::vector<double> linvelBounds_;
85 
87  std::vector<double> angvelBounds_;
88 
90  double stepSize_;
91 
93  unsigned int minControlSteps_;
94 
96  unsigned int maxControlSteps_;
97 
100 
102  mutable boost::mutex mutex_;
103 
104  MorseEnvironment(const unsigned int controlDim, const std::vector<double> &controlBounds,
105  const unsigned int rigidBodies, const std::vector<double> &positionBounds,
106  const std::vector<double> &linvelBounds, const std::vector<double> &angvelBounds,
107  const double stepSize, const unsigned int minControlSteps, const unsigned int maxControlSteps)
108  : controlDim_(controlDim), controlBounds_(controlBounds), rigidBodies_(rigidBodies),
109  positionBounds_(positionBounds), linvelBounds_(linvelBounds), angvelBounds_(angvelBounds),
110  stepSize_(stepSize), minControlSteps_(minControlSteps), maxControlSteps_(maxControlSteps),
111  simRunning_(true)
112  {
113  // Replace infinite bounds with very large bounds, so, e.g., sampling can still work
114  for (unsigned int i = 0; i < positionBounds_.size(); i++)
115  {
116  if (positionBounds_[i]==std::numeric_limits<double>::infinity())
117  positionBounds_[i] = std::numeric_limits<double>::max()/2;
118  else if (positionBounds_[i]==-std::numeric_limits<double>::infinity())
119  positionBounds_[i] = -std::numeric_limits<double>::max()/2;
120  }
121  for (unsigned int i = 0; i < linvelBounds_.size(); i++)
122  {
123  if (linvelBounds_[i]==std::numeric_limits<double>::infinity())
124  linvelBounds_[i] = std::numeric_limits<double>::max()/2;
125  else if (linvelBounds_[i]==-std::numeric_limits<double>::infinity())
126  linvelBounds_[i] = -std::numeric_limits<double>::max()/2;
127  }
128  for (unsigned int i = 0; i < angvelBounds_.size(); i++)
129  {
130  if (angvelBounds_[i]==std::numeric_limits<double>::infinity())
131  angvelBounds_[i] = std::numeric_limits<double>::max()/2;
132  else if (angvelBounds_[i]==-std::numeric_limits<double>::infinity())
133  angvelBounds_[i] = -std::numeric_limits<double>::max()/2;
134  }
135  }
136 
138  {
139  }
140 
142  void getControlBounds(std::vector<double> &lower, std::vector<double> &upper) const;
143 
144 
145  // These functions require interprocess communication and are left to be implemented in Python
146 
148  virtual void readState(State *state) = 0;
149 
151  virtual void writeState(const State *state) = 0;
152 
154  virtual void applyControl(const std::vector<double> &control) = 0;
155 
157  virtual void worldStep(const double dur) = 0;
158  };
159  }
160 }
161 
162 #endif
bool simRunning_
Indicates whether the simulation has been shut down externally.
This class contains the MORSE constructs OMPL needs to know about when planning.
virtual void worldStep(const double dur)=0
Proceed with the simulation for the given number of seconds.
std::vector< double > linvelBounds_
Upper and lower bounds on linear velocity in each spatial dimension.
virtual void applyControl(const std::vector< double > &control)=0
Configure simulation to proceed under a new control.
const unsigned int controlDim_
The dimension of the control space for this simulation.
boost::mutex mutex_
Lock to use when performing simulations in the world.
const std::vector< double > controlBounds_
Upper and lower bounds for each control dimension.
void getControlBounds(std::vector< double > &lower, std::vector< double > &upper) const
Get the control bounds – the bounding box in which to sample controls.
virtual void readState(State *state)=0
Query the internal state of the simulation.
Main namespace. Contains everything in this library.
Definition: Cost.h:42
unsigned int minControlSteps_
The minimum number of times a control is applied in sequence.
unsigned int maxControlSteps_
The maximum number of times a control is applied in sequence.
double stepSize_
The simulation step size.
Definition of an abstract state.
Definition: State.h:50
std::vector< double > positionBounds_
Upper and lower bounds on position in each spatial dimension.
const unsigned int rigidBodies_
The number of rigid bodies in the simulation.
virtual void writeState(const State *state)=0
Overwrite the internal state of the simulation.
std::vector< double > angvelBounds_
Upper and lower bounds on angular velocity in each spatial dimension.