demos/OpenDERigidBodyPlanning.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Rice University 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Rice University nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Ioan Sucan */ 00036 00037 #include <ompl/extensions/opende/OpenDESimpleSetup.h> 00038 #include <ompl/base/goals/GoalRegion.h> 00039 #include <ompl/config.h> 00040 #include <iostream> 00041 #include <ode/ode.h> 00042 00043 namespace ob = ompl::base; 00044 namespace og = ompl::geometric; 00045 namespace oc = ompl::control; 00046 00048 00049 class RigidBodyEnvironment : public oc::OpenDEEnvironment 00050 { 00051 public: 00052 00053 RigidBodyEnvironment(void) : oc::OpenDEEnvironment() 00054 { 00055 createWorld(); 00056 } 00057 00058 virtual ~RigidBodyEnvironment(void) 00059 { 00060 destroyWorld(); 00061 } 00062 00063 /************************************************** 00064 * Implementation of functions needed by planning * 00065 **************************************************/ 00066 00067 virtual unsigned int getControlDimension(void) const 00068 { 00069 return 3; 00070 } 00071 00072 virtual void getControlBounds(std::vector<double> &lower, std::vector<double> &upper) const 00073 { 00074 static double maxForce = 0.2; 00075 lower.resize(3); 00076 lower[0] = -maxForce; 00077 lower[1] = -maxForce; 00078 lower[2] = -maxForce; 00079 00080 upper.resize(3); 00081 upper[0] = maxForce; 00082 upper[1] = maxForce; 00083 upper[2] = maxForce; 00084 } 00085 00086 virtual void applyControl(const double *control) const 00087 { 00088 dBodyAddForce(boxBody, control[0], control[1], control[2]); 00089 } 00090 00091 virtual bool isValidCollision(dGeomID /*geom1*/, dGeomID /*geom2*/, const dContact& /*contact*/) const 00092 { 00093 return false; 00094 } 00095 00096 virtual void setupContact(dGeomID /*geom1*/, dGeomID /*geom2*/, dContact &contact) const 00097 { 00098 contact.surface.mode = dContactSoftCFM | dContactApprox1; 00099 contact.surface.mu = 0.9; 00100 contact.surface.soft_cfm = 0.2; 00101 } 00102 00103 /**************************************************/ 00104 00105 00106 // OMPL does not require this function here; we implement it here 00107 // for convenience. This function is only OpenDE code to create a 00108 // simulation environment. At the end of the function, there is a 00109 // call to setPlanningParameters(), which configures members of 00110 // the base class needed by planners. 00111 void createWorld(void); 00112 00113 // Clear all OpenDE objects 00114 void destroyWorld(void); 00115 00116 // Set parameters needed by the base class (such as the bodies 00117 // that make up to state of the system we are planning for) 00118 void setPlanningParameters(void); 00119 00120 // the simulation world 00121 dWorldID bodyWorld; 00122 00123 // the space for all objects 00124 dSpaceID space; 00125 00126 // the car mass 00127 dMass m; 00128 00129 // the body geom 00130 dGeomID boxGeom; 00131 00132 // the body 00133 dBodyID boxBody; 00134 00135 }; 00136 00137 00138 // Define the goal we want to reach 00139 class RigidBodyGoal : public ob::GoalRegion 00140 { 00141 public: 00142 00143 RigidBodyGoal(const ob::SpaceInformationPtr &si) : ob::GoalRegion(si) 00144 { 00145 threshold_ = 0.5; 00146 } 00147 00148 virtual double distanceGoal(const ob::State *st) const 00149 { 00150 const double *pos = st->as<oc::OpenDEStateSpace::StateType>()->getBodyPosition(0); 00151 double dx = fabs(pos[0] - 30); 00152 double dy = fabs(pos[1] - 55); 00153 double dz = fabs(pos[2] - 35); 00154 return sqrt(dx * dx + dy * dy + dz * dz); 00155 } 00156 00157 }; 00158 00159 00160 // Define how we project a state 00161 class RigidBodyStateProjectionEvaluator : public ob::ProjectionEvaluator 00162 { 00163 public: 00164 00165 RigidBodyStateProjectionEvaluator(const ob::StateSpace *space) : ob::ProjectionEvaluator(space) 00166 { 00167 } 00168 00169 virtual unsigned int getDimension(void) const 00170 { 00171 return 3; 00172 } 00173 00174 virtual void defaultCellSizes(void) 00175 { 00176 cellSizes_.resize(3); 00177 cellSizes_[0] = 1; 00178 cellSizes_[1] = 1; 00179 cellSizes_[2] = 1; 00180 } 00181 00182 virtual void project(const ob::State *state, ob::EuclideanProjection &projection) const 00183 { 00184 const double *pos = state->as<oc::OpenDEStateSpace::StateType>()->getBodyPosition(0); 00185 projection[0] = pos[0]; 00186 projection[1] = pos[1]; 00187 projection[2] = pos[2]; 00188 } 00189 00190 }; 00191 00192 // Define our own space, to include a distance function we want and register a default projection 00193 class RigidBodyStateSpace : public oc::OpenDEStateSpace 00194 { 00195 public: 00196 00197 RigidBodyStateSpace(const oc::OpenDEEnvironmentPtr &env) : oc::OpenDEStateSpace(env) 00198 { 00199 } 00200 00201 virtual double distance(const ob::State *s1, const ob::State *s2) const 00202 { 00203 const double *p1 = s1->as<oc::OpenDEStateSpace::StateType>()->getBodyPosition(0); 00204 const double *p2 = s2->as<oc::OpenDEStateSpace::StateType>()->getBodyPosition(0); 00205 double dx = fabs(p1[0] - p2[0]); 00206 double dy = fabs(p1[1] - p2[1]); 00207 double dz = fabs(p1[2] - p2[2]); 00208 return sqrt(dx * dx + dy * dy + dz * dz); 00209 } 00210 00211 virtual void registerProjections(void) 00212 { 00213 registerDefaultProjection(ob::ProjectionEvaluatorPtr(new RigidBodyStateProjectionEvaluator(this))); 00214 } 00215 00216 }; 00217 00219 00220 int main(int, char **) 00221 { 00222 // initialize OpenDE 00223 dInitODE2(0); 00224 00225 // create the OpenDE environment 00226 oc::OpenDEEnvironmentPtr env(new RigidBodyEnvironment()); 00227 00228 // create the state space and the control space for planning 00229 RigidBodyStateSpace *stateSpace = new RigidBodyStateSpace(env); 00230 ob::StateSpacePtr stateSpacePtr = ob::StateSpacePtr(stateSpace); 00231 00232 // this will take care of setting a proper collision checker and the starting state for the planner as the initial OpenDE state 00233 oc::OpenDESimpleSetup ss(stateSpacePtr); 00234 00235 // set the goal we would like to reach 00236 ss.setGoal(ob::GoalPtr(new RigidBodyGoal(ss.getSpaceInformation()))); 00237 00238 ob::RealVectorBounds bounds(3); 00239 bounds.setLow(-200); 00240 bounds.setHigh(200); 00241 stateSpace->setVolumeBounds(bounds); 00242 00243 bounds.setLow(-20); 00244 bounds.setHigh(20); 00245 stateSpace->setLinearVelocityBounds(bounds); 00246 stateSpace->setAngularVelocityBounds(bounds); 00247 00248 ss.setup(); 00249 ss.print(); 00250 00251 if (ss.solve(10)) 00252 ss.getSolutionPath().asGeometric().printAsMatrix(std::cout); 00253 00254 dCloseODE(); 00255 00256 return 0; 00257 } 00258 00259 00260 00261 00262 00263 00264 00265 00267 00268 /*********************************************** 00269 * Member function implementations * 00270 ***********************************************/ 00271 00272 void RigidBodyEnvironment::createWorld(void) 00273 { 00274 // BEGIN SETTING UP AN OPENDE ENVIRONMENT 00275 // *********************************** 00276 00277 bodyWorld = dWorldCreate(); 00278 space = dHashSpaceCreate(0); 00279 00280 dWorldSetGravity(bodyWorld, 0, 0, -0.981); 00281 00282 double lx = 0.2; 00283 double ly = 0.2; 00284 double lz = 0.1; 00285 00286 dMassSetBox(&m, 1, lx, ly, lz); 00287 00288 boxGeom = dCreateBox(space, lx, ly, lz); 00289 boxBody = dBodyCreate(bodyWorld); 00290 dBodySetMass(boxBody, &m); 00291 dGeomSetBody(boxGeom, boxBody); 00292 00293 // ********************************* 00294 // END SETTING UP AN OPENDE ENVIRONMENT 00295 00296 setPlanningParameters(); 00297 } 00298 00299 void RigidBodyEnvironment::destroyWorld(void) 00300 { 00301 dSpaceDestroy(space); 00302 dWorldDestroy(bodyWorld); 00303 } 00304 00305 void RigidBodyEnvironment::setPlanningParameters(void) 00306 { 00307 // Fill in parameters for OMPL: 00308 world_ = bodyWorld; 00309 collisionSpaces_.push_back(space); 00310 stateBodies_.push_back(boxBody); 00311 stepSize_ = 0.05; 00312 maxContacts_ = 3; 00313 minControlSteps_ = 10; 00314 maxControlSteps_ = 500; 00315 }