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()
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  {
214  std::make_shared<RigidBodyStateProjectionEvaluator>(this));
215  }
216 
217 };
218 
220 
221 int main(int /*argc*/, char ** /*argv*/)
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...
double distance(const State *state1, const State *state2) const override
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
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 unsigned int getDimension() const =0
Return the dimension of the projection defined by this evaluator.
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 double distanceGoal(const State *st) const =0
Compute the distance to the goal (heuristic). This function is the one used in computing the distance...
virtual void registerProjections()
Register the projections for this state space. Usually, this is at least the default projection...
Definition: StateSpace.cpp:244
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.
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 void defaultCellSizes()
Set the default cell dimensions for this projection. The default implementation of this function is e...
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
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.
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.