00001 /* 00002 * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc. 00003 * All rights reserved. This program is made available under the terms of the 00004 * Eclipse Public License v1.0 which accompanies this distribution, and is 00005 * available at http://www.eclipse.org/legal/epl-v10.html 00006 * Contributors: 00007 * National Institute of Advanced Industrial Science and Technology (AIST) 00008 */ 00009 00010 #ifndef ODE_WORLD_H_INCLUDED 00011 #define ODE_WORLD_H_INCLUDED 00012 00013 #include <ode/ode.h> 00014 #include <hrpCorba/ModelLoader.hh> 00015 #include <hrpModel/World.h> 00016 #include <hrpModel/Body.h> 00017 #include <hrpModel/ForwardDynamics.h> 00018 #include <hrpUtil/Eigen4d.h> 00019 #include <string> 00020 #include <vector> 00021 00022 #include "ODE_Link.h" 00023 00024 static const bool USE_QUICKSTEP=true; 00025 static const int QUICKSTEP_NUM_ITERATIONS = 20; 00026 00027 #ifdef dDOUBLE 00028 static const dReal CFM = 10e-11; 00029 #else 00030 static const dReal CFM = 10e-6; 00031 #endif 00032 static const dReal ERP = 0.2; 00033 static const dReal CONTACT_MAX_CORRECTING_VEL = dInfinity; 00034 static const dReal CONTACT_SURFACE_LAYER = 0.0; 00035 00036 static const int COLLISION_MAX_POINT = 100; 00037 00038 static const int SURFACE_MODE = 0 00039 // | dContactMu2 00040 // | dContactFDir1 00041 | dContactBounce 00042 // | dContactSoftERP 00043 // | ContactSoftCFM 00044 // | dContactMotion1 00045 // | dContactMotion2 00046 // | dContactMotionN 00047 // | dContactSlip1 00048 // | dContactSlip2 00049 // | dContactApprox0 00050 // | dContactApprox1_1 00051 // | dContactApprox1_2 00052 // | dContactApprox1 00053 ; 00054 static const dReal SURFACE_MU = dInfinity; 00055 static const dReal SURFACE_MU2 = 0.0; 00056 static const dReal SURFACE_BOUNCE = 0.0; 00057 static const dReal SURFACE_BOUNCE_VEL = 0.0; 00058 static const dReal SURFACE_SOFT_ERP = 0.0; 00059 static const dReal SURFACE_SOFT_CFM = 0.0; 00060 static const dReal SURFACE_MOTION1 = 0.0; 00061 static const dReal SURFACE_MOTION2 = 0.0; 00062 static const dReal SURFACE_SLIP1 = 0.0; 00063 static const dReal SURFACE_SLIP2 = 0.0; 00064 static const dReal CONTACT_FDIR1_X = 0.0; 00065 static const dReal CONTACT_FDIR1_Y = 0.0; 00066 static const dReal CONTACT_FDIR1_Z = 0.0; 00067 00068 class ODE_World : public hrp::WorldBase 00069 { 00070 public: 00071 ODE_World(); 00072 ~ODE_World(); 00073 00078 void setGravityAcceleration(const dVector3& gravity); 00079 00084 void getGravityAcceleration(dVector3& gravity); 00085 00092 void addBody(OpenHRP::BodyInfo_ptr body, const char *name); 00093 00097 void initialize(); 00098 00102 void calcNextState(OpenHRP::CollisionSequence& corbaCollisionSequence); 00103 00104 void clearExternalForces(); 00105 00106 void useInternalCollisionDetector(bool use){ 00107 useInternalCollisionDetector_ = use; 00108 }; 00109 00110 void addCollisionPair(OpenHRP::LinkPair& linkPair); 00111 00112 dWorldID getWorldID() { return worldId; } 00113 dSpaceID getSpaceID() { return spaceId; } 00114 dJointGroupID getJointGroupID() { return contactgroupId; } 00115 00116 OpenHRP::CollisionSequence collisions; 00117 00118 struct LinkPair{ 00119 dBodyID bodyId1; 00120 dBodyID bodyId2; 00121 }; 00122 typedef std::vector<LinkPair> LinkPairArray; 00123 LinkPairArray linkPairs; 00124 00125 private: 00126 dWorldID worldId; 00127 dSpaceID spaceId; 00128 dJointGroupID contactgroupId; 00129 00130 bool useInternalCollisionDetector_; 00131 00132 void updateSensors(); 00133 }; 00134 00135 static void ODE_collideCallback(void* data, dGeomID o1, dGeomID o2); 00136 00137 class ODE_ForwardDynamics : public hrp::ForwardDynamics{ 00138 public : 00139 ODE_ForwardDynamics(hrp::BodyPtr body); 00140 00141 virtual void initialize(); 00142 virtual void calcNextState(); 00143 void updateSensors(); 00144 00145 private : 00146 void updateForceSensor(ODE_ForceSensor* sensor); 00147 }; 00148 typedef boost::shared_ptr<ODE_ForwardDynamics> ODE_ForwardDynamicsPtr; 00149 00150 #endif