ForwardDynamicsCBM.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  */
14 #include "Body.h"
15 #include "Link.h"
16 #include "LinkTraverse.h"
17 #include "Sensor.h"
18 #include "ForwardDynamicsCBM.h"
19 
20 using namespace hrp;
21 using namespace std;
22 
23 
24 static const bool CALC_ALL_JOINT_TORQUES = false;
25 static const bool ROOT_ATT_NORMALIZATION_ENABLED = false;
26 
27 static const bool debugMode = false;
28 
29 #include <boost/format.hpp>
30 
31 template<class TMatrix>
32 static void putMatrix(TMatrix& M, char* name)
33 {
34  if(M.cols() == 1){
35  std::cout << "Vector " << name << M << std::endl;
36  } else {
37  std::cout << "Matrix " << name << ": \n";
38  for(size_t i=0; i < M.size1(); i++){
39  for(size_t j=0; j < M.cols(); j++){
40  std::cout << boost::format(" %6.3f ") % M(i, j);
41  }
42  std::cout << std::endl;
43  }
44  }
45 }
46 
47 template<class TVector>
48 static void putVector(TVector& M, char* name)
49 {
50  std::cout << "Vector " << name << M << std::endl;
51 }
52 
53 
55  ForwardDynamics(body)
56 {
57 
58 }
59 
60 
62 {
63 
64 }
65 
66 
68 {
69  Link* root = body->rootLink();
70  unknown_rootDof = (root->jointType == Link::FREE_JOINT && !root->isHighGainMode ) ? 6 : 0;
71  given_rootDof = (root->jointType == Link::FREE_JOINT && root->isHighGainMode ) ? 6 : 0;
72 
73  torqueModeJoints.clear();
74  highGainModeJoints.clear();
75 
76  int numLinks = body->numLinks();
77 
78  for(int i=1; i < numLinks; ++i){
79  Link* link = body->link(i);
80  int jointType = link->jointType;
81  if(jointType == Link::ROTATIONAL_JOINT || jointType == Link::SLIDE_JOINT){
82  if(link->isHighGainMode){
83  highGainModeJoints.push_back(link);
84  } else {
85  torqueModeJoints.push_back(link);
86  }
87  }
88  }
89 
90  int n = unknown_rootDof + torqueModeJoints.size();
91  int m = highGainModeJoints.size();
92 
93  isNoUnknownAccelMode = (n == 0);
94 
95  M11.resize(n, n);
96  M12.resize(n, given_rootDof+m);
97  b1. resize(n, 1);
98  c1. resize(n);
99  d1. resize(n, 1);
100 
101  qGiven. resize(m);
102  dqGiven. resize(m);
103  ddqGiven.resize(given_rootDof+m, 1);
104 
105  qGivenPrev. resize(m);
106  dqGivenPrev.resize(m);
107 
108  ddqorg.resize(numLinks);
109  uorg. resize(numLinks);
110 
112 
114  calcMassMatrix();
115  }
116 
117  ddqGivenCopied = false;
118 
120 
122 
123  q0. resize(numLinks);
124  dq0.resize(numLinks);
125  dq. resize(numLinks);
126  ddq.resize(numLinks);
127 
129  }
130 }
131 
132 
134 {
138  }
139 }
140 
141 
143 {
144  Vector3 f, tau;
145  calcAccelFKandForceSensorValues(body->rootLink(), f, tau);
146 }
147 
148 
150 {
151  if(isNoUnknownAccelMode && !body->numSensors(Sensor::FORCE)){
152 
154 
155  } else {
156 
157  switch(integrationMode){
158 
159  case EULER_METHOD:
161  break;
162 
163  case RUNGEKUTTA_METHOD:
165  break;
166  }
167 
169  Matrix33& R = body->rootLink()->R;
170  Matrix33::ColXpr x = R.col(0);
171  Matrix33::ColXpr y = R.col(1);
172  Matrix33::ColXpr z = R.col(2);
173  x.normalize();
174  z = x.cross(y).normalized();
175  y = z.cross(x);
176  }
177  }
178 
179  if(sensorsEnabled){
181  }
182 
183  body->setVirtualJointForces();
184 
185  ddqGivenCopied = false;
186 }
187 
188 
190 {
194 
195  Link* root = body->rootLink();
196 
197  if(unknown_rootDof){
198  Vector3 p;
199  Matrix33 R;
200  SE3exp(p, R, root->p, root->R, root->w, root->vo, timeStep);
201  root->p = p;
202  root->R = R;
203 
204  root->vo += root->dvo * timeStep;
205  root->w += root->dw * timeStep;
206  }
207 
208  int n = torqueModeJoints.size();
209  for(int i=0; i < n; ++i){
210  Link* link = torqueModeJoints[i];
211  link->q += link->dq * timeStep;
212  link->dq += link->ddq * timeStep;
213  }
214 
216  calcMassMatrix();
217 }
218 
219 
221 {
222  int numHighGainJoints = highGainModeJoints.size();
223  Link* root = body->rootLink();
224 
225  if(given_rootDof){
226  pGiven = root->p;
227  RGiven = root->R;
228  voGiven = root->vo;
229  wGiven = root->w;
230  root->p = pGivenPrev;
231  root->R = RGivenPrev;
232  root->vo = voGivenPrev;
233  root->w = wGivenPrev;
234  }
235 
236  for(int i=0; i < numHighGainJoints; ++i){
237  Link* link = highGainModeJoints[i];
238  qGiven [i] = link->q;
239  dqGiven[i] = link->dq;
240  link->q = qGivenPrev[i];
241  link->dq = dqGivenPrev[i];
242  }
243 
245  p0 = root->p;
246  R0 = root->R;
247  vo0 = root->vo;
248  w0 = root->w;
249  }
250 
251  vo.setZero();
252  w.setZero();
253  dvo.setZero();
254  dw.setZero();
255 
256  int numLinks = body->numLinks();
257 
258  for(int i=1; i < numLinks; ++i){
259  Link* link = body->link(i);
260  q0 [i] = link->q;
261  dq0[i] = link->dq;
262  dq [i] = 0.0;
263  ddq[i] = 0.0;
264  }
265 
267 
270 
271  integrateRungeKuttaOneStep(1.0 / 6.0, timeStep / 2.0);
272 
274  calcMassMatrix();
276 
277  integrateRungeKuttaOneStep(2.0 / 6.0, timeStep / 2.0);
278 
280  calcMassMatrix();
282 
284 
286  calcMassMatrix();
288 
289  if(unknown_rootDof){
290  SE3exp(root->p, root->R, p0, R0, w0, vo0, timeStep);
291  root->vo = vo0 + (dvo + root->dvo / 6.0) * timeStep;
292  root->w = w0 + (dw + root->dw / 6.0) * timeStep;
293  }
294  if(given_rootDof){
295  root->p = pGiven;
296  root->R = RGiven;
297  root->vo = voGiven;
298  root->w = wGiven;
299  }
300 
301  for(size_t i=0; i < torqueModeJoints.size(); ++i){
302  Link* link = torqueModeJoints[i];
303  int index = link->index;
304  link->q = q0 [index] + (dq [index] + link->dq / 6.0) * timeStep;
305  link->dq = dq0[index] + (ddq[index] + link->ddq / 6.0) * timeStep;
306  }
307  for(size_t i=0; i < highGainModeJoints.size(); ++i){
308  Link* link = highGainModeJoints[i];
309  link->q = qGiven [i];
310  link->dq = dqGiven[i];
311  }
312 
314  calcMassMatrix();
315 
317 }
318 
319 
321 {
322  Link* root = body->rootLink();
323 
325  SE3exp(root->p, root->R, p0, R0, root->w, root->vo, dt);
326  root->vo = vo0 + root->dvo * dt;
327  root->w = w0 + root->dw * dt;
328 
329  vo += r * root->vo;
330  w += r * root->w;
331  dvo += r * root->dvo;
332  dw += r * root->dw;
333  }
334 
335  int n = body->numLinks();
336  for(int i=1; i < n; ++i){
337  Link* link = body->link(i);
338  link->q = q0 [i] + dt * link->dq;
339  link->dq = dq0[i] + dt * link->ddq;
340  dq [i] += r * link->dq;
341  ddq[i] += r * link->ddq;
342  }
343 }
344 
345 
347 {
348  if(given_rootDof){
349  Link* root = body->rootLink();
350  pGivenPrev = root->p;
351  RGivenPrev = root->R;
352  voGivenPrev = root->vo;
353  wGivenPrev = root->w;
354  }
355  for(size_t i=0; i < highGainModeJoints.size(); ++i){
356  Link* link = highGainModeJoints[i];
357  qGivenPrev [i] = link->q;
358  dqGivenPrev[i] = link->dq;
359  }
360 }
361 
362 
364 {
365  const LinkTraverse& traverse = body->linkTraverse();
366  int n = traverse.numLinks();
367 
368  Link* root = traverse[0];
369  root_w_x_v = root->w.cross(root->vo + root->w.cross(root->p));
370 
371  if(given_rootDof){
372  root->vo = root->v - root->w.cross(root->p);
373  }
374 
375  for(int i=0; i < n; ++i){
376  Link* link = traverse[i];
377  Link* parent = link->parent;
378 
379  if(parent){
380 
381  switch(link->jointType){
382 
383  case Link::SLIDE_JOINT:
384  link->p = parent->R * (link->b + link->q * link->d) + parent->p;
385  link->R = parent->R;
386  link->sw.setZero();
387  link->sv.noalias() = parent->R * link->d;
388  link->w = parent->w;
389  break;
390 
392  link->R.noalias() = parent->R * rodrigues(link->a, link->q);
393  link->p = parent->R * link->b + parent->p;
394  link->sw.noalias() = parent->R * link->a;
395  link->sv = link->p.cross(link->sw);
396  link->w = link->dq * link->sw + parent->w;
397  break;
398 
399  case Link::FIXED_JOINT:
400  default:
401  link->p = parent->R * link->b + parent->p;
402  link->R = parent->R;
403  link->w = parent->w;
404  link->vo = parent->vo;
405  link->sw.setZero();
406  link->sv.setZero();
407  link->cv.setZero();
408  link->cw.setZero();
409  goto COMMON_CALCS_FOR_ALL_JOINT_TYPES;
410  }
411 
412  link->vo = link->dq * link->sv + parent->vo;
413 
414  Vector3 dsv(parent->w.cross(link->sv) + parent->vo.cross(link->sw));
415  Vector3 dsw(parent->w.cross(link->sw));
416  link->cv = link->dq * dsv;
417  link->cw = link->dq * dsw;
418  }
419 
420  COMMON_CALCS_FOR_ALL_JOINT_TYPES:
421 
423  link->v = link->vo + link->w.cross(link->p);
424 
425  link->wc = link->R * link->c + link->p;
426  Matrix33 Iw(link->R * link->I * link->R.transpose());
427  Matrix33 c_hat(hat(link->wc));
428  link->Iww = link->m * (c_hat * c_hat.transpose()) + Iw;
429  link->Iwv = link->m * c_hat;
430 
431  Vector3 P(link->m * (link->vo + link->w.cross(link->wc)));
432  Vector3 L(link->Iww * link->w + link->m * link->wc.cross(link->vo));
433 
434  link->pf = link->w.cross(P);
435  link->ptau = link->vo.cross(P) + link->w.cross(L);
436 
437  }
438 }
439 
440 
447 {
448  Link* root = body->rootLink();
449  int numLinks = body->numLinks();
450 
451  // preserve and clear the joint accelerations
452  for(int i=1; i < numLinks; ++i){
453  Link* link = body->link(i);
454  ddqorg[i] = link->ddq;
455  uorg [i] = link->u;
456  link->ddq = 0.0;
457  }
458 
459  // preserve and clear the root link acceleration
460  dvoorg = root->dvo;
461  dworg = root->dw;
462  root->dvo = g - root_w_x_v; // dv = g, dw = 0
463  root->dw.setZero();
464 
466 
467  if(unknown_rootDof){
468  for(int i=0; i < 3; ++i){
469  root->dvo[i] += 1.0;
471  root->dvo[i] -= 1.0;
472  }
473  for(int i=0; i < 3; ++i){
474  root->dw[i] = 1.0;
475  Vector3 dw_x_p = root->dw.cross(root->p);
476  root->dvo -= dw_x_p;
478  root->dvo += dw_x_p;
479  root->dw[i] = 0.0;
480  }
481  }
482  if(given_rootDof){
483  for(int i=0; i < 3; ++i){
484  root->dvo[i] += 1.0;
486  root->dvo[i] -= 1.0;
487  }
488  for(int i=0; i < 3; ++i){
489  root->dw[i] = 1.0;
490  Vector3 dw_x_p = root->dw.cross(root->p);
491  root->dvo -= dw_x_p;
493  root->dvo += dw_x_p;
494  root->dw[i] = 0.0;
495  }
496  }
497 
498  for(size_t i=0; i < torqueModeJoints.size(); ++i){
499  Link* link = torqueModeJoints[i];
500  link->ddq = 1.0;
501  int j = i + unknown_rootDof;
503  M11(j, j) += link->Jm2; // motor inertia
504  link->ddq = 0.0;
505  }
506  for(size_t i=0; i < highGainModeJoints.size(); ++i){
507  Link* link = highGainModeJoints[i];
508  link->ddq = 1.0;
509  int j = i + given_rootDof;
511  link->ddq = 0.0;
512  }
513 
514  // subtract the constant term
515  for(int i=0; i < M11.cols(); ++i){
516  M11.col(i) -= b1;
517  }
518  for(int i=0; i < M12.cols(); ++i){
519  M12.col(i) -= b1;
520  }
521 
522  for(int i=1; i < numLinks; ++i){
523  Link* link = body->link(i);
524  link->ddq = ddqorg[i];
525  link->u = uorg [i];
526  }
527  root->dvo = dvoorg;
528  root->dw = dworg;
529 
530  accelSolverInitialized = false;
531 }
532 
533 
535 {
536  Vector3 f;
537  Vector3 tau;
538  Link* root = body->rootLink();
539  calcInverseDynamics(root, f, tau);
540 
541  if(unknown_rootDof){
542  tau -= root->p.cross(f);
543  M.block<6,1>(0, column) << f, tau;
544  }
545 
546  for(size_t i=0; i < torqueModeJoints.size(); ++i){
547  M(i + unknown_rootDof, column) = torqueModeJoints[i]->u;
548  }
549 }
550 
551 
553 {
554  Link* parent = link->parent;
555  if(parent){
556  link->dvo = parent->dvo + link->cv + link->sv * link->ddq;
557  link->dw = parent->dw + link->cw + link->sw * link->ddq;
558  }
559 
560  out_f = link->pf;
561  out_tau = link->ptau;
562 
563  if(link->child){
564  Vector3 f_c;
565  Vector3 tau_c;
566  calcInverseDynamics(link->child, f_c, tau_c);
567  out_f += f_c;
568  out_tau += tau_c;
569  }
570 
571  out_f += link->m * link->dvo + link->Iwv.transpose() * link->dw;
572  out_tau += link->Iwv * link->dvo + link->Iww * link->dw;
573 
574  link->u = link->sv.dot(out_f) + link->sw.dot(out_tau);
575 
576  if(link->sibling){
577  Vector3 f_s;
578  Vector3 tau_s;
579  calcInverseDynamics(link->sibling, f_s, tau_s);
580  out_f += f_s;
581  out_tau += tau_s;
582  }
583 }
584 
585 
587 {
588  fextTotal.setZero();
589  tauextTotal.setZero();
590 
591  int n = body->numLinks();
592  for(int i=0; i < n; ++i){
593  Link* link = body->link(i);
594  fextTotal += link->fext;
595  tauextTotal += link->tauext;
596  }
597 
598  tauextTotal -= body->rootLink()->p.cross(fextTotal);
599 }
600 
601 void ForwardDynamicsMM::calcd1(Link* link, Vector3& out_f, Vector3& out_tau)
602 {
603  out_f = -link->fext;
604  out_tau = -link->tauext;
605 
606  if(link->child){
607  Vector3 f_c;
608  Vector3 tau_c;
609  calcd1(link->child, f_c, tau_c);
610  out_f += f_c;
611  out_tau += tau_c;
612  }
613 
614  link->u = link->sv.dot(out_f) + link->sw.dot(out_tau);
615 
616  if(link->sibling){
617  Vector3 f_s;
618  Vector3 tau_s;
619  calcd1(link->sibling, f_s, tau_s);
620  out_f += f_s;
621  out_tau += tau_s;
622  }
623 }
624 
626 {
628 
629  if(!ddqGivenCopied){
630  if(given_rootDof){
631  Link* root = body->rootLink();
632  root->dvo = root->dv - root->dw.cross(root->p) - root->w.cross(root->v);
633  ddqGiven.head(3) = root->dvo;
634  ddqGiven.segment(3,3) = root->dw;
635  }
636  for(size_t i=0; i < highGainModeJoints.size(); ++i){
638  }
639  ddqGivenCopied = true;
640  }
641 
642  b1 += M12*ddqGiven;
643 
644  for(unsigned int i=1; i < body->numLinks(); ++i){
645  Link* link = body->link(i);
646  uorg [i] = link->u;
647  }
648  Vector3 f, tau;
649  Link* root = body->rootLink();
650  calcd1(root, f, tau);
651  for(int i=0; i<unknown_rootDof; i++){
652  d1(i, 0) = 0;
653  }
654  for(size_t i=0; i < torqueModeJoints.size(); ++i){
655  d1(i + unknown_rootDof, 0) = torqueModeJoints[i]->u;
656  }
657  for(unsigned int i=1; i < body->numLinks(); ++i){
658  Link* link = body->link(i);
659  link->u = uorg [i];
660  }
661 
662  accelSolverInitialized = true;
663  }
664 }
665 
666 //for ConstraintForceSolver
667 void ForwardDynamicsMM::solveUnknownAccels(Link* link, const Vector3& fext, const Vector3& tauext, const Vector3& rootfext, const Vector3& roottauext)
668 {
669  Vector3 fextorg = link->fext;
670  Vector3 tauextorg = link->tauext;
671  link->fext = fext;
672  link->tauext = tauext;
673  for(unsigned int i=1; i < body->numLinks(); ++i){
674  Link* link = body->link(i);
675  uorg [i] = link->u;
676  }
677  Vector3 f, tau;
678  Link* root = body->rootLink();
679  calcd1(root, f, tau);
680  for(int i=0; i<unknown_rootDof; i++){
681  d1(i, 0) = 0;
682  }
683  for(size_t i=0; i < torqueModeJoints.size(); ++i){
684  d1(i + unknown_rootDof, 0) = torqueModeJoints[i]->u;
685  }
686  for(unsigned int i=1; i < body->numLinks(); ++i){
687  Link* link = body->link(i);
688  link->u = uorg [i];
689  }
690  link->fext = fextorg;
691  link->tauext = tauextorg;
692 
693  solveUnknownAccels(rootfext,roottauext);
694 }
695 
696 void ForwardDynamicsMM::solveUnknownAccels(const Vector3& fext, const Vector3& tauext)
697 {
698  if(unknown_rootDof){
699  c1.head(3) = fext;
700  c1.segment(3,3) = tauext;
701  }
702 
703  for(size_t i=0; i < torqueModeJoints.size(); ++i){
705  }
706 
707  c1 -= d1;
708  c1 -= b1.col(0);
709 
710  dvector a;
711  if(c1.size()!=0) {
712  a = M11.colPivHouseholderQr().solve(c1);
713  }
714 
715  if(unknown_rootDof){
716  Link* root = body->rootLink();
717  root->dw = a.segment(3, 3);
718  Vector3 dv = a.head(3);
719  root->dvo = dv - root->dw.cross(root->p) - root_w_x_v;
720  }
721 
722  for(size_t i=0; i < torqueModeJoints.size(); ++i){
723  Link* link = torqueModeJoints[i];
724  link->ddq = c1(i + unknown_rootDof);
725  }
726 }
727 
728 
730 {
731  Link* parent = link->parent;
732 
733  if(parent){
734  link->dvo = parent->dvo + link->cv + link->sv * link->ddq;
735  link->dw = parent->dw + link->cw + link->sw * link->ddq;
736  }
737 
738  out_f = link->pf;
739  out_tau = link->ptau;
740 
741  for(Link* child = link->child; child; child = child->sibling){
742  Vector3 f, tau;
743  calcAccelFKandForceSensorValues(child, f, tau);
744  out_f += f;
745  out_tau += tau;
746  }
747 
749 
750  if(CALC_ALL_JOINT_TORQUES || info.hasSensorsAbove){
751 
752  Vector3 fg(-link->m * g);
753  Vector3 tg(link->wc.cross(fg));
754 
755  out_f -= fg;
756  out_tau -= tg;
757 
758  out_f -= link->fext;
759  out_tau -= link->tauext;
760 
761  out_f += link->m * link->dvo + link->Iwv.transpose() * link->dw;
762  out_tau += link->Iwv * link->dvo + link->Iww * link->dw;
763 
765  link->u = link->sv.dot(out_f) + link->sw.dot(out_tau);
766  }
767 
768  if(info.sensor){
769  ForceSensor* sensor = info.sensor;
770  Matrix33 sensorR (link->R * sensor->localR);
771  Vector3 sensorPos(link->p + link->R * sensor->localPos);
772  Vector3 f(-out_f);
773  sensor->f = sensorR.transpose() * f;
774  sensor->tau = sensorR.transpose() * (-out_tau - sensorPos.cross(f));
775  }
776  }
777 }
778 
779 
781 {
783 
784  int n = body->numLinks();
785 
786  forceSensorInfo.resize(n);
787 
788  for(int i=0; i < n; ++i){
789  forceSensorInfo[i].sensor = 0;
790  forceSensorInfo[i].hasSensorsAbove = false;
791  }
792 
793  if(sensorsEnabled){
794 
795  int numForceSensors = body->numSensors(Sensor::FORCE);
796  for(int i=0; i < numForceSensors; ++i){
797  ForceSensor* sensor = body->sensor<ForceSensor>(i);
798  forceSensorInfo[sensor->link->index].sensor = sensor;
799  }
800 
801  updateForceSensorInfo(body->rootLink(), false);
802  }
803 }
804 
805 
806 void ForwardDynamicsMM::updateForceSensorInfo(Link* link, bool hasSensorsAbove)
807 {
809  hasSensorsAbove |= (info.sensor != 0);
810  info.hasSensorsAbove = hasSensorsAbove;
811 
812  for(Link* child = link->child; child; child = child->sibling){
813  updateForceSensorInfo(child, hasSensorsAbove);
814  }
815 }
hrp::ForwardDynamics::initializeSensors
virtual void initializeSensors()
Definition: ForwardDynamics.cpp:93
hrp::ForwardDynamics::body
BodyPtr body
Definition: ForwardDynamics.h:71
hrp::Sensor::localR
Matrix33 localR
Definition: hrplib/hrpModel/Sensor.h:60
i
png_uint_32 i
Definition: png.h:2732
hrp::ForwardDynamicsMM::dvo
Vector3 dvo
Definition: ForwardDynamicsCBM.h:136
hrp::ForwardDynamicsMM::q0
std::vector< double > q0
Definition: ForwardDynamicsCBM.h:131
hrp::ForwardDynamicsMM::~ForwardDynamicsMM
~ForwardDynamicsMM()
Definition: ForwardDynamicsCBM.cpp:61
hrp::ForwardDynamicsMM::ForceSensorInfo
Definition: ForwardDynamicsCBM.h:119
hrp::ForwardDynamicsMM::accelSolverInitialized
bool accelSolverInitialized
Definition: ForwardDynamicsCBM.h:98
hrp::ForwardDynamicsMM::dworg
Vector3 dworg
Definition: ForwardDynamicsCBM.h:117
hrp::rodrigues
Matrix33 rodrigues(const Vector3 &axis, double q)
Definition: Eigen3d.h:27
hrp::ForwardDynamicsMM::fextTotal
Vector3 fextTotal
Definition: ForwardDynamicsCBM.h:108
hrp::ForwardDynamicsMM::updateForceSensorInfo
void updateForceSensorInfo(Link *link, bool hasSensorsAbove)
Definition: ForwardDynamicsCBM.cpp:806
hrp::ForwardDynamics::SE3exp
static void SE3exp(Vector3 &out_p, Matrix33 &out_R, const Vector3 &p0, const Matrix33 &R0, const Vector3 &w, const Vector3 &vo, double dt)
update position/orientation using spatial velocity
Definition: ForwardDynamics.cpp:70
Sensor.h
hrp::dvector
Eigen::VectorXd dvector
Definition: EigenTypes.h:14
hrp::ForwardDynamicsMM::ddq
std::vector< double > ddq
Definition: ForwardDynamicsCBM.h:139
hrp::ForwardDynamicsMM::calcMotionWithEulerMethod
void calcMotionWithEulerMethod()
Definition: ForwardDynamicsCBM.cpp:189
hrp::ForwardDynamicsMM::ddqorg
dvector ddqorg
Definition: ForwardDynamicsCBM.h:114
hrp::ForwardDynamicsMM::dq0
std::vector< double > dq0
Definition: ForwardDynamicsCBM.h:132
hrp::ForwardDynamicsMM::dqGivenPrev
dvector dqGivenPrev
Definition: ForwardDynamicsCBM.h:102
hrp::ForwardDynamicsMM::initializeAccelSolver
void initializeAccelSolver()
Definition: ForwardDynamicsCBM.cpp:625
autoplay.n
n
Definition: autoplay.py:12
hrp::ForwardDynamicsMM::calcNextState
virtual void calcNextState()
Definition: ForwardDynamicsCBM.cpp:149
hrp
Definition: ColdetModel.h:28
hrp::ForwardDynamicsMM::highGainModeJoints
std::vector< Link * > highGainModeJoints
Definition: ForwardDynamicsCBM.h:82
hrp::ForwardDynamics::updateSensorsFinal
virtual void updateSensorsFinal()
Definition: ForwardDynamics.cpp:103
swingTest.f
f
Definition: swingTest.py:6
hrp::ForceSensor::f
Vector3 f
Definition: hrplib/hrpModel/Sensor.h:74
hrp::ForceSensor
Definition: hrplib/hrpModel/Sensor.h:68
hrp::ForwardDynamicsMM::d1
dmatrix d1
Definition: ForwardDynamicsCBM.h:78
hrp::ForwardDynamicsMM::c1
dvector c1
Definition: ForwardDynamicsCBM.h:79
hrp::Sensor::link
Link * link
Definition: hrplib/hrpModel/Sensor.h:59
hrp::ForwardDynamics::integrationMode
enum hrp::ForwardDynamics::@2 integrationMode
hrp::ForwardDynamicsMM::vo0
Vector3 vo0
Definition: ForwardDynamicsCBM.h:129
hrp::ForwardDynamicsMM::integrateRungeKuttaOneStep
void integrateRungeKuttaOneStep(double r, double dt)
Definition: ForwardDynamicsCBM.cpp:320
hrp::ForceSensor::tau
Vector3 tau
Definition: hrplib/hrpModel/Sensor.h:75
hrp::ForwardDynamicsMM::setColumnOfMassMatrix
void setColumnOfMassMatrix(dmatrix &M, int column)
Definition: ForwardDynamicsCBM.cpp:534
hrp::ForwardDynamicsMM::pGiven
Vector3 pGiven
Definition: ForwardDynamicsCBM.h:93
hrp::Vector3
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
hrp::ForwardDynamicsMM::ddqGiven
dvector ddqGiven
Definition: ForwardDynamicsCBM.h:92
hrp::ForwardDynamics::timeStep
double timeStep
Definition: ForwardDynamics.h:73
hrp::ForwardDynamicsMM::given_rootDof
int given_rootDof
Definition: ForwardDynamicsCBM.h:86
hrp::ForwardDynamicsMM::preserveHighGainModeJointState
void preserveHighGainModeJointState()
Definition: ForwardDynamicsCBM.cpp:346
hrp::ForwardDynamicsMM::unknown_rootDof
int unknown_rootDof
Definition: ForwardDynamicsCBM.h:85
hrp::ForwardDynamicsMM::voGivenPrev
Vector3 voGivenPrev
Definition: ForwardDynamicsCBM.h:105
hrp::ForwardDynamicsMM::solveUnknownAccels
void solveUnknownAccels()
Definition: ForwardDynamicsCBM.cpp:133
hrp::ForwardDynamicsMM::R0
Matrix33 R0
Definition: ForwardDynamicsCBM.h:128
hrp::Sensor::localPos
Vector3 localPos
Definition: hrplib/hrpModel/Sensor.h:61
hrp::ForwardDynamicsMM::dvoorg
Vector3 dvoorg
Definition: ForwardDynamicsCBM.h:116
hrp::ForwardDynamicsMM::w0
Vector3 w0
Definition: ForwardDynamicsCBM.h:130
hrp::ForwardDynamicsMM::calcInverseDynamics
void calcInverseDynamics(Link *link, Vector3 &out_f, Vector3 &out_tau)
Definition: ForwardDynamicsCBM.cpp:552
hrp::ForwardDynamicsMM::wGivenPrev
Vector3 wGivenPrev
Definition: ForwardDynamicsCBM.h:106
hrp::ForwardDynamicsMM::calcPositionAndVelocityFK
void calcPositionAndVelocityFK()
Definition: ForwardDynamicsCBM.cpp:363
hrp::ForwardDynamics::RUNGEKUTTA_METHOD
@ RUNGEKUTTA_METHOD
Definition: ForwardDynamics.h:76
hrp::ForwardDynamicsMM::RGivenPrev
Matrix33 RGivenPrev
Definition: ForwardDynamicsCBM.h:104
hrp::ForwardDynamicsMM::isNoUnknownAccelMode
bool isNoUnknownAccelMode
Definition: ForwardDynamicsCBM.h:88
hrp::hat
Matrix33 hat(const Vector3 &c)
Definition: Eigen3d.h:80
hrp::dmatrix
Eigen::MatrixXd dmatrix
Definition: EigenTypes.h:13
hrp::ForwardDynamicsMM::wGiven
Vector3 wGiven
Definition: ForwardDynamicsCBM.h:96
hrp::ForwardDynamics::sensorsEnabled
bool sensorsEnabled
Definition: ForwardDynamics.h:74
hrp::ForwardDynamicsMM::dq
std::vector< double > dq
Definition: ForwardDynamicsCBM.h:138
hrp::ForwardDynamicsMM::calcMotionWithRungeKuttaMethod
void calcMotionWithRungeKuttaMethod()
Definition: ForwardDynamicsCBM.cpp:220
hrp::ForwardDynamicsMM::root_w_x_v
Vector3 root_w_x_v
Definition: ForwardDynamicsCBM.h:111
hrp::ForwardDynamicsMM::voGiven
Vector3 voGiven
Definition: ForwardDynamicsCBM.h:95
hrp::ForwardDynamics::g
Vector3 g
Definition: ForwardDynamics.h:72
LinkTraverse.h
The header file of the LinkTraverse class.
hrp::ForwardDynamicsMM::dqGiven
dvector dqGiven
Definition: ForwardDynamicsCBM.h:91
hrp::ForwardDynamicsMM::dw
Vector3 dw
Definition: ForwardDynamicsCBM.h:137
hrp::ForwardDynamicsMM::qGiven
dvector qGiven
Definition: ForwardDynamicsCBM.h:90
hrp::ForwardDynamicsMM::calcd1
void calcd1(Link *link, Vector3 &out_f, Vector3 &out_tau)
Definition: ForwardDynamicsCBM.cpp:601
hrp::ForwardDynamicsMM::w
Vector3 w
Definition: ForwardDynamicsCBM.h:135
hrp::ForwardDynamicsMM::initializeSensors
virtual void initializeSensors()
Definition: ForwardDynamicsCBM.cpp:780
hrp::LinkTraverse::numLinks
unsigned int numLinks() const
Definition: LinkTraverse.h:40
hrp::ForwardDynamicsMM::qGivenPrev
dvector qGivenPrev
Definition: ForwardDynamicsCBM.h:101
hrp::ForwardDynamicsMM::sumExternalForces
void sumExternalForces()
Definition: ForwardDynamicsCBM.cpp:586
hrp::BodyPtr
boost::shared_ptr< Body > BodyPtr
Definition: Body.h:315
name
png_infop png_charpp name
Definition: png.h:2379
hrp::ForwardDynamics::EULER_METHOD
@ EULER_METHOD
Definition: ForwardDynamics.h:76
debugMode
static const bool debugMode
Definition: ForwardDynamicsCBM.cpp:27
hrp::ForwardDynamicsMM::M12
dmatrix M12
Definition: ForwardDynamicsCBM.h:76
hrp::ForwardDynamicsMM::uorg
dvector uorg
Definition: ForwardDynamicsCBM.h:115
hrp::Sensor::FORCE
@ FORCE
Definition: hrplib/hrpModel/Sensor.h:33
hrp::ForwardDynamicsMM::pGivenPrev
Vector3 pGivenPrev
Definition: ForwardDynamicsCBM.h:103
hrp::ForwardDynamicsMM::p0
Vector3 p0
Definition: ForwardDynamicsCBM.h:127
hrp::ForwardDynamicsMM::M11
dmatrix M11
Definition: ForwardDynamicsCBM.h:75
hrp::ForwardDynamicsMM::b1
dmatrix b1
Definition: ForwardDynamicsCBM.h:77
putVector
static void putVector(TVector &M, char *name)
Definition: ForwardDynamicsCBM.cpp:48
Body.h
hrp::ForwardDynamicsMM::calcMassMatrix
void calcMassMatrix()
Definition: ForwardDynamicsCBM.cpp:446
CALC_ALL_JOINT_TORQUES
static const bool CALC_ALL_JOINT_TORQUES
Definition: ForwardDynamicsCBM.cpp:24
hrp::LinkTraverse
Definition: LinkTraverse.h:29
hrp::Matrix33
Eigen::Matrix3d Matrix33
Definition: EigenTypes.h:12
hrp::ForwardDynamics
Definition: ForwardDynamics.h:35
putMatrix
static void putMatrix(TMatrix &M, char *name)
Definition: ForwardDynamicsCBM.cpp:32
test.a
int a
Definition: test.py:1
hrp::ForwardDynamicsMM::forceSensorInfo
std::vector< ForceSensorInfo > forceSensorInfo
Definition: ForwardDynamicsCBM.h:124
hrp::ForwardDynamicsMM::ForwardDynamicsMM
ForwardDynamicsMM(BodyPtr body)
Definition: ForwardDynamicsCBM.cpp:54
info
backing_store_ptr info
Definition: jmemsys.h:181
hrp::ForwardDynamicsMM::tauextTotal
Vector3 tauextTotal
Definition: ForwardDynamicsCBM.h:109
ROOT_ATT_NORMALIZATION_ENABLED
static const bool ROOT_ATT_NORMALIZATION_ENABLED
Definition: ForwardDynamicsCBM.cpp:25
hrp::ForwardDynamicsMM::calcAccelFKandForceSensorValues
void calcAccelFKandForceSensorValues()
Definition: ForwardDynamicsCBM.cpp:142
hrp::ForwardDynamicsMM::torqueModeJoints
std::vector< Link * > torqueModeJoints
Definition: ForwardDynamicsCBM.h:81
hrp::ForwardDynamicsMM::ddqGivenCopied
bool ddqGivenCopied
Definition: ForwardDynamicsCBM.h:99
hrp::ForwardDynamicsMM::vo
Vector3 vo
Definition: ForwardDynamicsCBM.h:134
hrp::ForwardDynamicsMM::initialize
virtual void initialize()
Definition: ForwardDynamicsCBM.cpp:67
ForwardDynamicsCBM.h
hrp::ForwardDynamicsMM::RGiven
Matrix33 RGiven
Definition: ForwardDynamicsCBM.h:94


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