ODE_DynamicsSimulator_impl.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * National Institute of Advanced Industrial Science and Technology (AIST)
8  * General Robotix Inc.
9  */
10 
11 
13 
14 #include <hrpUtil/Eigen3d.h>
15 #include <hrpModel/Body.h>
16 #include <hrpModel/Link.h>
17 #include <hrpModel/LinkTraverse.h>
18 #include <hrpModel/JointPath.h>
19 #include <hrpModel/Sensor.h>
21 
22 #include <vector>
23 #include <map>
24 #include <algorithm>
25 
26 using namespace std;
27 using namespace hrp;
28 
29 static const bool USE_INTERNAL_COLLISION_DETECTOR = false;
30 static const bool USE_ODE_COLLISION_DETECTOR = true;
31 static const int debugMode = false;
32 static const bool enableTimeMeasure = false;
33 
34 namespace {
35 
36  struct IdLabel {
37  int id;
38  const char *label;
39  };
40  typedef map<int, const char*> IdToLabelMap;
41 
42  IdToLabelMap commandLabelMap;
43 
44  IdLabel commandLabels[] = {
45 
46  { DynamicsSimulator::POSITION_GIVEN, "Position Given (High Gain Mode)" },
47  { DynamicsSimulator::JOINT_VALUE, "Joint Value" },
48  { DynamicsSimulator::JOINT_VELOCITY, "Joint Velocity"},
49  { DynamicsSimulator::JOINT_ACCELERATION,"Joint Acceleration"},
50  { DynamicsSimulator::JOINT_TORQUE, "Joint Torque"},
51  { DynamicsSimulator::ABS_TRANSFORM, "Absolute Transform"},
52  { DynamicsSimulator::ABS_VELOCITY, "Absolute Velocity"},
53  { DynamicsSimulator::EXTERNAL_FORCE, "External Force"},
54  { -1, "" },
55  };
56 
57  void initializeCommandLabelMaps()
58  {
59  if(commandLabelMap.empty()){
60  int i = 0;
61  while(true){
62  IdLabel& idLabel = commandLabels[i++];
63  if(idLabel.id == -1){
64  break;
65  }
66  commandLabelMap[idLabel.id] = idLabel.label;
67  }
68  }
69  }
70 
71  const char* getLabelOfLinkDataType(DynamicsSimulator::LinkDataType type)
72  {
73  IdToLabelMap::iterator p = commandLabelMap.find(type);
74  return (p != commandLabelMap.end()) ? p->second : "Requesting Unknown Data Type";
75  }
76 };
77 
78 
79 template<typename X, typename X_ptr>
80 X_ptr checkCorbaServer(std::string n, CosNaming::NamingContext_var &cxt)
81 {
82  CosNaming::Name ncName;
83  ncName.length(1);
84  ncName[0].id = CORBA::string_dup(n.c_str());
85  ncName[0].kind = CORBA::string_dup("");
86  X_ptr srv = NULL;
87  try {
88  srv = X::_narrow(cxt->resolve(ncName));
89  } catch(const CosNaming::NamingContext::NotFound &exc) {
90  std::cerr << n << " not found: ";
91  switch(exc.why) {
92  case CosNaming::NamingContext::missing_node:
93  std::cerr << "Missing Node" << std::endl;
94  case CosNaming::NamingContext::not_context:
95  std::cerr << "Not Context" << std::endl;
96  break;
97  case CosNaming::NamingContext::not_object:
98  std::cerr << "Not Object" << std::endl;
99  break;
100  }
101  return (X_ptr)NULL;
102  } catch(CosNaming::NamingContext::CannotProceed &exc) {
103  std::cerr << "Resolve " << n << " CannotProceed" << std::endl;
104  } catch(CosNaming::NamingContext::AlreadyBound &exc) {
105  std::cerr << "Resolve " << n << " InvalidName" << std::endl;
106  }
107  return srv;
108 }
109 
110 
112  : orb_(CORBA::ORB::_duplicate(orb))
113 {
114  if(debugMode){
115  cout << "ODE_DynamicsSimulator_impl::ODE_DynamicsSimulator_impl()" << endl;
116  }
117 
118  // default integration method
120 
123  // resolve collisionchecker object
124  CollisionDetectorFactory_var collisionDetectorFactory;
125 
126  CORBA::Object_var nS = orb->resolve_initial_references("NameService");
127  CosNaming::NamingContext_var cxT;
128  cxT = CosNaming::NamingContext::_narrow(nS);
129  collisionDetectorFactory =
130  checkCorbaServer<CollisionDetectorFactory,
131  CollisionDetectorFactory_var>("CollisionDetectorFactory", cxT);
132  if (CORBA::is_nil(collisionDetectorFactory)) {
133  std::cerr << "CollisionDetectorFactory not found" << std::endl;
134  }
135 
136  try {
137  collisionDetector = collisionDetectorFactory->create();
138  }catch(...){
139  std::cerr << "failed to create CollisionDetector" << std::endl;
140  }
141  }
142 
143  collisions = new CollisionSequence;
144  collidingLinkPairs = new LinkPairSequence;
145  allCharacterPositions = new CharacterPositionSequence;
146  allCharacterSensorStates = new SensorStateSequence;
147 
148  needToUpdatePositions = true;
150 }
151 
152 
154 {
155  if(debugMode){
156  cout << "ODE_DynamicsSimulator_impl::~ODE_DynamicsSimulator_impl()" << endl;
157  }
159  collisionDetector->destroy();
160 }
161 
162 
164 {
165  if(debugMode){
166  cout << "ODE_DynamicsSimulator_impl::destroy()" << endl;
167  }
168 
169  PortableServer::POA_var poa_ = _default_POA();
170  PortableServer::ObjectId_var id = poa_->servant_to_id(this);
171  poa_->deactivate_object(id);
172 }
173 
174 
176 (
177  const char *name,
178  BodyInfo_ptr bodyInfo
179  )
180 {
181  if(debugMode){
182  cout << "ODE_DynamicsSimulator_impl::registerCharacter("
183  << name << ", " << bodyInfo << " )" << std::endl;
184  }
185 
186  world.addBody(bodyInfo, name);
187 
189  collisionDetector->registerCharacter(name, bodyInfo);
190  }
191 }
192 
193 
195 (
196  CORBA::Double timeStep,
197  OpenHRP::DynamicsSimulator::IntegrateMethod integrateOpt,
198  OpenHRP::DynamicsSimulator::SensorOption sensorOpt
199  )
200 {
201  if(debugMode){
202  cout << "ODE_DynamicsSimulator_impl::init(" << timeStep << ", ";
203  cout << (integrateOpt == OpenHRP::DynamicsSimulator::EULER ? "EULER, " : "RUNGE_KUTTA, ");
204  cout << (sensorOpt == OpenHRP::DynamicsSimulator::DISABLE_SENSOR ? "DISABLE_SENSOR" : "ENABLE_SENSOR");
205  std::cout << ")" << std::endl;
206  }
207 
208  world.setTimeStep(timeStep);
209  world.setCurrentTime(0.0);
210 
211  if(integrateOpt == OpenHRP::DynamicsSimulator::EULER){
212  world.setEulerMethod();
213  } else {
214  world.setRungeKuttaMethod();
215  }
216 
217  world.enableSensors((sensorOpt == OpenHRP::DynamicsSimulator::ENABLE_SENSOR));
218 
219  int n = world.numBodies();
220  for(int i=0; i < n; ++i){
221  world.body(i)->initializeConfiguration();
222  }
223 
224  _setupCharacterData();
225 
226 #ifdef MEASURE_TIME
227  processTime = 0;
228 #endif
229 }
230 
231 
233 (
234  const char *charName1,
235  const char *linkName1,
236  const char *charName2,
237  const char *linkName2,
238  const CORBA::Double staticFriction,
239  const CORBA::Double slipFriction,
240  const DblSequence6 & K,
241  const DblSequence6 & C,
242  const double culling_thresh,
243  const double restitution
244  )
245 {
246  const double epsilon = 0.0;
247 
248  if(debugMode){
249  cout << "DynamicsSimulator_impl::registerCollisionCheckPair("
250  << charName1 << ", " << linkName1 << ", "
251  << charName2 << ", " << linkName2 << ", "
252  << staticFriction << ", " << slipFriction;
253  if((K.length() == 6) && (C.length() == 6)){
254  cout << ",\n"
255  << "{ "
256  << K[CORBA::ULong(0)] << ", "
257  << K[CORBA::ULong(1)] << ", "
258  << K[CORBA::ULong(2)] << " }\n"
259  << "{ "
260  << K[CORBA::ULong(3)] << ", "
261  << K[CORBA::ULong(4)] << ", "
262  << K[CORBA::ULong(5)] << " }\n"
263  << ",\n"
264  << "{ "
265  << C[CORBA::ULong(0)] << ", "
266  << C[CORBA::ULong(1)] << ", "
267  << C[CORBA::ULong(2)] << " }\n"
268  << "{ "
269  << C[CORBA::ULong(3)] << ", "
270  << C[CORBA::ULong(4)] << ", "
271  << C[CORBA::ULong(5)] << " }\n"
272  << ")" << endl;
273  } else {
274  cout << ", NULL, NULL)" << endl;
275  }
276  }
277 
278  int bodyIndex1 = world.bodyIndex(charName1);
279  int bodyIndex2 = world.bodyIndex(charName2);
280 
281  if(bodyIndex1 >= 0 && bodyIndex2 >= 0){
282 
283  BodyPtr body1 = world.body(bodyIndex1);
284  BodyPtr body2 = world.body(bodyIndex2);
285 
286  std::string emptyString = "";
287  vector<Link*> links1;
288  if(emptyString == linkName1){
289  const LinkTraverse& traverse = body1->linkTraverse();
290  links1.resize(traverse.numLinks());
291  std::copy(traverse.begin(), traverse.end(), links1.begin());
292  } else {
293  links1.push_back(body1->link(linkName1));
294  }
295 
296  vector<Link*> links2;
297  if(emptyString == linkName2){
298  const LinkTraverse& traverse = body2->linkTraverse();
299  links2.resize(traverse.numLinks());
300  std::copy(traverse.begin(), traverse.end(), links2.begin());
301  } else {
302  links2.push_back(body2->link(linkName2));
303  }
304 
305  for(size_t i=0; i < links1.size(); ++i){
306  for(size_t j=0; j < links2.size(); ++j){
307  Link* link1 = links1[i];
308  Link* link2 = links2[j];
309 
310  if(link1 && link2 && link1 != link2){
311  LinkPair_var linkPair = new LinkPair();
312  linkPair->charName1 = CORBA::string_dup(charName1);
313  linkPair->linkName1 = CORBA::string_dup(link1->name.c_str());
314  linkPair->charName2 = CORBA::string_dup(charName2);
315  linkPair->linkName2 = CORBA::string_dup(link2->name.c_str());
316  linkPair->tolerance = 0;
318  collisionDetector->addCollisionPair(linkPair);
320  world.addCollisionPair(linkPair);
321  }
322  }
323  }
324  }
325 
326 }
327 
328 
330 (
331  const char *charName1,
332  const char *linkName1,
333  const char *charName2,
334  const char *linkName2,
335  const double tolerance
336  )
337 {
338  const double epsilon = 0.0;
339 
340  if(debugMode){
341  cout << "DynamicsSimulator_impl::registerIntersectionCheckPair("
342  << charName1 << ", " << linkName1 << ", "
343  << charName2 << ", " << linkName2 << ", "
344  << tolerance << ")" << endl;
345  }
346 
347  int bodyIndex1 = world.bodyIndex(charName1);
348  int bodyIndex2 = world.bodyIndex(charName2);
349 
350  if(bodyIndex1 >= 0 && bodyIndex2 >= 0){
351 
352  BodyPtr body1 = world.body(bodyIndex1);
353  BodyPtr body2 = world.body(bodyIndex2);
354 
355  std::string emptyString = "";
356  vector<Link*> links1;
357  if(emptyString == linkName1){
358  const LinkTraverse& traverse = body1->linkTraverse();
359  links1.resize(traverse.numLinks());
360  std::copy(traverse.begin(), traverse.end(), links1.begin());
361  } else {
362  links1.push_back(body1->link(linkName1));
363  }
364 
365  vector<Link*> links2;
366  if(emptyString == linkName2){
367  const LinkTraverse& traverse = body2->linkTraverse();
368  links2.resize(traverse.numLinks());
369  std::copy(traverse.begin(), traverse.end(), links2.begin());
370  } else {
371  links2.push_back(body2->link(linkName2));
372  }
373 
374  for(size_t i=0; i < links1.size(); ++i){
375  for(size_t j=0; j < links2.size(); ++j){
376  Link* link1 = links1[i];
377  Link* link2 = links2[j];
378 
379  if(link1 && link2 && link1 != link2){
380  LinkPair_var linkPair = new LinkPair();
381  linkPair->charName1 = CORBA::string_dup(charName1);
382  linkPair->linkName1 = CORBA::string_dup(link1->name.c_str());
383  linkPair->charName2 = CORBA::string_dup(charName2);
384  linkPair->linkName2 = CORBA::string_dup(link2->name.c_str());
385  linkPair->tolerance = 0;
387  collisionDetector->addCollisionPair(linkPair);
389  world.addCollisionPair(linkPair);
390  }
391  }
392  }
393  }
394 }
395 
397  (
398  const char* charName1,
399  const char* linkName1,
400  const char* charName2,
401  const char* linkName2,
402  const DblSequence3& link1LocalPos,
403  const DblSequence3& link2LocalPos,
404  const ExtraJointType jointType,
405  const DblSequence3& jointAxis,
406  const char* extraJointName)
407 {
408 }
409 
412 (
413  const char * characterName,
414  const char * extraJointName,
415  DblSequence6_out contactForce
416  )
417 {
418  if(debugMode){
419  cout << "DynamicsSimulator_impl::getConnectionConstraintForce("
420  << characterName << ", " << extraJointName << ")" << endl;
421  }
422 }
423 
424 
426 (
427  const char *characterName,
428  const char *sensorName,
429  DblSequence_out sensorOutput
430  )
431 {
432  if(debugMode){
433  cout << "DynamicsSimulator_impl::getCharacterSensorData("
434  << characterName << ", " << sensorName << ")";
435  }
436 
437  BodyPtr body = world.body(characterName);
438  if(!body){
439  std::cerr << "not found! :" << characterName << std::endl;
440  return;
441  }
442 
443  sensorOutput = new DblSequence;
444  Sensor* sensor = body->sensor<Sensor>(sensorName);
445 
446  if(sensor){
447  switch(sensor->type){
448 
449  case Sensor::FORCE:
450  {
451  ForceSensor* forceSensor = static_cast<ForceSensor*>(sensor);
452  sensorOutput->length(6);
453 #ifndef __WIN32__
454  setVector3(forceSensor->f, sensorOutput);
455  setVector3(forceSensor->tau, sensorOutput, 3);
456 #else
457  sensorOutput[CORBA::ULong(0)] = forceSensor->f(0); sensorOutput[CORBA::ULong(1)] = forceSensor->f(1); sensorOutput[CORBA::ULong(2)] = forceSensor->f(2);
458  sensorOutput[CORBA::ULong(3)] = forceSensor->tau(0); sensorOutput[CORBA::ULong(4)] = forceSensor->tau(1); sensorOutput[CORBA::ULong(5)] = forceSensor->tau(2);
459 #endif
460 
461  }
462  break;
463 
464  case Sensor::RATE_GYRO:
465  {
466  RateGyroSensor* gyro = static_cast<RateGyroSensor*>(sensor);
467  sensorOutput->length(3);
468 #ifndef __WIN32__
469  setVector3(gyro->w, sensorOutput);
470 #else
471  sensorOutput[CORBA::ULong(0)] = gyro->w(0); sensorOutput[CORBA::ULong(1)] = gyro->w(1); sensorOutput[CORBA::ULong(2)] = gyro->w(2);
472 #endif
473  }
474  break;
475 
477  {
478  AccelSensor* accelSensor = static_cast<AccelSensor*>(sensor);
479  sensorOutput->length(3);
480 #ifndef __WIN32__
481  setVector3(accelSensor->dv, sensorOutput);
482 #else
483  sensorOutput[CORBA::ULong(0)] = accelSensor->dv(0); sensorOutput[CORBA::ULong(1)] = accelSensor->dv(1); sensorOutput[CORBA::ULong(2)] = accelSensor->dv(2);
484 #endif
485  }
486  break;
487 
488  case Sensor::RANGE:
489  {
490  RangeSensor *rangeSensor = static_cast<RangeSensor*>(sensor);
491  sensorOutput->length(rangeSensor->distances.size());
492  for (unsigned int i=0; i<rangeSensor->distances.size(); i++){
493  sensorOutput[i] = rangeSensor->distances[i];
494  }
495  }
496  break;
497 
498  default:
499  break;
500  }
501  }
502 
503  if(debugMode){
504  cout << "DynamicsSimulator_impl::getCharacterSensorData - output\n"
505  << "( " << sensorOutput[CORBA::ULong(0)];
506 
507  CORBA::ULong i = 0;
508  while(true){
509  cout << sensorOutput[i++];
510  if(i == sensorOutput->length()) break;
511  cout << ",";
512  }
513 
514  cout << ")" << endl;
515  }
516 
517 }
518 
519 
521 {
522  if(debugMode){
523  cout << "DynamicsSimulator_impl::initSimulation()" << endl;
524  }
525 
526  world.initialize();
527 
529 
531  collisionDetector->queryContactDeterminationForDefinedPairs(allCharacterPositions.in(), collisions.out());
532  }
533 
534  if(enableTimeMeasure){
535  timeMeasureFinished = false;
536  timeMeasureStarted = false;
537  }
538 
539 }
540 
541 
543 {
544  if(enableTimeMeasure){
545  if(!timeMeasureStarted){
547  timeMeasureStarted = true;
548  }
549  }
550 
551  if(debugMode){
552  cout << "DynamicsSimulator_impl::stepSimulation()" << endl;
553  }
554 
556 
559 
561 
564 
567  collisionDetector->queryContactDeterminationForDefinedPairs(allCharacterPositions.in(), collisions.out());
568  }
570 
571  if(enableTimeMeasure){
572  if(world.currentTime() > 5.0 && !timeMeasureFinished){
573  timeMeasureFinished = true;
574  timeMeasure1.end();
575  cout << "Total elapsed time = " << timeMeasure1.totalTime() << "\n"
576  << "Internal elapsed time = " << timeMeasure2.totalTime()
577  << ", the avarage = " << timeMeasure2.averageTime() << endl;
578  cout << "Collision check time = " << timeMeasure3.totalTime() << endl;
579  }
580  }
581 
582 }
583 
584 
586 (
587  const char* characterName,
588  const char* linkName,
589  OpenHRP::DynamicsSimulator::LinkDataType type,
590  const DblSequence& wdata
591  )
592 {
593  if(debugMode){
594  cout << "DynamicsSimulator_impl::setCharacterLinkData("
595  << characterName << ", " << linkName << ", "
596  << getLabelOfLinkDataType(type) << ", ";
597  switch(type) {
598 
599  case OpenHRP::DynamicsSimulator::POSITION_GIVEN:
604  cout << wdata[CORBA::ULong(0)] << ")\n";
605  break;
606 
608  cout << wdata[CORBA::ULong(0)] << ", "
609  << wdata[CORBA::ULong(1)] << ", "
610  << wdata[CORBA::ULong(2)] << ",\n"
611  << wdata[CORBA::ULong(3)] << ", "
612  << wdata[CORBA::ULong(4)] << ", "
613  << wdata[CORBA::ULong(5)] << ",\n"
614  << wdata[CORBA::ULong(6)] << ","
615  << wdata[CORBA::ULong(7)] << ", "
616  << wdata[CORBA::ULong(8)] << ",\n "
617  << wdata[CORBA::ULong(9)] << ","
618  << wdata[CORBA::ULong(10)] << ", "
619  << wdata[CORBA::ULong(11)] << ")" << endl;
620  break;
621 
622  default: // 3x1
623  cout << wdata[CORBA::ULong(0)] << ", "
624  << wdata[CORBA::ULong(1)] << ", "
625  << wdata[CORBA::ULong(2)] << ")" << endl;
626  }
627  }
628 
629  BodyPtr body = world.body(characterName);
630  if(!body){
631  std::cerr << "not found! :" << characterName << std::endl;
632  return;
633  }
634  ODE_Link* link = (ODE_Link*)(body->link(linkName));
635  if(!link){
636  std::cerr << "not found! :" << linkName << std::endl;
637  return;
638  }
639 
640  switch(type) {
641 
642  case OpenHRP::DynamicsSimulator::POSITION_GIVEN:
643  link->isHighGainMode = (wdata[0] > 0.0);
644  if(link->isHighGainMode){
645  std::cerr << " HighGainMode is unsupported. " << std::endl;
646  }
647  break;
648 
650  if(link->jointType != Link::FIXED_JOINT)
651  link->q = wdata[0];
652  break;
653 
655  if(link->jointType != Link::FIXED_JOINT)
656  link->dq = wdata[0];
657  break;
658 
660  if(link->jointType != Link::FIXED_JOINT)
661  link->ddq = wdata[0];
662  break;
663 
665  if(link->jointType != Link::FIXED_JOINT){
666  link->u = wdata[0];
667  link->setTorque(wdata[0]);
668  }
669  break;
670 
672  {
673  link->p(0) = wdata[0];
674  link->p(1) = wdata[1];
675  link->p(2) = wdata[2];
676  Matrix33 R;
677  getMatrix33FromRowMajorArray(R, wdata.get_buffer(), 3);
678  link->setSegmentAttitude(R);
679  link->setTransform(link->p, R);
680  }
681  break;
682 
684  {
685  link->v(0) = wdata[0];
686  link->v(1) = wdata[1];
687  link->v(2) = wdata[2];
688  link->w(0) = wdata[3];
689  link->w(1) = wdata[4];
690  link->w(2) = wdata[5];
691  link->setAbsVelocity(link->v, link->w);
692  }
693  break;
694 
696  {
697  std::cerr << "ERROR - Invalid type: " << getLabelOfLinkDataType(type) << endl;
698  }
699  break;
700 
702  {
703  link->setForce(wdata[0], wdata[1], wdata[2] );
704  link->setTorque(wdata[3], wdata[4], wdata[5] );
705  break;
706  }
707 
708  default:
709  return;
710  }
711 
712  needToUpdatePositions = true;
713  needToUpdateSensorStates = true;
714 
715 }
716 
717 
719 (
720  const char * characterName,
721  const char * linkName,
722  OpenHRP::DynamicsSimulator::LinkDataType type,
723  DblSequence_out out_rdata
724  )
725 {
726  if(debugMode){
727  cout << "DynamicsSimulator_impl::getCharacterLinkData("
728  << characterName << ", " << linkName << ", "
729  << getLabelOfLinkDataType(type) << ")" << endl;
730  }
731 
732  BodyPtr body = world.body(characterName);
733  if(!body){
734  std::cerr << "not found! :" << characterName << std::endl;
735  return;
736  }
737  ODE_Link* link = (ODE_Link*)body->link(linkName);
738  if(!link){
739  std::cerr << "not found! :" << linkName << std::endl;
740  return;
741  }
742 
743  DblSequence_var rdata = new DblSequence;
744 
745  switch(type) {
746 
748  rdata->length(1);
749  rdata[0] = link->q;
750  break;
751 
753  rdata->length(1);
754  rdata[0] = link->dq;
755  break;
756 
758  std::cerr << "ERROR - Invalid type: " << getLabelOfLinkDataType(type) << endl;
759  rdata->length(1);
760  rdata[0] = 0;
761  break;
762 
764  rdata->length(1);
765  rdata[0] = link->u;
766  break;
767 
769  {
770  rdata->length(12);
771  hrp::Vector3 p;
772  hrp::Matrix33 R;
773  link->getTransform(p, R);
774  rdata[0] = p(0);
775  rdata[1] = p(1);
776  rdata[2] = p(2);
777  double* buf = rdata->get_buffer();
779  }
780  break;
781 
783  {
784  rdata->length(6);
785  const dReal* w = link->getAngularVel();
786  hrp::Vector3 v;
787  link->getLinearVel(v);
788  rdata[0] = v(0);
789  rdata[1] = v(1);
790  rdata[2] = v(2);
791  rdata[3] = w[0];
792  rdata[4] = w[1];
793  rdata[5] = w[2];
794  }
795  break;
796 
798  {
799  rdata->length(6);
800  const dReal* f = link->getForce();
801  const dReal* tau = link->getTorque();
802  rdata[0] = f[0];
803  rdata[1] = f[1];
804  rdata[2] = f[2];
805  rdata[3] = tau[0];
806  rdata[4] = tau[1];
807  rdata[5] = tau[2];
808  }
809  break;
810 
812  std::cerr << "ERROR - Invalid type: " << getLabelOfLinkDataType(type) << endl;
813  rdata->length(0);
814  }
815  break;
816 
817  default:
818  break;
819  }
820 
821  out_rdata = rdata._retn();
822 
823 }
824 
825 
827 (
828  const char * characterName,
829  OpenHRP::DynamicsSimulator::LinkDataType type,
830  DblSequence_out rdata
831  )
832 {
833  if(debugMode){
834  cout << "DynamicsSimulator_impl::getCharacterAllLinkData("
835  << characterName << ", "
836  << getLabelOfLinkDataType(type) << ")" << endl;
837  }
838 
839  BodyPtr body = world.body(characterName);
840  if(!body){
841  std::cerr << "not found! :" << characterName << std::endl;
842  return;
843  }
844  int n = body->numJoints();
845  rdata = new DblSequence();
846  rdata->length(n);
847 
848  switch(type) {
849 
851  for(int i=0; i < n; ++i){
852  (*rdata)[i] = body->joint(i)->q;
853  }
854  break;
855 
857  for(int i=0; i < n; ++i){
858  (*rdata)[i] = body->joint(i)->dq;
859  }
860  break;
861 
863  std::cerr << "ERROR - Invalid type: " << getLabelOfLinkDataType(type) << endl;
864  for(int i=0; i < n; ++i){
865  (*rdata)[i] = 0;
866  }
867  break;
868 
870  for(int i=0; i < n; ++i){
871  (*rdata)[i] = body->joint(i)->u;
872  }
873  break;
874 
875  default:
876  cerr << "ERROR - Invalid type: " << getLabelOfLinkDataType(type) << endl;
877  break;
878  }
879 
880 }
881 
882 
884 (
885  const char * characterName,
886  OpenHRP::DynamicsSimulator::LinkDataType type,
887  const DblSequence & wdata
888  )
889 {
890  if(debugMode){
891  cout << "DynamicsSimulator_impl::setCharacterAllLinkData("
892  << characterName << ", "
893  << getLabelOfLinkDataType(type) << ",\n(";
894  if(wdata.length()) cout << wdata[0];
895  for(CORBA::ULong i=0; i<wdata.length(); ++i){
896  cout << ", " << wdata[i];
897  }
898  cout << "))" << endl;
899  }
900 
901  BodyPtr body = world.body(characterName);
902  if(!body){
903  std::cerr << "not found! :" << characterName << std::endl;
904  return;
905  }
906 
907  int n = wdata.length();
908  if(n > body->numJoints()){
909  n = body->numJoints();
910  }
911 
912  switch(type) {
913 
915  for(int i=0; i < n; ++i){
916  if(body->joint(i)->jointType != Link::FIXED_JOINT)
917  body->joint(i)->q = wdata[i];
918  }
919  break;
920 
922  for(int i=0; i < n; ++i){
923  if(body->joint(i)->jointType != Link::FIXED_JOINT)
924  body->joint(i)->dq = wdata[i];
925  }
926  break;
927 
929  for(int i=0; i < n; ++i){
930  if(body->joint(i)->jointType != Link::FIXED_JOINT)
931  body->joint(i)->ddq = wdata[i];
932  }
933  break;
934 
936  for(int i=0; i < n; ++i){
937  if(body->joint(i)->jointType != Link::FIXED_JOINT){
938  body->joint(i)->u = wdata[i];
939  ((ODE_Link*)body->joint(i))->setTorque(wdata[i]);
940  }
941  }
942  break;
943 
944  default:
945  std::cerr << "ERROR - Invalid type: " << getLabelOfLinkDataType(type) << endl;
946  }
947 
948  needToUpdatePositions = true;
949  needToUpdateSensorStates = true;
950 
951 }
952 
954 (
955  const DblSequence3& wdata
956  )
957 {
958  if(wdata.length() != 3){
959  std::cerr << "setGVector : The data length is not three. " << std::endl;
960  return;
961  }
962 
963  dVector3 g;
964  g[0] = wdata[CORBA::ULong(0)];
965  g[1] = wdata[CORBA::ULong(1)];
966  g[2] = -wdata[CORBA::ULong(2)];
967  world.setGravityAcceleration(g);
968 
969  if(debugMode){
970  cout << "ODE_DynamicsSimulator_impl::setGVector("
971  << wdata[CORBA::ULong(0)] << ", "
972  << wdata[CORBA::ULong(1)] << ", "
973  << wdata[CORBA::ULong(2)] << ")" << endl;
974  }
975 }
976 
977 
979 (
980  DblSequence3_out wdata
981  )
982 {
983  wdata->length(3);
984  dVector3 g;
985  world.getGravityAcceleration(g);
986  (*wdata)[0] = g[0];
987  (*wdata)[1] = g[1];
988  (*wdata)[2] = g[2];
989 
990  if(debugMode){
991  cout << "DynamicsSimulator_impl::getGVector(";
992  cout << wdata[CORBA::ULong(0)] << ", "
993  << wdata[CORBA::ULong(1)] << ", "
994  << wdata[CORBA::ULong(2)] << ")" << endl;
995  }
996 }
997 
998 
1001  const char * characterName,
1002  OpenHRP::DynamicsSimulator::JointDriveMode jointMode
1003  )
1004 {
1005  bool isHighGainMode = (jointMode == OpenHRP::DynamicsSimulator::HIGH_GAIN_MODE);
1006 
1007  BodyPtr body = world.body(characterName);
1008  if(!body){
1009  std::cerr << "not found! :" << characterName << std::endl;
1010  return;
1011  }
1012 
1013  for(int i=1; i < body->numLinks(); ++i){
1014  body->link(i)->isHighGainMode = isHighGainMode;
1015  }
1016 
1017  if(isHighGainMode){
1018  std::cerr << " HighGainMode is unsupported. " << std::endl;
1019  }
1020 
1021  if(debugMode){
1022  cout << "DynamicsSimulator_impl::setCharacterAllJointModes(";
1023  cout << characterName << ", ";
1024  cout << (isHighGainMode ? "HIGH_GAIN_MODE" : "TORQUE_MODE");
1025  cout << ")" << endl;
1026  }
1027 
1028 }
1029 
1030 
1033  const char * characterName,
1034  const char * fromLink, const char * toLink,
1035  const LinkPosition& target
1036  )
1037 {
1038  if(debugMode){
1039  cout << "DynamicsSimulator_impl::calcCharacterInverseKinematics("
1040  << characterName << ", " << fromLink << ", " << toLink << ",\n"
1041  << "( "
1042  << target.p[0] << ", " << target.p[1] << ", " << target.p[2] << ",\n\n"
1043  << target.R[0] << ", " << target.R[1] << ", " << target.R[2] << ",\n"
1044  << target.R[3] << ", " << target.R[4] << ", " << target.R[5] << ",\n"
1045  << target.R[6] << ", " << target.R[7] << ", " << target.R[8] << endl;
1046  }
1047 
1048  bool solved = false;
1049 
1050  BodyPtr body = world.body(characterName);
1051  if(!body){
1052  std::cerr << "not found! :" << characterName << std::endl;
1053  return false;
1054  }
1055 
1056  JointPath path(body->link(fromLink), body->link(toLink));
1057 
1058  if(!path.empty()){
1059  Vector3 p(target.p[0], target.p[1], target.p[2]);
1060  Matrix33 R;
1061  for (int i=0; i<3; i++) {
1062  for (int j=0; j<3; j++){
1063  R(i,j) = target.R[3*i+j];
1064  }
1065  }
1066 
1067  solved = path.calcInverseKinematics(p, R);
1068 
1069  if(solved) {
1070  needToUpdatePositions = true;
1071  needToUpdateSensorStates = true;
1072  }
1073  }
1074 
1075  return solved;
1076 
1077  return 0;
1078 }
1079 
1080 
1083  const char * characterName
1084  )
1085 {
1086  if(debugMode){
1087  cout << "DynamicsSimulator_impl::calcCharacterForwardKinematics( "
1088  << characterName << endl;
1089  }
1090 
1091  BodyPtr body = world.body(characterName);
1092  if(!body){
1093  std::cerr << "not found! :" << characterName << std::endl;
1094  return;
1095  }
1096  body->calcForwardKinematics(true, true);
1097 
1098  for(int j=0; j<body->numLinks(); j++){
1099  ODE_Link* link = (ODE_Link*)body->link(j);
1100  link->setTransform(link->p, link->segmentAttitude());
1101  link->setAbsVelocity(link->v, link->w);
1102  }
1103 
1104  needToUpdatePositions = true;
1105  needToUpdateSensorStates = true;
1106 
1107 }
1108 
1109 
1111 {
1112 
1113  for(int i=0; i < world.numBodies(); ++i){
1114  hrp::BodyPtr b = world.body(i);
1115  b->calcForwardKinematics(true, true);
1116  for(int j=0; j<b->numLinks(); j++){
1117  ODE_Link* link = (ODE_Link*)b->link(j);
1118  link->setTransform(link->p, link->segmentAttitude());
1119  link->setAbsVelocity(link->v, link->w);
1120  }
1121  }
1122 
1123  needToUpdatePositions = true;
1124  needToUpdateSensorStates = true;
1125 
1126 }
1127 
1128 
1130 {
1131 
1134 
1135  if (checkAll){
1136  return collisionDetector->queryContactDeterminationForDefinedPairs(allCharacterPositions.in(), collisions.out());
1137  }else{
1138  return collisionDetector->queryIntersectionForDefinedPairs(checkAll, allCharacterPositions.in(), collidingLinkPairs.out());
1139  }
1140 
1141  return 0;
1142 
1143 }
1144 
1145 
1147 {
1148 
1151 
1152  DistanceSequence_var distances = new DistanceSequence;
1153  collisionDetector->queryDistanceForDefinedPairs(allCharacterPositions.in(), distances);
1154  return distances._retn();
1155 
1156  return NULL;
1157 }
1158 
1159 
1160 LinkPairSequence *ODE_DynamicsSimulator_impl::checkIntersection(CORBA::Boolean checkAll)
1161 {
1162 
1165 
1166  LinkPairSequence_var pairs = new LinkPairSequence;
1167  collisionDetector->queryIntersectionForDefinedPairs(checkAll, allCharacterPositions.in(), pairs);
1168  return pairs._retn();
1169 
1170  return NULL;
1171 }
1172 
1174 {
1175  if(debugMode){
1176  cout << "DynamicsSimulator_impl::getWorldState()\n";
1177  }
1178 
1180 
1181  wstate = new WorldState;
1182 
1183  wstate->time = world.currentTime();
1184  wstate->characterPositions = allCharacterPositions;
1185  wstate->collisions = collisions;
1186 
1187  if(debugMode){
1188  cout << "getWorldState - exit" << endl;
1189  }
1190 
1191 }
1192 
1193 
1194 void ODE_DynamicsSimulator_impl::getCharacterSensorState(const char* characterName, SensorState_out sstate)
1195 {
1196 
1197  int bodyIndex = world.bodyIndex(characterName);
1198 
1199  if(bodyIndex >= 0){
1202  }
1203  sstate = new SensorState(allCharacterSensorStates[bodyIndex]);
1204  } else {
1205  sstate = new SensorState;
1206  }
1207 
1208 }
1209 
1210 
1212 {
1213  if(debugMode){
1214  cout << "DynamicsSimulator_impl::_setupCharacterData()\n";
1215  }
1216 
1217  int n = world.numBodies();
1218  allCharacterPositions->length(n);
1219  allCharacterSensorStates->length(n);
1220 
1221  for(int i=0; i < n; ++i){
1222  BodyPtr body = world.body(i);
1223 
1224  int numLinks = body->numLinks();
1225  CharacterPosition& characterPosition = allCharacterPositions[i];
1226  characterPosition.characterName = CORBA::string_dup(body->name().c_str());
1227  LinkPositionSequence& linkPositions = characterPosition.linkPositions;
1228  linkPositions.length(numLinks);
1229 
1230  int numJoints = body->numJoints();
1231  SensorState& sensorState = allCharacterSensorStates[i];
1232  sensorState.q.length(numJoints);
1233  sensorState.dq.length(numJoints);
1234  sensorState.u.length(numJoints);
1235  sensorState.force.length(body->numSensors(Sensor::FORCE));
1236  sensorState.rateGyro.length(body->numSensors(Sensor::RATE_GYRO));
1237  sensorState.accel.length(body->numSensors(Sensor::ACCELERATION));
1238  sensorState.range.length(body->numSensors(Sensor::RANGE));
1239 
1240  if(debugMode){
1241  std::cout << "character[" << i << "], nlinks = " << numLinks << "\n";
1242  }
1243  }
1244 
1245  if(debugMode){
1246  cout << "_setupCharacterData() - exit" << endl;;
1247  }
1248 
1249 }
1250 
1251 
1253 {
1254  if(debugMode){
1255  cout << "DynamicsSimulator_impl::_updateCharacterPositions()\n";
1256  }
1257 
1258  int n = world.numBodies();
1259 
1260  {
1261 #pragma omp parallel for num_threads(3)
1262  for(int i=0; i < n; ++i){
1263  BodyPtr body = world.body(i);
1264  int numLinks = body->numLinks();
1265 
1266  CharacterPosition& characterPosition = allCharacterPositions[i];
1267 
1268  if(debugMode){
1269  cout << "character[" << i << "], nlinks = " << numLinks << "\n";
1270  }
1271 
1272  for(int j=0; j < numLinks; ++j) {
1273  Link* link = body->link(j);
1274  LinkPosition& linkPosition = characterPosition.linkPositions[j];
1275  setVector3(link->p, linkPosition.p);
1276  setMatrix33ToRowMajorArray(link->segmentAttitude(), linkPosition.R);
1277  }
1278  }
1279  }
1280  needToUpdatePositions = false;
1281 
1282  if(debugMode){
1283  cout << "_updateCharacterData() - exit" << endl;
1284  }
1285 
1286 }
1287 
1288 
1290 {
1291 
1292  if(debugMode){
1293  cout << "DynamicsSimulator_impl::_updateSensorStates()\n";
1294  }
1295 
1296  int numBodies = world.numBodies();
1297  for(int i=0; i < numBodies; ++i){
1298 
1299  SensorState& state = allCharacterSensorStates[i];
1300 
1301  BodyPtr body = world.body(i);
1302  int numJoints = body->numJoints();
1303 
1304  for(int j=0; j < numJoints; j++){
1305  ODE_Link* joint = (ODE_Link*)body->joint(j);
1306  state.q [j] = joint->q;
1307  state.dq[j] = joint->dq;
1308  state.u [j] = joint->u;
1309  }
1310 
1311  int n = body->numSensors(Sensor::FORCE);
1312  for(int id = 0; id < n; ++id){
1313  ForceSensor* sensor = body->sensor<ForceSensor>(id);
1314  setVector3(sensor->f, state.force[id], 0);
1315  setVector3(sensor->tau, state.force[id], 3);
1316  if(debugMode){
1317  std::cout << "Force Sensor: f:" << sensor->f << "tau:" << sensor->tau << "\n";
1318  }
1319  }
1320 
1321  n = body->numSensors(Sensor::RATE_GYRO);
1322  for(int id=0; id < n; ++id){
1323  RateGyroSensor* sensor = body->sensor<RateGyroSensor>(id);
1324  setVector3(sensor->w, state.rateGyro[id]);
1325  if(debugMode){
1326  std::cout << "Rate Gyro:" << sensor->w << "\n";
1327  }
1328  }
1329 
1330  n = body->numSensors(Sensor::ACCELERATION);
1331  for(int id=0; id < n; ++id){
1332  AccelSensor* sensor = body->sensor<AccelSensor>(id);
1333  setVector3(sensor->dv, state.accel[id]);
1334  if(debugMode){
1335  std::cout << "Accel:" << sensor->dv << std::endl;
1336  }
1337  }
1338 
1339  n = body->numSensors(Sensor::RANGE);
1340  for (int id=0; id < n; ++id){
1341  RangeSensor *rangeSensor = body->sensor<RangeSensor>(id);
1342  if (world.currentTime() >= rangeSensor->nextUpdateTime){
1343  Vector3 gp(rangeSensor->link->p + (rangeSensor->link->R)*rangeSensor->localPos);
1344  Matrix33 gR(rangeSensor->link->R*rangeSensor->localR);
1345  DblArray3 p;
1346  DblArray9 R;
1347  setVector3(gp, p);
1349  DblSequence_var data = collisionDetector->scanDistanceWithRay(p, R,
1350  rangeSensor->scanStep, rangeSensor->scanAngle);
1351  rangeSensor->distances.resize(data->length());
1352  for (unsigned int i=0; i<data->length(); i++){
1353  rangeSensor->distances[i] = data[i];
1354  }
1355  rangeSensor->nextUpdateTime += 1.0/rangeSensor->scanRate;
1356  state.range[id] = data;
1357  }
1358  }
1359 
1360  }
1361  needToUpdateSensorStates = false;
1362 
1363  if(debugMode){
1364  cout << "_updateCharacterData() - exit" << endl;
1365  }
1366 
1367 }
1368 
1369 
1370 
1377  const char *characterName,
1378  LinkPairSequence_out pairs
1379  )
1380 {
1381  if(debugMode){
1382  cout << "DynamicsSimulator_impl::getCharacterCollidingPairs("
1383  << characterName << ")" << endl;
1384  }
1385 
1386  if(!world.body(characterName)){
1387  std::cerr << "not found! :" << characterName << std::endl;
1388  return false;
1389  }
1390 
1391  std::vector<unsigned int> locations;
1392 
1393  for(unsigned int i=0; i < collisions->length(); ++i) {
1394 
1395  if( (strcmp(characterName, collisions[i].pair.charName1) == 0) ||
1396  (strcmp(characterName, collisions[i].pair.charName2) == 0))
1397  locations.push_back(i);
1398  }
1399 
1400  pairs->length(locations.size());
1401 
1402  unsigned long n=0;
1403  for(std::vector<unsigned int>::iterator iter = locations.begin();
1404  iter != locations.end(); ++iter) {
1405 
1406  strcpy(pairs[n].charName1, collisions[*iter].pair.charName1);
1407  strcpy(pairs[n].charName2, collisions[*iter].pair.charName2);
1408  strcpy(pairs[n].linkName1, collisions[*iter].pair.linkName1);
1409  strcpy(pairs[n].linkName2, collisions[*iter].pair.linkName2);
1410  }
1411 
1412  return true;
1413 }
1414 
1415 
1418  const char *characterName,
1419  const char *baseLink,
1420  const char *targetLink,
1421  DblSequence_out jacobian
1422  )
1423 {
1424  if(debugMode){
1425  cout << "DynamicsSimulator_impl::calcCharacterJacobian("
1426  << characterName << ", "
1427  << baseLink << ", "
1428  << targetLink << ")" << endl;
1429  }
1430 
1431  BodyPtr body = world.body(characterName);
1432  if(!body){
1433  std::cerr << "not found! :" << characterName << std::endl;
1434  return;
1435  }
1436 
1437  JointPath path(body->link(baseLink), body->link(targetLink));
1438  int height = 6;
1439  int width = path.numJoints();
1440  dmatrix J(height, width);
1441  path.calcJacobian(J);
1442 
1443  jacobian->length(height * width);
1444  int i = 0;
1445  for(int r=0; r < height; ++r){
1446  for(int c=0; c < width; ++c){
1447  (*jacobian)[i++] = J(r, c);
1448  }
1449  }
1450 
1451 }
1452 
1453 
1459  orb_(CORBA::ORB::_duplicate(orb))
1460 {
1461  initializeCommandLabelMaps();
1462 
1463  if(debugMode){
1464  cout << "DynamicsSimulatorFactory_impl::DynamicsSimulatorFactory_impl()" << endl;
1465  }
1466 }
1467 
1468 
1470 {
1471  if(debugMode){
1472  cout << "DynamicsSimulatorFactory_impl::~DynamicsSimulatorFactory_impl()" << endl;
1473  }
1474 
1475  PortableServer::POA_var poa = _default_POA();
1476  PortableServer::ObjectId_var id = poa -> servant_to_id(this);
1477  poa -> deactivate_object(id);
1478 }
1479 
1480 
1481 DynamicsSimulator_ptr DynamicsSimulatorFactory_impl::create()
1482 {
1483  if(debugMode){
1484  cout << "DynamicsSimulatorFactory_impl::create()" << endl;
1485  }
1486 
1488 
1489  PortableServer::ServantBase_var integratorrServant = integratorImpl;
1490  PortableServer::POA_var poa_ = _default_POA();
1491  PortableServer::ObjectId_var id =
1492  poa_ -> activate_object(integratorImpl);
1493 
1494  return integratorImpl -> _this();
1495 }
1496 
1497 
1499 {
1500  orb_->shutdown(false);
1501 }
hrp::Sensor::localR
Matrix33 localR
Definition: hrplib/hrpModel/Sensor.h:60
TimeMeasure::averageTime
double averageTime() const
Definition: hrplib/hrpUtil/TimeMeasure.h:50
ModelLoaderUtil.h
ODE_DynamicsSimulator_impl::_setupCharacterData
void _setupCharacterData()
Definition: ODE_DynamicsSimulator_impl.cpp:1211
width
png_infop png_uint_32 * width
Definition: png.h:2306
hrp::LinkTraverse::end
std::vector< Link * >::const_iterator end() const
Definition: LinkTraverse.h:68
ODE_DynamicsSimulator_impl::calcCharacterForwardKinematics
virtual void calcCharacterForwardKinematics(const char *characterName)
Definition: ODE_DynamicsSimulator_impl.cpp:1082
i
png_uint_32 i
Definition: png.h:2732
hrp::WorldBase::body
BodyPtr body(int index)
get body by index
Definition: hrplib/hrpModel/World.cpp:56
ODE_DynamicsSimulator_impl::getCharacterSensorState
virtual void getCharacterSensorState(const char *characterName, SensorState_out sstate)
Definition: ODE_DynamicsSimulator_impl.cpp:1194
hrpPrep.NotFound
NotFound
Definition: hrpPrep.py:129
ODE_DynamicsSimulator_impl::getWorldState
virtual void getWorldState(WorldState_out wstate)
Definition: ODE_DynamicsSimulator_impl.cpp:1173
ODE_DynamicsSimulator_impl::checkIntersection
virtual LinkPairSequence * checkIntersection(CORBA::Boolean checkAll)
Definition: ODE_DynamicsSimulator_impl.cpp:1160
ColladaUtil::dReal
double dReal
Definition: ColladaUtil.h:78
ODE_DynamicsSimulator_impl::getExtraJointConstraintForce
virtual void getExtraJointConstraintForce(const char *characterName, const char *extraJointName, DblSequence6_out contactForce)
Definition: ODE_DynamicsSimulator_impl.cpp:412
Sensor::ACCELERATION
@ ACCELERATION
Definition: server/UtDynamicsSimulator/Sensor.h:32
Sensor.h
hrp::LinkTraverse::begin
std::vector< Link * >::const_iterator begin() const
Definition: LinkTraverse.h:64
ODE_DynamicsSimulator_impl::getCharacterLinkData
virtual void getCharacterLinkData(const char *characterName, const char *link, OpenHRP::DynamicsSimulator::LinkDataType type, DblSequence_out rdata)
Definition: ODE_DynamicsSimulator_impl.cpp:719
hrp::WorldBase::currentTime
double currentTime(void) const
get current time
Definition: hrplib/hrpModel/World.h:116
hrp::RangeSensor::distances
std::vector< double > distances
Definition: hrplib/hrpModel/Sensor.h:123
debugMode
static const int debugMode
Definition: ODE_DynamicsSimulator_impl.cpp:31
ODE_DynamicsSimulator_impl::allCharacterSensorStates
SensorStateSequence_var allCharacterSensorStates
Definition: ODE_DynamicsSimulator_impl.h:51
Sensor::FORCE
@ FORCE
Definition: server/UtDynamicsSimulator/Sensor.h:30
USE_INTERNAL_COLLISION_DETECTOR
static const bool USE_INTERNAL_COLLISION_DETECTOR
Definition: ODE_DynamicsSimulator_impl.cpp:29
ODE_World::calcNextState
void calcNextState(OpenHRP::CollisionSequence &corbaCollisionSequence)
compute forward dynamics and update current state
Definition: ODE_World.cpp:104
hrp::Sensor::type
int type
Definition: hrplib/hrpModel/Sensor.h:57
ODE_DynamicsSimulator_impl::registerIntersectionCheckPair
virtual void registerIntersectionCheckPair(const char *char1, const char *name1, const char *char2, const char *name2, const double tolerance)
Definition: ODE_DynamicsSimulator_impl.cpp:330
ODE_DynamicsSimulator_impl
Definition: ODE_DynamicsSimulator_impl.h:33
ODE_DynamicsSimulator_impl::registerCollisionCheckPair
virtual void registerCollisionCheckPair(const char *char1, const char *name1, const char *char2, const char *name2, CORBA::Double staticFriction, CORBA::Double slipFriction, const DblSequence6 &K, const DblSequence6 &C, const double culling_thresh, const double restitution)
Definition: ODE_DynamicsSimulator_impl.cpp:233
autoplay.n
n
Definition: autoplay.py:12
hrp
Definition: ColdetModel.h:28
hrp::RangeSensor::scanAngle
double scanAngle
Definition: hrplib/hrpModel/Sensor.h:122
swingTest.f
f
Definition: swingTest.py:6
hrp::ForceSensor::f
Vector3 f
Definition: hrplib/hrpModel/Sensor.h:74
Sensor::RATE_GYRO
@ RATE_GYRO
Definition: server/UtDynamicsSimulator/Sensor.h:31
hrp::ForceSensor
Definition: hrplib/hrpModel/Sensor.h:68
hrp::RangeSensor::scanRate
double scanRate
Definition: hrplib/hrpModel/Sensor.h:122
checkCorbaServer
X_ptr checkCorbaServer(std::string n, CosNaming::NamingContext_var &cxt)
Definition: ODE_DynamicsSimulator_impl.cpp:80
b
long b
Definition: jpegint.h:371
hrp::JointPath::numJoints
unsigned int numJoints() const
Definition: JointPath.h:42
DynamicsSimulatorFactory_impl::DynamicsSimulatorFactory_impl
DynamicsSimulatorFactory_impl(CORBA::ORB_ptr orb)
Definition: AistDynamicsSimulator/DynamicsSimulator_impl.cpp:1454
CONSTRAINT_FORCE
@ CONSTRAINT_FORCE
Definition: BridgeConf.h:51
type
png_infop png_charp png_int_32 png_int_32 int * type
Definition: png.h:2330
ODE_DynamicsSimulator_impl::world
ODE_World world
Definition: ODE_DynamicsSimulator_impl.h:41
hrp::Sensor::link
Link * link
Definition: hrplib/hrpModel/Sensor.h:59
ODE_DynamicsSimulator_impl::init
virtual void init(CORBA::Double timeStep, OpenHRP::DynamicsSimulator::IntegrateMethod integrateOpt, OpenHRP::DynamicsSimulator::SensorOption sensorOpt)
Definition: ODE_DynamicsSimulator_impl.cpp:195
hrp::WorldBase::numBodies
unsigned int numBodies()
get the number of bodies in this world
Definition: hrplib/hrpModel/World.h:44
ODE_DynamicsSimulator_impl::calcCharacterJacobian
virtual void calcCharacterJacobian(const char *characterName, const char *baseLink, const char *targetLink, DblSequence_out jacobian)
Definition: ODE_DynamicsSimulator_impl.cpp:1417
ODE_DynamicsSimulator_impl.h
ODE_DynamicsSimulator_impl::~ODE_DynamicsSimulator_impl
~ODE_DynamicsSimulator_impl()
Definition: ODE_DynamicsSimulator_impl.cpp:153
ODE_DynamicsSimulator_impl::getCharacterAllLinkData
virtual void getCharacterAllLinkData(const char *characterName, OpenHRP::DynamicsSimulator::LinkDataType type, DblSequence_out wdata)
Definition: ODE_DynamicsSimulator_impl.cpp:827
TimeMeasure::begin
void begin()
Definition: hrplib/hrpUtil/TimeMeasure.h:33
hrp::ForceSensor::tau
Vector3 tau
Definition: hrplib/hrpModel/Sensor.h:75
ODE_DynamicsSimulator_impl::timeMeasure1
TimeMeasure timeMeasure1
Definition: ODE_DynamicsSimulator_impl.h:54
ODE_DynamicsSimulator_impl::getCharacterCollidingPairs
virtual CORBA::Boolean getCharacterCollidingPairs(const char *characterName, LinkPairSequence_out pairs)
Definition: ODE_DynamicsSimulator_impl.cpp:1376
ODE_DynamicsSimulator_impl::checkCollision
virtual bool checkCollision(bool checkAll)
Definition: ODE_DynamicsSimulator_impl.cpp:1129
DynamicsSimulatorFactory_impl::orb_
CORBA::ORB_var orb_
Definition: AistDynamicsSimulator/DynamicsSimulator_impl.h:222
ODE_DynamicsSimulator_impl::timeMeasureFinished
bool timeMeasureFinished
Definition: ODE_DynamicsSimulator_impl.h:57
ODE_DynamicsSimulator_impl::collidingLinkPairs
LinkPairSequence_var collidingLinkPairs
Definition: ODE_DynamicsSimulator_impl.h:46
hrp::Vector3
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
ODE_DynamicsSimulator_impl::stepSimulation
virtual void stepSimulation()
Definition: ODE_DynamicsSimulator_impl.cpp:542
USE_ODE_COLLISION_DETECTOR
static const bool USE_ODE_COLLISION_DETECTOR
Definition: ODE_DynamicsSimulator_impl.cpp:30
hrp::Sensor::localPos
Vector3 localPos
Definition: hrplib/hrpModel/Sensor.h:61
ODE_World::clearExternalForces
void clearExternalForces()
Definition: ODE_World.cpp:193
hrp::RangeSensor
Definition: hrplib/hrpModel/Sensor.h:115
ODE_DynamicsSimulator_impl::_updateCharacterPositions
void _updateCharacterPositions()
Definition: ODE_DynamicsSimulator_impl.cpp:1252
enableTimeMeasure
static const bool enableTimeMeasure
Definition: ODE_DynamicsSimulator_impl.cpp:32
hrp::JointPath::calcInverseKinematics
virtual bool calcInverseKinematics(const Vector3 &end_p, const Matrix33 &end_R)
Definition: JointPath.cpp:252
JointPath.h
The header file of the LinkPath and JointPath classes.
hrp::RateGyroSensor::w
Vector3 w
Definition: hrplib/hrpModel/Sensor.h:88
ODE_DynamicsSimulator_impl::collisions
CollisionSequence_var collisions
Definition: ODE_DynamicsSimulator_impl.h:45
hrp::AccelSensor
Definition: hrplib/hrpModel/Sensor.h:95
hrp::dmatrix
Eigen::MatrixXd dmatrix
Definition: EigenTypes.h:13
hrp::JointPath::empty
bool empty() const
Definition: JointPath.h:38
ODE_DynamicsSimulator_impl::calcWorldForwardKinematics
virtual void calcWorldForwardKinematics()
Definition: ODE_DynamicsSimulator_impl.cpp:1110
LinkTraverse.h
The header file of the LinkTraverse class.
ODE_DynamicsSimulator_impl::getCharacterSensorValues
virtual void getCharacterSensorValues(const char *characterName, const char *sensorName, DblSequence_out values)
Definition: ODE_DynamicsSimulator_impl.cpp:426
ABS_TRANSFORM
@ ABS_TRANSFORM
Definition: BridgeConf.h:41
hrp::LinkTraverse::numLinks
unsigned int numLinks() const
Definition: LinkTraverse.h:40
DynamicsSimulatorFactory_impl::~DynamicsSimulatorFactory_impl
~DynamicsSimulatorFactory_impl()
Definition: AistDynamicsSimulator/DynamicsSimulator_impl.cpp:1465
ODE_DynamicsSimulator_impl::timeMeasureStarted
bool timeMeasureStarted
Definition: ODE_DynamicsSimulator_impl.h:58
ODE_World::useInternalCollisionDetector
void useInternalCollisionDetector(bool use)
Definition: ODE_World.h:106
hrp::BodyPtr
boost::shared_ptr< Body > BodyPtr
Definition: Body.h:315
JOINT_VALUE
@ JOINT_VALUE
Definition: BridgeConf.h:36
name
png_infop png_charpp name
Definition: png.h:2379
ODE_DynamicsSimulator_impl::timeMeasure3
TimeMeasure timeMeasure3
Definition: ODE_DynamicsSimulator_impl.h:56
DynamicsSimulatorFactory_impl::shutdown
void shutdown()
Definition: AistDynamicsSimulator/DynamicsSimulator_impl.cpp:1494
ODE_DynamicsSimulator_impl::setCharacterLinkData
virtual void setCharacterLinkData(const char *characterName, const char *link, OpenHRP::DynamicsSimulator::LinkDataType type, const DblSequence &data)
Definition: ODE_DynamicsSimulator_impl.cpp:586
buf
png_bytep buf
Definition: png.h:2726
ODE_DynamicsSimulator_impl::getGVector
virtual void getGVector(DblSequence3_out wdata)
Definition: ODE_DynamicsSimulator_impl.cpp:979
data
JSAMPIMAGE data
Definition: jpeglib.h:945
ODE_DynamicsSimulator_impl::setCharacterAllJointModes
virtual void setCharacterAllJointModes(const char *characterName, OpenHRP::DynamicsSimulator::JointDriveMode jointMode)
Definition: ODE_DynamicsSimulator_impl.cpp:1000
ODE_DynamicsSimulator_impl::initSimulation
virtual void initSimulation()
Definition: ODE_DynamicsSimulator_impl.cpp:520
ODE_DynamicsSimulator_impl::needToUpdateSensorStates
bool needToUpdateSensorStates
Definition: ODE_DynamicsSimulator_impl.h:52
JOINT_TORQUE
@ JOINT_TORQUE
Definition: BridgeConf.h:39
ODE_DynamicsSimulator_impl::calcCharacterInverseKinematics
virtual CORBA::Boolean calcCharacterInverseKinematics(const char *characterName, const char *baseLink, const char *targetLink, const LinkPosition &target)
Definition: ODE_DynamicsSimulator_impl.cpp:1032
ODE_DynamicsSimulator_impl::needToUpdatePositions
bool needToUpdatePositions
Definition: ODE_DynamicsSimulator_impl.h:49
hrp::setVector3
void setVector3(const Vector3 &v3, V &v, size_t top=0)
Definition: Eigen3d.h:130
ODE_World::initialize
void initialize()
initialize this world. This must be called after all bodies are registered.
Definition: ODE_World.cpp:173
autoplay.c
int c
Definition: autoplay.py:16
ODE_DynamicsSimulator_impl::collisionDetector
CollisionDetector_var collisionDetector
Definition: ODE_DynamicsSimulator_impl.h:43
hrp::WorldBase::bodyIndex
int bodyIndex(const std::string &name)
get index of body by name
Definition: hrplib/hrpModel/World.cpp:49
hrp::RangeSensor::scanStep
double scanStep
Definition: hrplib/hrpModel/Sensor.h:122
hrp::JointPath::calcJacobian
void calcJacobian(dmatrix &out_J, const Vector3 &local_p=Vector3::Zero()) const
Definition: JointPath.cpp:128
Body.h
JOINT_ACCELERATION
@ JOINT_ACCELERATION
Definition: BridgeConf.h:38
ODE_DynamicsSimulator_impl::setCharacterAllLinkData
virtual void setCharacterAllLinkData(const char *characterName, OpenHRP::DynamicsSimulator::LinkDataType type, const DblSequence &wdata)
Definition: ODE_DynamicsSimulator_impl.cpp:884
ABS_ACCELERATION
@ ABS_ACCELERATION
Definition: BridgeConf.h:43
hrp::Sensor
Definition: hrplib/hrpModel/Sensor.h:27
hrp::LinkTraverse
Definition: LinkTraverse.h:29
hrp::Matrix33
Eigen::Matrix3d Matrix33
Definition: EigenTypes.h:12
Eigen3d.h
height
png_infop png_uint_32 png_uint_32 * height
Definition: png.h:2306
ODE_DynamicsSimulator_impl::setGVector
virtual void setGVector(const DblSequence3 &wdata)
Definition: ODE_DynamicsSimulator_impl.cpp:954
JOINT_VELOCITY
@ JOINT_VELOCITY
Definition: BridgeConf.h:37
ODE_DynamicsSimulator_impl::allCharacterPositions
CharacterPositionSequence_var allCharacterPositions
Definition: ODE_DynamicsSimulator_impl.h:48
ODE_DynamicsSimulator_impl::registerExtraJoint
virtual void registerExtraJoint(const char *charName1, const char *linkName1, const char *charName2, const char *linkName2, const DblSequence3 &link1LocalPos, const DblSequence3 &link2LocalPos, const ExtraJointType jointType, const DblSequence3 &jointAxis, const char *extraJointName)
Definition: ODE_DynamicsSimulator_impl.cpp:397
hrp::setMatrix33ToRowMajorArray
void setMatrix33ToRowMajorArray(const Matrix33 &m33, Array &a, size_t top=0)
Definition: Eigen3d.h:105
ODE_DynamicsSimulator_impl::destroy
virtual void destroy()
Definition: ODE_DynamicsSimulator_impl.cpp:163
ODE_DynamicsSimulator_impl::registerCharacter
virtual void registerCharacter(const char *name, BodyInfo_ptr binfo)
Definition: ODE_DynamicsSimulator_impl.cpp:176
hrp::WorldBase::setRungeKuttaMethod
void setRungeKuttaMethod()
choose runge-kutta method for integration
Definition: hrplib/hrpModel/World.cpp:186
ABS_VELOCITY
@ ABS_VELOCITY
Definition: BridgeConf.h:42
ODE_DynamicsSimulator_impl::ODE_DynamicsSimulator_impl
ODE_DynamicsSimulator_impl(CORBA::ORB_ptr orb)
Definition: ODE_DynamicsSimulator_impl.cpp:111
DynamicsSimulatorFactory_impl::create
DynamicsSimulator_ptr create()
Definition: AistDynamicsSimulator/DynamicsSimulator_impl.cpp:1477
hrp::getMatrix33FromRowMajorArray
void getMatrix33FromRowMajorArray(Matrix33 &m33, const Array &a, size_t top=0)
Definition: Eigen3d.h:118
TimeMeasure::end
void end()
Definition: hrplib/hrpUtil/TimeMeasure.h:37
hrp::RateGyroSensor
Definition: hrplib/hrpModel/Sensor.h:82
ODE_DynamicsSimulator_impl::timeMeasure2
TimeMeasure timeMeasure2
Definition: ODE_DynamicsSimulator_impl.h:55
TimeMeasure::totalTime
double totalTime() const
Definition: hrplib/hrpUtil/TimeMeasure.h:48
ODE_DynamicsSimulator_impl::checkDistance
virtual DistanceSequence * checkDistance()
Definition: ODE_DynamicsSimulator_impl.cpp:1146
hrp::RangeSensor::nextUpdateTime
double nextUpdateTime
Definition: hrplib/hrpModel/Sensor.h:124
hrp::AccelSensor::dv
Vector3 dv
Definition: hrplib/hrpModel/Sensor.h:104
EXTERNAL_FORCE
@ EXTERNAL_FORCE
Definition: BridgeConf.h:40
hrp::JointPath
Definition: JointPath.h:26
ODE_DynamicsSimulator_impl::_updateSensorStates
void _updateSensorStates()
Definition: ODE_DynamicsSimulator_impl.cpp:1289


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Wed Sep 7 2022 02:51:03