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/ode/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 }
Definition of a goal region.
Definition: GoalRegion.h:112
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:135
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:80
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.
Definition of an abstract state.
Definition: State.h:114
This namespace contains sampling based planning routines shared by both planning under geometric cons...
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...
Create the set of classes typically needed to solve a control problem when forward propagation is com...
virtual unsigned int getDimension() const =0
Return the dimension of the projection defined by this evaluator.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
void registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
Register the default projection for this state space.
Definition: StateSpace.cpp:752
virtual void registerProjections()
Register the projections for this state space. Usually, this is at least the default projection....
Definition: StateSpace.cpp:233
virtual unsigned int getControlDimension() const =0
Number of parameters (double values) needed to specify a control input.
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 class contains the OpenDE constructs OMPL needs to know about when planning.
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
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 applyControl(const double *control) const =0
Application of a control. This function sets the forces/torques/velocities for bodies in the simulati...
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:641
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition: Control.h:77
State space representing OpenDE states.
virtual void setupContact(dGeomID geom1, dGeomID geom2, dContact &contact) const
Parameters to set when contacts are created between geom1 and geom2.
virtual void project(const State *state, Eigen::Ref< Eigen::VectorXd > projection) const =0
Compute the projection as an array of double values.
virtual void defaultCellSizes()
Set the default cell dimensions for this projection. The default implementation of this function is e...
The lower and upper bounds for an Rn space.