openhrp3: ODE_World.cpp Source File

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 #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                 //TODO
00134 
00135  //               contact.geom.g1 = link1->geomId;
00136  //               contact.geom.g2 = link2->geomId;
00137                 //std::cout << "out pos " << contact.geom.pos[0] << "  " << contact.geom.pos[1]  << "  " <<contact.geom.pos[2]  << std::endl;
00138                 //std::cout << "out normal " << contact.geom.normal[0] << "  " << contact.geom.normal[1]  << "  " <<contact.geom.normal[2]  << std::endl;
00139                 //std::cout << "out depth " << contact.geom.depth << std::endl;
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             //std::cout << std::endl;
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         // for sensor
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     //hrp::Vector3 sensorPos(link->p + link->R * sensor->localPos);
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 }