00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef OMPL_EXTENSION_ODE_ENVIRONMENT_
00038 #define OMPL_EXTENSION_ODE_ENVIRONMENT_
00039
00040 #include "ompl/config.h"
00041 #if OMPL_EXTENSION_ODE == 0
00042 # error ODE extension not built
00043 #endif
00044
00045 #include "ompl/util/ClassForward.h"
00046
00047 #include <ode/ode.h>
00048 #include <vector>
00049 #include <string>
00050 #include <map>
00051 #include <boost/thread/mutex.hpp>
00052
00053 namespace ompl
00054 {
00055 namespace control
00056 {
00057
00059 ClassForward(ODEEnvironment);
00060
00065 class ODEEnvironment
00066 {
00067 public:
00068
00070 dWorldID world_;
00071
00073 std::vector<dSpaceID> collisionSpaces_;
00074
00078 std::vector<dBodyID> stateBodies_;
00079
00081 std::map<dGeomID, std::string>
00082 geomNames_;
00083
00085 bool verboseContacts_;
00086
00088 dJointGroupID contactGroup_;
00089
00091 unsigned int maxContacts_;
00092
00094 double stepSize_;
00095
00097 unsigned int maxControlSteps_;
00098
00100 unsigned int minControlSteps_;
00101
00103 mutable boost::mutex mutex_;
00104
00105 ODEEnvironment(void) : world_(NULL), verboseContacts_(false), maxContacts_(3), stepSize_(0.05), maxControlSteps_(100), minControlSteps_(5)
00106 {
00107 contactGroup_ = dJointGroupCreate(0);
00108 }
00109
00110 virtual ~ODEEnvironment(void)
00111 {
00112 if (contactGroup_)
00113 dJointGroupDestroy(contactGroup_);
00114 }
00115
00117 virtual unsigned int getControlDimension(void) const = 0;
00118
00120 virtual void getControlBounds(std::vector<double> &lower, std::vector<double> &upper) const = 0;
00121
00125 virtual void applyControl(const double *control) const = 0;
00126
00131 virtual bool isValidCollision(dGeomID geom1, dGeomID geom2, const dContact& contact) const;
00132
00136 virtual unsigned int getMaxContacts(dGeomID geom1, dGeomID geom2) const;
00137
00139 virtual void setupContact(dGeomID geom1, dGeomID geom2, dContact &contact) const;
00140
00142 std::string getGeomName(dGeomID geom) const;
00143
00145 void setGeomName(dGeomID geom, const std::string &name);
00146
00147 };
00148 }
00149 }
00150
00151 #endif