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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines