Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 #include "ODE_World.h"
00011 #include "ODE_ModelLoaderUtil.h"
00012
00013 static const dReal DEFAULT_GRAVITY_ACCELERATION = -9.80665;
00014
00015 ODE_World::ODE_World()
00016 {
00017 dInitODE();
00018 worldId = dWorldCreate();
00019 spaceId = dHashSpaceCreate(0);
00020 contactgroupId = dJointGroupCreate(0);
00021
00022 dWorldSetCFM(worldId, CFM);
00023 dWorldSetERP(worldId, ERP);
00024 dWorldSetContactMaxCorrectingVel(worldId, CONTACT_MAX_CORRECTING_VEL);
00025 dWorldSetContactSurfaceLayer(worldId, CONTACT_SURFACE_LAYER);
00026 if(USE_QUICKSTEP)
00027 dWorldSetQuickStepNumIterations(worldId, QUICKSTEP_NUM_ITERATIONS);
00028
00029 dWorldSetGravity(worldId, 0, 0, DEFAULT_GRAVITY_ACCELERATION);
00030 }
00031
00032 ODE_World::~ODE_World()
00033 {
00034 for(int i=0; i<numBodies(); i++){
00035 hrp::BodyPtr b = body(i);
00036 ODE_Link* link = (ODE_Link*)(b->rootLink());
00037 link->destroy();
00038 }
00039 dJointGroupDestroy(contactgroupId);
00040 dSpaceDestroy(spaceId);
00041 dWorldDestroy(worldId);
00042 dCloseODE();
00043 }
00044
00045 void ODE_World::setGravityAcceleration(const dVector3& gravity)
00046 {
00047 dWorldSetGravity(worldId, gravity[0], gravity[1], gravity[2]);
00048 }
00049
00050 void ODE_World::getGravityAcceleration(dVector3& gravity)
00051 {
00052 dWorldGetGravity(worldId, gravity);
00053 }
00054
00055 void ODE_World::addBody(OpenHRP::BodyInfo_ptr bodyInfo, const char *name)
00056 {
00057 hrp::BodyPtr body(new hrp::Body());
00058 ODE_loadBodyFromBodyInfo(body, this, bodyInfo);
00059 body->setName(name);
00060
00061 hrp::WorldBase::addBody(body);
00062 }
00063
00064 void ODE_World::addCollisionPair(OpenHRP::LinkPair& linkPair){
00065 const char* bodyName[2];
00066 bodyName[0] = linkPair.charName1;
00067 bodyName[1] = linkPair.charName2;
00068
00069 const char* linkName[2];
00070 linkName[0] = linkPair.linkName1;
00071 linkName[1] = linkPair.linkName2;
00072
00073 hrp::BodyPtr body1 = this->body(bodyName[0]);
00074 if(body1 == NULL){
00075 std::cout << "ODE_World::addCollisionPair : Body ";
00076 std::cout << bodyName[0] << " is not found." << std::endl;
00077 return;
00078 }
00079 hrp::BodyPtr body2 = this->body(bodyName[1]);
00080 if(body2 == NULL){
00081 std::cout << "ODE_World::addCollisionPair : Body ";
00082 std::cout << bodyName[1] << " is not found." << std::endl;
00083 return;
00084 }
00085 ODE_Link* link1 = (ODE_Link*)body1->link(linkName[0]);
00086 if(link1 == NULL){
00087 std::cout << "ODE_World::addCollisionPair : Link ";
00088 std::cout << linkName[0] << " is not found." << std::endl;
00089 return;
00090 }
00091 ODE_Link* link2 = (ODE_Link*)body2->link(linkName[1]);
00092 if(link2 == NULL){
00093 std::cout << "ODE_World::addCollisionPair : Link ";
00094 std::cout << linkName[1] << " is not found." << std::endl;
00095 return;
00096 }
00097
00098 LinkPair _linkPair;
00099 _linkPair.bodyId1 = link1->bodyId;
00100 _linkPair.bodyId2 = link2->bodyId;
00101 linkPairs.push_back(_linkPair);
00102 }
00103
00104 void ODE_World::calcNextState(OpenHRP::CollisionSequence& corbaCollisionSequence){
00105 if(useInternalCollisionDetector_){
00106 int n = linkPairs.size();
00107 collisions.length(n);
00108 for(int i=0; i<n; i++)
00109 collisions[i].points.length(0);
00110 dSpaceCollide(spaceId, (void *)this, &ODE_collideCallback);
00111 corbaCollisionSequence = collisions;
00112 }else{
00113 for(int i=0; i<corbaCollisionSequence.length(); i++){
00114 OpenHRP::Collision& _collision = corbaCollisionSequence[i];
00115 std::string charName1 = (std::string)_collision.pair.charName1;
00116 std::string linkName1 = (std::string)_collision.pair.linkName1;
00117 ODE_Link* link1 = (ODE_Link*)body(charName1)->link(linkName1);
00118 std::string charName2 = (std::string)_collision.pair.charName2;
00119 std::string linkName2 = (std::string)_collision.pair.linkName2;
00120 ODE_Link* link2 = (ODE_Link*)body(charName2)->link(linkName2);
00121
00122 OpenHRP::CollisionPointSequence& points = _collision.points;
00123 int n = points.length();
00124 for(int j=0; j<n; j++){
00125 dContact contact;
00126 contact.geom.pos[0] = points[j].position[0];
00127 contact.geom.pos[1] = points[j].position[1];
00128 contact.geom.pos[2] = points[j].position[2];
00129 contact.geom.normal[0] = -points[j].normal[0];
00130 contact.geom.normal[1] = -points[j].normal[1];
00131 contact.geom.normal[2] = -points[j].normal[2];
00132 contact.geom.depth = points[j].idepth;
00133
00134
00135
00136
00137
00138
00139
00140
00141 contact.surface.mode = SURFACE_MODE;
00142 contact.surface.mu = SURFACE_MU;
00143 contact.surface.mu2 = SURFACE_MU2;
00144 contact.surface.bounce = SURFACE_BOUNCE;
00145 contact.surface.bounce_vel = SURFACE_BOUNCE_VEL;
00146 contact.surface.soft_erp = SURFACE_SOFT_ERP;
00147 contact.surface.soft_cfm = SURFACE_SOFT_CFM;
00148 contact.surface.motion1 = SURFACE_MOTION1;
00149 contact.surface.motion2 = SURFACE_MOTION2;
00150 contact.surface.slip1 = SURFACE_SLIP1;
00151 contact.surface.slip2 = SURFACE_SLIP2;
00152 contact.fdir1[0] = CONTACT_FDIR1_X;
00153 contact.fdir1[1] = CONTACT_FDIR1_Y;
00154 contact.fdir1[2] = CONTACT_FDIR1_Z;
00155
00156 dJointID c = dJointCreateContact(worldId, contactgroupId, &contact);
00157 dJointAttach(c, link1->bodyId, link2->bodyId);
00158 }
00159
00160 }
00161 }
00162
00163 if(USE_QUICKSTEP)
00164 dWorldQuickStep(worldId, timeStep_);
00165 else
00166 dWorldStep(worldId, timeStep_);
00167
00168 updateSensors();
00169
00170 currentTime_ += timeStep_;
00171 }
00172
00173 void ODE_World::initialize(){
00174 for(int i=0; i<numBodies(); i++){
00175 hrp::BodyPtr b = body(i);
00176 for(int j=0; j<b->numLinks(); j++){
00177 ODE_Link* link = (ODE_Link*)b->link(j);
00178 if(link->jointType==ODE_Link::FIXED_JOINT)
00179 dJointSetFixed(link->odeJointId);
00180 }
00181
00182
00183 BodyInfo& info = bodyInfoArray[i];
00184 info.forwardDynamics.reset(new ODE_ForwardDynamics(b));
00185 info.forwardDynamics->setTimeStep(timeStep_);
00186 info.forwardDynamics->enableSensors(sensorsAreEnabled);
00187 info.forwardDynamics->initialize();
00188
00189 }
00190
00191 }
00192
00193 void ODE_World::clearExternalForces(){
00194 dJointGroupEmpty(contactgroupId);
00195 }
00196
00197 void ODE_World::updateSensors(){
00198 for(int i=0; i<numBodies(); i++){
00199 BodyInfo& info = bodyInfoArray[i];
00200 ODE_ForwardDynamicsPtr forwardDynamics = boost::dynamic_pointer_cast<ODE_ForwardDynamics>(info.forwardDynamics);
00201 forwardDynamics->updateSensors();
00202 }
00203 }
00204
00205 void ODE_collideCallback(void *data, dGeomID o1, dGeomID o2){
00206
00207 dContact contact[COLLISION_MAX_POINT];
00208 ODE_World* world = (ODE_World*)data;
00209 OpenHRP::CollisionSequence& collisions = world->collisions;
00210
00211 ODE_World::LinkPairArray& linkPairs = world->linkPairs;
00212 int collisionIndex = -1;
00213 dBodyID body1 = dGeomGetBody(o1);
00214 dBodyID body2 = dGeomGetBody(o2);
00215
00216 for(int i=0; i<linkPairs.size(); i++){
00217 if( (linkPairs[i].bodyId1 == body1 && linkPairs[i].bodyId2 == body2) ||
00218 (linkPairs[i].bodyId1 == body2 && linkPairs[i].bodyId2 == body1) ){
00219 collisionIndex = i;
00220 break;
00221 }
00222 }
00223 if(collisionIndex == -1)
00224 return;
00225
00226 int n= dCollide(o1, o2, COLLISION_MAX_POINT, &contact[0].geom, sizeof(dContact));
00227 collisions[collisionIndex].points.length(n);
00228 for(int i=0; i<n; i++){
00229 collisions[collisionIndex].points[i].position[0] = contact[i].geom.pos[0];
00230 collisions[collisionIndex].points[i].position[1] = contact[i].geom.pos[1];
00231 collisions[collisionIndex].points[i].position[2] = contact[i].geom.pos[2];
00232 collisions[collisionIndex].points[i].normal[0] = contact[i].geom.normal[0];
00233 collisions[collisionIndex].points[i].normal[1] = contact[i].geom.normal[1];
00234 collisions[collisionIndex].points[i].normal[2] = contact[i].geom.normal[2];
00235 collisions[collisionIndex].points[i].idepth = contact[i].geom.depth;
00236
00237 contact[i].surface.mode = SURFACE_MODE;
00238 contact[i].surface.mu = SURFACE_MU;
00239 contact[i].surface.mu2 = SURFACE_MU2;
00240 contact[i].surface.bounce = SURFACE_BOUNCE;
00241 contact[i].surface.bounce_vel = SURFACE_BOUNCE_VEL;
00242 contact[i].surface.soft_erp = SURFACE_SOFT_ERP;
00243 contact[i].surface.soft_cfm = SURFACE_SOFT_CFM;
00244 contact[i].surface.motion1 = SURFACE_MOTION1;
00245 contact[i].surface.motion2 = SURFACE_MOTION2;
00246 contact[i].surface.slip1 = SURFACE_SLIP1;
00247 contact[i].surface.slip2 = SURFACE_SLIP2;
00248 contact[i].fdir1[0] = CONTACT_FDIR1_X;
00249 contact[i].fdir1[1] = CONTACT_FDIR1_Y;
00250 contact[i].fdir1[2] = CONTACT_FDIR1_Z;
00251
00252 dJointID c = dJointCreateContact(world->getWorldID(), world->getJointGroupID(), &contact[i]);
00253 dJointAttach(c, dGeomGetBody(contact[i].geom.g1), dGeomGetBody(contact[i].geom.g2));
00254 }
00255
00256 }
00257
00258 ODE_ForwardDynamics::ODE_ForwardDynamics(hrp::BodyPtr body) :
00259 ForwardDynamics(body)
00260 {
00261 }
00262
00263 void ODE_ForwardDynamics::calcNextState(){
00264 }
00265
00266 void ODE_ForwardDynamics::initialize(){
00267 initializeSensors();
00268 }
00269
00270 #ifndef M_2PI
00271 #define M_2PI 6.28318530717958647692
00272 #endif
00273 void ODE_ForwardDynamics::updateSensors(){
00274 for(int i=0; i<body->numLinks(); i++){
00275 ODE_Link* link = (ODE_Link*)body->link(i);
00276 const dReal* _w = link->getAngularVel();
00277 link->w << _w[0], _w[1], _w[2];
00278 hrp::Matrix33 R;
00279 link->getTransform(link->p, R);
00280 link->setSegmentAttitude(R);
00281 link->getLinearVel(link->v);
00282
00283 link->dq = link->getVelocity();
00284 link->q += link->dq * timeStep;
00285 dReal q =link->getAngle();
00286
00287 if(link->jointType == ODE_Link::ROTATIONAL_JOINT && fabs(q-link->q) > M_PI ){
00288 dReal oldq=link->q;
00289 int k = link->q/M_2PI;
00290 if( link->q>=0 )
00291 if( q>=0 )
00292 link->q = k * M_2PI + q;
00293 else
00294 link->q = (k+1) * M_2PI + q;
00295 else
00296 if( q<0 )
00297 link->q = k * M_2PI + q;
00298 else
00299 link->q = (k-1) * M_2PI + q;
00300 }else
00301 link->q = q;
00302 }
00303 updateSensorsFinal();
00304
00305 int n = body->numSensors(hrp::Sensor::FORCE);
00306 for(int i=0; i < n; ++i){
00307 updateForceSensor((ODE_ForceSensor*)body->sensor<hrp::ForceSensor>(i));
00308 }
00309 }
00310
00311 void ODE_ForwardDynamics::updateForceSensor(ODE_ForceSensor* sensor){
00312 ODE_Link* link = (ODE_Link*)sensor->link;
00313 dJointFeedback* fb = dJointGetFeedback(link->odeJointId);
00314 hrp::Vector3 f(fb->f2[0], fb->f2[1], fb->f2[2]);
00315 hrp::Vector3 tau(fb->t2[0], fb->t2[1], fb->t2[2]);
00316 hrp::Matrix33 sensorR(link->R * sensor->localR);
00317 hrp::Vector3 fs(sensorR.transpose() * f);
00318
00319 hrp::Vector3 sensorPos(link->R * (sensor->localPos - link->parent->c));
00320 hrp::Vector3 ts(sensorR.transpose() * (tau - sensorPos.cross(f)));
00321
00322 sensor->f = fs;
00323 sensor->tau = ts;
00324 }