Loading...
Searching...
No Matches
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
43namespace ob = ompl::base;
44namespace og = ompl::geometric;
45namespace oc = ompl::control;
46
48
49class RigidBodyEnvironment : public oc::OpenDEEnvironment
50{
51public:
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
135class RigidBodyGoal : public ob::GoalRegion
136{
137public:
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
154class RigidBodyStateProjectionEvaluator : public ob::ProjectionEvaluator
155{
156public:
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
184class RigidBodyStateSpace : public oc::OpenDEStateSpace
185{
186public:
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
209int 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
254void 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
281void RigidBodyEnvironment::destroyWorld()
282{
283 dSpaceDestroy(space);
284 dWorldDestroy(bodyWorld);
285}
286
287void 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}
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...
Definition of a goal region.
Definition GoalRegion.h:48
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...
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
virtual void defaultCellSizes()
Set the default cell dimensions for this projection. The default implementation of this function is e...
virtual unsigned int getDimension() const =0
Return the dimension of the projection defined by this evaluator.
virtual void project(const State *state, Eigen::Ref< Eigen::VectorXd > projection) const =0
Compute the projection as an array of double values.
The lower and upper bounds for an Rn space.
ompl::base::State StateType
Define the type of state allocated by this space.
Definition StateSpace.h:78
virtual void registerProjections()
Register the projections for this state space. Usually, this is at least the default projection....
void registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
Register the default projection for this state space.
const T * as() const
Cast this instance to a desired type.
Definition State.h:66
This class contains the OpenDE constructs OMPL needs to know about when planning.
virtual unsigned int getControlDimension() const =0
Number of parameters (double values) needed to specify a control input.
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.
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...
virtual void setupContact(dGeomID geom1, dGeomID geom2, dContact &contact) const
Parameters to set when contacts are created between geom1 and geom2.
Create the set of classes typically needed to solve a control problem when forward propagation is com...
State space representing OpenDE states.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition Control.h:45
This namespace contains code that is specific to planning under geometric constraints.