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() : oc::OpenDEEnvironment()
54  {
55  createWorld();
56  }
57 
58  ~RigidBodyEnvironment() override
59  {
60  destroyWorld();
61  }
62 
63  /**************************************************
64  * Implementation of functions needed by planning *
65  **************************************************/
66 
67  unsigned int getControlDimension() const override
68  {
69  return 3;
70  }
71 
72  void getControlBounds(std::vector<double> &lower, std::vector<double> &upper) const override
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  void applyControl(const double *control) const override
87  {
88  dBodyAddForce(boxBody, control[0], control[1], control[2]);
89  }
90 
91  bool isValidCollision(dGeomID /*geom1*/, dGeomID /*geom2*/, const dContact& /*contact*/) const override
92  {
93  return false;
94  }
95 
96  void setupContact(dGeomID /*geom1*/, dGeomID /*geom2*/, dContact &contact) const override
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();
112 
113  // Clear all OpenDE objects
114  void destroyWorld();
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();
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  double distanceGoal(const ob::State *st) const override
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  unsigned int getDimension() const override
170  {
171  return 3;
172  }
173 
174  void defaultCellSizes() override
175  {
176  cellSizes_.resize(3);
177  cellSizes_[0] = 1;
178  cellSizes_[1] = 1;
179  cellSizes_[2] = 1;
180  }
181 
182  void project(const ob::State *state, ob::EuclideanProjection &projection) const override
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  double distance(const ob::State *s1, const ob::State *s2) const override
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  void registerProjections() override
212  {
213  registerDefaultProjection(
214  std::make_shared<RigidBodyStateProjectionEvaluator>(this));
215  }
216 
217 };
218 
220 
221 int main(int, char **)
222 {
223  // initialize OpenDE
224  dInitODE2(0);
225 
226  // create the OpenDE environment
227  oc::OpenDEEnvironmentPtr env(std::make_shared<RigidBodyEnvironment>());
228 
229  // create the state space and the control space for planning
230  auto stateSpace = std::make_shared<RigidBodyStateSpace>(env);
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(stateSpace);
234 
235  // set the goal we would like to reach
236  ss.setGoal(std::make_shared<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()
273 {
274  // BEGIN SETTING UP AN OPENDE ENVIRONMENT
275  // ***********************************
276 
277  bodyWorld = dWorldCreate();
278  space = dHashSpaceCreate(nullptr);
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()
300 {
301  dSpaceDestroy(space);
302  dWorldDestroy(bodyWorld);
303 }
304 
305 void RigidBodyEnvironment::setPlanningParameters()
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...
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...
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition: Control.h:44
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.
This class contains the OpenDE constructs OMPL needs to know about when planning. ...
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
State space representing OpenDE states.
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:577
virtual unsigned int getControlDimension() const =0
Number of parameters (double values) needed to specify a control input.
A 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:70
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:49
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: Cost.h:44
The lower and upper bounds for an Rn space.
Definition of a goal region.
Definition: GoalRegion.h:47
virtual void setupContact(dGeomID geom1, dGeomID geom2, dContact &contact) const
Parameters to set when contacts are created between geom1 and geom2.
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:47
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
A shared pointer wrapper for ompl::control::OpenDEEnvironment.