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/ode/OpenDEEnvironment.h"
44 
45 namespace ompl
46 {
47  namespace control
48  {
50  class OpenDEStateSpace : public base::CompoundStateSpace
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 
165  const OpenDEEnvironmentPtr &getEnvironment() const
166  {
167  return env_;
168  }
169 
171  unsigned int getNrBodies() const
172  {
173  return env_->stateBodies_.size();
174  }
175 
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 
217  base::StateSamplerPtr allocDefaultStateSampler() const override;
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
OpenDEEnvironmentPtr env_
Representation of the OpenDE parameters OMPL needs to plan.
const double * getBodyPosition(unsigned int body) const
Get the position (x, y, z) of the body at index body.
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.
@ STATE_COLLISION_VALUE_BIT
Index of bit in StateType::collision indicating whether a state is in collision or not....
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.
void setAngularVelocityBounds(const base::RealVectorBounds &bounds)
Set the bounds for each of the angular velocity subspaces.
virtual void readState(base::State *state) const
Read the parameters of the OpenDE bodies and store them in state.
Definition of an abstract state.
Definition: State.h:113
void freeState(base::State *state) const override
Free the memory of the allocated state.
OpenDE State. This is a compound state that allows accessing the properties of the bodies the state s...
base::State * allocState() const override
Allocate a state that can store a point in the described space.
A shared pointer wrapper for ompl::control::OpenDEEnvironment.
const base::SO3StateSpace::StateType & getBodyRotation(unsigned int body) const
Get the quaternion of the body at index body.
void setDefaultBounds()
By default, the volume bounds enclosing the geometry of the environment are computed to include all o...
@ STATE_COLLISION_KNOWN_BIT
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...
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....
const double * getBodyLinearVelocity(unsigned int body) const
Get the linear velocity (x, y, z) of the body at index body.
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:142
const OpenDEEnvironmentPtr & getEnvironment() const
Get the OpenDE environment this state space corresponds to.
@ STATE_VALIDITY_VALUE_BIT
Index of bit in StateType::collision indicating whether a state is valid or not. Initially the value ...
base::StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
int collision
Flag containing information about state validity.
base::StateSamplerPtr allocStateSampler() const override
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
bool satisfiesBoundsExceptRotation(const StateType *state) const
This is a convenience function provided for optimization purposes. It checks whether a state satisfie...
void setVolumeBounds(const base::RealVectorBounds &bounds)
Set the bounds for each of the position subspaces.
virtual bool evaluateCollision(const base::State *state) const
Fill the OpenDEStateSpace::STATE_COLLISION_VALUE_BIT of StateType::collision member of a state,...
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:641
const double * getBodyAngularVelocity(unsigned int body) const
Get the angular velocity (x, y, z) of the body at index body.
void setLinearVelocityBounds(const base::RealVectorBounds &bounds)
Set the bounds for each of the linear velocity subspaces.
@ STATE_VALIDITY_KNOWN_BIT
Index of bit in StateType::collision indicating whether it is known if a state is in valid or not....
unsigned int getNrBodies() const
Get the number of bodies state is maintained for.
Main namespace. Contains everything in this library.
The lower and upper bounds for an Rn space.