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  {
253  og::PathGeometric p = ss.getSolutionPath().asGeometric();
254  std::vector<ob::State*> &states = p.getStates();
255  for (unsigned int i = 0 ; i < states.size() ; ++i)
256  {
257  const double *pos = states[i]->as<ob::CompoundState>()->as<ob::RealVectorStateSpace::StateType>(0)->values;
258  std::cout << pos[0] << " " << pos[1] << " " << pos[2] << std::endl;
259  }
260  }
261 
262  dCloseODE();
263 
264  return 0;
265 }
266 
267 
268 
269 
270 
271 
272 
273 
275 
276 /***********************************************
277  * Member function implementations *
278  ***********************************************/
279 
280 void RigidBodyEnvironment::createWorld(void)
281 {
282  // BEGIN SETTING UP AN OPENDE ENVIRONMENT
283  // ***********************************
284 
285  bodyWorld = dWorldCreate();
286  space = dHashSpaceCreate(0);
287 
288  dWorldSetGravity(bodyWorld, 0, 0, -0.981);
289 
290  double lx = 0.2;
291  double ly = 0.2;
292  double lz = 0.1;
293 
294  dMassSetBox(&m, 1, lx, ly, lz);
295 
296  boxGeom = dCreateBox(space, lx, ly, lz);
297  boxBody = dBodyCreate(bodyWorld);
298  dBodySetMass(boxBody, &m);
299  dGeomSetBody(boxGeom, boxBody);
300 
301  // *********************************
302  // END SETTING UP AN OPENDE ENVIRONMENT
303 
304  setPlanningParameters();
305 }
306 
307 void RigidBodyEnvironment::destroyWorld(void)
308 {
309  dSpaceDestroy(space);
310  dWorldDestroy(bodyWorld);
311 }
312 
313 void RigidBodyEnvironment::setPlanningParameters(void)
314 {
315  // Fill in parameters for OMPL:
316  world_ = bodyWorld;
317  collisionSpaces_.push_back(space);
318  stateBodies_.push_back(boxBody);
319  stepSize_ = 0.05;
320  maxContacts_ = 3;
321  minControlSteps_ = 10;
322  maxControlSteps_ = 500;
323 }