All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
OpenDERigidBodyPlanning.cpp
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 #include <ompl/extensions/opende/OpenDESimpleSetup.h>
38 #include <ompl/base/goals/GoalRegion.h>
39 #include <ompl/config.h>
40 #include <iostream>
41 #include <ode/ode.h>
42 
43 namespace ob = ompl::base;
44 namespace og = ompl::geometric;
45 namespace oc = ompl::control;
46 
48 
49 class RigidBodyEnvironment : public oc::OpenDEEnvironment
50 {
51 public:
52 
53  RigidBodyEnvironment(void) : oc::OpenDEEnvironment()
54  {
55  createWorld();
56  }
57 
58  virtual ~RigidBodyEnvironment(void)
59  {
60  destroyWorld();
61  }
62 
63  /**************************************************
64  * Implementation of functions needed by planning *
65  **************************************************/
66 
67  virtual unsigned int getControlDimension(void) const
68  {
69  return 3;
70  }
71 
72  virtual void getControlBounds(std::vector<double> &lower, std::vector<double> &upper) const
73  {
74  static double maxForce = 0.2;
75  lower.resize(3);
76  lower[0] = -maxForce;
77  lower[1] = -maxForce;
78  lower[2] = -maxForce;
79 
80  upper.resize(3);
81  upper[0] = maxForce;
82  upper[1] = maxForce;
83  upper[2] = maxForce;
84  }
85 
86  virtual void applyControl(const double *control) const
87  {
88  dBodyAddForce(boxBody, control[0], control[1], control[2]);
89  }
90 
91  virtual bool isValidCollision(dGeomID /*geom1*/, dGeomID /*geom2*/, const dContact& /*contact*/) const
92  {
93  return false;
94  }
95 
96  virtual void setupContact(dGeomID /*geom1*/, dGeomID /*geom2*/, dContact &contact) const
97  {
98  contact.surface.mode = dContactSoftCFM | dContactApprox1;
99  contact.surface.mu = 0.9;
100  contact.surface.soft_cfm = 0.2;
101  }
102 
103  /**************************************************/
104 
105 
106  // OMPL does not require this function here; we implement it here
107  // for convenience. This function is only OpenDE code to create a
108  // simulation environment. At the end of the function, there is a
109  // call to setPlanningParameters(), which configures members of
110  // the base class needed by planners.
111  void createWorld(void);
112 
113  // Clear all OpenDE objects
114  void destroyWorld(void);
115 
116  // Set parameters needed by the base class (such as the bodies
117  // that make up to state of the system we are planning for)
118  void setPlanningParameters(void);
119 
120  // the simulation world
121  dWorldID bodyWorld;
122 
123  // the space for all objects
124  dSpaceID space;
125 
126  // the car mass
127  dMass m;
128 
129  // the body geom
130  dGeomID boxGeom;
131 
132  // the body
133  dBodyID boxBody;
134 
135 };
136 
137 
138 // Define the goal we want to reach
139 class RigidBodyGoal : public ob::GoalRegion
140 {
141 public:
142 
143  RigidBodyGoal(const ob::SpaceInformationPtr &si) : ob::GoalRegion(si)
144  {
145  threshold_ = 0.5;
146  }
147 
148  virtual double distanceGoal(const ob::State *st) const
149  {
150  const double *pos = st->as<oc::OpenDEStateSpace::StateType>()->getBodyPosition(0);
151  double dx = fabs(pos[0] - 30);
152  double dy = fabs(pos[1] - 55);
153  double dz = fabs(pos[2] - 35);
154  return sqrt(dx * dx + dy * dy + dz * dz);
155  }
156 
157 };
158 
159 
160 // Define how we project a state
161 class RigidBodyStateProjectionEvaluator : public ob::ProjectionEvaluator
162 {
163 public:
164 
165  RigidBodyStateProjectionEvaluator(const ob::StateSpace *space) : ob::ProjectionEvaluator(space)
166  {
167  }
168 
169  virtual unsigned int getDimension(void) const
170  {
171  return 3;
172  }
173 
174  virtual void defaultCellSizes(void)
175  {
176  cellSizes_.resize(3);
177  cellSizes_[0] = 1;
178  cellSizes_[1] = 1;
179  cellSizes_[2] = 1;
180  }
181 
182  virtual void project(const ob::State *state, ob::EuclideanProjection &projection) const
183  {
184  const double *pos = state->as<oc::OpenDEStateSpace::StateType>()->getBodyPosition(0);
185  projection[0] = pos[0];
186  projection[1] = pos[1];
187  projection[2] = pos[2];
188  }
189 
190 };
191 
192 // Define our own space, to include a distance function we want and register a default projection
193 class RigidBodyStateSpace : public oc::OpenDEStateSpace
194 {
195 public:
196 
197  RigidBodyStateSpace(const oc::OpenDEEnvironmentPtr &env) : oc::OpenDEStateSpace(env)
198  {
199  }
200 
201  virtual double distance(const ob::State *s1, const ob::State *s2) const
202  {
203  const double *p1 = s1->as<oc::OpenDEStateSpace::StateType>()->getBodyPosition(0);
204  const double *p2 = s2->as<oc::OpenDEStateSpace::StateType>()->getBodyPosition(0);
205  double dx = fabs(p1[0] - p2[0]);
206  double dy = fabs(p1[1] - p2[1]);
207  double dz = fabs(p1[2] - p2[2]);
208  return sqrt(dx * dx + dy * dy + dz * dz);
209  }
210 
211  virtual void registerProjections(void)
212  {
213  registerDefaultProjection(ob::ProjectionEvaluatorPtr(new RigidBodyStateProjectionEvaluator(this)));
214  }
215 
216 };
217 
219 
220 int main(int, char **)
221 {
222  // initialize OpenDE
223  dInitODE2(0);
224 
225  // create the OpenDE environment
226  oc::OpenDEEnvironmentPtr env(new RigidBodyEnvironment());
227 
228  // create the state space and the control space for planning
229  RigidBodyStateSpace *stateSpace = new RigidBodyStateSpace(env);
230  ob::StateSpacePtr stateSpacePtr = ob::StateSpacePtr(stateSpace);
231 
232  // this will take care of setting a proper collision checker and the starting state for the planner as the initial OpenDE state
233  oc::OpenDESimpleSetup ss(stateSpacePtr);
234 
235  // set the goal we would like to reach
236  ss.setGoal(ob::GoalPtr(new RigidBodyGoal(ss.getSpaceInformation())));
237 
238  ob::RealVectorBounds bounds(3);
239  bounds.setLow(-200);
240  bounds.setHigh(200);
241  stateSpace->setVolumeBounds(bounds);
242 
243  bounds.setLow(-20);
244  bounds.setHigh(20);
245  stateSpace->setLinearVelocityBounds(bounds);
246  stateSpace->setAngularVelocityBounds(bounds);
247 
248  ss.setup();
249  ss.print();
250 
251  if (ss.solve(10))
252  ss.getSolutionPath().asGeometric().printAsMatrix(std::cout);
253 
254  dCloseODE();
255 
256  return 0;
257 }
258 
259 
260 
261 
262 
263 
264 
265 
267 
268 /***********************************************
269  * Member function implementations *
270  ***********************************************/
271 
272 void RigidBodyEnvironment::createWorld(void)
273 {
274  // BEGIN SETTING UP AN OPENDE ENVIRONMENT
275  // ***********************************
276 
277  bodyWorld = dWorldCreate();
278  space = dHashSpaceCreate(0);
279 
280  dWorldSetGravity(bodyWorld, 0, 0, -0.981);
281 
282  double lx = 0.2;
283  double ly = 0.2;
284  double lz = 0.1;
285 
286  dMassSetBox(&m, 1, lx, ly, lz);
287 
288  boxGeom = dCreateBox(space, lx, ly, lz);
289  boxBody = dBodyCreate(bodyWorld);
290  dBodySetMass(boxBody, &m);
291  dGeomSetBody(boxGeom, boxBody);
292 
293  // *********************************
294  // END SETTING UP AN OPENDE ENVIRONMENT
295 
296  setPlanningParameters();
297 }
298 
299 void RigidBodyEnvironment::destroyWorld(void)
300 {
301  dSpaceDestroy(space);
302  dWorldDestroy(bodyWorld);
303 }
304 
305 void RigidBodyEnvironment::setPlanningParameters(void)
306 {
307  // Fill in parameters for OMPL:
308  world_ = bodyWorld;
309  collisionSpaces_.push_back(space);
310  stateBodies_.push_back(boxBody);
311  stepSize_ = 0.05;
312  maxContacts_ = 3;
313  minControlSteps_ = 10;
314  maxControlSteps_ = 500;
315 }
Create the set of classes typically needed to solve a control problem when forward propagation is com...
virtual void applyControl(const double *control) const =0
Application of a control. This function sets the forces/torques/velocities for bodies in the simulati...
A boost shared pointer wrapper for ompl::base::StateSpace.
virtual double distanceGoal(const State *st) const =0
Compute the distance to the goal (heuristic). This function is the one used in computing the distance...
CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:543
virtual unsigned int getDimension(void) const =0
Return the dimension of the projection defined by this evaluator.
virtual void getControlBounds(std::vector< double > &lower, std::vector< double > &upper) const =0
Get the control bounds – the bounding box in which to sample controls.
void registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
Register the default projection for this state space.
Definition: StateSpace.cpp:726
virtual void setupContact(dGeomID geom1, dGeomID geom2, dContact &contact) const
Parameters to set when contacts are created between geom1 and geom2.
const T * as(void) const
Cast this instance to a desired type.
Definition: State.h:74
virtual void registerProjections(void)
Register the projections for this state space. Usually, this is at least the default projection...
Definition: StateSpace.cpp:216
This class contains the OpenDE constructs OMPL needs to know about when planning. ...
State space representing OpenDE states.
virtual bool isValidCollision(dGeomID geom1, dGeomID geom2, const dContact &contact) const
Decide whether a collision is a valid one or not. In some cases, collisions between some bodies can b...
A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.
A boost shared pointer wrapper for ompl::base::SpaceInformation.
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition: StateSpace.h:73
boost::numeric::ublas::vector< double > EuclideanProjection
The datatype for state projections. This class contains a real vector.
Definition of an abstract state.
Definition: State.h:50
virtual double distance(const State *state1, const State *state2) const
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
virtual void project(const State *state, EuclideanProjection &projection) const =0
Compute the projection as an array of double values.
The lower and upper bounds for an Rn space.
virtual unsigned int getControlDimension(void) const =0
Number of parameters (double values) needed to specify a control input.
Definition of a goal region.
Definition: GoalRegion.h:50
A boost shared pointer wrapper for ompl::base::Goal.
virtual void defaultCellSizes(void)
Set the default cell dimensions for this projection. The default implementation of this function is e...
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
A boost shared pointer wrapper for ompl::control::OpenDEEnvironment.