00001
00002
00003
00004
00005
00006
00007
00008
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
00124 world.setRungeKuttaMethod();
00125
00126 if(!USE_INTERNAL_COLLISION_DETECTOR){
00127
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:
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:
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
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 }