ODE_DynamicsSimulator_impl.cpp
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  * General Robotix Inc. 
00009  */
00010 
00011 
00012 #include "ODE_DynamicsSimulator_impl.h"
00013 
00014 #include <hrpUtil/Eigen3d.h>
00015 #include <hrpModel/Body.h>
00016 #include <hrpModel/Link.h>
00017 #include <hrpModel/LinkTraverse.h>
00018 #include <hrpModel/JointPath.h>
00019 #include <hrpModel/Sensor.h>
00020 #include <hrpModel/ModelLoaderUtil.h>
00021 
00022 #include <vector>
00023 #include <map>
00024 #include <algorithm>
00025 
00026 using namespace std;
00027 using namespace hrp;
00028 
00029 static const bool USE_INTERNAL_COLLISION_DETECTOR = false;
00030 static const bool USE_ODE_COLLISION_DETECTOR = true;
00031 static const int debugMode = false;
00032 static const bool enableTimeMeasure = false;
00033 
00034 namespace {
00035 
00036     struct IdLabel {
00037         int id;
00038         const char *label;
00039     };
00040     typedef map<int, const char*> IdToLabelMap;
00041 
00042     IdToLabelMap commandLabelMap;
00043 
00044     IdLabel commandLabels[] = {
00045 
00046         { DynamicsSimulator::POSITION_GIVEN,    "Position Given (High Gain Mode)" },
00047         { DynamicsSimulator::JOINT_VALUE,       "Joint Value" },
00048         { DynamicsSimulator::JOINT_VELOCITY,    "Joint Velocity"},
00049         { DynamicsSimulator::JOINT_ACCELERATION,"Joint Acceleration"},
00050         { DynamicsSimulator::JOINT_TORQUE,              "Joint Torque"},
00051         { DynamicsSimulator::ABS_TRANSFORM,             "Absolute Transform"},
00052         { DynamicsSimulator::ABS_VELOCITY,              "Absolute Velocity"},
00053         { DynamicsSimulator::EXTERNAL_FORCE,    "External Force"},
00054         { -1, "" },
00055     };
00056 
00057     void initializeCommandLabelMaps()
00058     {
00059         if(commandLabelMap.empty()){
00060             int i = 0;
00061             while(true){
00062                 IdLabel& idLabel = commandLabels[i++];
00063                 if(idLabel.id == -1){
00064                     break;
00065                 }
00066                 commandLabelMap[idLabel.id] = idLabel.label;
00067             }
00068         }
00069     }
00070 
00071     const char* getLabelOfLinkDataType(DynamicsSimulator::LinkDataType type)
00072     {
00073         IdToLabelMap::iterator p = commandLabelMap.find(type);
00074         return (p != commandLabelMap.end()) ? p->second : "Requesting Unknown Data Type";
00075     }
00076 };
00077 
00078 
00079 template<typename X, typename X_ptr>
00080 X_ptr checkCorbaServer(std::string n, CosNaming::NamingContext_var &cxt)
00081 {
00082     CosNaming::Name ncName;
00083     ncName.length(1);
00084     ncName[0].id = CORBA::string_dup(n.c_str());
00085     ncName[0].kind = CORBA::string_dup("");
00086     X_ptr srv = NULL;
00087     try {
00088         srv = X::_narrow(cxt->resolve(ncName));
00089     } catch(const CosNaming::NamingContext::NotFound &exc) {
00090         std::cerr << n << " not found: ";
00091         switch(exc.why) {
00092         case CosNaming::NamingContext::missing_node:
00093             std::cerr << "Missing Node" << std::endl;
00094         case CosNaming::NamingContext::not_context:
00095             std::cerr << "Not Context" << std::endl;
00096             break;
00097         case CosNaming::NamingContext::not_object:
00098             std::cerr << "Not Object" << std::endl;
00099             break;
00100         }
00101         return (X_ptr)NULL;
00102     } catch(CosNaming::NamingContext::CannotProceed &exc) {
00103         std::cerr << "Resolve " << n << " CannotProceed" << std::endl;
00104     } catch(CosNaming::NamingContext::AlreadyBound &exc) {
00105         std::cerr << "Resolve " << n << " InvalidName" << std::endl;
00106     }
00107     return srv;
00108 }
00109 
00110 
00111 ODE_DynamicsSimulator_impl::ODE_DynamicsSimulator_impl(CORBA::ORB_ptr orb)
00112     : orb_(CORBA::ORB::_duplicate(orb))
00113 {
00114     if(debugMode){
00115         cout << "ODE_DynamicsSimulator_impl::ODE_DynamicsSimulator_impl()" << endl;
00116     }
00117 
00118     // default integration method
00119     world.setRungeKuttaMethod();
00120 
00121     world.useInternalCollisionDetector(USE_ODE_COLLISION_DETECTOR);
00122     if(!USE_INTERNAL_COLLISION_DETECTOR){
00123         // resolve collisionchecker object
00124         CollisionDetectorFactory_var collisionDetectorFactory;
00125         
00126         CORBA::Object_var nS = orb->resolve_initial_references("NameService");
00127         CosNaming::NamingContext_var cxT;
00128         cxT = CosNaming::NamingContext::_narrow(nS);
00129         collisionDetectorFactory =
00130             checkCorbaServer<CollisionDetectorFactory,
00131             CollisionDetectorFactory_var>("CollisionDetectorFactory", cxT);
00132         if (CORBA::is_nil(collisionDetectorFactory)) {
00133             std::cerr << "CollisionDetectorFactory not found" << std::endl;
00134         }
00135 
00136         try {
00137             collisionDetector = collisionDetectorFactory->create();
00138         }catch(...){
00139             std::cerr << "failed to create CollisionDetector" << std::endl;
00140         }
00141     }
00142 
00143     collisions = new CollisionSequence;
00144     collidingLinkPairs = new LinkPairSequence;
00145     allCharacterPositions = new CharacterPositionSequence;
00146     allCharacterSensorStates = new SensorStateSequence;
00147 
00148     needToUpdatePositions = true;
00149     needToUpdateSensorStates = true;
00150 }
00151 
00152 
00153 ODE_DynamicsSimulator_impl::~ODE_DynamicsSimulator_impl()
00154 {
00155     if(debugMode){
00156         cout << "ODE_DynamicsSimulator_impl::~ODE_DynamicsSimulator_impl()" << endl;
00157     }
00158     if(!USE_INTERNAL_COLLISION_DETECTOR)
00159         collisionDetector->destroy();
00160 }
00161 
00162 
00163 void ODE_DynamicsSimulator_impl::destroy()
00164 {
00165     if(debugMode){
00166         cout << "ODE_DynamicsSimulator_impl::destroy()" << endl;
00167     }
00168 
00169     PortableServer::POA_var poa_ = _default_POA();
00170     PortableServer::ObjectId_var id = poa_->servant_to_id(this);
00171     poa_->deactivate_object(id);
00172 }
00173 
00174 
00175 void ODE_DynamicsSimulator_impl::registerCharacter
00176 (
00177     const char *name,
00178     BodyInfo_ptr bodyInfo
00179     )
00180 {
00181     if(debugMode){
00182         cout << "ODE_DynamicsSimulator_impl::registerCharacter("
00183              << name << ", " << bodyInfo << " )" << std::endl;
00184     }
00185 
00186     world.addBody(bodyInfo, name);
00187 
00188     if(!USE_INTERNAL_COLLISION_DETECTOR){
00189         collisionDetector->registerCharacter(name, bodyInfo);
00190     }
00191 }
00192 
00193 
00194 void ODE_DynamicsSimulator_impl::init
00195 (
00196     CORBA::Double timeStep,
00197     OpenHRP::DynamicsSimulator::IntegrateMethod integrateOpt,
00198     OpenHRP::DynamicsSimulator::SensorOption sensorOpt
00199     )
00200 {
00201     if(debugMode){
00202         cout << "ODE_DynamicsSimulator_impl::init(" << timeStep << ", ";
00203         cout << (integrateOpt == OpenHRP::DynamicsSimulator::EULER ? "EULER, " : "RUNGE_KUTTA, ");
00204         cout << (sensorOpt == OpenHRP::DynamicsSimulator::DISABLE_SENSOR ? "DISABLE_SENSOR" : "ENABLE_SENSOR");
00205         std::cout << ")" << std::endl;
00206     }
00207 
00208     world.setTimeStep(timeStep);
00209     world.setCurrentTime(0.0);
00210 
00211     if(integrateOpt == OpenHRP::DynamicsSimulator::EULER){
00212         world.setEulerMethod();
00213     } else {
00214         world.setRungeKuttaMethod();
00215     }
00216 
00217     world.enableSensors((sensorOpt == OpenHRP::DynamicsSimulator::ENABLE_SENSOR));
00218 
00219     int n = world.numBodies();
00220     for(int i=0; i < n; ++i){
00221         world.body(i)->initializeConfiguration();
00222     }
00223 
00224     _setupCharacterData();
00225 
00226 #ifdef MEASURE_TIME
00227     processTime = 0;
00228 #endif
00229 }
00230 
00231 
00232 void ODE_DynamicsSimulator_impl::registerCollisionCheckPair
00233 (
00234     const char *charName1,
00235     const char *linkName1,
00236     const char *charName2,
00237     const char *linkName2,
00238     const CORBA::Double staticFriction,
00239     const CORBA::Double slipFriction,
00240     const DblSequence6 & K,
00241     const DblSequence6 & C,
00242     const double culling_thresh,
00243     const double restitution
00244     )
00245 {
00246     const double epsilon = 0.0;
00247 
00248     if(debugMode){
00249         cout << "DynamicsSimulator_impl::registerCollisionCheckPair("
00250              << charName1 << ", " << linkName1 << ", "
00251              << charName2 << ", " << linkName2 << ", "
00252              << staticFriction << ", " << slipFriction;
00253         if((K.length() == 6) && (C.length() == 6)){
00254             cout << ",\n"
00255                  << "{ "
00256                  << K[CORBA::ULong(0)] << ", "
00257                  << K[CORBA::ULong(1)] << ", "
00258                  << K[CORBA::ULong(2)] << " }\n"
00259                  << "{ "
00260                  << K[CORBA::ULong(3)] << ", "
00261                  << K[CORBA::ULong(4)] << ", "
00262                  << K[CORBA::ULong(5)] << " }\n"
00263                  << ",\n"
00264                  << "{ "
00265                  << C[CORBA::ULong(0)] << ", "
00266                  << C[CORBA::ULong(1)] << ", "
00267                  << C[CORBA::ULong(2)] << " }\n"
00268                  << "{ "
00269                  << C[CORBA::ULong(3)] << ", "
00270                  << C[CORBA::ULong(4)] << ", "
00271                  << C[CORBA::ULong(5)] << " }\n"
00272                  << ")" << endl;
00273         } else {
00274             cout << ", NULL, NULL)" << endl;
00275         }
00276     }
00277 
00278     int bodyIndex1 = world.bodyIndex(charName1);
00279     int bodyIndex2 = world.bodyIndex(charName2);
00280 
00281     if(bodyIndex1 >= 0 && bodyIndex2 >= 0){
00282 
00283         BodyPtr body1 = world.body(bodyIndex1);
00284         BodyPtr body2 = world.body(bodyIndex2);
00285 
00286         std::string emptyString = "";
00287         vector<Link*> links1;
00288         if(emptyString == linkName1){
00289             const LinkTraverse& traverse = body1->linkTraverse();
00290             links1.resize(traverse.numLinks());
00291             std::copy(traverse.begin(), traverse.end(), links1.begin());
00292         } else {
00293             links1.push_back(body1->link(linkName1));
00294         }
00295 
00296         vector<Link*> links2;
00297         if(emptyString == linkName2){
00298             const LinkTraverse& traverse = body2->linkTraverse();
00299             links2.resize(traverse.numLinks());
00300             std::copy(traverse.begin(), traverse.end(), links2.begin());
00301         } else {
00302             links2.push_back(body2->link(linkName2));
00303         }
00304 
00305         for(size_t i=0; i < links1.size(); ++i){
00306             for(size_t j=0; j < links2.size(); ++j){
00307                 Link* link1 = links1[i];
00308                 Link* link2 = links2[j];
00309 
00310                 if(link1 && link2 && link1 != link2){
00311                     LinkPair_var linkPair = new LinkPair();
00312                     linkPair->charName1  = CORBA::string_dup(charName1);
00313                     linkPair->linkName1 = CORBA::string_dup(link1->name.c_str());
00314                     linkPair->charName2  = CORBA::string_dup(charName2);
00315                     linkPair->linkName2 = CORBA::string_dup(link2->name.c_str());
00316                     linkPair->tolerance = 0;
00317                     if(!USE_INTERNAL_COLLISION_DETECTOR)
00318                         collisionDetector->addCollisionPair(linkPair);
00319                     if(USE_ODE_COLLISION_DETECTOR)
00320                         world.addCollisionPair(linkPair);
00321                 }
00322             }
00323         }
00324     }
00325 
00326 }
00327 
00328 
00329 void ODE_DynamicsSimulator_impl::registerIntersectionCheckPair
00330 (
00331     const char *charName1,
00332     const char *linkName1,
00333     const char *charName2,
00334     const char *linkName2,
00335     const double tolerance
00336     )
00337 {
00338     const double epsilon = 0.0;
00339 
00340     if(debugMode){
00341         cout << "DynamicsSimulator_impl::registerIntersectionCheckPair("
00342              << charName1 << ", " << linkName1 << ", "
00343              << charName2 << ", " << linkName2 << ", "
00344              << tolerance << ")" << endl;
00345     }
00346 
00347     int bodyIndex1 = world.bodyIndex(charName1);
00348     int bodyIndex2 = world.bodyIndex(charName2);
00349 
00350     if(bodyIndex1 >= 0 && bodyIndex2 >= 0){
00351 
00352         BodyPtr body1 = world.body(bodyIndex1);
00353         BodyPtr body2 = world.body(bodyIndex2);
00354 
00355         std::string emptyString = "";
00356         vector<Link*> links1;
00357         if(emptyString == linkName1){
00358             const LinkTraverse& traverse = body1->linkTraverse();
00359             links1.resize(traverse.numLinks());
00360             std::copy(traverse.begin(), traverse.end(), links1.begin());
00361         } else {
00362             links1.push_back(body1->link(linkName1));
00363         }
00364 
00365         vector<Link*> links2;
00366         if(emptyString == linkName2){
00367             const LinkTraverse& traverse = body2->linkTraverse();
00368             links2.resize(traverse.numLinks());
00369             std::copy(traverse.begin(), traverse.end(), links2.begin());
00370         } else {
00371             links2.push_back(body2->link(linkName2));
00372         }
00373 
00374         for(size_t i=0; i < links1.size(); ++i){
00375             for(size_t j=0; j < links2.size(); ++j){
00376                 Link* link1 = links1[i];
00377                 Link* link2 = links2[j];
00378 
00379                 if(link1 && link2 && link1 != link2){
00380                     LinkPair_var linkPair = new LinkPair();
00381                     linkPair->charName1  = CORBA::string_dup(charName1);
00382                     linkPair->linkName1 = CORBA::string_dup(link1->name.c_str());
00383                     linkPair->charName2  = CORBA::string_dup(charName2);
00384                     linkPair->linkName2 = CORBA::string_dup(link2->name.c_str());
00385                     linkPair->tolerance = 0;
00386                     if(!USE_INTERNAL_COLLISION_DETECTOR)
00387                         collisionDetector->addCollisionPair(linkPair);
00388                     if(USE_ODE_COLLISION_DETECTOR)
00389                         world.addCollisionPair(linkPair);
00390                 }
00391             }
00392         }
00393     }
00394 }
00395 
00396 void ODE_DynamicsSimulator_impl::registerExtraJoint
00397                 (
00398                         const char*     charName1,
00399                         const char*     linkName1,
00400                         const char*     charName2,
00401                         const char*     linkName2,
00402                         const DblSequence3&     link1LocalPos,
00403                         const DblSequence3&     link2LocalPos,
00404                         const ExtraJointType jointType,
00405                         const DblSequence3&     jointAxis,
00406                         const char*                     extraJointName)
00407 {
00408 }
00409 
00411 void ODE_DynamicsSimulator_impl::getExtraJointConstraintForce
00412 (
00413     const char * characterName,
00414     const char * extraJointName,
00415     DblSequence6_out contactForce
00416     )
00417 {
00418     if(debugMode){
00419         cout << "DynamicsSimulator_impl::getConnectionConstraintForce("
00420              << characterName << ", " << extraJointName << ")" << endl;
00421     }
00422 }
00423 
00424 
00425 void ODE_DynamicsSimulator_impl::getCharacterSensorValues
00426 (
00427     const char *characterName,
00428     const char *sensorName,
00429     DblSequence_out sensorOutput
00430     )
00431 {
00432     if(debugMode){
00433         cout << "DynamicsSimulator_impl::getCharacterSensorData("
00434              << characterName << ", " << sensorName << ")";
00435     }
00436 
00437     BodyPtr body = world.body(characterName);
00438     if(!body){
00439         std::cerr << "not found! :" << characterName << std::endl;
00440         return;
00441     }
00442 
00443     sensorOutput = new DblSequence;
00444     Sensor* sensor = body->sensor<Sensor>(sensorName);
00445 
00446     if(sensor){
00447         switch(sensor->type){
00448 
00449         case Sensor::FORCE:
00450         {
00451             ForceSensor* forceSensor = static_cast<ForceSensor*>(sensor);
00452             sensorOutput->length(6);
00453 #ifndef __WIN32__
00454             setVector3(forceSensor->f, sensorOutput);
00455             setVector3(forceSensor->tau, sensorOutput, 3);
00456 #else
00457             sensorOutput[CORBA::ULong(0)] = forceSensor->f(0); sensorOutput[CORBA::ULong(1)] = forceSensor->f(1); sensorOutput[CORBA::ULong(2)] = forceSensor->f(2);
00458             sensorOutput[CORBA::ULong(3)] = forceSensor->tau(0); sensorOutput[CORBA::ULong(4)] = forceSensor->tau(1); sensorOutput[CORBA::ULong(5)] = forceSensor->tau(2);
00459 #endif
00460                 
00461         }
00462         break;
00463 
00464         case Sensor::RATE_GYRO:
00465         {
00466             RateGyroSensor* gyro = static_cast<RateGyroSensor*>(sensor);
00467             sensorOutput->length(3);
00468 #ifndef __WIN32__
00469             setVector3(gyro->w, sensorOutput);
00470 #else   
00471             sensorOutput[CORBA::ULong(0)] = gyro->w(0); sensorOutput[CORBA::ULong(1)] = gyro->w(1); sensorOutput[CORBA::ULong(2)] = gyro->w(2);
00472 #endif
00473         }
00474         break;
00475 
00476         case Sensor::ACCELERATION:
00477         {
00478             AccelSensor* accelSensor = static_cast<AccelSensor*>(sensor);
00479             sensorOutput->length(3);
00480 #ifndef __WIN32__
00481             setVector3(accelSensor->dv, sensorOutput);
00482 #else
00483             sensorOutput[CORBA::ULong(0)] = accelSensor->dv(0); sensorOutput[CORBA::ULong(1)] = accelSensor->dv(1); sensorOutput[CORBA::ULong(2)] = accelSensor->dv(2);
00484 #endif
00485         }
00486         break;
00487 
00488         case Sensor::RANGE:
00489         {
00490             RangeSensor *rangeSensor = static_cast<RangeSensor*>(sensor);
00491             sensorOutput->length(rangeSensor->distances.size());
00492             for (unsigned int i=0; i<rangeSensor->distances.size(); i++){
00493                 sensorOutput[i] = rangeSensor->distances[i];
00494             }
00495         }
00496         break;
00497 
00498         default:
00499             break;
00500         }
00501     }
00502 
00503     if(debugMode){
00504         cout << "DynamicsSimulator_impl::getCharacterSensorData - output\n"
00505              << "( " << sensorOutput[CORBA::ULong(0)];
00506 
00507         CORBA::ULong i = 0;
00508         while(true){
00509             cout << sensorOutput[i++];
00510             if(i == sensorOutput->length()) break;
00511             cout << ",";
00512         }
00513 
00514         cout << ")" << endl;
00515     }
00516 
00517 }
00518 
00519 
00520 void ODE_DynamicsSimulator_impl::initSimulation()
00521 {
00522     if(debugMode){
00523         cout << "DynamicsSimulator_impl::initSimulation()" << endl;
00524     }
00525 
00526     world.initialize();
00527 
00528     _updateCharacterPositions();
00529 
00530     if(!USE_ODE_COLLISION_DETECTOR){
00531         collisionDetector->queryContactDeterminationForDefinedPairs(allCharacterPositions.in(), collisions.out());
00532     }
00533 
00534     if(enableTimeMeasure){
00535         timeMeasureFinished = false;
00536         timeMeasureStarted = false;
00537     }
00538 
00539 }
00540 
00541 
00542 void ODE_DynamicsSimulator_impl::stepSimulation()
00543 {
00544     if(enableTimeMeasure){
00545         if(!timeMeasureStarted){
00546             timeMeasure1.begin();
00547             timeMeasureStarted = true;
00548         }
00549     }
00550 
00551     if(debugMode){
00552         cout << "DynamicsSimulator_impl::stepSimulation()" << endl;
00553     }
00554 
00555     world.clearExternalForces();
00556 
00557     if(enableTimeMeasure) timeMeasure2.begin();
00558     world.calcNextState(collisions);
00559 
00560     needToUpdateSensorStates = true;
00561 
00562     _updateCharacterPositions();
00563     if(enableTimeMeasure) timeMeasure2.end();
00564 
00565     if(enableTimeMeasure) timeMeasure3.begin();
00566     if(!USE_ODE_COLLISION_DETECTOR){
00567         collisionDetector->queryContactDeterminationForDefinedPairs(allCharacterPositions.in(), collisions.out());
00568     }
00569     if(enableTimeMeasure) timeMeasure3.end();
00570 
00571     if(enableTimeMeasure){
00572         if(world.currentTime() > 5.0 && !timeMeasureFinished){
00573             timeMeasureFinished = true;
00574             timeMeasure1.end();
00575             cout << "Total elapsed time = " << timeMeasure1.totalTime() << "\n"
00576                  << "Internal elapsed time = " << timeMeasure2.totalTime()
00577                  << ", the avarage = " << timeMeasure2.averageTime() << endl;
00578             cout << "Collision check time = " << timeMeasure3.totalTime() << endl;
00579         }
00580     }
00581 
00582 }
00583 
00584 
00585 void ODE_DynamicsSimulator_impl::setCharacterLinkData
00586 (
00587     const char* characterName,
00588     const char* linkName,
00589     OpenHRP::DynamicsSimulator::LinkDataType type,
00590     const DblSequence& wdata
00591     )
00592 {
00593     if(debugMode){
00594         cout << "DynamicsSimulator_impl::setCharacterLinkData("
00595              << characterName << ", " << linkName << ", "
00596              << getLabelOfLinkDataType(type) << ", ";
00597         switch(type) {
00598 
00599         case OpenHRP::DynamicsSimulator::POSITION_GIVEN:
00600         case OpenHRP::DynamicsSimulator::JOINT_VALUE:
00601         case OpenHRP::DynamicsSimulator::JOINT_VELOCITY:
00602         case OpenHRP::DynamicsSimulator::JOINT_ACCELERATION:
00603         case OpenHRP::DynamicsSimulator::JOINT_TORQUE:
00604             cout << wdata[CORBA::ULong(0)] << ")\n";
00605             break;
00606 
00607         case OpenHRP::DynamicsSimulator::ABS_TRANSFORM: // 12x1
00608             cout << wdata[CORBA::ULong(0)] << ", "
00609                  << wdata[CORBA::ULong(1)] << ", "
00610                  << wdata[CORBA::ULong(2)] << ",\n"
00611                  << wdata[CORBA::ULong(3)] << ", "
00612                  << wdata[CORBA::ULong(4)] << ", "
00613                  << wdata[CORBA::ULong(5)] << ",\n"
00614                  << wdata[CORBA::ULong(6)] << ","
00615                  << wdata[CORBA::ULong(7)] << ", "
00616                  << wdata[CORBA::ULong(8)] << ",\n "
00617                  << wdata[CORBA::ULong(9)] << ","
00618                  << wdata[CORBA::ULong(10)] << ", "
00619                  << wdata[CORBA::ULong(11)] << ")" << endl;
00620             break;
00621 
00622         default: // 3x1
00623             cout << wdata[CORBA::ULong(0)] << ", "
00624                  << wdata[CORBA::ULong(1)] << ", "
00625                  << wdata[CORBA::ULong(2)] << ")" << endl;
00626         }
00627     }
00628 
00629     BodyPtr body = world.body(characterName);
00630     if(!body){
00631         std::cerr << "not found! :" << characterName << std::endl;
00632         return;
00633     }
00634     ODE_Link* link = (ODE_Link*)(body->link(linkName));
00635     if(!link){
00636         std::cerr << "not found! :" << linkName << std::endl;
00637         return;
00638     }
00639 
00640     switch(type) {
00641 
00642     case OpenHRP::DynamicsSimulator::POSITION_GIVEN:
00643         link->isHighGainMode = (wdata[0] > 0.0);
00644         if(link->isHighGainMode){
00645             std::cerr << " HighGainMode is unsupported. " << std::endl;
00646         }
00647         break;
00648 
00649     case OpenHRP::DynamicsSimulator::JOINT_VALUE:
00650         if(link->jointType != Link::FIXED_JOINT)
00651             link->q = wdata[0];
00652         break;
00653 
00654     case OpenHRP::DynamicsSimulator::JOINT_VELOCITY:
00655         if(link->jointType != Link::FIXED_JOINT)
00656             link->dq = wdata[0];
00657         break;
00658 
00659     case OpenHRP::DynamicsSimulator::JOINT_ACCELERATION:
00660         if(link->jointType != Link::FIXED_JOINT)
00661             link->ddq = wdata[0];
00662         break;
00663 
00664     case OpenHRP::DynamicsSimulator::JOINT_TORQUE:
00665         if(link->jointType != Link::FIXED_JOINT){
00666             link->u = wdata[0];
00667             link->setTorque(wdata[0]);
00668         }
00669         break;
00670 
00671     case OpenHRP::DynamicsSimulator::ABS_TRANSFORM:
00672     {
00673         link->p(0) = wdata[0];
00674         link->p(1) = wdata[1];
00675         link->p(2) = wdata[2];
00676         Matrix33 R;
00677         getMatrix33FromRowMajorArray(R, wdata.get_buffer(), 3);
00678         link->setSegmentAttitude(R);
00679         link->setTransform(link->p, R);
00680      }
00681     break;
00682         
00683     case OpenHRP::DynamicsSimulator::ABS_VELOCITY:
00684     {
00685         link->v(0) = wdata[0];
00686         link->v(1) = wdata[1];
00687         link->v(2) = wdata[2];
00688         link->w(0) = wdata[3];
00689         link->w(1) = wdata[4];
00690         link->w(2) = wdata[5];
00691         link->setAbsVelocity(link->v, link->w);
00692     }
00693     break;
00694 
00695     case OpenHRP::DynamicsSimulator::ABS_ACCELERATION:
00696     {
00697         std::cerr << "ERROR - Invalid type: " << getLabelOfLinkDataType(type) << endl;
00698     }
00699     break;
00700 
00701     case OpenHRP::DynamicsSimulator::EXTERNAL_FORCE:  
00702     {
00703         link->setForce(wdata[0], wdata[1], wdata[2] );
00704         link->setTorque(wdata[3], wdata[4], wdata[5] );
00705         break;
00706     }
00707         
00708     default:
00709         return;
00710     }
00711 
00712     needToUpdatePositions = true;
00713     needToUpdateSensorStates = true;
00714 
00715 }
00716 
00717 
00718 void ODE_DynamicsSimulator_impl::getCharacterLinkData
00719 (
00720     const char * characterName,
00721     const char * linkName,
00722     OpenHRP::DynamicsSimulator::LinkDataType type,
00723     DblSequence_out out_rdata
00724     )
00725 {
00726     if(debugMode){
00727         cout << "DynamicsSimulator_impl::getCharacterLinkData("
00728              << characterName << ", " << linkName << ", "
00729              << getLabelOfLinkDataType(type) << ")" << endl;
00730     }
00731 
00732     BodyPtr body = world.body(characterName);
00733     if(!body){
00734         std::cerr << "not found! :" << characterName << std::endl;
00735         return;
00736     }
00737     ODE_Link* link = (ODE_Link*)body->link(linkName);
00738     if(!link){
00739         std::cerr << "not found! :" << linkName << std::endl;
00740         return;
00741     }
00742 
00743     DblSequence_var rdata = new DblSequence;
00744 
00745     switch(type) {
00746 
00747     case OpenHRP::DynamicsSimulator::JOINT_VALUE:
00748         rdata->length(1);
00749         rdata[0] = link->q;
00750         break;
00751 
00752     case OpenHRP::DynamicsSimulator::JOINT_VELOCITY:
00753         rdata->length(1);
00754         rdata[0] = link->dq;
00755         break;
00756 
00757     case OpenHRP::DynamicsSimulator::JOINT_ACCELERATION:
00758         std::cerr << "ERROR - Invalid type: " << getLabelOfLinkDataType(type) << endl;
00759         rdata->length(1);
00760         rdata[0] = 0;
00761         break;
00762 
00763     case OpenHRP::DynamicsSimulator::JOINT_TORQUE:
00764         rdata->length(1);
00765         rdata[0] = link->u;
00766         break;
00767 
00768     case OpenHRP::DynamicsSimulator::ABS_TRANSFORM:
00769     {
00770         rdata->length(12);
00771         hrp::Vector3 p;
00772         hrp::Matrix33 R;
00773         link->getTransform(p, R);
00774         rdata[0] = p(0);
00775         rdata[1] = p(1);
00776         rdata[2] = p(2);
00777         double* buf = rdata->get_buffer();
00778         setMatrix33ToRowMajorArray(R, buf, 3);
00779     }
00780     break;
00781 
00782     case OpenHRP::DynamicsSimulator::ABS_VELOCITY:
00783         {
00784         rdata->length(6);
00785         const dReal* w = link->getAngularVel();
00786         hrp::Vector3 v;
00787         link->getLinearVel(v);
00788         rdata[0] = v(0);
00789         rdata[1] = v(1);
00790         rdata[2] = v(2);
00791         rdata[3] = w[0];
00792         rdata[4] = w[1];
00793         rdata[5] = w[2];
00794         }
00795         break;
00796 
00797     case OpenHRP::DynamicsSimulator::EXTERNAL_FORCE:
00798         {
00799         rdata->length(6);
00800         const dReal* f = link->getForce();
00801         const dReal* tau = link->getTorque();
00802         rdata[0] = f[0];
00803         rdata[1] = f[1];
00804         rdata[2] = f[2];
00805         rdata[3] = tau[0];
00806         rdata[4] = tau[1];
00807         rdata[5] = tau[2];
00808         }
00809         break;
00810 
00811     case OpenHRP::DynamicsSimulator::CONSTRAINT_FORCE: {
00812         std::cerr << "ERROR - Invalid type: " << getLabelOfLinkDataType(type) << endl;
00813         rdata->length(0);
00814     }
00815         break;
00816 
00817     default:
00818         break;
00819     }
00820 
00821     out_rdata = rdata._retn();
00822 
00823 }
00824 
00825 
00826 void ODE_DynamicsSimulator_impl::getCharacterAllLinkData
00827 (
00828     const char * characterName,
00829     OpenHRP::DynamicsSimulator::LinkDataType type,
00830     DblSequence_out rdata
00831     )
00832 {
00833     if(debugMode){
00834         cout << "DynamicsSimulator_impl::getCharacterAllLinkData("
00835              << characterName << ", "
00836              << getLabelOfLinkDataType(type) << ")" << endl;
00837     }
00838 
00839     BodyPtr body = world.body(characterName);
00840     if(!body){
00841         std::cerr << "not found! :" << characterName << std::endl;
00842         return;
00843     }
00844     int n = body->numJoints();
00845     rdata = new DblSequence();
00846     rdata->length(n);
00847 
00848     switch(type) {
00849 
00850     case OpenHRP::DynamicsSimulator::JOINT_VALUE:
00851         for(int i=0; i < n; ++i){
00852             (*rdata)[i] =  body->joint(i)->q;
00853         }
00854         break;
00855 
00856     case OpenHRP::DynamicsSimulator::JOINT_VELOCITY:
00857         for(int i=0; i < n; ++i){
00858             (*rdata)[i] =  body->joint(i)->dq;
00859         }
00860         break;
00861 
00862     case OpenHRP::DynamicsSimulator::JOINT_ACCELERATION:
00863         std::cerr << "ERROR - Invalid type: " << getLabelOfLinkDataType(type) << endl;
00864         for(int i=0; i < n; ++i){
00865             (*rdata)[i] = 0;
00866         }
00867         break;
00868 
00869     case OpenHRP::DynamicsSimulator::JOINT_TORQUE:
00870         for(int i=0; i < n; ++i){
00871             (*rdata)[i] = body->joint(i)->u;
00872         }
00873         break;
00874 
00875     default:
00876         cerr << "ERROR - Invalid type: " << getLabelOfLinkDataType(type) << endl;
00877         break;
00878     }
00879 
00880 }
00881 
00882 
00883 void ODE_DynamicsSimulator_impl::setCharacterAllLinkData
00884 (
00885     const char * characterName,
00886     OpenHRP::DynamicsSimulator::LinkDataType type,
00887     const DblSequence & wdata
00888     )
00889 {
00890     if(debugMode){
00891         cout << "DynamicsSimulator_impl::setCharacterAllLinkData("
00892              << characterName << ", "
00893              << getLabelOfLinkDataType(type) << ",\n(";
00894         if(wdata.length()) cout << wdata[0];
00895         for(CORBA::ULong i=0; i<wdata.length(); ++i){
00896             cout << ", " << wdata[i];
00897         }
00898         cout << "))" << endl;
00899     }
00900 
00901     BodyPtr body = world.body(characterName);
00902     if(!body){
00903         std::cerr << "not found! :" << characterName << std::endl;
00904         return;
00905     }
00906 
00907     int n = wdata.length();
00908     if(n > body->numJoints()){
00909         n = body->numJoints();
00910     }
00911 
00912     switch(type) {
00913 
00914     case OpenHRP::DynamicsSimulator::JOINT_VALUE:
00915         for(int i=0; i < n; ++i){
00916             if(body->joint(i)->jointType != Link::FIXED_JOINT)
00917                 body->joint(i)->q = wdata[i];
00918         }
00919         break;
00920 
00921     case OpenHRP::DynamicsSimulator::JOINT_VELOCITY:
00922         for(int i=0; i < n; ++i){
00923             if(body->joint(i)->jointType != Link::FIXED_JOINT)
00924                 body->joint(i)->dq = wdata[i];
00925         }
00926         break;
00927 
00928     case OpenHRP::DynamicsSimulator::JOINT_ACCELERATION:
00929         for(int i=0; i < n; ++i){
00930             if(body->joint(i)->jointType != Link::FIXED_JOINT)
00931                 body->joint(i)->ddq = wdata[i];
00932         }
00933         break;
00934 
00935     case OpenHRP::DynamicsSimulator::JOINT_TORQUE:
00936         for(int i=0; i < n; ++i){
00937             if(body->joint(i)->jointType != Link::FIXED_JOINT){
00938                 body->joint(i)->u = wdata[i];
00939                 ((ODE_Link*)body->joint(i))->setTorque(wdata[i]);
00940             }
00941         }
00942         break;
00943 
00944     default:
00945         std::cerr << "ERROR - Invalid type: " << getLabelOfLinkDataType(type) << endl;
00946     }
00947 
00948     needToUpdatePositions = true;
00949     needToUpdateSensorStates = true;
00950 
00951 }
00952 
00953 void ODE_DynamicsSimulator_impl::setGVector
00954 (
00955     const DblSequence3& wdata
00956     )
00957 {
00958     if(wdata.length() != 3){
00959         std::cerr << "setGVector : The data length is not three. " << std::endl;
00960         return;
00961     }
00962 
00963     dVector3 g;
00964     g[0] = wdata[CORBA::ULong(0)];
00965     g[1] = wdata[CORBA::ULong(1)];
00966     g[2] = -wdata[CORBA::ULong(2)];
00967     world.setGravityAcceleration(g);
00968 
00969     if(debugMode){
00970         cout << "ODE_DynamicsSimulator_impl::setGVector("
00971              << wdata[CORBA::ULong(0)] << ", "
00972              << wdata[CORBA::ULong(1)] << ", "
00973              << wdata[CORBA::ULong(2)] << ")" << endl;
00974     }
00975 }
00976 
00977 
00978 void ODE_DynamicsSimulator_impl::getGVector
00979 (
00980     DblSequence3_out wdata
00981     )
00982 {
00983     wdata->length(3);
00984     dVector3 g;
00985     world.getGravityAcceleration(g);
00986     (*wdata)[0] = g[0];
00987     (*wdata)[1] = g[1];
00988     (*wdata)[2] = g[2];
00989 
00990     if(debugMode){
00991         cout << "DynamicsSimulator_impl::getGVector(";
00992         cout << wdata[CORBA::ULong(0)] << ", "
00993              << wdata[CORBA::ULong(1)] << ", "
00994              << wdata[CORBA::ULong(2)] << ")" << endl;
00995     }
00996 }
00997 
00998 
00999 void ODE_DynamicsSimulator_impl::setCharacterAllJointModes
01000 (
01001     const char * characterName,
01002     OpenHRP::DynamicsSimulator::JointDriveMode jointMode
01003     )
01004 {
01005     bool isHighGainMode = (jointMode == OpenHRP::DynamicsSimulator::HIGH_GAIN_MODE);
01006 
01007     BodyPtr body = world.body(characterName);
01008     if(!body){
01009         std::cerr << "not found! :" << characterName << std::endl;
01010         return;
01011     }
01012 
01013     for(int i=1; i < body->numLinks(); ++i){
01014         body->link(i)->isHighGainMode = isHighGainMode;
01015     }
01016     
01017     if(isHighGainMode){
01018         std::cerr << " HighGainMode is unsupported. " << std::endl;
01019     }
01020 
01021     if(debugMode){
01022         cout << "DynamicsSimulator_impl::setCharacterAllJointModes(";
01023         cout << characterName << ", ";
01024         cout << (isHighGainMode ? "HIGH_GAIN_MODE" : "TORQUE_MODE");
01025         cout << ")" << endl;
01026     }
01027 
01028 }
01029 
01030 
01031 CORBA::Boolean ODE_DynamicsSimulator_impl::calcCharacterInverseKinematics
01032 (
01033     const char * characterName,
01034     const char * fromLink, const char * toLink,
01035     const LinkPosition& target
01036     )
01037 {
01038     if(debugMode){
01039         cout << "DynamicsSimulator_impl::calcCharacterInverseKinematics("
01040              << characterName << ", " << fromLink << ", " << toLink << ",\n"
01041              << "( "
01042              << target.p[0] << ", " << target.p[1] << ", " << target.p[2] << ",\n\n"
01043              << target.R[0] << ", " << target.R[1] << ", " << target.R[2] << ",\n"
01044              << target.R[3] << ", " << target.R[4] << ", " << target.R[5] << ",\n"
01045              << target.R[6] << ", " << target.R[7] << ", " << target.R[8] << endl;
01046     }
01047 
01048     bool solved = false;
01049 
01050     BodyPtr body = world.body(characterName);
01051     if(!body){
01052         std::cerr << "not found! :" << characterName << std::endl;
01053         return false;
01054     }
01055 
01056     JointPath path(body->link(fromLink), body->link(toLink));
01057 
01058     if(!path.empty()){
01059         Vector3 p(target.p[0], target.p[1], target.p[2]);
01060         Matrix33 R;
01061         for (int i=0; i<3; i++) {
01062             for (int j=0; j<3; j++){
01063                 R(i,j) = target.R[3*i+j];
01064             }
01065         }
01066 
01067         solved = path.calcInverseKinematics(p, R);
01068 
01069         if(solved) {
01070             needToUpdatePositions = true;
01071             needToUpdateSensorStates = true;
01072         }
01073     }
01074 
01075     return solved;
01076 
01077     return 0;
01078 }
01079 
01080 
01081 void ODE_DynamicsSimulator_impl::calcCharacterForwardKinematics
01082 (
01083     const char * characterName
01084     )
01085 {
01086     if(debugMode){
01087         cout << "DynamicsSimulator_impl::calcCharacterForwardKinematics( "
01088              << characterName << endl;
01089     }
01090 
01091     BodyPtr body = world.body(characterName);
01092     if(!body){
01093         std::cerr << "not found! :" << characterName << std::endl;
01094         return;
01095     }
01096     body->calcForwardKinematics(true, true);
01097     
01098     for(int j=0; j<body->numLinks(); j++){
01099         ODE_Link* link = (ODE_Link*)body->link(j);
01100         link->setTransform(link->p, link->segmentAttitude());
01101         link->setAbsVelocity(link->v, link->w);
01102     }
01103 
01104     needToUpdatePositions = true;
01105     needToUpdateSensorStates = true;
01106 
01107 }
01108 
01109 
01110 void ODE_DynamicsSimulator_impl::calcWorldForwardKinematics()
01111 {
01112 
01113     for(int i=0; i < world.numBodies(); ++i){
01114         hrp::BodyPtr b = world.body(i);
01115         b->calcForwardKinematics(true, true);
01116         for(int j=0; j<b->numLinks(); j++){
01117             ODE_Link* link = (ODE_Link*)b->link(j);
01118             link->setTransform(link->p, link->segmentAttitude());
01119             link->setAbsVelocity(link->v, link->w);
01120         }
01121     }
01122 
01123     needToUpdatePositions = true;
01124     needToUpdateSensorStates = true;
01125 
01126 }
01127 
01128 
01129 bool ODE_DynamicsSimulator_impl::checkCollision(bool checkAll) 
01130 {
01131 
01132     calcWorldForwardKinematics();
01133     _updateCharacterPositions();
01134 
01135     if (checkAll){
01136        return collisionDetector->queryContactDeterminationForDefinedPairs(allCharacterPositions.in(), collisions.out());
01137     }else{
01138        return collisionDetector->queryIntersectionForDefinedPairs(checkAll, allCharacterPositions.in(), collidingLinkPairs.out());
01139     }
01140 
01141     return 0;
01142 
01143 }
01144 
01145 
01146 DistanceSequence *ODE_DynamicsSimulator_impl::checkDistance()
01147 {
01148 
01149     calcWorldForwardKinematics();
01150     _updateCharacterPositions();
01151 
01152     DistanceSequence_var distances = new DistanceSequence;
01153     collisionDetector->queryDistanceForDefinedPairs(allCharacterPositions.in(), distances);
01154     return distances._retn();
01155 
01156     return NULL;
01157 }
01158 
01159 
01160 LinkPairSequence *ODE_DynamicsSimulator_impl::checkIntersection(CORBA::Boolean checkAll)
01161 {
01162 
01163     calcWorldForwardKinematics();
01164     _updateCharacterPositions();
01165 
01166     LinkPairSequence_var pairs = new LinkPairSequence;
01167     collisionDetector->queryIntersectionForDefinedPairs(checkAll, allCharacterPositions.in(), pairs);
01168     return pairs._retn();
01169 
01170     return NULL;
01171 }
01172 
01173 void ODE_DynamicsSimulator_impl::getWorldState(WorldState_out wstate)
01174 {
01175     if(debugMode){
01176         cout << "DynamicsSimulator_impl::getWorldState()\n";
01177     }
01178 
01179     if (needToUpdatePositions) _updateCharacterPositions();
01180 
01181     wstate = new WorldState;
01182 
01183     wstate->time = world.currentTime();
01184     wstate->characterPositions = allCharacterPositions;
01185     wstate->collisions = collisions;
01186 
01187     if(debugMode){
01188         cout << "getWorldState - exit" << endl;
01189     }
01190     
01191 }
01192 
01193 
01194 void ODE_DynamicsSimulator_impl::getCharacterSensorState(const char* characterName, SensorState_out sstate)
01195 {
01196 
01197     int bodyIndex = world.bodyIndex(characterName);
01198 
01199     if(bodyIndex >= 0){
01200         if(needToUpdateSensorStates){
01201             _updateSensorStates();
01202         }
01203         sstate = new SensorState(allCharacterSensorStates[bodyIndex]);
01204     } else {
01205         sstate = new SensorState;
01206     }
01207 
01208 }
01209 
01210 
01211 void ODE_DynamicsSimulator_impl::_setupCharacterData()
01212 {
01213     if(debugMode){
01214         cout << "DynamicsSimulator_impl::_setupCharacterData()\n";
01215     }
01216 
01217     int n = world.numBodies();
01218     allCharacterPositions->length(n);
01219     allCharacterSensorStates->length(n);
01220 
01221     for(int i=0; i < n; ++i){
01222         BodyPtr body = world.body(i);
01223 
01224         int numLinks = body->numLinks();
01225         CharacterPosition& characterPosition = allCharacterPositions[i];
01226         characterPosition.characterName = CORBA::string_dup(body->name().c_str());
01227         LinkPositionSequence& linkPositions = characterPosition.linkPositions;
01228         linkPositions.length(numLinks);
01229 
01230         int numJoints = body->numJoints();
01231         SensorState& sensorState = allCharacterSensorStates[i];
01232         sensorState.q.length(numJoints);
01233         sensorState.dq.length(numJoints);
01234         sensorState.u.length(numJoints);
01235         sensorState.force.length(body->numSensors(Sensor::FORCE));
01236         sensorState.rateGyro.length(body->numSensors(Sensor::RATE_GYRO));
01237         sensorState.accel.length(body->numSensors(Sensor::ACCELERATION));
01238         sensorState.range.length(body->numSensors(Sensor::RANGE));
01239 
01240         if(debugMode){
01241             std::cout << "character[" << i << "], nlinks = " << numLinks << "\n";
01242         }
01243     }
01244 
01245     if(debugMode){
01246         cout << "_setupCharacterData() - exit" << endl;;
01247     }
01248 
01249 }
01250 
01251 
01252 void ODE_DynamicsSimulator_impl::_updateCharacterPositions()
01253 {
01254     if(debugMode){
01255         cout << "DynamicsSimulator_impl::_updateCharacterPositions()\n";
01256     }
01257 
01258     int n = world.numBodies();
01259 
01260     {   
01261 #pragma omp parallel for num_threads(3)
01262         for(int i=0; i < n; ++i){
01263             BodyPtr body = world.body(i);
01264             int numLinks = body->numLinks();
01265                         
01266             CharacterPosition& characterPosition = allCharacterPositions[i];
01267                         
01268             if(debugMode){
01269                 cout << "character[" << i << "], nlinks = " << numLinks << "\n";
01270             }
01271                         
01272             for(int j=0; j < numLinks; ++j) {
01273                 Link* link = body->link(j);
01274                 LinkPosition& linkPosition = characterPosition.linkPositions[j];
01275                 setVector3(link->p, linkPosition.p);
01276                 setMatrix33ToRowMajorArray(link->segmentAttitude(), linkPosition.R);
01277             }
01278         }
01279     }
01280     needToUpdatePositions = false;
01281 
01282     if(debugMode){
01283         cout << "_updateCharacterData() - exit" << endl;
01284     }
01285 
01286 }
01287 
01288 
01289 void ODE_DynamicsSimulator_impl::_updateSensorStates()
01290 {
01291 
01292     if(debugMode){
01293         cout << "DynamicsSimulator_impl::_updateSensorStates()\n";
01294     }
01295 
01296     int numBodies = world.numBodies();
01297     for(int i=0; i < numBodies; ++i){
01298 
01299         SensorState& state = allCharacterSensorStates[i];
01300 
01301         BodyPtr body = world.body(i);
01302         int numJoints = body->numJoints();
01303 
01304         for(int j=0; j < numJoints; j++){
01305             ODE_Link* joint = (ODE_Link*)body->joint(j);
01306             state.q [j] = joint->q;
01307             state.dq[j] = joint->dq;
01308             state.u [j] = joint->u;
01309         }
01310 
01311         int n = body->numSensors(Sensor::FORCE);
01312         for(int id = 0; id < n; ++id){
01313             ForceSensor* sensor = body->sensor<ForceSensor>(id);
01314             setVector3(sensor->f,   state.force[id], 0);
01315             setVector3(sensor->tau, state.force[id], 3);
01316             if(debugMode){
01317                 std::cout << "Force Sensor: f:" << sensor->f << "tau:" << sensor->tau << "\n";
01318             }
01319         }
01320 
01321         n = body->numSensors(Sensor::RATE_GYRO);
01322         for(int id=0; id < n; ++id){
01323             RateGyroSensor* sensor = body->sensor<RateGyroSensor>(id);
01324             setVector3(sensor->w, state.rateGyro[id]);
01325             if(debugMode){
01326                 std::cout << "Rate Gyro:" << sensor->w << "\n";
01327             }
01328         }
01329 
01330         n = body->numSensors(Sensor::ACCELERATION);
01331         for(int id=0; id < n; ++id){
01332             AccelSensor* sensor = body->sensor<AccelSensor>(id);
01333             setVector3(sensor->dv, state.accel[id]);
01334             if(debugMode){
01335                 std::cout << "Accel:" << sensor->dv << std::endl;
01336             }
01337         }               
01338 
01339         n = body->numSensors(Sensor::RANGE);
01340         for (int id=0; id < n; ++id){
01341             RangeSensor *rangeSensor = body->sensor<RangeSensor>(id);
01342             if (world.currentTime() >= rangeSensor->nextUpdateTime){
01343                 Vector3 gp(rangeSensor->link->p + (rangeSensor->link->R)*rangeSensor->localPos);
01344                 Matrix33 gR(rangeSensor->link->R*rangeSensor->localR);
01345                 DblArray3 p;
01346                 DblArray9 R;
01347                 setVector3(gp, p);
01348                 setMatrix33ToRowMajorArray(gR, R);
01349                 DblSequence_var data = collisionDetector->scanDistanceWithRay(p, R,
01350                                                                               rangeSensor->scanStep, rangeSensor->scanAngle);
01351                 rangeSensor->distances.resize(data->length());
01352                 for (unsigned int i=0; i<data->length(); i++){
01353                     rangeSensor->distances[i] = data[i];
01354                 }
01355                 rangeSensor->nextUpdateTime += 1.0/rangeSensor->scanRate;
01356                 state.range[id] = data;
01357             }
01358         }
01359 
01360     }
01361     needToUpdateSensorStates = false;
01362 
01363     if(debugMode){
01364         cout << "_updateCharacterData() - exit" << endl;
01365     }
01366 
01367 }
01368 
01369 
01370 
01375 CORBA::Boolean ODE_DynamicsSimulator_impl::getCharacterCollidingPairs
01376 (
01377     const char *characterName,
01378     LinkPairSequence_out pairs
01379     )
01380 {
01381     if(debugMode){
01382         cout << "DynamicsSimulator_impl::getCharacterCollidingPairs("
01383              << characterName << ")" << endl;
01384     }
01385 
01386     if(!world.body(characterName)){
01387         std::cerr << "not found! :" << characterName << std::endl;
01388         return false;
01389     }
01390 
01391     std::vector<unsigned int> locations;
01392 
01393     for(unsigned int i=0; i < collisions->length(); ++i) {
01394 
01395         if(  (strcmp(characterName, collisions[i].pair.charName1) == 0)  ||
01396              (strcmp(characterName, collisions[i].pair.charName2) == 0))
01397             locations.push_back(i);
01398     }
01399 
01400     pairs->length(locations.size());
01401 
01402     unsigned long n=0;
01403     for(std::vector<unsigned int>::iterator iter = locations.begin();
01404         iter != locations.end(); ++iter) {
01405 
01406         strcpy(pairs[n].charName1, collisions[*iter].pair.charName1);
01407         strcpy(pairs[n].charName2, collisions[*iter].pair.charName2);
01408         strcpy(pairs[n].linkName1, collisions[*iter].pair.linkName1);
01409         strcpy(pairs[n].linkName2, collisions[*iter].pair.linkName2);
01410     }
01411 
01412     return true;
01413 }
01414 
01415 
01416 void ODE_DynamicsSimulator_impl::calcCharacterJacobian
01417 (
01418     const char *characterName,
01419     const char *baseLink,
01420     const char *targetLink,
01421     DblSequence_out jacobian
01422     )
01423 {
01424     if(debugMode){
01425         cout << "DynamicsSimulator_impl::calcCharacterJacobian("
01426              << characterName << ", "
01427              << baseLink << ", "
01428              << targetLink << ")" << endl;
01429     }
01430 
01431     BodyPtr body = world.body(characterName);
01432     if(!body){
01433         std::cerr << "not found! :" << characterName << std::endl;
01434         return;
01435     }
01436 
01437     JointPath path(body->link(baseLink), body->link(targetLink));
01438     int height = 6;
01439     int width = path.numJoints();
01440     dmatrix J(height, width);
01441     path.calcJacobian(J);
01442 
01443     jacobian->length(height * width);
01444     int i = 0;
01445     for(int r=0; r < height; ++r){
01446         for(int c=0; c < width; ++c){
01447             (*jacobian)[i++] = J(r, c);
01448         }
01449     }
01450 
01451 }
01452 
01453 
01458 DynamicsSimulatorFactory_impl::DynamicsSimulatorFactory_impl(CORBA::ORB_ptr     orb) :
01459     orb_(CORBA::ORB::_duplicate(orb))
01460 {
01461     initializeCommandLabelMaps();
01462 
01463     if(debugMode){
01464         cout << "DynamicsSimulatorFactory_impl::DynamicsSimulatorFactory_impl()" << endl;
01465     }
01466 }
01467 
01468 
01469 DynamicsSimulatorFactory_impl::~DynamicsSimulatorFactory_impl()
01470 {
01471     if(debugMode){
01472         cout << "DynamicsSimulatorFactory_impl::~DynamicsSimulatorFactory_impl()" << endl;
01473     }
01474 
01475     PortableServer::POA_var poa = _default_POA();
01476     PortableServer::ObjectId_var id = poa -> servant_to_id(this);
01477     poa -> deactivate_object(id);
01478 }
01479 
01480 
01481 DynamicsSimulator_ptr DynamicsSimulatorFactory_impl::create()
01482 {
01483     if(debugMode){
01484         cout << "DynamicsSimulatorFactory_impl::create()" << endl;
01485     }
01486 
01487     ODE_DynamicsSimulator_impl* integratorImpl = new ODE_DynamicsSimulator_impl(orb_);
01488 
01489     PortableServer::ServantBase_var integratorrServant = integratorImpl;
01490     PortableServer::POA_var poa_ = _default_POA();
01491     PortableServer::ObjectId_var id =
01492         poa_ -> activate_object(integratorImpl);
01493 
01494     return integratorImpl -> _this();
01495 }
01496 
01497 
01498 void DynamicsSimulatorFactory_impl::shutdown()
01499 {
01500     orb_->shutdown(false);
01501 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:55