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


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:53