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_OPENDE_ENVIRONMENT_
00038 #define OMPL_EXTENSION_OPENDE_ENVIRONMENT_
00039
00040 #include "ompl/config.h"
00041 #if OMPL_EXTENSION_OPENDE == 0
00042 # error OpenDE 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
00060 ClassForward(OpenDEEnvironment);
00062
00067 class OpenDEEnvironment
00068 {
00069 public:
00070
00072 dWorldID world_;
00073
00075 std::vector<dSpaceID> collisionSpaces_;
00076
00080 std::vector<dBodyID> stateBodies_;
00081
00083 std::map<dGeomID, std::string>
00084 geomNames_;
00085
00087 bool verboseContacts_;
00088
00090 dJointGroupID contactGroup_;
00091
00093 unsigned int maxContacts_;
00094
00096 double stepSize_;
00097
00099 unsigned int maxControlSteps_;
00100
00102 unsigned int minControlSteps_;
00103
00105 mutable boost::mutex mutex_;
00106
00107 OpenDEEnvironment(void) : world_(NULL), verboseContacts_(false), maxContacts_(3), stepSize_(0.05), maxControlSteps_(100), minControlSteps_(5)
00108 {
00109 contactGroup_ = dJointGroupCreate(0);
00110 }
00111
00112 virtual ~OpenDEEnvironment(void)
00113 {
00114 if (contactGroup_)
00115 dJointGroupDestroy(contactGroup_);
00116 }
00117
00119 virtual unsigned int getControlDimension(void) const = 0;
00120
00122 virtual void getControlBounds(std::vector<double> &lower, std::vector<double> &upper) const = 0;
00123
00127 virtual void applyControl(const double *control) const = 0;
00128
00133 virtual bool isValidCollision(dGeomID geom1, dGeomID geom2, const dContact& contact) const;
00134
00138 virtual unsigned int getMaxContacts(dGeomID geom1, dGeomID geom2) const;
00139
00141 virtual void setupContact(dGeomID geom1, dGeomID geom2, dContact &contact) const;
00142
00144 std::string getGeomName(dGeomID geom) const;
00145
00147 void setGeomName(dGeomID geom, const std::string &name);
00148
00149 };
00150 }
00151 }
00152
00153 #endif