00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00016 #include <vector>
00017 #include <map>
00018
00019 #include "DynamicsSimulator_impl.h"
00020 #include "ModelLoaderUtil.h"
00021 #include "Sensor.h"
00022 #include <psim.h>
00023
00024 using namespace OpenHRP;
00025 using namespace std;
00026
00027
00028
00029 static const bool enableTimeMeasure = false;
00030
00031
00032
00033
00034
00035 namespace {
00036
00037 struct IdLabel {
00038 int id;
00039 const char* label;
00040 };
00041 typedef map<int, const char*> IdToLabelMap;
00042
00043 IdToLabelMap commandLabelMaps[2];
00044
00045 IdToLabelMap commandLabelMap;
00046
00047 IdLabel commandLabels[] = {
00048
00049 { DynamicsSimulator::POSITION_GIVEN, "Position Given (High Gain Mode)" },
00050 { DynamicsSimulator::JOINT_VALUE, "Joint Value" },
00051 { DynamicsSimulator::JOINT_VELOCITY, "Joint Velocity"},
00052 { DynamicsSimulator::JOINT_ACCELERATION,"Joint Acceleration"},
00053 { DynamicsSimulator::JOINT_TORQUE, "Joint Torque"},
00054 { DynamicsSimulator::ABS_TRANSFORM, "Absolute Transform"},
00055 { DynamicsSimulator::ABS_VELOCITY, "Absolute Velocity"},
00056 { DynamicsSimulator::EXTERNAL_FORCE, "External Force"},
00057 { -1, "" },
00058 };
00059
00060 void initializeCommandLabelMaps()
00061 {
00062 if(commandLabelMap.empty()){
00063 int i = 0;
00064 while(true){
00065 IdLabel& idLabel = commandLabels[i++];
00066 if(idLabel.id == -1){
00067 break;
00068 }
00069 commandLabelMap[idLabel.id] = idLabel.label;
00070 }
00071 }
00072 }
00073
00074 const char* getLabelOfLinkDataType(DynamicsSimulator::LinkDataType type)
00075 {
00076 IdToLabelMap::iterator p = commandLabelMap.find(type);
00077 return (p != commandLabelMap.end()) ? p->second : "Requesting Unknown Data Type";
00078 }
00079 };
00080
00081
00082 template<typename X, typename X_ptr>
00083 X_ptr checkCorbaServer(std::string n, CosNaming::NamingContext_var &cxt)
00084 {
00085 CosNaming::Name ncName;
00086 ncName.length(1);
00087 ncName[0].id = CORBA::string_dup(n.c_str());
00088 ncName[0].kind = CORBA::string_dup("");
00089 X_ptr srv = NULL;
00090 try {
00091 srv = X::_narrow(cxt->resolve(ncName));
00092 } catch(const CosNaming::NamingContext::NotFound &exc) {
00093 std::cerr << n << " not found: ";
00094 switch(exc.why) {
00095 case CosNaming::NamingContext::missing_node:
00096 std::cerr << "Missing Node" << std::endl;
00097 case CosNaming::NamingContext::not_context:
00098 std::cerr << "Not Context" << std::endl;
00099 break;
00100 case CosNaming::NamingContext::not_object:
00101 std::cerr << "Not Object" << std::endl;
00102 break;
00103 }
00104 return (X_ptr)NULL;
00105 } catch(CosNaming::NamingContext::CannotProceed &exc) {
00106 std::cerr << "Resolve " << n << " CannotProceed" << std::endl;
00107 } catch(CosNaming::NamingContext::AlreadyBound &exc) {
00108 std::cerr << "Resolve " << n << " InvalidName" << std::endl;
00109 }
00110 return srv;
00111 }
00112
00113
00114 DynamicsSimulator_impl::DynamicsSimulator_impl(CORBA::ORB_ptr orb)
00115 : orb_(CORBA::ORB::_duplicate(orb))
00116 {
00117 world.setCurrentTime(0.0);
00118 world.setRungeKuttaMethod();
00119
00120
00121 CollisionDetectorFactory_var collisionDetectorFactory;
00122
00123 CORBA::Object_var nS = orb->resolve_initial_references("NameService");
00124 CosNaming::NamingContext_var cxT;
00125 cxT = CosNaming::NamingContext::_narrow(nS);
00126 collisionDetectorFactory =
00127 checkCorbaServer<CollisionDetectorFactory,
00128 CollisionDetectorFactory_var>("CollisionDetectorFactory", cxT);
00129 if (CORBA::is_nil(collisionDetectorFactory)) {
00130 std::cerr << "CollisionDetectorFactory not found" << std::endl;
00131 }
00132
00133 collisionDetector = collisionDetectorFactory->create();
00134
00135 collisions = new CollisionSequence;
00136 collidingLinkPairs = new LinkPairSequence;
00137 allCharacterPositions = new CharacterPositionSequence;
00138 allCharacterSensorStates = new SensorStateSequence;
00139
00140 needToUpdatePositions = true;
00141 needToUpdateSensorStates = true;
00142 }
00143
00144
00145 DynamicsSimulator_impl::~DynamicsSimulator_impl()
00146 {
00147 }
00148
00149
00150 void DynamicsSimulator_impl::destroy()
00151 {
00152 PortableServer::POA_var poa_ = _default_POA();
00153 PortableServer::ObjectId_var id = poa_->servant_to_id(this);
00154 poa_->deactivate_object(id);
00155 }
00156
00157
00158 void DynamicsSimulator_impl::registerCharacter(
00159 const char *name,
00160 BodyInfo_ptr body)
00161 {
00162
00163 loadBodyFromBodyInfo(&world, name, body);
00164 collisionDetector->registerCharacter(name, body);
00165
00166 }
00167
00168
00169 void DynamicsSimulator_impl::init(
00170 CORBA::Double _timestep,
00171 OpenHRP::DynamicsSimulator::IntegrateMethod integrateOpt,
00172 OpenHRP::DynamicsSimulator::SensorOption sensorOpt)
00173 {
00174
00175
00176 world.setTimeStep(_timestep);
00177 world.setCurrentTime(0.0);
00178
00179 if(integrateOpt == OpenHRP::DynamicsSimulator::EULER){
00180 world.setEulerMethod();
00181 } else {
00182 world.setRungeKuttaMethod();
00183 }
00184
00185 world.enableSensors((sensorOpt == OpenHRP::DynamicsSimulator::ENABLE_SENSOR));
00186
00187 _setupCharacterData();
00188
00189 isFirstSimulationLoop = true;
00190
00191 #ifdef MEASURE_TIME
00192 processTime = 0;
00193 #endif
00194 }
00195
00196
00197 static void joint_traverse_sub(Joint* cur, std::vector<Joint*>& jlist)
00198 {
00199 if(!cur) return;
00200
00201 jlist.push_back(cur);
00202 joint_traverse_sub(cur->brother, jlist);
00203 joint_traverse_sub(cur->child, jlist);
00204 }
00205
00206 static void joint_traverse(Joint* r, std::vector<Joint*>& jlist)
00207 {
00208 jlist.push_back(r);
00209 joint_traverse_sub(r->child, jlist);
00210 }
00211
00212
00213 void DynamicsSimulator_impl::registerCollisionCheckPair(
00214 const char *charName1,
00215 const char *linkName1,
00216 const char *charName2,
00217 const char *linkName2,
00218 const CORBA::Double staticFriction,
00219 const CORBA::Double slipFriction,
00220 const DblSequence6 & K,
00221 const DblSequence6 & C,
00222 const double culling_thresh,
00223 const double restitution)
00224 {
00225 const double epsilon = 0.0;
00226
00227
00228 std::string emptyString = "";
00229 std::vector<Joint*> joints1;
00230 std::vector<Joint*> joints2;
00231 pSim* chain = world.Chain();
00232
00233 if(emptyString == linkName1)
00234 {
00235 Joint* r = chain->FindCharacterRoot(charName1);
00236 if(r) joint_traverse(r, joints1);
00237 }
00238 else
00239 {
00240 Joint* jnt1 = chain->FindJoint(linkName1, charName1);
00241 if(jnt1) joints1.push_back(jnt1);
00242 }
00243 if(emptyString == linkName2)
00244 {
00245 Joint* r = chain->FindCharacterRoot(charName2);
00246 if(r) joint_traverse(r, joints2);
00247 }
00248 else
00249 {
00250 Joint* jnt2 = chain->FindJoint(linkName2, charName2);
00251 if(jnt2) joints2.push_back(jnt2);
00252 }
00253
00254 for(size_t i=0; i < joints1.size(); ++i)
00255 {
00256 Joint* j1 = joints1[i];
00257 for(size_t j=0; j < joints2.size(); ++j)
00258 {
00259 Joint* j2 = joints2[j];
00260 if(j1 && j2 && j1 != j2)
00261 {
00262
00263 world.addCollisionCheckLinkPair(j1, j2, staticFriction, slipFriction, epsilon);
00264 LinkPair_var linkPair = new LinkPair();
00265 linkPair->charName1 = CORBA::string_dup(charName1);
00266 linkPair->linkName1 = CORBA::string_dup(j1->basename);
00267 linkPair->charName2 = CORBA::string_dup(charName2);
00268 linkPair->linkName2 = CORBA::string_dup(j2->basename);
00269 linkPair->tolerance = 0;
00270 collisionDetector->addCollisionPair(linkPair);
00271 }
00272 }
00273 }
00274 }
00275
00276 void DynamicsSimulator_impl::registerIntersectionCheckPair(
00277 const char *charName1,
00278 const char *linkName1,
00279 const char *charName2,
00280 const char *linkName2,
00281 const double tolerance)
00282 {
00283
00284
00285 std::string emptyString = "";
00286 std::vector<Joint*> joints1;
00287 std::vector<Joint*> joints2;
00288 pSim* chain = world.Chain();
00289
00290 if(emptyString == linkName1)
00291 {
00292 Joint* r = chain->FindCharacterRoot(charName1);
00293 if(r) joint_traverse(r, joints1);
00294 }
00295 else
00296 {
00297 Joint* jnt1 = chain->FindJoint(linkName1, charName1);
00298 if(jnt1) joints1.push_back(jnt1);
00299 }
00300 if(emptyString == linkName2)
00301 {
00302 Joint* r = chain->FindCharacterRoot(charName2);
00303 if(r) joint_traverse(r, joints2);
00304 }
00305 else
00306 {
00307 Joint* jnt2 = chain->FindJoint(linkName2, charName2);
00308 if(jnt2) joints2.push_back(jnt2);
00309 }
00310
00311 for(size_t i=0; i < joints1.size(); ++i)
00312 {
00313 Joint* j1 = joints1[i];
00314 for(size_t j=0; j < joints2.size(); ++j)
00315 {
00316 Joint* j2 = joints2[j];
00317 if(j1 && j2 && j1 != j2)
00318 {
00319 LinkPair_var linkPair = new LinkPair();
00320 linkPair->charName1 = CORBA::string_dup(charName1);
00321 linkPair->linkName1 = CORBA::string_dup(j1->basename);
00322 linkPair->charName2 = CORBA::string_dup(charName2);
00323 linkPair->linkName2 = CORBA::string_dup(j2->basename);
00324 linkPair->tolerance = tolerance;
00325 collisionDetector->addCollisionPair(linkPair);
00326 }
00327 }
00328 }
00329 }
00330
00331 void DynamicsSimulator_impl::registerExtraJoint
00332 (
00333 const char* charName1,
00334 const char* linkName1,
00335 const char* charName2,
00336 const char* linkName2,
00337 const DblSequence3& link1LocalPos,
00338 const DblSequence3& link2LocalPos,
00339 const ExtraJointType jointType,
00340 const DblSequence3& jointAxis,
00341 const char* extraJointName)
00342 {
00343 }
00344
00346 void DynamicsSimulator_impl::getExtraJointConstraintForce(
00347 const char * characterName,
00348 const char * extraJointName,
00349 DblSequence6_out contactForce)
00350 {
00351 }
00352
00353
00354 static void vec3_to_seq(const fVec3& vec, DblSequence_out& seq, size_t offset = 0)
00355 {
00356 seq[CORBA::ULong(offset++)] = vec(0);
00357 seq[CORBA::ULong(offset++)] = vec(1);
00358 seq[CORBA::ULong(offset)] = vec(2);
00359 }
00360
00361 static void seq_to_vec3(const DblSequence3& seq, fVec3& vec)
00362 {
00363 vec(0) = seq[0];
00364 vec(1) = seq[1];
00365 vec(2) = seq[2];
00366 }
00367
00368 void DynamicsSimulator_impl::getCharacterSensorValues(
00369 const char *characterName,
00370 const char *sensorName,
00371 DblSequence_out sensorOutput)
00372 {
00373 sensorOutput = new DblSequence;
00374 Sensor* sensor = world.findSensor(sensorName, characterName);
00375
00376 if(sensor)
00377 {
00378 switch(sensor->type)
00379 {
00380 case Sensor::FORCE:
00381 {
00382 ForceSensor* forceSensor = static_cast<ForceSensor*>(sensor);
00383 sensorOutput->length(6);
00384 #ifndef __WIN32__
00385 vec3_to_seq(forceSensor->f, sensorOutput);
00386 vec3_to_seq(forceSensor->tau, sensorOutput, 3);
00387 #else
00388 sensorOutput[CORBA::ULong(0)] = forceSensor->f(0);
00389 sensorOutput[CORBA::ULong(1)] = forceSensor->f(1);
00390 sensorOutput[CORBA::ULong(2)] = forceSensor->f(2);
00391 sensorOutput[CORBA::ULong(3)] = forceSensor->tau(0);
00392 sensorOutput[CORBA::ULong(4)] = forceSensor->tau(1);
00393 sensorOutput[CORBA::ULong(5)] = forceSensor->tau(2);
00394 #endif
00395 }
00396 break;
00397
00398 case Sensor::RATE_GYRO:
00399 {
00400 RateGyroSensor* gyro = static_cast<RateGyroSensor*>(sensor);
00401 sensorOutput->length(3);
00402 #ifndef __WIN32__
00403 vec3_to_seq(gyro->w, sensorOutput);
00404 #else
00405 sensorOutput[CORBA::ULong(0)] = gyro->w(0);
00406 sensorOutput[CORBA::ULong(1)] = gyro->w(1);
00407 sensorOutput[CORBA::ULong(2)] = gyro->w(2);
00408 #endif
00409 }
00410 break;
00411
00412 case Sensor::ACCELERATION:
00413 {
00414 AccelSensor* accelSensor = static_cast<AccelSensor*>(sensor);
00415 sensorOutput->length(3);
00416 #ifndef __WIN32__
00417 vec3_to_seq(accelSensor->dv, sensorOutput);
00418 #else
00419 sensorOutput[CORBA::ULong(0)] = accelSensor->dv(0);
00420 sensorOutput[CORBA::ULong(1)] = accelSensor->dv(1);
00421 sensorOutput[CORBA::ULong(2)] = accelSensor->dv(2);
00422 #endif
00423 }
00424 break;
00425
00426 default:
00427 break;
00428 }
00429 }
00430 }
00431
00432 void DynamicsSimulator_impl::initSimulation()
00433 {
00434 world.initialize();
00435
00436 _updateCharacterPositions();
00437 collisionDetector->queryContactDeterminationForDefinedPairs(allCharacterPositions.in(), collisions.out());
00438
00439 if(enableTimeMeasure){
00440 timeMeasureFinished = false;
00441 timeMeasure1.begin();
00442 }
00443 }
00444
00445 void DynamicsSimulator_impl::stepSimulation()
00446 {
00447 if(enableTimeMeasure) timeMeasure2.begin();
00448 world.calcNextState(collisions);
00449 if(enableTimeMeasure) timeMeasure2.end();
00450
00451 if(enableTimeMeasure){
00452 cout << " " << timeMeasure2.time() << "\n";
00453 }
00454
00455 needToUpdateSensorStates = true;
00456
00457 _updateCharacterPositions();
00458
00459 collisionDetector->queryContactDeterminationForDefinedPairs(allCharacterPositions.in(), collisions.out());
00460
00461 if(enableTimeMeasure)
00462 {
00463 if(world.currentTime() > 10.0 && !timeMeasureFinished)
00464 {
00465 timeMeasureFinished = true;
00466 timeMeasure1.end();
00467 cout << "Total elapsed time = " << timeMeasure1.totalTime() << "\n";
00468 cout << "Internal elapsed time = " << timeMeasure2.totalTime();
00469 cout << ", the avarage = " << timeMeasure2.avarageTime() << endl;;
00470
00471 }
00472 }
00473 }
00474
00475
00476 void DynamicsSimulator_impl::setCharacterLinkData(
00477 const char* characterName,
00478 const char* linkName,
00479 OpenHRP::DynamicsSimulator::LinkDataType type,
00480 const DblSequence& wdata)
00481 {
00482
00483 Joint* joint = world.Chain()->FindJoint(linkName, characterName);
00484 if(!joint) return;
00485
00486 switch(type) {
00487
00488 case OpenHRP::DynamicsSimulator::POSITION_GIVEN:
00489 joint->t_given = !(wdata[0] > 0.0);
00490 break;
00491
00492 case OpenHRP::DynamicsSimulator::JOINT_VALUE:
00493 joint->SetJointValue(wdata[0]);
00494 break;
00495
00496 case OpenHRP::DynamicsSimulator::JOINT_VELOCITY:
00497 joint->SetJointVel(wdata[0]);
00498 break;
00499
00500 case OpenHRP::DynamicsSimulator::JOINT_ACCELERATION:
00501 joint->SetJointAcc(wdata[0]);
00502 break;
00503
00504 case OpenHRP::DynamicsSimulator::JOINT_TORQUE:
00505 joint->SetJointForce(wdata[0]);
00506 break;
00507
00508 case OpenHRP::DynamicsSimulator::ABS_TRANSFORM:
00509 {
00510 fVec3 abs_pos(wdata[0], wdata[1], wdata[2]);
00511 fMat33 abs_att(wdata[3], wdata[4], wdata[5], wdata[6], wdata[7], wdata[8], wdata[9], wdata[10], wdata[11]);
00512 joint->SetJointValue(abs_pos, abs_att);
00513 break;
00514 }
00515
00516 case OpenHRP::DynamicsSimulator::ABS_VELOCITY:
00517 {
00518 fVec3 abs_lin_vel(wdata[0], wdata[1], wdata[2]);
00519 fVec3 abs_ang_vel(wdata[3], wdata[4], wdata[5]);
00520 joint->rel_lin_vel.mul(abs_lin_vel, joint->abs_att);
00521 joint->rel_ang_vel.mul(abs_ang_vel, joint->abs_att);
00522 break;
00523 }
00524
00525 case OpenHRP::DynamicsSimulator::EXTERNAL_FORCE:
00526 {
00527
00528
00529 joint->ext_force(0) = wdata[0];
00530 joint->ext_force(1) = wdata[1];
00531 joint->ext_force(2) = wdata[2];
00532 fVec3 n0(wdata[3], wdata[4], wdata[5]);
00533 fVec3 np;
00534 np.cross(joint->abs_pos, joint->ext_force);
00535 joint->ext_moment.sub(n0, np);
00536 break;
00537 }
00538
00539 default:
00540 return;
00541 }
00542
00543 needToUpdatePositions = true;
00544 needToUpdateSensorStates = true;
00545 }
00546
00547
00548 void DynamicsSimulator_impl::getCharacterLinkData(
00549 const char * characterName,
00550 const char * linkName,
00551 OpenHRP::DynamicsSimulator::LinkDataType type,
00552 DblSequence_out out_rdata)
00553 {
00554
00555 Joint* joint = world.Chain()->FindJoint(linkName, characterName);
00556 assert(joint);
00557
00558 DblSequence_var rdata = new DblSequence;
00559
00560 switch(type) {
00561
00562 case OpenHRP::DynamicsSimulator::JOINT_VALUE:
00563 rdata->length(1);
00564 rdata[0] = joint->q;
00565 break;
00566
00567 case OpenHRP::DynamicsSimulator::JOINT_VELOCITY:
00568 rdata->length(1);
00569 rdata[0] = joint->qd;
00570 break;
00571
00572 case OpenHRP::DynamicsSimulator::JOINT_ACCELERATION:
00573 rdata->length(1);
00574 rdata[0] = joint->qdd;
00575 break;
00576
00577 case OpenHRP::DynamicsSimulator::JOINT_TORQUE:
00578 rdata->length(1);
00579 rdata[0] = joint->tau;
00580 break;
00581
00582 case OpenHRP::DynamicsSimulator::ABS_TRANSFORM:
00583 {
00584 fEulerPara ep;
00585 ep.set(joint->abs_att);
00586 rdata->length(7);
00587 rdata[0] = joint->abs_pos(0);
00588 rdata[1] = joint->abs_pos(1);
00589 rdata[2] = joint->abs_pos(2);
00590 rdata[3] = ep.Ang();
00591 rdata[4] = ep.Axis()(0);
00592 rdata[5] = ep.Axis()(1);
00593 rdata[6] = ep.Axis()(2);
00594 break;
00595 }
00596
00597 case OpenHRP::DynamicsSimulator::ABS_VELOCITY:
00598 {
00599 fVec3 v0, w0;
00600 v0.mul(joint->abs_att, joint->loc_lin_vel);
00601 w0.mul(joint->abs_att, joint->loc_ang_vel);
00602 rdata->length(6);
00603 rdata[0] = v0(0);
00604 rdata[1] = v0(1);
00605 rdata[2] = v0(2);
00606 rdata[3] = w0(0);
00607 rdata[4] = w0(1);
00608 rdata[5] = w0(2);
00609 break;
00610 }
00611
00612 case OpenHRP::DynamicsSimulator::EXTERNAL_FORCE:
00613 {
00614
00615
00616 rdata->length(6);
00617 rdata[0] = joint->ext_force(0);
00618 rdata[1] = joint->ext_force(1);
00619 rdata[2] = joint->ext_force(2);
00620 fVec3 np, n0;
00621 np.cross(joint->abs_pos, joint->ext_force);
00622 n0.add(joint->ext_moment, np);
00623 rdata[3] = n0(0);
00624 rdata[4] = n0(1);
00625 rdata[5] = n0(2);
00626 break;
00627 }
00628
00629 default:
00630 break;
00631 }
00632
00633 out_rdata = rdata._retn();
00634 }
00635
00636
00637 void DynamicsSimulator_impl::getCharacterAllLinkData(
00638 const char * characterName,
00639 OpenHRP::DynamicsSimulator::LinkDataType type,
00640 DblSequence_out rdata)
00641 {
00642
00643 rdata = new DblSequence();
00644
00645 world.getAllCharacterData(characterName, type, rdata);
00646 }
00647
00648
00649 void DynamicsSimulator_impl::setCharacterAllLinkData(
00650 const char * characterName,
00651 OpenHRP::DynamicsSimulator::LinkDataType type,
00652 const DblSequence & wdata)
00653 {
00654
00655 world.setAllCharacterData(characterName, type, wdata);
00656 }
00657
00658
00659 void DynamicsSimulator_impl::setGVector(
00660 const DblSequence3& wdata)
00661 {
00662 assert(wdata.length() == 3);
00663
00664
00665 fVec3 g;
00666 seq_to_vec3(wdata, g);
00667 world.setGravityAcceleration(g);
00668
00669 }
00670
00671
00672 void DynamicsSimulator_impl::getGVector(
00673 DblSequence3_out wdata)
00674 {
00675 wdata->length(3);
00676 fVec3 g = world.getGravityAcceleration();
00677 (*wdata)[0] = g(0);
00678 (*wdata)[1] = g(1);
00679 (*wdata)[2] = g(2);
00680 }
00681
00682
00683 void DynamicsSimulator_impl::setCharacterAllJointModes(
00684 const char * characterName,
00685 OpenHRP::DynamicsSimulator::JointDriveMode jointMode)
00686 {
00687
00688 bool isTorqueMode = (jointMode != OpenHRP::DynamicsSimulator::HIGH_GAIN_MODE);
00689
00690 world.Chain()->SetCharacterTorqueGiven(characterName, isTorqueMode);
00691
00692 }
00693
00694
00695 CORBA::Boolean DynamicsSimulator_impl::calcCharacterInverseKinematics(
00696 const char * characterName,
00697 const char * fromLink, const char * toLink,
00698 const LinkPosition& target)
00699 {
00700 bool solved = false;
00701
00702
00703
00704 return solved;
00705 }
00706
00707
00708 void DynamicsSimulator_impl::calcCharacterForwardKinematics(
00709 const char * characterName)
00710 {
00711
00712 world.Chain()->CalcPosition();
00713
00714 needToUpdatePositions = true;
00715 needToUpdateSensorStates = true;
00716 }
00717
00718
00719 void DynamicsSimulator_impl::calcWorldForwardKinematics()
00720 {
00721
00722 world.Chain()->CalcPosition();
00723
00724 needToUpdatePositions = true;
00725 needToUpdateSensorStates = true;
00726 }
00727
00728
00729 void DynamicsSimulator_impl::getWorldState(WorldState_out wstate)
00730 {
00731
00732 if (needToUpdatePositions) _updateCharacterPositions();
00733
00734 wstate = new WorldState;
00735
00736 wstate->time = world.currentTime();
00737 wstate->characterPositions = allCharacterPositions;
00738 wstate->collisions = collisions;
00739 }
00740
00741
00742 void DynamicsSimulator_impl::getCharacterSensorState(const char* characterName, SensorState_out sstate)
00743 {
00744
00745 int index = -1;
00746 int n_char = world.numCharacter();
00747 for(int i=0; i<n_char; i++)
00748 {
00749 Joint* j = world.rootJoint(i);
00750 if(!strcmp(j->CharName(), characterName))
00751 {
00752 index = i;
00753 break;
00754 }
00755 }
00756 if(index >= 0)
00757 {
00758 if(needToUpdateSensorStates)
00759 {
00760 _updateSensorStates();
00761 }
00762 sstate = new SensorState(allCharacterSensorStates[index]);
00763 }
00764 else
00765 {
00766 sstate = new SensorState;
00767 }
00768 }
00769
00770
00771 void DynamicsSimulator_impl::_setupCharacterData()
00772 {
00773 int nchar = world.numCharacter();
00774
00775 allCharacterPositions->length(nchar);
00776 allCharacterSensorStates->length(nchar);
00777
00778
00779
00780 for(int i=0; i<nchar; i++)
00781 {
00782 Joint* j = world.rootJoint(i);
00783
00784 CharacterPosition& characterPosition = allCharacterPositions[i];
00785 characterPosition.characterName = CORBA::string_dup(j->CharName());
00786 int n_links = world.numLinks(i);
00787
00788 LinkPositionSequence& linkPositions = characterPosition.linkPositions;
00789 linkPositions.length(n_links);
00790 SensorState& sensorState = allCharacterSensorStates[i];
00791 int n_joints = world.numJoints(i);
00792
00793 sensorState.q.length(n_joints);
00794 sensorState.dq.length(n_joints);
00795 sensorState.u.length(n_joints);
00796 sensorState.force.length(world.numSensors(Sensor::FORCE, j->CharName()));
00797 sensorState.rateGyro.length(world.numSensors(Sensor::RATE_GYRO, j->CharName()));
00798 sensorState.accel.length(world.numSensors(Sensor::ACCELERATION, j->CharName()));
00799 }
00800 }
00801
00802
00803 void DynamicsSimulator_impl::_updateCharacterPositions()
00804 {
00805 world.getAllCharacterPositions(allCharacterPositions.inout());
00806 needToUpdatePositions = false;
00807
00808 #if 0
00809 int n_char = allCharacterPositions->length();
00810 for(int i=0; i<n_char; i++)
00811 {
00812 CharacterPosition& cpos = allCharacterPositions[i];
00813 int n_link = cpos.linkPositions.length();
00814 for(int j=0; j<n_link; j++)
00815 {
00816 LinkPosition& lpos = cpos.linkPositions[j];
00817 }
00818 }
00819 #endif
00820 }
00821
00822 void DynamicsSimulator_impl::_updateSensorStates()
00823 {
00824 world.getAllSensorStates(allCharacterSensorStates);
00825 needToUpdateSensorStates = false;
00826 }
00827
00828
00833 CORBA::Boolean DynamicsSimulator_impl::getCharacterCollidingPairs(
00834 const char *characterName,
00835 LinkPairSequence_out pairs)
00836 {
00837 std::vector<unsigned int> locations;
00838
00839 for(unsigned int i=0; i < collisions->length(); ++i) {
00840
00841 if( (strcmp(characterName, collisions[i].pair.charName1) == 0) ||
00842 (strcmp(characterName, collisions[i].pair.charName2) == 0))
00843 locations.push_back(i);
00844 }
00845
00846 pairs->length(locations.size());
00847
00848 unsigned long n=0;
00849 for(std::vector<unsigned int>::iterator iter = locations.begin();
00850 iter != locations.end(); ++iter) {
00851
00852 strcpy(pairs[n].charName1, collisions[*iter].pair.charName1);
00853 strcpy(pairs[n].charName2, collisions[*iter].pair.charName2);
00854 strcpy(pairs[n].linkName1, collisions[*iter].pair.linkName1);
00855 strcpy(pairs[n].linkName2, collisions[*iter].pair.linkName2);
00856 }
00857
00858 return true;
00859 }
00860
00861
00862 void DynamicsSimulator_impl::calcCharacterJacobian(
00863 const char *characterName,
00864 const char *baseLink,
00865 const char *targetLink,
00866 DblSequence_out jacobian)
00867 {
00868 fMat J;
00869 world.calcCharacterJacobian(characterName, baseLink, targetLink, J);
00870
00871 int height = J.row();
00872 int width = J.col();
00873
00874 jacobian->length(height * width);
00875 int i = 0;
00876 for(int r=0; r < height; ++r){
00877 for(int c=0; c < width; ++c){
00878 (*jacobian)[i++] = J(r, c);
00879 }
00880 }
00881 }
00882
00883 bool DynamicsSimulator_impl::checkCollision(bool checkAll)
00884 {
00885 calcWorldForwardKinematics();
00886 _updateCharacterPositions();
00887 if (checkAll){
00888 return collisionDetector->queryContactDeterminationForDefinedPairs(allCharacterPositions.in(), collisions.out());
00889 }else{
00890 return collisionDetector->queryIntersectionForDefinedPairs(checkAll, allCharacterPositions.in(), collidingLinkPairs.out());
00891 }
00892 }
00893
00894 DistanceSequence *DynamicsSimulator_impl::checkDistance()
00895 {
00896 calcWorldForwardKinematics();
00897 _updateCharacterPositions();
00898 DistanceSequence_var distances = new DistanceSequence;
00899 collisionDetector->queryDistanceForDefinedPairs(allCharacterPositions.in(), distances);
00900 return distances._retn();
00901 }
00902
00903 LinkPairSequence *DynamicsSimulator_impl::checkIntersection(CORBA::Boolean checkAll)
00904 {
00905 calcWorldForwardKinematics();
00906 _updateCharacterPositions();
00907 LinkPairSequence_var pairs = new LinkPairSequence;
00908 collisionDetector->queryIntersectionForDefinedPairs(checkAll, allCharacterPositions.in(), pairs);
00909 return pairs._retn();
00910 }
00911
00916 DynamicsSimulatorFactory_impl::DynamicsSimulatorFactory_impl(CORBA::ORB_ptr orb) :
00917 orb_(CORBA::ORB::_duplicate(orb))
00918 {
00919 initializeCommandLabelMaps();
00920 }
00921
00922
00923 DynamicsSimulatorFactory_impl::~DynamicsSimulatorFactory_impl()
00924 {
00925 PortableServer::POA_var poa = _default_POA();
00926 PortableServer::ObjectId_var id = poa -> servant_to_id(this);
00927 poa -> deactivate_object(id);
00928 }
00929
00930
00931 DynamicsSimulator_ptr DynamicsSimulatorFactory_impl::create()
00932 {
00933 DynamicsSimulator_impl* integratorImpl = new DynamicsSimulator_impl(orb_);
00934
00935 PortableServer::ServantBase_var integratorrServant = integratorImpl;
00936 PortableServer::POA_var poa_ = _default_POA();
00937 PortableServer::ObjectId_var id =
00938 poa_ -> activate_object(integratorImpl);
00939
00940 return integratorImpl -> _this();
00941 }
00942
00943
00944 void DynamicsSimulatorFactory_impl::shutdown()
00945 {
00946 orb_->shutdown(false);
00947 }