ForwardDynamicsABM.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 "ForwardDynamicsABM.h"
15 #include "Body.h"
16 #include "Link.h"
17 #include "LinkTraverse.h"
18 #include "Sensor.h"
19 #include <hrpUtil/EigenTypes.h>
20 
21 
22 using namespace hrp;
23 using namespace std;
24 
25 
26 static const bool debugMode = false;
27 static const bool rootAttitudeNormalizationEnabled = false;
28 
29 
31  ForwardDynamics(body),
32  q0(body->numLinks()),
33  dq0(body->numLinks()),
34  dq(body->numLinks()),
35  ddq(body->numLinks())
36 {
37 
38 }
39 
40 
42 {
43 
44 }
45 
46 
48 {
51 }
52 
53 
55 {
56  calcABMPhase1();
58 }
59 
60 
62 {
64  calcABMPhase3();
65 }
66 
67 
69 {
70  switch(integrationMode){
71 
72  case EULER_METHOD:
74  break;
75 
76  case RUNGEKUTTA_METHOD:
78  break;
79  }
80 
82  Matrix33& R = body->rootLink()->R;
83  Matrix33::ColXpr x = R.col(0);
84  Matrix33::ColXpr y = R.col(1);
85  Matrix33::ColXpr z = R.col(2);
86  x.normalize();
87  z = x.cross(y).normalized();
88  y = z.cross(x);
89  }
90 
92 
93  if(sensorsEnabled){
95  }
96 
97  body->setVirtualJointForces();
98 }
99 
100 
102 {
103  calcABMLastHalf();
104 
105  if(sensorsEnabled){
107  }
108 
109  Link* root = body->rootLink();
110 
111  if(root->jointType != Link::FIXED_JOINT){
112  Vector3 p;
113  Matrix33 R;
114  SE3exp(p, R, root->p, root->R, root->w, root->vo, timeStep);
115  root->p = p;
116  root->R = R;
117 
118  root->vo += root->dvo * timeStep;
119  root->w += root->dw * timeStep;
120  }
121 
122  int n = body->numLinks();
123  for(int i=1; i < n; ++i){
124  Link* link = body->link(i);
125  link->q += link->dq * timeStep;
126  link->dq += link->ddq * timeStep;
127  }
128 }
129 
130 
132 {
133  Link* root = body->rootLink();
134 
135  if(root->jointType != Link::FIXED_JOINT){
136 
137  SE3exp(root->p, root->R, p0, R0, root->w, root->vo, dt);
138  root->vo = vo0 + root->dvo * dt;
139  root->w = w0 + root->dw * dt;
140 
141  vo += r * root->vo;
142  w += r * root->w;
143  dvo += r * root->dvo;
144  dw += r * root->dw;
145  }
146 
147  int n = body->numLinks();
148  for(int i=1; i < n; ++i){
149 
150  Link* link = body->link(i);
151 
152  link->q = q0[i] + dt * link->dq;
153  link->dq = dq0[i] + dt * link->ddq;
154 
155  dq[i] += r * link->dq;
156  ddq[i] += r * link->ddq;
157  }
158 }
159 
160 
162 {
163  Link* root = body->rootLink();
164 
165  if(root->jointType != Link::FIXED_JOINT){
166  p0 = root->p;
167  R0 = root->R;
168  vo0 = root->vo;
169  w0 = root->w;
170  }
171 
172  vo.setZero();
173  w.setZero();
174  dvo.setZero();
175  dw.setZero();
176 
177  int n = body->numLinks();
178  for(int i=1; i < n; ++i){
179  Link* link = body->link(i);
180  q0[i] = link->q;
181  dq0[i] = link->dq;
182  dq[i] = 0.0;
183  ddq[i] = 0.0;
184  }
185 
186  calcABMLastHalf();
187 
188  if(sensorsEnabled){
190  }
191 
192  integrateRungeKuttaOneStep(1.0 / 6.0, timeStep / 2.0);
193  calcABMPhase1();
194  calcABMPhase2();
195  calcABMPhase3();
196 
197  integrateRungeKuttaOneStep(2.0 / 6.0, timeStep / 2.0);
198  calcABMPhase1();
199  calcABMPhase2();
200  calcABMPhase3();
201 
203  calcABMPhase1();
204  calcABMPhase2();
205  calcABMPhase3();
206 
207  if(root->jointType != Link::FIXED_JOINT){
208  SE3exp(root->p, root->R, p0, R0, w0, vo0, timeStep);
209  root->vo = vo0 + (dvo + root->dvo / 6.0) * timeStep;
210  root->w = w0 + (dw + root->dw / 6.0) * timeStep;
211  }
212 
213  for(int i=1; i < n; ++i){
214  Link* link = body->link(i);
215  link->q = q0[i] + ( dq[i] + link->dq / 6.0) * timeStep;
216  link->dq = dq0[i] + (ddq[i] + link->ddq / 6.0) * timeStep;
217  }
218 }
219 
220 
222 {
223  const LinkTraverse& traverse = body->linkTraverse();
224  int n = traverse.numLinks();
225 
226  for(int i=0; i < n; ++i){
227  Link* link = traverse[i];
228  Link* parent = link->parent;
229 
230  if(parent){
231  switch(link->jointType){
232 
234  link->R.noalias() = parent->R * rodrigues(link->a, link->q);
235  link->p = parent->R * link->b + parent->p;
236  link->sw.noalias() = parent->R * link->a;
237  link->sv = link->p.cross(link->sw);
238  link->w = link->dq * link->sw + parent->w;
239  break;
240 
241  case Link::SLIDE_JOINT:
242  link->p = parent->R * (link->b + link->q * link->d) + parent->p;
243  link->R = parent->R;
244  link->sw.setZero();
245  link->sv.noalias() = parent->R * link->d;
246  link->w = parent->w;
247  break;
248 
249  case Link::FIXED_JOINT:
250  default:
251  link->p = parent->R * link->b + parent->p;
252  link->R = parent->R;
253  link->w = parent->w;
254  link->vo = parent->vo;
255  link->sw.setZero();
256  link->sv.setZero();
257  link->cv.setZero();
258  link->cw.setZero();
259  goto COMMON_CALCS_FOR_ALL_JOINT_TYPES;
260  }
261 
262  // Common for ROTATE and SLIDE
263  link->vo = link->dq * link->sv + parent->vo;
264  Vector3 dsv(parent->w.cross(link->sv) + parent->vo.cross(link->sw));
265  Vector3 dsw(parent->w.cross(link->sw));
266  link->cv = link->dq * dsv;
267  link->cw = link->dq * dsw;
268  }
269 
270  COMMON_CALCS_FOR_ALL_JOINT_TYPES:
271 
272  link->v = link->vo + link->w.cross(link->p);
273 
274  link->wc = link->R * link->c + link->p;
275 
276  // compute I^s (Eq.(6.24) of Kajita's textbook))
277  Matrix33 Iw(link->R * link->I * link->R.transpose());
278 
279  Matrix33 c_hat(hat(link->wc));
280  link->Iww = link->m * (c_hat * c_hat.transpose()) + Iw;
281 
282  link->Ivv <<
283  link->m, 0.0, 0.0,
284  0.0, link->m, 0.0,
285  0.0, 0.0, link->m;
286 
287  link->Iwv = link->m * c_hat;
288 
289  // compute P and L (Eq.(6.25) of Kajita's textbook)
290  Vector3 P(link->m * (link->vo + link->w.cross(link->wc)));
291  Vector3 L(link->Iww * link->w + link->m * link->wc.cross(link->vo));
292 
293  link->pf = link->w.cross(P);
294  link->ptau = link->vo.cross(P) + link->w.cross(L);
295 
296  Vector3 fg(-link->m * g);
297  Vector3 tg(link->wc.cross(fg));
298 
299  link->pf -= fg;
300  link->ptau -= tg;
301  }
302 }
303 
304 
306 {
307  const LinkTraverse& traverse = body->linkTraverse();
308  int n = traverse.numLinks();
309 
310  for(int i = n-1; i >= 0; --i){
311  Link* link = traverse[i];
312 
313  link->pf -= link->fext;
314  link->ptau -= link->tauext;
315 
316  // compute articulated inertia (Eq.(6.48) of Kajita's textbook)
317  for(Link* child = link->child; child; child = child->sibling){
318 
319  if(child->jointType == Link::FIXED_JOINT){
320  link->Ivv += child->Ivv;
321  link->Iwv += child->Iwv;
322  link->Iww += child->Iww;
323 
324  }else{
325  Vector3 hhv_dd(child->hhv / child->dd);
326  link->Ivv += child->Ivv - child->hhv * hhv_dd.transpose();
327  link->Iwv += child->Iwv - child->hhw * hhv_dd.transpose();
328  link->Iww += child->Iww - child->hhw * (child->hhw / child->dd).transpose();
329  }
330 
331  link->pf += child->Ivv * child->cv + child->Iwv.transpose() * child->cw + child->pf;
332  link->ptau += child->Iwv * child->cv + child->Iww * child->cw + child->ptau;
333 
334  if(child->jointType != Link::FIXED_JOINT){
335  double uu_dd = child->uu / child->dd;
336  link->pf += uu_dd * child->hhv;
337  link->ptau += uu_dd * child->hhw;
338  }
339  }
340 
341  if(i > 0){
342  if(link->jointType != Link::FIXED_JOINT){
343  // hh = Ia * s
344  link->hhv = link->Ivv * link->sv + link->Iwv.transpose() * link->sw;
345  link->hhw = link->Iwv * link->sv + link->Iww * link->sw;
346  // dd = Ia * s * s^T
347  link->dd = link->sv.dot(link->hhv) + link->sw.dot(link->hhw) + link->Jm2;
348  // uu = u - hh^T*c + s^T*pp
349  link->uu = link->u - (link->hhv.dot(link->cv) + link->hhw.dot(link->cw)
350  + link->sv.dot(link->pf) + link->sw.dot(link->ptau));
351  }
352  }
353  }
354 }
355 
356 
357 // A part of phase 2 (inbound loop) that can be calculated before external forces are given
359 {
360  const LinkTraverse& traverse = body->linkTraverse();
361  int n = traverse.numLinks();
362 
363  for(int i = n-1; i >= 0; --i){
364  Link* link = traverse[i];
365 
366  for(Link* child = link->child; child; child = child->sibling){
367 
368  if(child->jointType == Link::FIXED_JOINT){
369  link->Ivv += child->Ivv;
370  link->Iwv += child->Iwv;
371  link->Iww += child->Iww;
372 
373  }else{
374  Vector3 hhv_dd(child->hhv / child->dd);
375  link->Ivv += child->Ivv - child->hhv * hhv_dd.transpose();
376  link->Iwv += child->Iwv - child->hhw * hhv_dd.transpose();
377  link->Iww += child->Iww - child->hhw * (child->hhw / child->dd).transpose();
378  }
379 
380  link->pf += child->Ivv * child->cv + child->Iwv.transpose() * child->cw;
381  link->ptau += child->Iwv * child->cv + child->Iww * child->cw;
382  }
383 
384  if(i > 0){
385  if(link->jointType != Link::FIXED_JOINT){
386  link->hhv = link->Ivv * link->sv + link->Iwv.transpose() * link->sw;
387  link->hhw = link->Iwv * link->sv + link->Iww * link->sw;
388  link->dd = link->sv.dot(link->hhv) + link->sw.dot(link->hhw) + link->Jm2;
389  link->uu = - (link->hhv.dot(link->cv) + link->hhw.dot(link->cw));
390  }
391  }
392  }
393 }
394 
395 
396 // A remaining part of phase 2 that requires external forces
398 {
399  const LinkTraverse& traverse = body->linkTraverse();
400  int n = traverse.numLinks();
401 
402  for(int i = n-1; i >= 0; --i){
403  Link* link = traverse[i];
404 
405  link->pf -= link->fext;
406  link->ptau -= link->tauext;
407 
408  for(Link* child = link->child; child; child = child->sibling){
409  link->pf += child->pf;
410  link->ptau += child->ptau;
411 
412  if(child->jointType != Link::FIXED_JOINT){
413  double uu_dd = child->uu / child->dd;
414  link->pf += uu_dd * child->hhv;
415  link->ptau += uu_dd * child->hhw;
416  }
417  }
418 
419  if(i > 0){
420  if(link->jointType != Link::FIXED_JOINT)
421  link->uu += link->u - (link->sv.dot(link->pf) + link->sw.dot(link->ptau));
422  }
423  }
424 }
425 
426 
428 {
429  const LinkTraverse& traverse = body->linkTraverse();
430 
431  Link* root = traverse[0];
432 
433  if(root->jointType == Link::FREE_JOINT){
434 
435  // - | Ivv trans(Iwv) | * | dvo | = | pf |
436  // | Iwv Iww | | dw | | ptau |
437 
438  dmatrix Ia(6,6);
439  Ia << root->Ivv, root->Iwv.transpose(),
440  root->Iwv, root->Iww;
441 
442  dvector p(6);
443  p << root->pf, root->ptau;
444  p *= -1.0;
445 
446  dvector pm(Ia.colPivHouseholderQr().solve(p));
447 
448  root->dvo = pm.head(3);
449  root->dw = pm.tail(3);
450 
451  } else {
452  root->dvo.setZero();
453  root->dw.setZero();
454  }
455 
456  int n = traverse.numLinks();
457  for(int i=1; i < n; ++i){
458  Link* link = traverse[i];
459  Link* parent = link->parent;
460  if(link->jointType != Link::FIXED_JOINT){
461  link->ddq = (link->uu - (link->hhv.dot(parent->dvo) + link->hhw.dot(parent->dw))) / link->dd;
462  link->dvo = parent->dvo + link->cv + link->sv * link->ddq;
463  link->dw = parent->dw + link->cw + link->sw * link->ddq;
464  }else{
465  link->ddq = 0.0;
466  link->dvo = parent->dvo;
467  link->dw = parent->dw;
468  }
469  }
470 }
471 
472 
474 {
475  int n = body->numSensors(Sensor::FORCE);
476  for(int i=0; i < n; ++i){
477  updateForceSensor(body->sensor<ForceSensor>(i));
478  }
479 }
480 
481 
483 {
484  Link* link = sensor->link;
485 
486  // | f | = | Ivv trans(Iwv) | * | dvo | + | pf |
487  // | tau | | Iwv Iww | | dw | | ptau |
488 
489  Vector3 f (-(link->Ivv * link->dvo + link->Iwv.transpose() * link->dw + link->pf));
490  Vector3 tau(-(link->Iwv * link->dvo + link->Iww * link->dw + link->ptau));
491 
492  Matrix33 sensorR(link->R * sensor->localR);
493  Vector3 fs(sensorR.transpose() * f);
494  Vector3 sensorPos(link->p + link->R * sensor->localPos);
495  Vector3 ts(sensorR.transpose() * (tau - sensorPos.cross(f)));
496 
497  sensor->f = fs;
498  sensor->tau = ts;
499 
500  if(debugMode){
501  cout << "sensor " << sensor->name << ": ";
502  cout << "f = " << f;
503  cout << "R = " << sensorR;
504  cout << "sensor->f = " << sensor->f;
505  cout << endl;
506  }
507 }
508 
509 
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::ForwardDynamicsABM::calcMotionWithEulerMethod
void calcMotionWithEulerMethod()
Definition: ForwardDynamicsABM.cpp:101
hrp::ForwardDynamicsABM::updateForceSensor
void updateForceSensor(ForceSensor *sensor)
Definition: ForwardDynamicsABM.cpp:482
hrp::ForwardDynamicsABM::vo
Vector3 vo
Definition: ForwardDynamicsABM.h:80
hrp::rodrigues
Matrix33 rodrigues(const Vector3 &axis, double q)
Definition: Eigen3d.h:27
hrp::ForwardDynamicsABM::calcABMPhase2
void calcABMPhase2()
Definition: ForwardDynamicsABM.cpp:305
hrp::ForwardDynamicsABM::q0
std::vector< double > q0
Definition: ForwardDynamicsABM.h:77
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
hrp::ForwardDynamicsABM::calcABMPhase1
void calcABMPhase1()
Definition: ForwardDynamicsABM.cpp:221
Sensor.h
hrp::dvector
Eigen::VectorXd dvector
Definition: EigenTypes.h:14
hrp::ForwardDynamicsABM::dq0
std::vector< double > dq0
Definition: ForwardDynamicsABM.h:78
hrp::ForwardDynamicsABM::ddq
std::vector< double > ddq
Definition: ForwardDynamicsABM.h:85
hrp::ForwardDynamicsABM::calcABMPhase2Part2
void calcABMPhase2Part2()
Definition: ForwardDynamicsABM.cpp:397
rootAttitudeNormalizationEnabled
static const bool rootAttitudeNormalizationEnabled
Definition: ForwardDynamicsABM.cpp:27
hrp::ForwardDynamicsABM::w0
Vector3 w0
Definition: ForwardDynamicsABM.h:76
autoplay.n
n
Definition: autoplay.py:12
hrp::ForwardDynamicsABM::R0
Matrix33 R0
Definition: ForwardDynamicsABM.h:74
hrp
Definition: ColdetModel.h:28
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::ForwardDynamicsABM::~ForwardDynamicsABM
~ForwardDynamicsABM()
Definition: ForwardDynamicsABM.cpp:41
hrp::ForwardDynamicsABM::calcABMFirstHalf
void calcABMFirstHalf()
Definition: ForwardDynamicsABM.cpp:54
hrp::Sensor::link
Link * link
Definition: hrplib/hrpModel/Sensor.h:59
hrp::ForwardDynamics::integrationMode
enum hrp::ForwardDynamics::@2 integrationMode
hrp::ForceSensor::tau
Vector3 tau
Definition: hrplib/hrpModel/Sensor.h:75
hrp::ForwardDynamicsABM::dvo
Vector3 dvo
Definition: ForwardDynamicsABM.h:82
hrp::Vector3
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
hrp::ForwardDynamicsABM::calcABMLastHalf
void calcABMLastHalf()
Definition: ForwardDynamicsABM.cpp:61
hrp::ForwardDynamics::timeStep
double timeStep
Definition: ForwardDynamics.h:73
hrp::ForwardDynamicsABM::updateForceSensors
void updateForceSensors()
Definition: ForwardDynamicsABM.cpp:473
hrp::Sensor::name
std::string name
Definition: hrplib/hrpModel/Sensor.h:56
hrp::Sensor::localPos
Vector3 localPos
Definition: hrplib/hrpModel/Sensor.h:61
hrp::ForwardDynamics::RUNGEKUTTA_METHOD
@ RUNGEKUTTA_METHOD
Definition: ForwardDynamics.h:76
hrp::ForwardDynamicsABM::calcNextState
virtual void calcNextState()
Definition: ForwardDynamicsABM.cpp:68
hrp::hat
Matrix33 hat(const Vector3 &c)
Definition: Eigen3d.h:80
hrp::dmatrix
Eigen::MatrixXd dmatrix
Definition: EigenTypes.h:13
hrp::ForwardDynamics::sensorsEnabled
bool sensorsEnabled
Definition: ForwardDynamics.h:74
hrp::ForwardDynamics::g
Vector3 g
Definition: ForwardDynamics.h:72
LinkTraverse.h
The header file of the LinkTraverse class.
hrp::ForwardDynamicsABM::integrateRungeKuttaOneStep
void integrateRungeKuttaOneStep(double r, double dt)
Definition: ForwardDynamicsABM.cpp:131
hrp::LinkTraverse::numLinks
unsigned int numLinks() const
Definition: LinkTraverse.h:40
hrp::ForwardDynamicsABM::initialize
virtual void initialize()
Definition: ForwardDynamicsABM.cpp:47
hrp::ForwardDynamicsABM::vo0
Vector3 vo0
Definition: ForwardDynamicsABM.h:75
hrp::ForwardDynamicsABM::w
Vector3 w
Definition: ForwardDynamicsABM.h:81
hrp::BodyPtr
boost::shared_ptr< Body > BodyPtr
Definition: Body.h:315
hrp::ForwardDynamics::EULER_METHOD
@ EULER_METHOD
Definition: ForwardDynamics.h:76
hrp::ForwardDynamicsABM::calcABMPhase2Part1
void calcABMPhase2Part1()
Definition: ForwardDynamicsABM.cpp:358
debugMode
static const bool debugMode
Definition: ForwardDynamicsABM.cpp:26
hrp::Sensor::FORCE
@ FORCE
Definition: hrplib/hrpModel/Sensor.h:33
hrp::ForwardDynamicsABM::calcABMPhase3
void calcABMPhase3()
Definition: ForwardDynamicsABM.cpp:427
Body.h
hrp::ForwardDynamicsABM::dq
std::vector< double > dq
Definition: ForwardDynamicsABM.h:84
EigenTypes.h
hrp::LinkTraverse
Definition: LinkTraverse.h:29
hrp::ForwardDynamicsABM::p0
Vector3 p0
Definition: ForwardDynamicsABM.h:73
hrp::Matrix33
Eigen::Matrix3d Matrix33
Definition: EigenTypes.h:12
hrp::ForwardDynamicsABM::calcMotionWithRungeKuttaMethod
void calcMotionWithRungeKuttaMethod()
Definition: ForwardDynamicsABM.cpp:161
hrp::ForwardDynamics
Definition: ForwardDynamics.h:35
hrp::ForwardDynamicsABM::dw
Vector3 dw
Definition: ForwardDynamicsABM.h:83
hrp::ForwardDynamicsABM::ForwardDynamicsABM
ForwardDynamicsABM(BodyPtr body)
Definition: ForwardDynamicsABM.cpp:30
ForwardDynamicsABM.h


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