UtDynamicsSimulator/DynamicsSimulator_impl.cpp
Go to the documentation of this file.
1 // -*- mode: c++; indent-tabs-mode: t; tab-width: 4; c-basic-offset: 4; -*-
2 /*
3  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
4  * All rights reserved. This program is made available under the terms of the
5  * Eclipse Public License v1.0 which accompanies this distribution, and is
6  * available at http://www.eclipse.org/legal/epl-v10.html
7  * Contributors:
8  * The University of Tokyo
9  * National Institute of Advanced Industrial Science and Technology (AIST)
10  * General Robotix Inc.
11  */
16 #include <vector>
17 #include <map>
18 
19 #include "DynamicsSimulator_impl.h"
20 #include "ModelLoaderUtil.h"
21 #include "Sensor.h"
22 #include <psim.h>
23 
24 using namespace OpenHRP;
25 using namespace std;
26 
27 
28 //#define INTEGRATOR_DEBUG
29 static const bool enableTimeMeasure = false;
30 
31 //#include <fstream>
32 //static std::ofstream logfile("impl.log");
33 //static std::ofstream logfile;
34 
35 namespace {
36 
37  struct IdLabel {
38  int id;
39  const char* label;
40  };
41  typedef map<int, const char*> IdToLabelMap;
42 
43  IdToLabelMap commandLabelMaps[2];
44 
45  IdToLabelMap commandLabelMap;
46 
47  IdLabel commandLabels[] = {
48 
49  { DynamicsSimulator::POSITION_GIVEN, "Position Given (High Gain Mode)" },
50  { DynamicsSimulator::JOINT_VALUE, "Joint Value" },
51  { DynamicsSimulator::JOINT_VELOCITY, "Joint Velocity"},
52  { DynamicsSimulator::JOINT_ACCELERATION,"Joint Acceleration"},
53  { DynamicsSimulator::JOINT_TORQUE, "Joint Torque"},
54  { DynamicsSimulator::ABS_TRANSFORM, "Absolute Transform"},
55  { DynamicsSimulator::ABS_VELOCITY, "Absolute Velocity"},
56  { DynamicsSimulator::EXTERNAL_FORCE, "External Force"},
57  { -1, "" },
58  };
59 
60  void initializeCommandLabelMaps()
61  {
62  if(commandLabelMap.empty()){
63  int i = 0;
64  while(true){
65  IdLabel& idLabel = commandLabels[i++];
66  if(idLabel.id == -1){
67  break;
68  }
69  commandLabelMap[idLabel.id] = idLabel.label;
70  }
71  }
72  }
73 
74  const char* getLabelOfLinkDataType(DynamicsSimulator::LinkDataType type)
75  {
76  IdToLabelMap::iterator p = commandLabelMap.find(type);
77  return (p != commandLabelMap.end()) ? p->second : "Requesting Unknown Data Type";
78  }
79 };
80 
81 
82 template<typename X, typename X_ptr>
83 X_ptr checkCorbaServer(std::string n, CosNaming::NamingContext_var &cxt)
84 {
85  CosNaming::Name ncName;
86  ncName.length(1);
87  ncName[0].id = CORBA::string_dup(n.c_str());
88  ncName[0].kind = CORBA::string_dup("");
89  X_ptr srv = NULL;
90  try {
91  srv = X::_narrow(cxt->resolve(ncName));
92  } catch(const CosNaming::NamingContext::NotFound &exc) {
93  std::cerr << n << " not found: ";
94  switch(exc.why) {
95  case CosNaming::NamingContext::missing_node:
96  std::cerr << "Missing Node" << std::endl;
97  case CosNaming::NamingContext::not_context:
98  std::cerr << "Not Context" << std::endl;
99  break;
100  case CosNaming::NamingContext::not_object:
101  std::cerr << "Not Object" << std::endl;
102  break;
103  }
104  return (X_ptr)NULL;
105  } catch(CosNaming::NamingContext::CannotProceed &exc) {
106  std::cerr << "Resolve " << n << " CannotProceed" << std::endl;
107  } catch(CosNaming::NamingContext::AlreadyBound &exc) {
108  std::cerr << "Resolve " << n << " InvalidName" << std::endl;
109  }
110  return srv;
111 }
112 
113 
115  : orb_(CORBA::ORB::_duplicate(orb))
116 {
117  world.setCurrentTime(0.0);
118  world.setRungeKuttaMethod();
119 
120  // resolve collisionchecker object
121  CollisionDetectorFactory_var collisionDetectorFactory;
122 
123  CORBA::Object_var nS = orb->resolve_initial_references("NameService");
124  CosNaming::NamingContext_var cxT;
125  cxT = CosNaming::NamingContext::_narrow(nS);
126  collisionDetectorFactory =
127  checkCorbaServer<CollisionDetectorFactory,
128  CollisionDetectorFactory_var>("CollisionDetectorFactory", cxT);
129  if (CORBA::is_nil(collisionDetectorFactory)) {
130  std::cerr << "CollisionDetectorFactory not found" << std::endl;
131  }
132 
133  collisionDetector = collisionDetectorFactory->create();
134 
135  collisions = new CollisionSequence;
136  collidingLinkPairs = new LinkPairSequence;
137  allCharacterPositions = new CharacterPositionSequence;
138  allCharacterSensorStates = new SensorStateSequence;
139 
140  needToUpdatePositions = true;
141  needToUpdateSensorStates = true;
142 }
143 
144 
146 {
147 }
148 
149 
151 {
152  PortableServer::POA_var poa_ = _default_POA();
153  PortableServer::ObjectId_var id = poa_->servant_to_id(this);
154  poa_->deactivate_object(id);
155 }
156 
157 
159  const char *name,
160  BodyInfo_ptr body)
161 {
162 // logfile << "registerCharacter(" << name << ", " << body->name() << ")" << endl;
163  loadBodyFromBodyInfo(&world, name, body);
164  collisionDetector->registerCharacter(name, body);
165 // logfile << "total dof = " << world.Chain()->NumDOF() << endl;
166 }
167 
168 
170  CORBA::Double _timestep,
171  OpenHRP::DynamicsSimulator::IntegrateMethod integrateOpt,
172  OpenHRP::DynamicsSimulator::SensorOption sensorOpt)
173 {
174 // logfile << "init" << endl;
175 
176  world.setTimeStep(_timestep);
177  world.setCurrentTime(0.0);
178 
179  if(integrateOpt == OpenHRP::DynamicsSimulator::EULER){
180  world.setEulerMethod();
181  } else {
182  world.setRungeKuttaMethod();
183  }
184 
185  world.enableSensors((sensorOpt == OpenHRP::DynamicsSimulator::ENABLE_SENSOR));
186 
187  _setupCharacterData();
188 
189  isFirstSimulationLoop = true;
190 
191 #ifdef MEASURE_TIME
192  processTime = 0;
193 #endif
194 }
195 
196 
197 static void joint_traverse_sub(Joint* cur, std::vector<Joint*>& jlist)
198 {
199  if(!cur) return;
200 // logfile << "joint_traverse_sub: " << cur->name << ", n_dof = " << cur->n_dof << endl;
201  jlist.push_back(cur);
202  joint_traverse_sub(cur->brother, jlist);
203  joint_traverse_sub(cur->child, jlist);
204 }
205 
206 static void joint_traverse(Joint* r, std::vector<Joint*>& jlist)
207 {
208  jlist.push_back(r);
209  joint_traverse_sub(r->child, jlist);
210 }
211 
212 
214  const char *charName1,
215  const char *linkName1,
216  const char *charName2,
217  const char *linkName2,
218  const CORBA::Double staticFriction,
219  const CORBA::Double slipFriction,
220  const DblSequence6 & K,
221  const DblSequence6 & C,
222  const double culling_thresh,
223  const double restitution)
224 {
225  const double epsilon = 0.0;
226 // logfile << "registerCollisionCheckPair" << endl;
227 
228  std::string emptyString = "";
229  std::vector<Joint*> joints1;
230  std::vector<Joint*> joints2;
231  pSim* chain = world.Chain();
232 
233  if(emptyString == linkName1)
234  {
235  Joint* r = chain->FindCharacterRoot(charName1);
236  if(r) joint_traverse(r, joints1);
237  }
238  else
239  {
240  Joint* jnt1 = chain->FindJoint(linkName1, charName1);
241  if(jnt1) joints1.push_back(jnt1);
242  }
243  if(emptyString == linkName2)
244  {
245  Joint* r = chain->FindCharacterRoot(charName2);
246  if(r) joint_traverse(r, joints2);
247  }
248  else
249  {
250  Joint* jnt2 = chain->FindJoint(linkName2, charName2);
251  if(jnt2) joints2.push_back(jnt2);
252  }
253 
254  for(size_t i=0; i < joints1.size(); ++i)
255  {
256  Joint* j1 = joints1[i];
257  for(size_t j=0; j < joints2.size(); ++j)
258  {
259  Joint* j2 = joints2[j];
260  if(j1 && j2 && j1 != j2)
261  {
262 // logfile << "pair = " << j1->name << ", " << j2->name << endl;
263  world.addCollisionCheckLinkPair(j1, j2, staticFriction, slipFriction, epsilon);
264  LinkPair_var linkPair = new LinkPair();
265  linkPair->charName1 = CORBA::string_dup(charName1);
266  linkPair->linkName1 = CORBA::string_dup(j1->basename);
267  linkPair->charName2 = CORBA::string_dup(charName2);
268  linkPair->linkName2 = CORBA::string_dup(j2->basename);
269  linkPair->tolerance = 0;
270  collisionDetector->addCollisionPair(linkPair);
271  }
272  }
273  }
274 }
275 
277  const char *charName1,
278  const char *linkName1,
279  const char *charName2,
280  const char *linkName2,
281  const double tolerance)
282 {
283 // logfile << "registerCollisionCheckPair" << endl;
284 
285  std::string emptyString = "";
286  std::vector<Joint*> joints1;
287  std::vector<Joint*> joints2;
288  pSim* chain = world.Chain();
289 
290  if(emptyString == linkName1)
291  {
292  Joint* r = chain->FindCharacterRoot(charName1);
293  if(r) joint_traverse(r, joints1);
294  }
295  else
296  {
297  Joint* jnt1 = chain->FindJoint(linkName1, charName1);
298  if(jnt1) joints1.push_back(jnt1);
299  }
300  if(emptyString == linkName2)
301  {
302  Joint* r = chain->FindCharacterRoot(charName2);
303  if(r) joint_traverse(r, joints2);
304  }
305  else
306  {
307  Joint* jnt2 = chain->FindJoint(linkName2, charName2);
308  if(jnt2) joints2.push_back(jnt2);
309  }
310 
311  for(size_t i=0; i < joints1.size(); ++i)
312  {
313  Joint* j1 = joints1[i];
314  for(size_t j=0; j < joints2.size(); ++j)
315  {
316  Joint* j2 = joints2[j];
317  if(j1 && j2 && j1 != j2)
318  {
319  LinkPair_var linkPair = new LinkPair();
320  linkPair->charName1 = CORBA::string_dup(charName1);
321  linkPair->linkName1 = CORBA::string_dup(j1->basename);
322  linkPair->charName2 = CORBA::string_dup(charName2);
323  linkPair->linkName2 = CORBA::string_dup(j2->basename);
324  linkPair->tolerance = tolerance;
325  collisionDetector->addCollisionPair(linkPair);
326  }
327  }
328  }
329 }
330 
332  (
333  const char* charName1,
334  const char* linkName1,
335  const char* charName2,
336  const char* linkName2,
337  const DblSequence3& link1LocalPos,
338  const DblSequence3& link2LocalPos,
339  const ExtraJointType jointType,
340  const DblSequence3& jointAxis,
341  const char* extraJointName)
342 {
343 }
344 
347  const char * characterName,
348  const char * extraJointName,
349  DblSequence6_out contactForce)
350 {
351 }
352 
353 
354 static void vec3_to_seq(const fVec3& vec, DblSequence_out& seq, size_t offset = 0)
355 {
356  seq[CORBA::ULong(offset++)] = vec(0);
357  seq[CORBA::ULong(offset++)] = vec(1);
358  seq[CORBA::ULong(offset)] = vec(2);
359 }
360 
361 static void seq_to_vec3(const DblSequence3& seq, fVec3& vec)
362 {
363  vec(0) = seq[0];
364  vec(1) = seq[1];
365  vec(2) = seq[2];
366 }
367 
369  const char *characterName,
370  const char *sensorName,
371  DblSequence_out sensorOutput)
372 {
373  sensorOutput = new DblSequence;
374  Sensor* sensor = world.findSensor(sensorName, characterName);
375 
376  if(sensor)
377  {
378  switch(sensor->type)
379  {
380  case Sensor::FORCE:
381  {
382  ForceSensor* forceSensor = static_cast<ForceSensor*>(sensor);
383  sensorOutput->length(6);
384 #ifndef __WIN32__
385  vec3_to_seq(forceSensor->f, sensorOutput);
386  vec3_to_seq(forceSensor->tau, sensorOutput, 3);
387 #else
388  sensorOutput[CORBA::ULong(0)] = forceSensor->f(0);
389  sensorOutput[CORBA::ULong(1)] = forceSensor->f(1);
390  sensorOutput[CORBA::ULong(2)] = forceSensor->f(2);
391  sensorOutput[CORBA::ULong(3)] = forceSensor->tau(0);
392  sensorOutput[CORBA::ULong(4)] = forceSensor->tau(1);
393  sensorOutput[CORBA::ULong(5)] = forceSensor->tau(2);
394 #endif
395  }
396  break;
397 
398  case Sensor::RATE_GYRO:
399  {
400  RateGyroSensor* gyro = static_cast<RateGyroSensor*>(sensor);
401  sensorOutput->length(3);
402 #ifndef __WIN32__
403  vec3_to_seq(gyro->w, sensorOutput);
404 #else
405  sensorOutput[CORBA::ULong(0)] = gyro->w(0);
406  sensorOutput[CORBA::ULong(1)] = gyro->w(1);
407  sensorOutput[CORBA::ULong(2)] = gyro->w(2);
408 #endif
409  }
410  break;
411 
412  case Sensor::ACCELERATION:
413  {
414  AccelSensor* accelSensor = static_cast<AccelSensor*>(sensor);
415  sensorOutput->length(3);
416 #ifndef __WIN32__
417  vec3_to_seq(accelSensor->dv, sensorOutput);
418 #else
419  sensorOutput[CORBA::ULong(0)] = accelSensor->dv(0);
420  sensorOutput[CORBA::ULong(1)] = accelSensor->dv(1);
421  sensorOutput[CORBA::ULong(2)] = accelSensor->dv(2);
422 #endif
423  }
424  break;
425 
426  default:
427  break;
428  }
429  }
430 }
431 
433 {
434  world.initialize();
435 
436  _updateCharacterPositions();
437  collisionDetector->queryContactDeterminationForDefinedPairs(allCharacterPositions.in(), collisions.out());
438 
439  if(enableTimeMeasure){
440  timeMeasureFinished = false;
441  timeMeasure1.begin();
442  }
443 }
444 
446 {
447  if(enableTimeMeasure) timeMeasure2.begin();
448  world.calcNextState(collisions);
449  if(enableTimeMeasure) timeMeasure2.end();
450 
451  if(enableTimeMeasure){
452  cout << " " << timeMeasure2.time() << "\n";
453  }
454 
455  needToUpdateSensorStates = true;
456 
457  _updateCharacterPositions();
458 
459  collisionDetector->queryContactDeterminationForDefinedPairs(allCharacterPositions.in(), collisions.out());
460 
462  {
463  if(world.currentTime() > 10.0 && !timeMeasureFinished)
464  {
465  timeMeasureFinished = true;
466  timeMeasure1.end();
467  cout << "Total elapsed time = " << timeMeasure1.totalTime() << "\n";
468  cout << "Internal elapsed time = " << timeMeasure2.totalTime();
469  cout << ", the avarage = " << timeMeasure2.avarageTime() << endl;;
470  //cout << "Collision check time = " << timeMeasure3.totalTime() << endl;
471  }
472  }
473 }
474 
475 
477  const char* characterName,
478  const char* linkName,
479  OpenHRP::DynamicsSimulator::LinkDataType type,
480  const DblSequence& wdata)
481 {
482 // logfile << "setCharacterLinkData(" << characterName << ", " << linkName << ")" << endl;
483  Joint* joint = world.Chain()->FindJoint(linkName, characterName);
484  if(!joint) return;
485 
486  switch(type) {
487 
488  case OpenHRP::DynamicsSimulator::POSITION_GIVEN:
489  joint->t_given = !(wdata[0] > 0.0);
490  break;
491 
493  joint->SetJointValue(wdata[0]);
494  break;
495 
497  joint->SetJointVel(wdata[0]);
498  break;
499 
501  joint->SetJointAcc(wdata[0]);
502  break;
503 
505  joint->SetJointForce(wdata[0]);
506  break;
507 
509  {
510  fVec3 abs_pos(wdata[0], wdata[1], wdata[2]);
511  fMat33 abs_att(wdata[3], wdata[4], wdata[5], wdata[6], wdata[7], wdata[8], wdata[9], wdata[10], wdata[11]); // wdata is in row major order
512  joint->SetJointValue(abs_pos, abs_att);
513  break;
514  }
515 
517  {
518  fVec3 abs_lin_vel(wdata[0], wdata[1], wdata[2]);
519  fVec3 abs_ang_vel(wdata[3], wdata[4], wdata[5]);
520  joint->rel_lin_vel.mul(abs_lin_vel, joint->abs_att);
521  joint->rel_ang_vel.mul(abs_ang_vel, joint->abs_att);
522  break;
523  }
524 
526  {
527  // original: local frame?, around world origin
528  // new: local frame, around joint origin
529  joint->ext_force(0) = wdata[0];
530  joint->ext_force(1) = wdata[1];
531  joint->ext_force(2) = wdata[2];
532  fVec3 n0(wdata[3], wdata[4], wdata[5]);
533  fVec3 np;
534  np.cross(joint->abs_pos, joint->ext_force);
535  joint->ext_moment.sub(n0, np);
536  break;
537  }
538 
539  default:
540  return;
541  }
542 
543  needToUpdatePositions = true;
544  needToUpdateSensorStates = true;
545 }
546 
547 
549  const char * characterName,
550  const char * linkName,
551  OpenHRP::DynamicsSimulator::LinkDataType type,
552  DblSequence_out out_rdata)
553 {
554 // logfile << "getCharacterLinkData" << endl;
555  Joint* joint = world.Chain()->FindJoint(linkName, characterName);
556  assert(joint);
557 
558  DblSequence_var rdata = new DblSequence;
559 
560  switch(type) {
561 
563  rdata->length(1);
564  rdata[0] = joint->q;
565  break;
566 
568  rdata->length(1);
569  rdata[0] = joint->qd;
570  break;
571 
573  rdata->length(1);
574  rdata[0] = joint->qdd;
575  break;
576 
578  rdata->length(1);
579  rdata[0] = joint->tau;
580  break;
581 
583  {
584  fEulerPara ep;
585  ep.set(joint->abs_att);
586  rdata->length(7);
587  rdata[0] = joint->abs_pos(0);
588  rdata[1] = joint->abs_pos(1);
589  rdata[2] = joint->abs_pos(2);
590  rdata[3] = ep.Ang();
591  rdata[4] = ep.Axis()(0);
592  rdata[5] = ep.Axis()(1);
593  rdata[6] = ep.Axis()(2);
594  break;
595  }
596 
598  {
599  fVec3 v0, w0;
600  v0.mul(joint->abs_att, joint->loc_lin_vel);
601  w0.mul(joint->abs_att, joint->loc_ang_vel);
602  rdata->length(6);
603  rdata[0] = v0(0);
604  rdata[1] = v0(1);
605  rdata[2] = v0(2);
606  rdata[3] = w0(0);
607  rdata[4] = w0(1);
608  rdata[5] = w0(2);
609  break;
610  }
611 
613  {
614  // original: local frame, around joint origin
615  // new: local frame?, around world origin
616  rdata->length(6);
617  rdata[0] = joint->ext_force(0);
618  rdata[1] = joint->ext_force(1);
619  rdata[2] = joint->ext_force(2);
620  fVec3 np, n0;
621  np.cross(joint->abs_pos, joint->ext_force);
622  n0.add(joint->ext_moment, np);
623  rdata[3] = n0(0);
624  rdata[4] = n0(1);
625  rdata[5] = n0(2);
626  break;
627  }
628 
629  default:
630  break;
631  }
632 
633  out_rdata = rdata._retn();
634 }
635 
636 
638  const char * characterName,
639  OpenHRP::DynamicsSimulator::LinkDataType type,
640  DblSequence_out rdata)
641 {
642 // logfile << "getCharacterAllLinkData" << endl;
643  rdata = new DblSequence();
644 
645  world.getAllCharacterData(characterName, type, rdata);
646 }
647 
648 
650  const char * characterName,
651  OpenHRP::DynamicsSimulator::LinkDataType type,
652  const DblSequence & wdata)
653 {
654 // logfile << "setCharacterAllLinkData: " << getLabelOfLinkDataType(type) << endl;
655  world.setAllCharacterData(characterName, type, wdata);
656 }
657 
658 
660  const DblSequence3& wdata)
661 {
662  assert(wdata.length() == 3);
663 
664 // logfile << "setGVector" << endl;
665  fVec3 g;
666  seq_to_vec3(wdata, g);
667  world.setGravityAcceleration(g);
668 
669 }
670 
671 
673  DblSequence3_out wdata)
674 {
675  wdata->length(3);
676  fVec3 g = world.getGravityAcceleration();
677  (*wdata)[0] = g(0);
678  (*wdata)[1] = g(1);
679  (*wdata)[2] = g(2);
680 }
681 
682 
684  const char * characterName,
685  OpenHRP::DynamicsSimulator::JointDriveMode jointMode)
686 {
687 // logfile << "setCharacterAllJointModes" << endl;
688  bool isTorqueMode = (jointMode != OpenHRP::DynamicsSimulator::HIGH_GAIN_MODE);
689 
690  world.Chain()->SetCharacterTorqueGiven(characterName, isTorqueMode);
691 
692 }
693 
694 
696  const char * characterName,
697  const char * fromLink, const char * toLink,
698  const LinkPosition& target)
699 {
700  bool solved = false;
701 
702  // TODO
703 
704  return solved;
705 }
706 
707 
709  const char * characterName)
710 {
711 // logfile << "calcCharacterForwardKinematics" << endl;
712  world.Chain()->CalcPosition();
713 
714  needToUpdatePositions = true;
715  needToUpdateSensorStates = true;
716 }
717 
718 
720 {
721 // logfile << "calcWorldForwardKinematics" << endl;
722  world.Chain()->CalcPosition();
723 
724  needToUpdatePositions = true;
725  needToUpdateSensorStates = true;
726 }
727 
728 
729 void DynamicsSimulator_impl::getWorldState(WorldState_out wstate)
730 {
731 // logfile << "getWorldState" << endl;
732  if (needToUpdatePositions) _updateCharacterPositions();
733 
734  wstate = new WorldState;
735 
736  wstate->time = world.currentTime();
737  wstate->characterPositions = allCharacterPositions;
738  wstate->collisions = collisions;
739 }
740 
741 
742 void DynamicsSimulator_impl::getCharacterSensorState(const char* characterName, SensorState_out sstate)
743 {
744 // logfile << "getCharacterSensorState(" << characterName << ")" << endl;
745  int index = -1;
746  int n_char = world.numCharacter();
747  for(int i=0; i<n_char; i++)
748  {
749  Joint* j = world.rootJoint(i);
750  if(!strcmp(j->CharName(), characterName))
751  {
752  index = i;
753  break;
754  }
755  }
756  if(index >= 0)
757  {
758  if(needToUpdateSensorStates)
759  {
760  _updateSensorStates();
761  }
762  sstate = new SensorState(allCharacterSensorStates[index]);
763  }
764  else
765  {
766  sstate = new SensorState;
767  }
768 }
769 
770 
772 {
773  int nchar = world.numCharacter();
774 
775  allCharacterPositions->length(nchar);
776  allCharacterSensorStates->length(nchar);
777 
778 // logfile << "_setupCharacterData" << endl;
779 // logfile << "total DOF = " << chain->NumDOF() << endl;
780  for(int i=0; i<nchar; i++)
781  {
782  Joint* j = world.rootJoint(i);
783 // logfile << "root = " << j->name << endl;
784  CharacterPosition& characterPosition = allCharacterPositions[i];
785  characterPosition.characterName = CORBA::string_dup(j->CharName());
786  int n_links = world.numLinks(i);
787 // logfile << "n_links = " << n_links << endl;
788  LinkPositionSequence& linkPositions = characterPosition.linkPositions;
789  linkPositions.length(n_links);
790  SensorState& sensorState = allCharacterSensorStates[i];
791  int n_joints = world.numJoints(i);
792 // logfile << "n_joints = " << n_joints << endl;
793  sensorState.q.length(n_joints);
794  sensorState.dq.length(n_joints);
795  sensorState.u.length(n_joints);
796  sensorState.force.length(world.numSensors(Sensor::FORCE, j->CharName()));
797  sensorState.rateGyro.length(world.numSensors(Sensor::RATE_GYRO, j->CharName()));
798  sensorState.accel.length(world.numSensors(Sensor::ACCELERATION, j->CharName()));
799  }
800 }
801 
802 
804 {
805  world.getAllCharacterPositions(allCharacterPositions.inout());
806  needToUpdatePositions = false;
807 
808 #if 0
809  int n_char = allCharacterPositions->length();
810  for(int i=0; i<n_char; i++)
811  {
812  CharacterPosition& cpos = allCharacterPositions[i];
813  int n_link = cpos.linkPositions.length();
814  for(int j=0; j<n_link; j++)
815  {
816  LinkPosition& lpos = cpos.linkPositions[j];
817  }
818  }
819 #endif
820 }
821 
823 {
824  world.getAllSensorStates(allCharacterSensorStates);
825  needToUpdateSensorStates = false;
826 }
827 
828 
834  const char *characterName,
835  LinkPairSequence_out pairs)
836 {
837  std::vector<unsigned int> locations;
838 
839  for(unsigned int i=0; i < collisions->length(); ++i) {
840 
841  if( (strcmp(characterName, collisions[i].pair.charName1) == 0) ||
842  (strcmp(characterName, collisions[i].pair.charName2) == 0))
843  locations.push_back(i);
844  }
845 
846  pairs->length(locations.size());
847 
848  unsigned long n=0;
849  for(std::vector<unsigned int>::iterator iter = locations.begin();
850  iter != locations.end(); ++iter) {
851 
852  strcpy(pairs[n].charName1, collisions[*iter].pair.charName1);
853  strcpy(pairs[n].charName2, collisions[*iter].pair.charName2);
854  strcpy(pairs[n].linkName1, collisions[*iter].pair.linkName1);
855  strcpy(pairs[n].linkName2, collisions[*iter].pair.linkName2);
856  }
857 
858  return true;
859 }
860 
861 
863  const char *characterName,
864  const char *baseLink,
865  const char *targetLink,
866  DblSequence_out jacobian)
867 {
868  fMat J;
869  world.calcCharacterJacobian(characterName, baseLink, targetLink, J);
870 
871  int height = J.row();
872  int width = J.col();
873 
874  jacobian->length(height * width);
875  int i = 0;
876  for(int r=0; r < height; ++r){
877  for(int c=0; c < width; ++c){
878  (*jacobian)[i++] = J(r, c);
879  }
880  }
881 }
882 
883 bool DynamicsSimulator_impl::checkCollision(bool checkAll)
884 {
885  calcWorldForwardKinematics();
886  _updateCharacterPositions();
887  if (checkAll){
888  return collisionDetector->queryContactDeterminationForDefinedPairs(allCharacterPositions.in(), collisions.out());
889  }else{
890  return collisionDetector->queryIntersectionForDefinedPairs(checkAll, allCharacterPositions.in(), collidingLinkPairs.out());
891  }
892 }
893 
894 DistanceSequence *DynamicsSimulator_impl::checkDistance()
895 {
896  calcWorldForwardKinematics();
897  _updateCharacterPositions();
898  DistanceSequence_var distances = new DistanceSequence;
899  collisionDetector->queryDistanceForDefinedPairs(allCharacterPositions.in(), distances);
900  return distances._retn();
901 }
902 
903 LinkPairSequence *DynamicsSimulator_impl::checkIntersection(CORBA::Boolean checkAll)
904 {
905  calcWorldForwardKinematics();
906  _updateCharacterPositions();
907  LinkPairSequence_var pairs = new LinkPairSequence;
908  collisionDetector->queryIntersectionForDefinedPairs(checkAll, allCharacterPositions.in(), pairs);
909  return pairs._retn();
910 }
911 
917  orb_(CORBA::ORB::_duplicate(orb))
918 {
919  initializeCommandLabelMaps();
920 }
921 
922 
924 {
925  PortableServer::POA_var poa = _default_POA();
926  PortableServer::ObjectId_var id = poa -> servant_to_id(this);
927  poa -> deactivate_object(id);
928 }
929 
930 
931 DynamicsSimulator_ptr DynamicsSimulatorFactory_impl::create()
932 {
933  DynamicsSimulator_impl* integratorImpl = new DynamicsSimulator_impl(orb_);
934 
935  PortableServer::ServantBase_var integratorrServant = integratorImpl;
936  PortableServer::POA_var poa_ = _default_POA();
937  PortableServer::ObjectId_var id =
938  poa_ -> activate_object(integratorImpl);
939 
940  return integratorImpl -> _this();
941 }
942 
943 
945 {
946  orb_->shutdown(false);
947 }
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)
NotFound
Definition: hrpPrep.py:129
fVec3 loc_lin_vel
linear velocity in local frame
Definition: chain.h:743
void add(const fVec3 &vec1, const fVec3 &vec2)
Definition: fMatrix3.cpp:888
3x3 matrix class.
Definition: fMatrix3.h:29
int c
Definition: autoplay.py:16
X_ptr checkCorbaServer(std::string n, CosNaming::NamingContext_var &cxt)
Joint * child
pointer to the child joint
Definition: chain.h:685
png_infop png_charp png_int_32 png_int_32 int * type
Definition: png.h:2332
virtual void getCharacterSensorState(const char *characterName, SensorState_out sstate)
int t_given
torque or motion controlled
Definition: chain.h:709
void cross(const fVec3 &vec1, const fVec3 &vec2)
Cross product.
Definition: fMatrix3.cpp:944
Forward dynamics computation based on Assembly-Disassembly Algorithm.
friend class Chain
Definition: chain.h:540
png_infop png_charpp name
Definition: png.h:2382
fVec3 loc_ang_vel
angular velocity in local frame
Definition: chain.h:744
int col() const
Returns the number of columns.
Definition: fMatrix.h:154
virtual void getGVector(DblSequence3_out wdata)
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)
HRPMODEL_API bool loadBodyFromBodyInfo(BodyPtr body, OpenHRP::BodyInfo_ptr bodyInfo, bool loadGeometryForCollisionDetection=false, Link *(*f)()=NULL)
friend fVec3 & Axis(fEulerPara &ep)
Access the vector part (a*sin(theta/2))
Definition: fEulerPara.h:59
png_uint_32 i
Definition: png.h:2735
static const bool enableTimeMeasure
char * basename
joint base name (without the character name)
Definition: chain.h:692
virtual void getCharacterLinkData(const char *characterName, const char *link, OpenHRP::DynamicsSimulator::LinkDataType type, DblSequence_out rdata)
void set(const fVec3 &v, double s)
Set the elements.
Definition: fEulerPara.h:75
int SetJointAcc(double _qdd)
Definition: joint.cpp:580
double q
joint value (for 1DOF joints)
Definition: chain.h:724
double tau
joint force/torque (for 1DOF joints)
Definition: chain.h:733
png_infop png_uint_32 * width
Definition: png.h:2309
Euler parameter class.
Definition: fEulerPara.h:28
virtual void getExtraJointConstraintForce(const char *characterName, const char *extraJointName, DblSequence6_out contactForce)
virtual void init(CORBA::Double timeStep, OpenHRP::DynamicsSimulator::IntegrateMethod integrateOpt, OpenHRP::DynamicsSimulator::SensorOption sensorOpt)
virtual void calcCharacterForwardKinematics(const char *characterName)
int SetJointForce(double _tau)
Definition: joint.cpp:783
virtual CORBA::Boolean calcCharacterInverseKinematics(const char *characterName, const char *baseLink, const char *targetLink, const LinkPosition &target)
virtual void setGVector(const DblSequence3 &wdata)
png_infop png_uint_32 png_uint_32 * height
Definition: png.h:2309
double qd
joint velocity (for 1DOF joints)
Definition: chain.h:725
list index
fVec3 rel_ang_vel
Definition: chain.h:729
fVec3 ext_force
external force
Definition: chain.h:736
fMat33 abs_att
absolute orientation
Definition: chain.h:742
friend double & Ang(fEulerPara &ep)
Access the scalar part (cos(theta/2))
Definition: fEulerPara.h:47
virtual LinkPairSequence * checkIntersection(CORBA::Boolean checkAll)
Joint * brother
pointer to the brother joint
Definition: chain.h:684
int row() const
Returns the number of rows.
Definition: fMatrix.h:158
virtual void getWorldState(WorldState_out wstate)
id
virtual void setCharacterLinkData(const char *characterName, const char *link, OpenHRP::DynamicsSimulator::LinkDataType type, const DblSequence &data)
fVec3 rel_lin_vel
Definition: chain.h:728
char * CharName() const
Returns the character name.
Definition: chain.h:648
void sub(const fVec3 &vec1, const fVec3 &vec2)
Definition: fMatrix3.cpp:902
int SetJointValue(double _q)
Definition: joint.cpp:538
virtual void getCharacterAllLinkData(const char *characterName, OpenHRP::DynamicsSimulator::LinkDataType type, DblSequence_out wdata)
double qdd
joint acceleration (for 1DOF joints)
Definition: chain.h:726
Joint * FindJoint(const char *jname, const char *charname=0)
Find a joint from name.
Definition: chain.cpp:391
3-element vector class.
Definition: fMatrix3.h:206
virtual void registerIntersectionCheckPair(const char *char1, const char *name1, const char *char2, const char *name2, const double tolerance)
static void joint_traverse(Joint *r, std::vector< Joint *> &jlist)
virtual void setCharacterAllJointModes(const char *characterName, OpenHRP::DynamicsSimulator::JointDriveMode jointMode)
fVec3 ext_moment
external moment around the local frame
Definition: chain.h:737
static void vec3_to_seq(const fVec3 &vec, DblSequence_out &seq, size_t offset=0)
fVec3 abs_pos
absolute position
Definition: chain.h:741
void mul(const fVec3 &vec, double d)
Definition: fMatrix3.cpp:916
Main class for forward dynamics computation.
Definition: psim.h:446
virtual void setCharacterAllLinkData(const char *characterName, OpenHRP::DynamicsSimulator::LinkDataType type, const DblSequence &wdata)
The class for representing a joint.
Definition: chain.h:538
static void seq_to_vec3(const DblSequence3 &seq, fVec3 &vec)
virtual CORBA::Boolean getCharacterCollidingPairs(const char *characterName, LinkPairSequence_out pairs)
Chain()
Definition: chain.cpp:46
int SetJointVel(double _qd)
Definition: joint.cpp:562
virtual void registerCharacter(const char *name, BodyInfo_ptr binfo)
virtual void calcCharacterJacobian(const char *characterName, const char *baseLink, const char *targetLink, DblSequence_out jacobian)
static void joint_traverse_sub(Joint *cur, std::vector< Joint *> &jlist)
Matrix of generic size. The elements are stored in a one-dimensional array in row-major order...
Definition: fMatrix.h:46
virtual void getCharacterSensorValues(const char *characterName, const char *sensorName, DblSequence_out values)
Joint * FindCharacterRoot(const char *charname)
Find the root joint of the character with name charname.
Definition: chain.cpp:429


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