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:
83  {
84  }
85 
87  const double *getBodyPosition(unsigned int body) const
88  {
89  return as<base::RealVectorStateSpace::StateType>(body * 4)->values;
90  }
91 
93  double *getBodyPosition(unsigned int body)
94  {
95  return as<base::RealVectorStateSpace::StateType>(body * 4)->values;
96  }
97 
99  const base::SO3StateSpace::StateType &getBodyRotation(unsigned int body) const
100  {
101  return *as<base::SO3StateSpace::StateType>(body * 4 + 3);
102  }
103 
106  {
107  return *as<base::SO3StateSpace::StateType>(body * 4 + 3);
108  }
109 
111  const double *getBodyLinearVelocity(unsigned int body) const
112  {
113  return as<base::RealVectorStateSpace::StateType>(body * 4 + 1)->values;
114  }
115 
117  double *getBodyLinearVelocity(unsigned int body)
118  {
119  return as<base::RealVectorStateSpace::StateType>(body * 4 + 1)->values;
120  }
121 
123  const double *getBodyAngularVelocity(unsigned int body) const
124  {
125  return as<base::RealVectorStateSpace::StateType>(body * 4 + 2)->values;
126  }
127 
129  double *getBodyAngularVelocity(unsigned int body)
130  {
131  return as<base::RealVectorStateSpace::StateType>(body * 4 + 2)->values;
132  }
133 
140  mutable int collision;
141  };
142 
161  OpenDEStateSpace(OpenDEEnvironmentPtr env, double positionWeight = 1.0, double linVelWeight = 0.5,
162  double angVelWeight = 0.5, double orientationWeight = 1.0);
163 
164  ~OpenDEStateSpace() override = default;
165 
168  {
169  return env_;
170  }
171 
173  unsigned int getNrBodies() const
174  {
175  return env_->stateBodies_.size();
176  }
177 
183  void setDefaultBounds();
184 
186  void setVolumeBounds(const base::RealVectorBounds &bounds);
187 
190 
193 
196  virtual void readState(base::State *state) const;
197 
202  virtual void writeState(const base::State *state) const;
203 
211  bool satisfiesBoundsExceptRotation(const StateType *state) const;
212 
213  base::State *allocState() const override;
214  void freeState(base::State *state) const override;
215  void copyState(base::State *destination, const base::State *source) const override;
216  void interpolate(const base::State *from, const base::State *to, const double t,
217  base::State *state) const override;
218 
220  base::StateSamplerPtr allocStateSampler() const override;
221 
225  virtual bool evaluateCollision(const base::State *source) const;
226 
227  protected:
230  };
231  }
232 }
233 
234 #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.
virtual bool evaluateCollision(const base::State *source) const
Fill the OpenDEStateSpace::STATE_COLLISION_VALUE_BIT of StateType::collision member of a state...
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.
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, const 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...
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:577
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.