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


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