OpenDEStateSpace.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: Ioan Sucan */
36 
37 #ifndef OMPL_EXTENSION_OPENDE_STATE_SPACE_
38 #define OMPL_EXTENSION_OPENDE_STATE_SPACE_
39 
40 #include "ompl/base/StateSpace.h"
41 #include "ompl/base/spaces/RealVectorStateSpace.h"
42 #include "ompl/base/spaces/SO3StateSpace.h"
43 #include "ompl/extensions/opende/OpenDEEnvironment.h"
44 
45 namespace ompl
46 {
47  namespace control
48  {
51  {
52  public:
53  enum
54  {
75  };
76 
80  {
81  public:
82  StateType() = default;
83 
85  const double *getBodyPosition(unsigned int body) const
86  {
87  return as<base::RealVectorStateSpace::StateType>(body * 4)->values;
88  }
89 
91  double *getBodyPosition(unsigned int body)
92  {
93  return as<base::RealVectorStateSpace::StateType>(body * 4)->values;
94  }
95 
97  const base::SO3StateSpace::StateType &getBodyRotation(unsigned int body) const
98  {
99  return *as<base::SO3StateSpace::StateType>(body * 4 + 3);
100  }
101 
104  {
105  return *as<base::SO3StateSpace::StateType>(body * 4 + 3);
106  }
107 
109  const double *getBodyLinearVelocity(unsigned int body) const
110  {
111  return as<base::RealVectorStateSpace::StateType>(body * 4 + 1)->values;
112  }
113 
115  double *getBodyLinearVelocity(unsigned int body)
116  {
117  return as<base::RealVectorStateSpace::StateType>(body * 4 + 1)->values;
118  }
119 
121  const double *getBodyAngularVelocity(unsigned int body) const
122  {
123  return as<base::RealVectorStateSpace::StateType>(body * 4 + 2)->values;
124  }
125 
127  double *getBodyAngularVelocity(unsigned int body)
128  {
129  return as<base::RealVectorStateSpace::StateType>(body * 4 + 2)->values;
130  }
131 
138  mutable int collision{0};
139  };
140 
159  OpenDEStateSpace(OpenDEEnvironmentPtr env, double positionWeight = 1.0, double linVelWeight = 0.5,
160  double angVelWeight = 0.5, double orientationWeight = 1.0);
161 
162  ~OpenDEStateSpace() override = default;
163 
166  {
167  return env_;
168  }
169 
171  unsigned int getNrBodies() const
172  {
173  return env_->stateBodies_.size();
174  }
175 
181  void setDefaultBounds();
182 
184  void setVolumeBounds(const base::RealVectorBounds &bounds);
185 
188 
191 
194  virtual void readState(base::State *state) const;
195 
200  virtual void writeState(const base::State *state) const;
201 
209  bool satisfiesBoundsExceptRotation(const StateType *state) const;
210 
211  base::State *allocState() const override;
212  void freeState(base::State *state) const override;
213  void copyState(base::State *destination, const base::State *source) const override;
214  void interpolate(const base::State *from, const base::State *to, double t,
215  base::State *state) const override;
216 
218  base::StateSamplerPtr allocStateSampler() const override;
219 
223  virtual bool evaluateCollision(const base::State *state) const;
224 
225  protected:
228  };
229  }
230 }
231 
232 #endif
const double * getBodyPosition(unsigned int body) const
Get the position (x, y, z) of the body at index body.
Index of bit in StateType::collision indicating whether it is known if a state is in collision or not...
virtual void writeState(const base::State *state) const
Set the parameters of the OpenDE bodies to be the ones read from state. The code will technically wor...
Index of bit in StateType::collision indicating whether a state is valid or not. Initially the value ...
Definition of a compound state.
Definition: State.h:86
base::State * allocState() const override
Allocate a state that can store a point in the described space.
void setVolumeBounds(const base::RealVectorBounds &bounds)
Set the bounds for each of the position subspaces.
double * getBodyLinearVelocity(unsigned int body)
Get the linear velocity (x, y, z) of the body at index body.
base::SO3StateSpace::StateType & getBodyRotation(unsigned int body)
Get the quaternion of the body at index body.
const base::SO3StateSpace::StateType & getBodyRotation(unsigned int body) const
Get the quaternion of the body at index body.
A shared pointer wrapper for ompl::base::StateSampler.
virtual bool evaluateCollision(const base::State *state) const
Fill the OpenDEStateSpace::STATE_COLLISION_VALUE_BIT of StateType::collision member of a state...
Index of bit in StateType::collision indicating whether a state is in collision or not...
void setAngularVelocityBounds(const base::RealVectorBounds &bounds)
Set the bounds for each of the angular velocity subspaces.
void interpolate(const base::State *from, const base::State *to, double t, base::State *state) const override
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state...
OpenDE State. This is a compound state that allows accessing the properties of the bodies the state s...
double * getBodyPosition(unsigned int body)
Get the position (x, y, z) of the body at index body.
const double * getBodyAngularVelocity(unsigned int body) const
Get the angular velocity (x, y, z) of the body at index body.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
State space representing OpenDE states.
bool satisfiesBoundsExceptRotation(const StateType *state) const
This is a convenience function provided for optimization purposes. It checks whether a state satisfie...
void setDefaultBounds()
By default, the volume bounds enclosing the geometry of the environment are computed to include all o...
const double * getBodyLinearVelocity(unsigned int body) const
Get the linear velocity (x, y, z) of the body at index body.
A space to allow the composition of state spaces.
Definition: StateSpace.h:573
The definition of a state in SO(3) represented as a unit quaternion.
Definition: SO3StateSpace.h:90
unsigned int getNrBodies() const
Get the number of bodies state is maintained for.
void freeState(base::State *state) const override
Free the memory of the allocated state.
const OpenDEEnvironmentPtr & getEnvironment() const
Get the OpenDE environment this state space corresponds to.
Index of bit in StateType::collision indicating whether it is known if a state is in valid or not...
base::StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
Definition of an abstract state.
Definition: State.h:49
OpenDEStateSpace(OpenDEEnvironmentPtr env, double positionWeight=1.0, double linVelWeight=0.5, double angVelWeight=0.5, double orientationWeight=1.0)
Construct a state space representing OpenDE states.
The lower and upper bounds for an Rn space.
base::StateSamplerPtr allocStateSampler() const override
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
void copyState(base::State *destination, const base::State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap. ...
void setLinearVelocityBounds(const base::RealVectorBounds &bounds)
Set the bounds for each of the linear velocity subspaces.
double * getBodyAngularVelocity(unsigned int body)
Get the angular velocity (x, y, z) of the body at index body.
int collision
Flag containing information about state validity.
A shared pointer wrapper for ompl::control::OpenDEEnvironment.
virtual void readState(base::State *state) const
Read the parameters of the OpenDE bodies and store them in state.
OpenDEEnvironmentPtr env_
Representation of the OpenDE parameters OMPL needs to plan.