ODE_World.h
Go to the documentation of this file.
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


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:18