00001
00002
00003
00004
00005
00006
00007
00008
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
00119 world.setRungeKuttaMethod();
00120
00121 world.useInternalCollisionDetector(USE_ODE_COLLISION_DETECTOR);
00122 if(!USE_INTERNAL_COLLISION_DETECTOR){
00123
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:
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:
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 }