Body.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  */
9 
15 #include "Body.h"
16 #include "Link.h"
17 #include "JointPath.h"
18 #include "Sensor.h"
19 #include "Light.h"
22 #include <map>
23 #include <cstdlib>
24 
25 using namespace hrp;
26 using namespace std;
27 
28 static const bool PUT_DEBUG_MESSAGE = true;
29 
31 
32 #ifndef uint
33 typedef unsigned int uint;
34 #endif
35 
36 namespace hrp {
37 
39  {
41  int ikTypeId;
43  virtual void onJointPathUpdated();
44  public:
45  CustomizedJointPath(Body* body, Link* baseLink, Link* targetLink);
46  virtual ~CustomizedJointPath();
47  virtual bool calcInverseKinematics(const Vector3& end_p, const Matrix33& end_R);
48  virtual bool hasAnalyticalIK();
49  };
50 }
51 
52 
54 {
55  if(customizerHandle){
56  customizerInterface->destroy(customizerHandle);
57  }
58 
59  delete rootLink_;
60 
61  // delete sensors
62  for(unsigned int sensorType =0; sensorType < numSensorTypes(); ++sensorType){
63  int n = numSensors(sensorType);
64  for(int i=0; i < n; ++i){
65  Sensor* s = sensor(sensorType, i);
66  if(s){
67  Sensor::destroy(s);
68  }
69  }
70  }
71 }
72 
73 
75 {
76  rootLink_ = 0;
77 
78  customizerHandle = 0;
79  customizerInterface = 0;
80  bodyHandleEntity.body = this;
81  bodyHandle = &bodyHandleEntity;
82 }
83 
84 
86  : allSensors(Sensor::NUM_SENSOR_TYPES)
87 {
88  initialize();
89 
90  rootLink_ = new Link;
91  rootLink_->body = this;
92 
93  defaultRootPosition.setZero();
94  defaultRootAttitude.setIdentity();
95 }
96 
97 
98 Body::Body(const Body& org)
99  : name_(org.name_),
100  modelName_(org.modelName_),
101  allSensors(Sensor::NUM_SENSOR_TYPES)
102 {
103  initialize();
104 
105  setRootLink(new Link(*org.rootLink()));
106 
109 
110  // copy sensors
111  std::map<Link*, int> linkToIndexMap;
112 
113  for(unsigned int i=0; i < org.linkTraverse_.numLinks(); ++i){
114  Link* lnk = org.linkTraverse_[i];
115  linkToIndexMap[lnk] = i;
116  }
117 
118  int n = org.numSensorTypes();
119  for(int sensorType = 0; sensorType < n; ++sensorType){
120  for(unsigned int i=0; i < org.numSensors(sensorType); ++i){
121  Sensor* orgSensor = org.sensor(sensorType, i);
122  if (orgSensor){
123  int linkIndex = linkToIndexMap[orgSensor->link];
124  Link* newLink = linkTraverse_[linkIndex];
125  Sensor* cloneSensor = createSensor(newLink, sensorType, orgSensor->id, orgSensor->name);
126  *cloneSensor = *orgSensor;
127  }
128  }
129  }
130 
131  // do deep copy of extraJoint
132  for(size_t i=0; i < org.extraJoints.size(); ++i){
133  const ExtraJoint& orgExtraJoint = org.extraJoints[i];
134  ExtraJoint extraJoint(orgExtraJoint);
135  for(int j=0; j < 2; ++j){
136  extraJoint.link[j] = link(orgExtraJoint.link[j]->index);
137  }
138  }
139 
140  if(org.customizerInterface){
142  }
143 }
144 
145 
147 {
148  if(rootLink_){
149  delete rootLink_;
150  }
151  rootLink_ = link;
152 
153  updateLinkTree();
154 }
155 
156 
158 {
161 }
162 
163 
165 {
166  out_p = defaultRootPosition;
167  out_R = defaultRootAttitude;
168 }
169 
170 
172 {
173  Link* empty = new Link;
174  empty->body = this;
175  empty->jointId = jointId;
176  empty->p.setZero();
177  empty->R.setIdentity();
178  empty->v.setZero();
179  empty->w.setZero();
180  empty->dv.setZero();
181  empty->dw.setZero();
182  empty->q = 0.0;
183  empty->dq = 0.0;
184  empty->ddq = 0.0;
185  empty->u = 0.0;
186  empty->a.setZero();
187  empty->d.setZero();
188  empty->b.setZero();
189  empty->Rs.setIdentity();
190  empty->m = 0.0;
191  empty->I.setZero();
192  empty->c.setZero();
193  empty->wc.setZero();
194  empty->vo.setZero();
195  empty->dvo.setZero();
196  empty->fext.setZero();
197  empty->tauext.setZero();
198  empty->Jm2 = 0.0;
199  empty->ulimit = 0.0;
200  empty->llimit = 0.0;
201  empty->uvlimit = 0.0;
202  empty->lvlimit = 0.0;
203  empty->defaultJointValue = 0.0;
204  empty->Ir = 0.0;
205  empty->gearRatio = 1.0;
206 
207  return empty;
208 }
209 
210 
212 {
213  nameToLinkMap.clear();
215 
216  int n = linkTraverse_.numLinks();
217  jointIdToLinkArray.clear();
218  jointIdToLinkArray.resize(n, 0);
219  int maxJointID = -1;
220 
221  for(int i=0; i < n; ++i){
222  Link* link = linkTraverse_[i];
223  link->body = this;
224  link->index = i;
225  nameToLinkMap[link->name] = link;
226 
227  int id = link->jointId;
228  if(id >= 0){
229  int size = jointIdToLinkArray.size();
230  if(id >= size){
231  jointIdToLinkArray.resize(id + 1, 0);
232  }
233  jointIdToLinkArray[id] = link;
234  if(id > maxJointID){
235  maxJointID = id;
236  }
237  }
238  }
239 
240  jointIdToLinkArray.resize(maxJointID + 1);
241 
242  for(size_t i=0; i < jointIdToLinkArray.size(); ++i){
243  if(!jointIdToLinkArray[i]){
245  std::cerr << "Warning: Model " << modelName_ <<
246  " has empty joint ID in the valid IDs." << std::endl;
247  }
248  }
249 
250  calcTotalMass();
251 
253 }
254 
255 
260 Link* Body::link(const std::string& name) const
261 {
262  NameToLinkMap::const_iterator p = nameToLinkMap.find(name);
263  return (p != nameToLinkMap.end()) ? p->second : 0;
264 }
265 
266 
268 {
271 
272  rootLink_->v.setZero();
273  rootLink_->dv.setZero();
274  rootLink_->w.setZero();
275  rootLink_->dw.setZero();
276  rootLink_->vo.setZero();
277  rootLink_->dvo.setZero();
278  rootLink_->constraintForces.clear();
279 
280  int n = linkTraverse_.numLinks();
281  for(int i=0; i < n; ++i){
282  Link* link = linkTraverse_[i];
283  link->u = 0.0;
284  link->q = 0.0;
285  link->dq = 0.0;
286  link->ddq = 0.0;
287  link->constraintForces.clear();
288  }
289 
290  calcForwardKinematics(true, true);
291 
293 }
294 
295 
296 
298 {
299  totalMass_ = 0.0;
300 
301  int n = linkTraverse_.numLinks();
302  for(int i=0; i < n; ++i){
303  totalMass_ += linkTraverse_[i]->m;
304  }
305 
306  return totalMass_;
307 }
308 
309 
311 {
312  totalMass_ = 0.0;
313 
314  Vector3 mc(Vector3::Zero());
315 
316  int n = linkTraverse_.numLinks();
317  for(int i=0; i < n; i++){
318  Link* link = linkTraverse_[i];
319  link->wc = link->p + link->R * link->c;
320  mc += link->m * link->wc;
321  totalMass_ += link->m;
322  }
323 
324  return mc / totalMass_;
325 }
326 
327 
339 {
340  // buffers for the unit vector method
341  dmatrix b1;
342  dvector ddqorg;
343  dvector uorg;
344  Vector3 dvoorg;
345  Vector3 dworg;
346  Vector3 root_w_x_v;
347  Vector3 g(0, 0, 9.8);
348 
349  uint nJ = numJoints();
350  int totaldof = nJ;
351  if( !isStaticModel_ ) totaldof += 6;
352 
353  out_M.resize(totaldof,totaldof);
354  b1.resize(totaldof, 1);
355 
356  // preserve and clear the joint accelerations
357  ddqorg.resize(nJ);
358  uorg.resize(nJ);
359  for(uint i = 0; i < nJ; ++i){
360  Link* ptr = joint(i);
361  ddqorg[i] = ptr->ddq;
362  uorg [i] = ptr->u;
363  ptr->ddq = 0.0;
364  }
365 
366  // preserve and clear the root link acceleration
367  dvoorg = rootLink_->dvo;
368  dworg = rootLink_->dw;
369  root_w_x_v = rootLink_->w.cross(rootLink_->vo + rootLink_->w.cross(rootLink_->p));
370  rootLink_->dvo = g - root_w_x_v; // dv = g, dw = 0
371  rootLink_->dw.setZero();
372 
373  setColumnOfMassMatrix(b1, 0);
374 
375  if( !isStaticModel_ ){
376  for(int i=0; i < 3; ++i){
377  rootLink_->dvo[i] += 1.0;
378  setColumnOfMassMatrix(out_M, i);
379  rootLink_->dvo[i] -= 1.0;
380  }
381  for(int i=0; i < 3; ++i){
382  rootLink_->dw[i] = 1.0;
383  Vector3 dw_x_p = rootLink_->dw.cross(rootLink_->p); // spatial acceleration caused by ang. acc.
384  rootLink_->dvo -= dw_x_p;
385  setColumnOfMassMatrix(out_M, i + 3);
386  rootLink_->dvo += dw_x_p;
387  rootLink_->dw[i] = 0.0;
388  }
389  }
390 
391  for(uint i = 0; i < nJ; ++i){
392  Link* ptr = joint(i);
393  ptr->ddq = 1.0;
394  int j = i + 6;
395  setColumnOfMassMatrix(out_M, j);
396  out_M(j, j) += ptr->Jm2; // motor inertia
397  ptr->ddq = 0.0;
398  }
399 
400  // subtract the constant term
401  for(int i = 0; i < out_M.cols(); ++i){
402  out_M.col(i) -= b1;
403  }
404 
405  // recover state
406  for(uint i = 0; i < nJ; ++i){
407  Link* ptr = joint(i);
408  ptr->ddq = ddqorg[i];
409  ptr->u = uorg [i];
410  }
411  rootLink_->dvo = dvoorg;
412  rootLink_->dw = dworg;
413 }
414 
415 
416 void Body::setColumnOfMassMatrix(dmatrix& out_M, int column)
417 {
418  Vector3 f;
419  Vector3 tau;
420 
422 
423  if( !isStaticModel_ ){
424  tau -= rootLink_->p.cross(f);
425  out_M.block<6,1>(0, column) << f, tau;
426  }
427 
428  int n = numJoints();
429  for(int i = 0; i < n; ++i){
430  Link* ptr = joint(i);
431  out_M(i + 6, column) = ptr->u;
432  }
433 }
434 
435 
436 /*
437  * see Kajita et al. Humanoid Robot Ohm-sha, p.210
438  */
440 {
441  Link* parent = ptr->parent;
442  if(parent){
443  Vector3 dsv,dsw,sv,sw;
444 
445  if(ptr->jointType != Link::FIXED_JOINT){
446  sw.noalias() = parent->R * ptr->a;
447  sv = ptr->p.cross(sw);
448  }else{
449  sw.setZero();
450  sv.setZero();
451  }
452 
453  ptr->vo = parent->vo + sv * ptr->dq; // Added by Rafa
454  ptr->w = parent->w + sw * ptr->dq; // Added by Rafa
455 
456  dsv = parent->w.cross(sv) + parent->vo.cross(sw);
457  dsw = parent->w.cross(sw);
458 
459  ptr->dw = parent->dw + dsw * ptr->dq + sw * ptr->ddq;
460  ptr->dvo = parent->dvo + dsv * ptr->dq + sv * ptr->ddq;
461 
462  ptr->sw = sw;
463  ptr->sv = sv;
464  }
465 
466  Vector3 c, P, L;
467  Matrix33 I, c_hat;
468 
469  c = ptr->R * ptr->c + ptr->p;
470  I.noalias() = ptr->R * ptr->I * ptr->R.transpose();
471  c_hat = hat(c);
472  I.noalias() += ptr->m * c_hat * c_hat.transpose();
473  P.noalias() = ptr->m * (ptr->vo + ptr->w.cross(c));
474  L = ptr->m * c.cross(ptr->vo) + I * ptr->w;
475 
476  out_f = ptr->m * (ptr->dvo + ptr->dw.cross(c)) + ptr->w.cross(P);
477  out_tau = ptr->m * c.cross(ptr->dvo) + I * ptr->dw + ptr->vo.cross(P) + ptr->w.cross(L);
478 
479  if(ptr->child){
480  Vector3 f_c;
481  Vector3 tau_c;
482  calcInverseDynamics(ptr->child, f_c, tau_c);
483  out_f += f_c;
484  out_tau += tau_c;
485  }
486 
487  ptr->u = ptr->sv.dot(out_f) + ptr->sw.dot(out_tau);
488 
489  if(ptr->sibling){
490  Vector3 f_s;
491  Vector3 tau_s;
492  calcInverseDynamics(ptr->sibling, f_s, tau_s);
493  out_f += f_s;
494  out_tau += tau_s;
495  }
496 }
497 
498 
504 {
505  out_P.setZero();
506  out_L.setZero();
507 
508  Vector3 dwc; // Center of mass speed in world frame
509  Vector3 P; // Linear momentum of the link
510  Vector3 L; // Angular momentum with respect to the world frame origin
511  Vector3 Llocal; // Angular momentum with respect to the center of mass of the link
512 
513  int n = linkTraverse_.numLinks();
514  for(int i=0; i < n; i++){
515  Link* link = linkTraverse_[i];
516 
517  dwc = link->v + link->w.cross(link->R * link->c);
518 
519  P = link->m * dwc;
520 
521  //L = cross(link->wc, P) + link->R * link->I * trans(link->R) * link->w;
522  Llocal.noalias() = link->I * link->R.transpose()*link->w;
523  L = link->wc.cross(P) + link->R * Llocal;
524 
525  out_P += P;
526  out_L += L;
527  }
528 }
529 
531 {
532  out_P.setZero();
533  out_L.setZero();
534 
535  dmatrix J,H;
536  calcCMJacobian(NULL,J);
538 
539  dvector dq;
540  int n = numJoints();
541  dq.resize(n);
542  for(int i=0; i < n; i++){
543  Link* link = joint(i);
544  dq[i] = link->dq;
545  }
546  dvector v;
547  v.resize(n+3+3);
548  v << dq, rootLink_->v, rootLink_->w;
549 
550  out_P = totalMass_ * J * v;
551  out_L = H * v;
552 }
553 
554 void Body::calcForwardKinematics(bool calcVelocity, bool calcAcceleration)
555 {
556  linkTraverse_.calcForwardKinematics(calcVelocity, calcAcceleration);
557 }
558 
559 
560 Light* Body::createLight(Link* link, int lightType, const std::string& name)
561 {
562  Light *light = new Light(link, lightType, name);
564  return light;
565 }
566 
567 Sensor* Body::createSensor(Link* link, int sensorType, int id, const std::string& name)
568 {
569  Sensor* sensor = 0;
570 
571  if(sensorType < Sensor::NUM_SENSOR_TYPES && id >= 0){
572 
573  SensorArray& sensors = allSensors[sensorType];
574  int n = sensors.size();
575  if(id >= n){
576  sensors.resize(id + 1, 0);
577  }
578  sensor = sensors[id];
579  if(sensor){
580  std::cerr << "duplicated sensor Id is specified(id = "
581  << id << ", name = " << name << ")" << std::endl;
582 
583  nameToSensorMap.erase(sensor->name);
584  } else {
585  sensor = Sensor::create(sensorType);
586  }
587  if(sensor){
588  sensor->id = id;
589  sensors[id] = sensor;
590  sensor->link = link;
591  sensor->name = name;
593  link->sensors.push_back(sensor);
594  }
595  }
596 
597  return sensor;
598 }
599 
600 void Body::addSensor(Sensor* sensor, int sensorType, int id ){
601  if(sensorType < Sensor::NUM_SENSOR_TYPES && id >= 0){
602  SensorArray& sensors = allSensors[sensorType];
603  int n = sensors.size();
604  if(id >= n){
605  sensors.resize(id + 1, 0);
606  }
607  Sensor* sameId = sensors[id];
608  if(sameId){
609  std::cerr << "duplicated sensor Id is specified(id = "
610  << id << ", name = " << sensor->name << ")" << std::endl;
611 
612  nameToSensorMap.erase(sameId->name);
613  }
614  sensors[id] = sensor;
615  nameToSensorMap[sensor->name] = sensor;
616  }
617 }
618 
619 
621 {
622  for(unsigned int i=0; i < numSensorTypes(); ++i){
623  for(unsigned int j=0; j < numSensors(i); ++j){
624  if(sensor(i,j))
625  sensor(i, j)->clear();
626  }
627  }
628 }
629 
630 
631 JointPathPtr Body::getJointPath(Link* baseLink, Link* targetLink)
632 {
634  return JointPathPtr(new CustomizedJointPath(this, baseLink, targetLink));
635  } else {
636  return JointPathPtr(new JointPath(baseLink, targetLink));
637  }
638 }
639 
640 
642 {
645  }
646 }
647 
648 
650 {
651  int n = linkTraverse_.numLinks();
652  for(int i=0; i < n; ++i){
653  Link* link = linkTraverse_[i];
654  link->fext.setZero();
655  link->tauext.setZero();
656  }
657 }
658 
659 
661 {
662  const int n = linkTraverse_.numLinks();
663  for(int i=0; i < n; ++i){
664  Link* link = linkTraverse_[i];
665  if(link->coldetModel){
666  link->coldetModel->setPosition(link->segmentAttitude(), link->p);
667  }
668  }
669 }
670 
671 
672 void Body::putInformation(std::ostream &out)
673 {
674  out << "Body: model name = " << modelName_ << " name = " << name_ << "\n\n";
675 
676  int n = numLinks();
677  for(int i=0; i < n; ++i){
678  out << *link(i);
679  }
680  out << std::endl;
681 }
682 
683 
688 {
692  }
693 
694  BodyCustomizerInterface* interface = findBodyCustomizer(modelName_);
695 
696  return interface ? installCustomizer(interface) : false;
697 }
698 
699 
700 bool Body::installCustomizer(BodyCustomizerInterface * customizerInterface)
701 {
702  if(this->customizerInterface){
703  if(customizerHandle){
705  customizerHandle = 0;
706  }
707  this->customizerInterface = 0;
708  }
709 
712  if(customizerHandle){
714  }
715  }
716 
717  return (customizerHandle != 0);
718 }
719 
720 
721 std::ostream& operator<< (std::ostream& out, Body& body)
722 {
723  body.putInformation(out);
724  return out;
725 }
726 
727 
728 static inline Link* extractLink(BodyHandle bodyHandle, int linkIndex)
729 {
730  return static_cast<BodyHandleEntity*>(bodyHandle)->body->link(linkIndex);
731 }
732 
733 
734 static int getLinkIndexFromName(BodyHandle bodyHandle, const char* linkName)
735 {
736  Body* body = static_cast<BodyHandleEntity*>(bodyHandle)->body;
737  Link* link = body->link(linkName);
738  return (link ? link->index : -1);
739 }
740 
741 
742 static const char* getLinkName(BodyHandle bodyHandle, int linkIndex)
743 {
744  return extractLink(bodyHandle, linkIndex)->name.c_str();
745 }
746 
747 
748 static double* getJointValuePtr(BodyHandle bodyHandle, int linkIndex)
749 {
750  return &(extractLink(bodyHandle,linkIndex)->q);
751 }
752 
753 
754 static double* getJointVelocityPtr(BodyHandle bodyHandle, int linkIndex)
755 {
756  return &(extractLink(bodyHandle, linkIndex)->dq);
757 }
758 
759 
760 static double* getJointTorqueForcePtr(BodyHandle bodyHandle, int linkIndex)
761 {
762  return &(extractLink(bodyHandle, linkIndex)->u);
763 }
764 
765 
767 {
768  static BodyInterface interface = {
771  getLinkName,
775  };
776 
777  return &interface;
778 }
779 
780 
781 CustomizedJointPath::CustomizedJointPath(Body* body, Link* baseLink, Link* targetLink) :
782  JointPath(baseLink, targetLink),
783  body(body)
784 {
786 }
787 
788 
790 {
791 
792 }
793 
794 
796 {
799  if(ikTypeId){
801  } else {
802  // try reversed path
805  if(ikTypeId){
807  }
808  }
809 }
810 
811 
813 {
814  bool solved;
815 
816  if(ikTypeId == 0 || isBestEffortIKMode){
817 
818  solved = JointPath::calcInverseKinematics(end_p, end_R);
819 
820  } else {
821 
822  std::vector<double> qorg(numJoints());
823 
824  for(unsigned int i=0; i < numJoints(); ++i){
825  qorg[i] = joint(i)->q;
826  }
827 
828  const Link* targetLink = endLink();
829  const Link* baseLink_ = baseLink();
830 
831  Vector3 p_relative;
832  Matrix33 R_relative;
834  p_relative.noalias() = baseLink_->R.transpose() * (end_p - baseLink_->p);
835  R_relative.noalias() = baseLink_->R.transpose() * end_R;
836  } else {
837  p_relative.noalias() = end_R.transpose() * (baseLink_->p - end_p);
838  R_relative.noalias() = end_R.transpose() * baseLink_->R;
839  }
840  solved = body->customizerInterface->
841  calcAnalyticIk(body->customizerHandle, ikTypeId, p_relative, R_relative);
842 
843  if(solved){
844 
846 
847  Vector3 dp(end_p - targetLink->p);
848  Vector3 omega(omegaFromRot((targetLink->R*targetLink->Rs).transpose() * end_R));
849 
850  double errsqr = dp.dot(dp) + omega.dot(omega);
851 
852  if(errsqr < maxIKErrorSqr){
853  solved = true;
854  } else {
855  solved = false;
856  for(unsigned int i=0; i < numJoints(); ++i){
857  joint(i)->q = qorg[i];
858  }
860  }
861  }
862  }
863 
864  return solved;
865 }
866 
867 
869 {
870  return (ikTypeId != 0);
871 }
872 
874 {
875  // prepare subm, submwc
877  if (base){
878  jp = getJointPath(rootLink(), base);
879  Link *skip = jp->joint(0);
880  skip->subm = rootLink()->m;
881  skip->submwc = rootLink()->m*rootLink()->wc;
882  Link *l = rootLink()->child;
883  if (l){
884  if (l != skip) {
885  l->calcSubMassCM();
886  skip->subm += l->subm;
887  skip->submwc += l->submwc;
888  }
889  l = l->sibling;
890  while(l){
891  if (l != skip){
892  l->calcSubMassCM();
893  skip->subm += l->subm;
894  skip->submwc += l->submwc;
895  }
896  l = l->sibling;
897  }
898  }
899 
900  // assuming there is no branch between base and root
901  for (unsigned int i=1; i<jp->numJoints(); i++){
902  l = jp->joint(i);
903  l->subm = l->parent->m + l->parent->subm;
904  l->submwc = l->parent->m*l->parent->wc + l->parent->submwc;
905  }
906 
907  J.resize(3, numJoints());
908  }else{
909  rootLink()->calcSubMassCM();
910  J.resize(3, numJoints()+6);
911  }
912 
913  // compute Jacobian
914  std::vector<int> sgn(numJoints(), 1);
915  if (jp) {
916  for (unsigned int i=0; i<jp->numJoints(); i++) sgn[jp->joint(i)->jointId] = -1;
917  }
918 
919  for (unsigned int i=0; i<numJoints(); i++){
920  Link *j = joint(i);
921  switch(j->jointType){
923  {
924  Vector3 omega(sgn[j->jointId]*j->R*j->a);
925  Vector3 arm((j->submwc-j->subm*j->p)/totalMass_);
926  Vector3 dp(omega.cross(arm));
927  J.col(j->jointId) = dp;
928  break;
929  }
930  case Link::SLIDE_JOINT:
931  {
932  Vector3 dp((j->subm/totalMass_)*sgn[j->jointId]*j->R*j->d);
933  J.col(j->jointId) = dp;
934  break;
935  }
936  default:
937  std::cerr << "calcCMJacobian() : unsupported jointType("
938  << j->jointType << ")" << std::endl;
939  }
940  }
941  if (!base){
942  int c = numJoints();
943  J(0, c ) = 1.0; J(0, c+1) = 0.0; J(0, c+2) = 0.0;
944  J(1, c ) = 0.0; J(1, c+1) = 1.0; J(1, c+2) = 0.0;
945  J(2, c ) = 0.0; J(2, c+1) = 0.0; J(2, c+2) = 1.0;
946 
947  Vector3 dp(rootLink()->submwc/totalMass_ - rootLink()->p);
948  J(0, c+3) = 0.0; J(0, c+4) = dp(2); J(0, c+5) = -dp(1);
949  J(1, c+3) = -dp(2); J(1, c+4) = 0.0; J(1, c+5) = dp(0);
950  J(2, c+3) = dp(1); J(2, c+4) = -dp(0); J(2, c+5) = 0.0;
951  }
952 }
953 
955 {
956  // prepare subm, submwc
958 
959  dmatrix M;
960  calcCMJacobian(base, M);
961  M.conservativeResize(3, numJoints());
962  M *= totalMass();
963 
964  if (base){
965  jp = getJointPath(rootLink(), base);
966  Link *skip = jp->joint(0);
967  skip->subm = rootLink()->m;
968  skip->submwc = rootLink()->m*rootLink()->wc;
969  Link *l = rootLink()->child;
970  if (l){
971  if (l != skip) {
972  l->calcSubMassCM();
973  skip->subm += l->subm;
974  skip->submwc += l->submwc;
975  }
976  l = l->sibling;
977  while(l){
978  if (l != skip){
979  l->calcSubMassCM();
980  skip->subm += l->subm;
981  skip->submwc += l->submwc;
982  }
983  l = l->sibling;
984  }
985  }
986 
987  // assuming there is no branch between base and root
988  for (unsigned int i=1; i<jp->numJoints(); i++){
989  l = jp->joint(i);
990  l->subm = l->parent->m + l->parent->subm;
991  l->submwc = l->parent->m*l->parent->wc + l->parent->submwc;
992  }
993 
994  H.resize(3, numJoints());
995  }else{
996  rootLink()->calcSubMassCM();
997  H.resize(3, numJoints()+6);
998  }
999 
1000  // compute Jacobian
1001  std::vector<int> sgn(numJoints(), 1);
1002  if (jp) {
1003  for (unsigned int i=0; i<jp->numJoints(); i++) sgn[jp->joint(i)->jointId] = -1;
1004  }
1005 
1006  for (unsigned int i=0; i<numJoints(); i++){
1007  Link *j = joint(i);
1008  switch(j->jointType){
1010  {
1011  Vector3 omega(sgn[j->jointId]*j->R*j->a);
1012  Vector3 Mcol = M.col(j->jointId);
1013  Matrix33 jsubIw;
1014  j->calcSubMassInertia(jsubIw);
1015  Vector3 dp = jsubIw*omega;
1016  if (j->subm!=0) dp += (j->submwc/j->subm).cross(Mcol);
1017  H.col(j->jointId) = dp;
1018  break;
1019  }
1020  case Link::SLIDE_JOINT:
1021  {
1022  if(j->subm!=0){
1023  Vector3 Mcol =M.col(j->jointId);
1024  Vector3 dp = (j->submwc/j->subm).cross(Mcol);
1025  H.col(j->jointId) = dp;
1026  }
1027  break;
1028  }
1029  default:
1030  std::cerr << "calcCMJacobian() : unsupported jointType("
1031  << j->jointType << ")" << std::endl;
1032  }
1033  }
1034  if (!base){
1035  int c = numJoints();
1036  H.block(0, c, 3, 3).setZero();
1037  Matrix33 Iw;
1038  rootLink_->calcSubMassInertia(Iw);
1039  H.block(0, c+3, 3, 3) = Iw;
1040  Vector3 cm = calcCM();
1041  Matrix33 cm_cross;
1042  cm_cross <<
1043  0.0, -cm(2), cm(1),
1044  cm(2), 0.0, -cm(0),
1045  -cm(1), cm(0), 0.0;
1046  H.block(0,0,3,c) -= cm_cross * M;
1047  }
1048 }
unsigned int numSensors(int sensorType) const
Definition: Body.h:162
Link * createEmptyJoint(int jointId)
Definition: Body.cpp:171
std::vector< SensorArray > allSensors
Definition: Body.h:290
void initialize()
Definition: Body.cpp:74
virtual void onJointPathUpdated()
Definition: Body.cpp:795
int c
Definition: autoplay.py:16
BodyCustomizerInitializeAnalyticIkFunc initializeAnalyticIk
Link * rootLink() const
Definition: Body.h:144
Eigen::MatrixXd dmatrix
Definition: EigenTypes.h:13
static void destroy(Sensor *sensor)
virtual bool calcInverseKinematics(const Vector3 &end_p, const Matrix33 &end_R)
Definition: Body.cpp:812
double calcTotalMass()
Definition: Body.cpp:297
static const int BODY_INTERFACE_VERSION
unsigned int numJoints() const
Definition: Body.h:87
unsigned int numLinks() const
Definition: Body.h:112
static int getLinkIndexFromName(BodyHandle bodyHandle, const char *linkName)
Definition: Body.cpp:734
png_voidp ptr
Definition: png.h:2063
Body()
Definition: Body.cpp:85
void setDefaultRootPosition(const Vector3 &p, const Matrix33 &R)
Definition: Body.cpp:157
Vector3 calcCM()
Definition: Body.cpp:310
void calcCMJacobian(Link *base, dmatrix &J)
compute CoM Jacobian
Definition: Body.cpp:873
virtual ~Body()
Definition: Body.cpp:53
png_uint_32 size
Definition: png.h:1521
virtual ~CustomizedJointPath()
Definition: Body.cpp:789
static Link * extractLink(BodyHandle bodyHandle, int linkIndex)
Definition: Body.cpp:728
double maxIKErrorSqr
Definition: JointPath.h:89
NameToLinkMap nameToLinkMap
Definition: Body.h:286
bool isCustomizedIkPathReversed
Definition: Body.cpp:42
friend class CustomizedJointPath
Definition: Body.h:312
void setRootLink(Link *link)
Definition: Body.cpp:146
void addSensor(Sensor *sensor, int sensorType, int id)
Definition: Body.cpp:600
void getDefaultRootPosition(Vector3 &out_p, Matrix33 &out_R)
Definition: Body.cpp:164
Link * link(int index) const
Definition: Body.h:121
void * BodyHandle
Definition: Body.h:40
png_uint_32 i
Definition: png.h:2735
Eigen::VectorXd dvector
Definition: EigenTypes.h:14
static BodyInterface * bodyInterface()
Definition: Body.cpp:766
Sensor * createSensor(Link *link, int sensorType, int id, const std::string &name)
Definition: Body.cpp:567
void calcTotalMomentum(Vector3 &out_P, Vector3 &out_L)
Definition: Body.cpp:503
BodyCustomizerCreateFunc create
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
unsigned int uint
Definition: Body.cpp:33
unsigned int numSensorTypes() const
Definition: Body.h:166
Vector3 defaultRootPosition
Definition: Body.h:299
std::vector< Sensor * > SensorArray
Definition: Body.h:289
BodyCustomizerSetVirtualJointForcesFunc setVirtualJointForces
void setColumnOfMassMatrix(dmatrix &M, int column)
Definition: Body.cpp:416
Eigen::Matrix3d Matrix33
Definition: EigenTypes.h:12
void updateLinkTree()
Definition: Body.cpp:211
BodyCustomizerDestroyFunc destroy
void clearSensorValues()
Definition: Body.cpp:620
static bool pluginsInDefaultDirectoriesLoaded
Definition: Body.cpp:30
void calcForwardKinematics(bool calcVelocity=false, bool calcAcceleration=false) const
void initializeConfiguration()
Definition: Body.cpp:267
void calcMassMatrix(dmatrix &out_M)
Definition: Body.cpp:338
std::string name_
Definition: Body.h:277
virtual void clear()
virtual void find(Link *root, bool doUpward=false, bool doDownward=true)
void updateLinkColdetModelPositions()
Definition: Body.cpp:660
double totalMass_
Definition: Body.h:297
NameToLightMap nameToLightMap
Definition: Body.h:295
Sensor * sensor(int sensorType, int sensorId) const
Definition: Body.h:158
BodyCustomizerInterface * customizerInterface
Definition: Body.h:304
static double * getJointVelocityPtr(BodyHandle bodyHandle, int linkIndex)
Definition: Body.cpp:754
const std::string & name()
Definition: Body.h:56
static Sensor * create(int type)
list index
The header file of the LinkPath and JointPath classes.
Light * createLight(Link *link, int lightType, const std::string &name)
Definition: Body.cpp:560
NameToSensorMap nameToSensorMap
Definition: Body.h:293
CustomizedJointPath(Body *body, Link *baseLink, Link *targetLink)
Definition: Body.cpp:781
std::vector< ExtraJoint > extraJoints
Definition: Body.h:256
std::string modelName_
Definition: Body.h:278
Link * link[2]
Definition: Body.h:252
bool installCustomizer()
Definition: Body.cpp:687
Light * light(const std::string &name)
Definition: Body.h:149
static const char * getLinkName(BodyHandle bodyHandle, int linkIndex)
Definition: Body.cpp:742
unsigned int numJoints() const
Definition: JointPath.h:42
Vector3 cross(const Vector3 &v1, const Vector3 &v2)
Definition: Tvmet2Eigen.h:15
The definitions of the body customizer interface for increasing binary compatibility.
bool isBestEffortIKMode
Definition: JointPath.h:90
void calcTotalMomentumFromJacobian(Vector3 &out_P, Vector3 &out_L)
Definition: Body.cpp:530
Matrix33 hat(const Vector3 &c)
Definition: Eigen3d.h:80
virtual bool hasAnalyticalIK()
Definition: Body.cpp:868
virtual bool calcInverseKinematics(const Vector3 &end_p, const Matrix33 &end_R)
Definition: JointPath.cpp:252
Matrix33 defaultRootAttitude
Definition: Body.h:300
Link * endLink() const
Definition: JointPath.h:54
void calcForwardKinematics(bool calcVelocity=false, bool calcAcceleration=false) const
Definition: JointPath.h:62
void calcAngularMomentumJacobian(Link *base, dmatrix &H)
compute Angular Momentum Jacobian around CoM of base
Definition: Body.cpp:954
std::ostream & operator<<(std::ostream &out, Body &body)
Definition: Body.cpp:721
Link * baseLink() const
Definition: JointPath.h:50
double sgn(double x)
Definition: fMatrix3.cpp:246
HRP_UTIL_EXPORT Vector3 omegaFromRot(const Matrix33 &r)
Definition: Eigen3d.cpp:63
Definition: Body.h:44
JointPathPtr getJointPath(Link *baseLink, Link *targetLink)
Definition: Body.cpp:631
BodyHandle bodyHandle
Definition: Body.h:306
boost::shared_ptr< JointPath > JointPathPtr
Definition: Body.h:29
static const bool PUT_DEBUG_MESSAGE
Definition: Body.cpp:28
bool isStaticModel_
Definition: Body.h:274
Link * rootLink_
Definition: Body.h:275
Link * joint(int index) const
Definition: JointPath.h:46
void setVirtualJointForcesSub()
Definition: Body.cpp:641
void putInformation(std::ostream &out)
Definition: Body.cpp:672
Link * joint(int id) const
Definition: Body.h:97
BodyCustomizerHandle customizerHandle
Definition: Body.h:303
void clearExternalForces()
Definition: Body.cpp:649
void calcForwardKinematics(bool calcVelocity=false, bool calcAcceleration=false)
Definition: Body.cpp:554
HRPMODEL_API int loadBodyCustomizers(const std::string pathString, BodyInterface *bodyInterface)
LinkTraverse linkTraverse_
Definition: Body.h:283
unsigned int numLinks() const
Definition: LinkTraverse.h:40
static double * getJointValuePtr(BodyHandle bodyHandle, int linkIndex)
Definition: Body.cpp:748
void calcInverseDynamics(Link *link, Vector3 &out_f, Vector3 &out_tau)
Definition: Body.cpp:439
LinkArray jointIdToLinkArray
Definition: Body.h:281
static double * getJointTorqueForcePtr(BodyHandle bodyHandle, int linkIndex)
Definition: Body.cpp:760


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