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  RigidBodyEnvironment()
53  {
54  createWorld();
55  }
56 
57  ~RigidBodyEnvironment() override
58  {
59  destroyWorld();
60  }
61 
62  /**************************************************
63  * Implementation of functions needed by planning *
64  **************************************************/
65 
66  unsigned int getControlDimension() const override
67  {
68  return 3;
69  }
70 
71  void getControlBounds(std::vector<double> &lower, std::vector<double> &upper) const override
72  {
73  static double maxForce = 0.2;
74  lower.resize(3);
75  lower[0] = -maxForce;
76  lower[1] = -maxForce;
77  lower[2] = -maxForce;
78 
79  upper.resize(3);
80  upper[0] = maxForce;
81  upper[1] = maxForce;
82  upper[2] = maxForce;
83  }
84 
85  void applyControl(const double *control) const override
86  {
87  dBodyAddForce(boxBody, control[0], control[1], control[2]);
88  }
89 
90  bool isValidCollision(dGeomID /*geom1*/, dGeomID /*geom2*/, const dContact & /*contact*/) const override
91  {
92  return false;
93  }
94 
95  void setupContact(dGeomID /*geom1*/, dGeomID /*geom2*/, dContact &contact) const override
96  {
97  contact.surface.mode = dContactSoftCFM | dContactApprox1;
98  contact.surface.mu = 0.9;
99  contact.surface.soft_cfm = 0.2;
100  }
101 
102  /**************************************************/
103 
104  // OMPL does not require this function here; we implement it here
105  // for convenience. This function is only OpenDE code to create a
106  // simulation environment. At the end of the function, there is a
107  // call to setPlanningParameters(), which configures members of
108  // the base class needed by planners.
109  void createWorld();
110 
111  // Clear all OpenDE objects
112  void destroyWorld();
113 
114  // Set parameters needed by the base class (such as the bodies
115  // that make up to state of the system we are planning for)
116  void setPlanningParameters();
117 
118  // the simulation world
119  dWorldID bodyWorld;
120 
121  // the space for all objects
122  dSpaceID space;
123 
124  // the car mass
125  dMass m;
126 
127  // the body geom
128  dGeomID boxGeom;
129 
130  // the body
131  dBodyID boxBody;
132 };
133 
134 // Define the goal we want to reach
135 class RigidBodyGoal : public ob::GoalRegion
136 {
137 public:
138  RigidBodyGoal(const ob::SpaceInformationPtr &si) : ob::GoalRegion(si)
139  {
140  threshold_ = 0.5;
141  }
142 
143  double distanceGoal(const ob::State *st) const override
144  {
145  const double *pos = st->as<oc::OpenDEStateSpace::StateType>()->getBodyPosition(0);
146  double dx = fabs(pos[0] - 30);
147  double dy = fabs(pos[1] - 55);
148  double dz = fabs(pos[2] - 35);
149  return sqrt(dx * dx + dy * dy + dz * dz);
150  }
151 };
152 
153 // Define how we project a state
154 class RigidBodyStateProjectionEvaluator : public ob::ProjectionEvaluator
155 {
156 public:
157  RigidBodyStateProjectionEvaluator(const ob::StateSpace *space) : ob::ProjectionEvaluator(space)
158  {
159  }
160 
161  unsigned int getDimension() const override
162  {
163  return 3;
164  }
165 
166  void defaultCellSizes() override
167  {
168  cellSizes_.resize(3);
169  cellSizes_[0] = 1;
170  cellSizes_[1] = 1;
171  cellSizes_[2] = 1;
172  }
173 
174  void project(const ob::State *state, Eigen::Ref<Eigen::VectorXd> projection) const override
175  {
176  const double *pos = state->as<oc::OpenDEStateSpace::StateType>()->getBodyPosition(0);
177  projection[0] = pos[0];
178  projection[1] = pos[1];
179  projection[2] = pos[2];
180  }
181 };
182 
183 // Define our own space, to include a distance function we want and register a default projection
184 class RigidBodyStateSpace : public oc::OpenDEStateSpace
185 {
186 public:
187  RigidBodyStateSpace(const oc::OpenDEEnvironmentPtr &env) : oc::OpenDEStateSpace(env)
188  {
189  }
190 
191  double distance(const ob::State *s1, const ob::State *s2) const override
192  {
193  const double *p1 = s1->as<oc::OpenDEStateSpace::StateType>()->getBodyPosition(0);
194  const double *p2 = s2->as<oc::OpenDEStateSpace::StateType>()->getBodyPosition(0);
195  double dx = fabs(p1[0] - p2[0]);
196  double dy = fabs(p1[1] - p2[1]);
197  double dz = fabs(p1[2] - p2[2]);
198  return sqrt(dx * dx + dy * dy + dz * dz);
199  }
200 
201  void registerProjections() override
202  {
203  registerDefaultProjection(std::make_shared<RigidBodyStateProjectionEvaluator>(this));
204  }
205 };
206 
208 
209 int main(int /*argc*/, char ** /*argv*/)
210 {
211  // initialize OpenDE
212  dInitODE2(0);
213 
214  // create the OpenDE environment
215  oc::OpenDEEnvironmentPtr env(std::make_shared<RigidBodyEnvironment>());
216 
217  // create the state space and the control space for planning
218  auto stateSpace = std::make_shared<RigidBodyStateSpace>(env);
219 
220  // this will take care of setting a proper collision checker and the starting state for the planner as the initial
221  // OpenDE state
222  oc::OpenDESimpleSetup ss(stateSpace);
223 
224  // set the goal we would like to reach
225  ss.setGoal(std::make_shared<RigidBodyGoal>(ss.getSpaceInformation()));
226 
227  ob::RealVectorBounds bounds(3);
228  bounds.setLow(-200);
229  bounds.setHigh(200);
230  stateSpace->setVolumeBounds(bounds);
231 
232  bounds.setLow(-20);
233  bounds.setHigh(20);
234  stateSpace->setLinearVelocityBounds(bounds);
235  stateSpace->setAngularVelocityBounds(bounds);
236 
237  ss.setup();
238  ss.print();
239 
240  if (ss.solve(10))
241  ss.getSolutionPath().asGeometric().printAsMatrix(std::cout);
242 
243  dCloseODE();
244 
245  return 0;
246 }
247 
249 
250 /***********************************************
251  * Member function implementations *
252  ***********************************************/
253 
254 void RigidBodyEnvironment::createWorld()
255 {
256  // BEGIN SETTING UP AN OPENDE ENVIRONMENT
257  // ***********************************
258 
259  bodyWorld = dWorldCreate();
260  space = dHashSpaceCreate(nullptr);
261 
262  dWorldSetGravity(bodyWorld, 0, 0, -0.981);
263 
264  double lx = 0.2;
265  double ly = 0.2;
266  double lz = 0.1;
267 
268  dMassSetBox(&m, 1, lx, ly, lz);
269 
270  boxGeom = dCreateBox(space, lx, ly, lz);
271  boxBody = dBodyCreate(bodyWorld);
272  dBodySetMass(boxBody, &m);
273  dGeomSetBody(boxGeom, boxBody);
274 
275  // *********************************
276  // END SETTING UP AN OPENDE ENVIRONMENT
277 
278  setPlanningParameters();
279 }
280 
281 void RigidBodyEnvironment::destroyWorld()
282 {
283  dSpaceDestroy(space);
284  dWorldDestroy(bodyWorld);
285 }
286 
287 void RigidBodyEnvironment::setPlanningParameters()
288 {
289  // Fill in parameters for OMPL:
290  world_ = bodyWorld;
291  collisionSpaces_.push_back(space);
292  stateBodies_.push_back(boxBody);
293  stepSize_ = 0.05;
294  maxContacts_ = 3;
295  minControlSteps_ = 10;
296  maxControlSteps_ = 500;
297 }
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 project(const State *state, Eigen::Ref< Eigen::VectorXd > projection) const =0
Compute the projection as an array of double values.
virtual void registerProjections()
Register the projections for this state space. Usually, this is at least the default projection...
Definition: StateSpace.cpp:232
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
Definition of an abstract state.
Definition: State.h:49
This namespace contains sampling based planning routines shared by both planning under geometric cons...
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.