Rigid Body Dynamics Library
Dynamics.cc
Go to the documentation of this file.
1 /*
2  * RBDL - Rigid Body Dynamics Library
3  * Copyright (c) 2011-2018 Martin Felis <martin@fysx.org>
4  *
5  * Licensed under the zlib license. See LICENSE for more details.
6  */
7 
8 #include <iostream>
9 #include <limits>
10 #include <assert.h>
11 #include <string.h>
12 
13 #include "rbdl/rbdl_mathutils.h"
14 #include "rbdl/Logging.h"
15 
16 #include "rbdl/Model.h"
17 #include "rbdl/Joint.h"
18 #include "rbdl/Body.h"
19 #include "rbdl/Dynamics.h"
20 #include "rbdl/Kinematics.h"
21 
22 namespace RigidBodyDynamics {
23 
24 using namespace Math;
25 
26 RBDL_DLLAPI void InverseDynamics (
27  Model &model,
28  const VectorNd &Q,
29  const VectorNd &QDot,
30  const VectorNd &QDDot,
31  VectorNd &Tau,
32  std::vector<SpatialVector> *f_ext) {
33  LOG << "-------- " << __func__ << " --------" << std::endl;
34 
35  // Reset the velocity of the root body
36  model.v[0].setZero();
37  model.a[0].set (0., 0., 0., -model.gravity[0], -model.gravity[1], -model.gravity[2]);
38 
39  for (unsigned int i = 1; i < model.mBodies.size(); i++) {
40  unsigned int q_index = model.mJoints[i].q_index;
41  unsigned int lambda = model.lambda[i];
42 
43  jcalc (model, i, Q, QDot);
44 
45  model.v[i] = model.X_lambda[i].apply(model.v[lambda]) + model.v_J[i];
46  model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]);
47 
48  if(model.mJoints[i].mJointType != JointTypeCustom){
49  if (model.mJoints[i].mDoFCount == 1) {
50  model.a[i] = model.X_lambda[i].apply(model.a[lambda])
51  + model.c[i]
52  + model.S[i] * QDDot[q_index];
53  } else if (model.mJoints[i].mDoFCount == 3) {
54  model.a[i] = model.X_lambda[i].apply(model.a[lambda])
55  + model.c[i]
56  + model.multdof3_S[i] * Vector3d (QDDot[q_index],
57  QDDot[q_index + 1],
58  QDDot[q_index + 2]);
59  }
60  }else if(model.mJoints[i].mJointType == JointTypeCustom){
61  unsigned int k = model.mJoints[i].custom_joint_index;
62  VectorNd customJointQDDot(model.mCustomJoints[k]->mDoFCount);
63  for(unsigned z = 0; z < model.mCustomJoints[k]->mDoFCount; ++z){
64  customJointQDDot[z] = QDDot[q_index+z];
65  }
66  model.a[i] = model.X_lambda[i].apply(model.a[lambda])
67  + model.c[i]
68  + model.mCustomJoints[k]->S * customJointQDDot;
69  }
70 
71  if (!model.mBodies[i].mIsVirtual) {
72  model.f[i] = model.I[i] * model.a[i] + crossf(model.v[i],model.I[i] * model.v[i]);
73  } else {
74  model.f[i].setZero();
75  }
76  }
77 
78  if (f_ext != NULL) {
79  for (unsigned int i = 1; i < model.mBodies.size(); i++) {
80  unsigned int lambda = model.lambda[i];
81  model.X_base[i] = model.X_lambda[i] * model.X_base[lambda];
82  model.f[i] -= model.X_base[i].toMatrixAdjoint() * (*f_ext)[i];
83  }
84  }
85 
86  for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) {
87  if(model.mJoints[i].mJointType != JointTypeCustom){
88  if (model.mJoints[i].mDoFCount == 1) {
89  Tau[model.mJoints[i].q_index] = model.S[i].dot(model.f[i]);
90  } else if (model.mJoints[i].mDoFCount == 3) {
91  Tau.block<3,1>(model.mJoints[i].q_index, 0)
92  = model.multdof3_S[i].transpose() * model.f[i];
93  }
94  } else if (model.mJoints[i].mJointType == JointTypeCustom) {
95  unsigned int k = model.mJoints[i].custom_joint_index;
96  Tau.block(model.mJoints[i].q_index,0,
97  model.mCustomJoints[k]->mDoFCount, 1)
98  = model.mCustomJoints[k]->S.transpose() * model.f[i];
99  }
100 
101  if (model.lambda[i] != 0) {
102  model.f[model.lambda[i]] = model.f[model.lambda[i]] + model.X_lambda[i].applyTranspose(model.f[i]);
103  }
104  }
105 }
106 
107 RBDL_DLLAPI void NonlinearEffects (
108  Model &model,
109  const VectorNd &Q,
110  const VectorNd &QDot,
111  VectorNd &Tau,
112  std::vector<Math::SpatialVector> *f_ext) {
113  LOG << "-------- " << __func__ << " --------" << std::endl;
114 
115  SpatialVector spatial_gravity (0., 0., 0., -model.gravity[0], -model.gravity[1], -model.gravity[2]);
116 
117  // Reset the velocity of the root body
118  model.v[0].setZero();
119  model.a[0] = spatial_gravity;
120 
121  for (unsigned int i = 1; i < model.mJointUpdateOrder.size(); i++) {
122  jcalc (model, model.mJointUpdateOrder[i], Q, QDot);
123  }
124 
125  for (unsigned int i = 1; i < model.mBodies.size(); i++) {
126  if (model.lambda[i] == 0) {
127  model.v[i] = model.v_J[i];
128  model.a[i] = model.X_lambda[i].apply(spatial_gravity);
129  } else {
130  model.v[i] = model.X_lambda[i].apply(model.v[model.lambda[i]]) + model.v_J[i];
131  model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]);
132  model.a[i] = model.X_lambda[i].apply(model.a[model.lambda[i]]) + model.c[i];
133  }
134 
135  if (!model.mBodies[i].mIsVirtual) {
136  model.f[i] = model.I[i] * model.a[i] + crossf(model.v[i],model.I[i] * model.v[i]);
137  if (f_ext != NULL && (*f_ext)[i] != SpatialVector::Zero()) {
138  model.f[i] -= model.X_base[i].toMatrixAdjoint() * (*f_ext)[i];
139  }
140  } else {
141  model.f[i].setZero();
142  }
143  }
144 
145  for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) {
146  if(model.mJoints[i].mJointType != JointTypeCustom){
147  if (model.mJoints[i].mDoFCount == 1) {
148  Tau[model.mJoints[i].q_index]
149  = model.S[i].dot(model.f[i]);
150  } else if (model.mJoints[i].mDoFCount == 3) {
151  Tau.block<3,1>(model.mJoints[i].q_index, 0)
152  = model.multdof3_S[i].transpose() * model.f[i];
153  }
154  } else if(model.mJoints[i].mJointType == JointTypeCustom) {
155  unsigned int k = model.mJoints[i].custom_joint_index;
156  Tau.block(model.mJoints[i].q_index,0,
157  model.mCustomJoints[k]->mDoFCount, 1)
158  = model.mCustomJoints[k]->S.transpose() * model.f[i];
159  }
160 
161  if (model.lambda[i] != 0) {
162  model.f[model.lambda[i]] = model.f[model.lambda[i]] + model.X_lambda[i].applyTranspose(model.f[i]);
163  }
164  }
165 }
166 
167 RBDL_DLLAPI void CompositeRigidBodyAlgorithm (
168  Model& model,
169  const VectorNd &Q,
170  MatrixNd &H,
171  bool update_kinematics) {
172  LOG << "-------- " << __func__ << " --------" << std::endl;
173 
174  assert (H.rows() == model.dof_count && H.cols() == model.dof_count);
175 
176  for (unsigned int i = 1; i < model.mBodies.size(); i++) {
177  if (update_kinematics) {
178  jcalc_X_lambda_S (model, i, Q);
179  }
180  model.Ic[i] = model.I[i];
181  }
182 
183  for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) {
184  if (model.lambda[i] != 0) {
185  model.Ic[model.lambda[i]] = model.Ic[model.lambda[i]] + model.X_lambda[i].applyTranspose(model.Ic[i]);
186  }
187 
188  unsigned int dof_index_i = model.mJoints[i].q_index;
189 
190  if (model.mJoints[i].mDoFCount == 1
191  && model.mJoints[i].mJointType != JointTypeCustom) {
192 
193  SpatialVector F = model.Ic[i] * model.S[i];
194  H(dof_index_i, dof_index_i) = model.S[i].dot(F);
195 
196  unsigned int j = i;
197  unsigned int dof_index_j = dof_index_i;
198 
199  while (model.lambda[j] != 0) {
200  F = model.X_lambda[j].applyTranspose(F);
201  j = model.lambda[j];
202  dof_index_j = model.mJoints[j].q_index;
203 
204  if(model.mJoints[j].mJointType != JointTypeCustom) {
205  if (model.mJoints[j].mDoFCount == 1) {
206  H(dof_index_i,dof_index_j) = F.dot(model.S[j]);
207  H(dof_index_j,dof_index_i) = H(dof_index_i,dof_index_j);
208  } else if (model.mJoints[j].mDoFCount == 3) {
209  Vector3d H_temp2 =
210  (F.transpose() * model.multdof3_S[j]).transpose();
211  LOG << F.transpose() << std::endl
212  << model.multdof3_S[j] << std::endl;
213  LOG << H_temp2.transpose() << std::endl;
214 
215  H.block<1,3>(dof_index_i,dof_index_j) = H_temp2.transpose();
216  H.block<3,1>(dof_index_j,dof_index_i) = H_temp2;
217  }
218  } else if (model.mJoints[j].mJointType == JointTypeCustom){
219  unsigned int k = model.mJoints[j].custom_joint_index;
220  unsigned int dof = model.mCustomJoints[k]->mDoFCount;
221  VectorNd H_temp2 =
222  (F.transpose() * model.mCustomJoints[k]->S).transpose();
223 
224  LOG << F.transpose()
225  << std::endl
226  << model.mCustomJoints[j]->S << std::endl;
227 
228  LOG << H_temp2.transpose() << std::endl;
229 
230  H.block(dof_index_i,dof_index_j,1,dof) = H_temp2.transpose();
231  H.block(dof_index_j,dof_index_i,dof,1) = H_temp2;
232  }
233  }
234  } else if (model.mJoints[i].mDoFCount == 3
235  && model.mJoints[i].mJointType != JointTypeCustom) {
236  Matrix63 F_63 = model.Ic[i].toMatrix() * model.multdof3_S[i];
237  H.block<3,3>(dof_index_i, dof_index_i) = model.multdof3_S[i].transpose() * F_63;
238 
239  unsigned int j = i;
240  unsigned int dof_index_j = dof_index_i;
241 
242  while (model.lambda[j] != 0) {
243  F_63 = model.X_lambda[j].toMatrixTranspose() * (F_63);
244  j = model.lambda[j];
245  dof_index_j = model.mJoints[j].q_index;
246 
247  if(model.mJoints[j].mJointType != JointTypeCustom){
248  if (model.mJoints[j].mDoFCount == 1) {
249  Vector3d H_temp2 = F_63.transpose() * (model.S[j]);
250 
251  H.block<3,1>(dof_index_i,dof_index_j) = H_temp2;
252  H.block<1,3>(dof_index_j,dof_index_i) = H_temp2.transpose();
253  } else if (model.mJoints[j].mDoFCount == 3) {
254  Matrix3d H_temp2 = F_63.transpose() * (model.multdof3_S[j]);
255 
256  H.block<3,3>(dof_index_i,dof_index_j) = H_temp2;
257  H.block<3,3>(dof_index_j,dof_index_i) = H_temp2.transpose();
258  }
259  } else if (model.mJoints[j].mJointType == JointTypeCustom){
260  unsigned int k = model.mJoints[j].custom_joint_index;
261  unsigned int dof = model.mCustomJoints[k]->mDoFCount;
262 
263  MatrixNd H_temp2 = F_63.transpose() * (model.mCustomJoints[k]->S);
264 
265  H.block(dof_index_i,dof_index_j,3,dof) = H_temp2;
266  H.block(dof_index_j,dof_index_i,dof,3) = H_temp2.transpose();
267  }
268  }
269  } else if (model.mJoints[i].mJointType == JointTypeCustom) {
270  unsigned int kI = model.mJoints[i].custom_joint_index;
271  unsigned int dofI = model.mCustomJoints[kI]->mDoFCount;
272 
273  MatrixNd F_Nd = model.Ic[i].toMatrix()
274  * model.mCustomJoints[kI]->S;
275 
276  H.block(dof_index_i, dof_index_i,dofI,dofI)
277  = model.mCustomJoints[kI]->S.transpose() * F_Nd;
278 
279  unsigned int j = i;
280  unsigned int dof_index_j = dof_index_i;
281 
282  while (model.lambda[j] != 0) {
283  F_Nd = model.X_lambda[j].toMatrixTranspose() * (F_Nd);
284  j = model.lambda[j];
285  dof_index_j = model.mJoints[j].q_index;
286 
287  if(model.mJoints[j].mJointType != JointTypeCustom){
288  if (model.mJoints[j].mDoFCount == 1) {
289  MatrixNd H_temp2 = F_Nd.transpose() * (model.S[j]);
290  H.block( dof_index_i, dof_index_j,
291  H_temp2.rows(),H_temp2.cols()) = H_temp2;
292  H.block(dof_index_j,dof_index_i,
293  H_temp2.cols(),H_temp2.rows()) = H_temp2.transpose();
294  } else if (model.mJoints[j].mDoFCount == 3) {
295  MatrixNd H_temp2 = F_Nd.transpose() * (model.multdof3_S[j]);
296  H.block(dof_index_i, dof_index_j,
297  H_temp2.rows(),H_temp2.cols()) = H_temp2;
298  H.block(dof_index_j, dof_index_i,
299  H_temp2.cols(),H_temp2.rows()) = H_temp2.transpose();
300  }
301  } else if (model.mJoints[j].mJointType == JointTypeCustom){
302  unsigned int k = model.mJoints[j].custom_joint_index;
303  unsigned int dof = model.mCustomJoints[k]->mDoFCount;
304 
305  MatrixNd H_temp2 = F_Nd.transpose() * (model.mCustomJoints[k]->S);
306 
307  H.block(dof_index_i,dof_index_j,3,dof) = H_temp2;
308  H.block(dof_index_j,dof_index_i,dof,3) = H_temp2.transpose();
309  }
310  }
311  }
312  }
313 }
314 
315 RBDL_DLLAPI void ForwardDynamics (
316  Model &model,
317  const VectorNd &Q,
318  const VectorNd &QDot,
319  const VectorNd &Tau,
320  VectorNd &QDDot,
321  std::vector<SpatialVector> *f_ext) {
322  LOG << "-------- " << __func__ << " --------" << std::endl;
323 
324  SpatialVector spatial_gravity (0., 0., 0., model.gravity[0], model.gravity[1], model.gravity[2]);
325 
326  unsigned int i = 0;
327 
328  LOG << "Q = " << Q.transpose() << std::endl;
329  LOG << "QDot = " << QDot.transpose() << std::endl;
330  LOG << "Tau = " << Tau.transpose() << std::endl;
331  LOG << "---" << std::endl;
332 
333  // Reset the velocity of the root body
334  model.v[0].setZero();
335 
336  for (i = 1; i < model.mBodies.size(); i++) {
337  unsigned int lambda = model.lambda[i];
338 
339  jcalc (model, i, Q, QDot);
340 
341  if (lambda != 0)
342  model.X_base[i] = model.X_lambda[i] * model.X_base[lambda];
343  else
344  model.X_base[i] = model.X_lambda[i];
345 
346  model.v[i] = model.X_lambda[i].apply( model.v[lambda]) + model.v_J[i];
347 
348  /*
349  LOG << "X_J (" << i << "):" << std::endl << X_J << std::endl;
350  LOG << "v_J (" << i << "):" << std::endl << v_J << std::endl;
351  LOG << "v_lambda" << i << ":" << std::endl << model.v.at(lambda) << std::endl;
352  LOG << "X_base (" << i << "):" << std::endl << model.X_base[i] << std::endl;
353  LOG << "X_lambda (" << i << "):" << std::endl << model.X_lambda[i] << std::endl;
354  LOG << "SpatialVelocity (" << i << "): " << model.v[i] << std::endl;
355  */
356  model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]);
357  model.I[i].setSpatialMatrix (model.IA[i]);
358 
359  model.pA[i] = crossf(model.v[i],model.I[i] * model.v[i]);
360 
361  if (f_ext != NULL && (*f_ext)[i] != SpatialVector::Zero()) {
362  LOG << "External force (" << i << ") = " << model.X_base[i].toMatrixAdjoint() * (*f_ext)[i] << std::endl;
363  model.pA[i] -= model.X_base[i].toMatrixAdjoint() * (*f_ext)[i];
364  }
365  }
366 
367  // ClearLogOutput();
368 
369  LOG << "--- first loop ---" << std::endl;
370 
371  for (i = model.mBodies.size() - 1; i > 0; i--) {
372  unsigned int q_index = model.mJoints[i].q_index;
373 
374  if (model.mJoints[i].mDoFCount == 1
375  && model.mJoints[i].mJointType != JointTypeCustom) {
376 
377  model.U[i] = model.IA[i] * model.S[i];
378  model.d[i] = model.S[i].dot(model.U[i]);
379  model.u[i] = Tau[q_index] - model.S[i].dot(model.pA[i]);
380  // LOG << "u[" << i << "] = " << model.u[i] << std::endl;
381 
382  unsigned int lambda = model.lambda[i];
383  if (lambda != 0) {
384  SpatialMatrix Ia = model.IA[i]
385  - model.U[i]
386  * (model.U[i] / model.d[i]).transpose();
387 
388  SpatialVector pa = model.pA[i]
389  + Ia * model.c[i]
390  + model.U[i] * model.u[i] / model.d[i];
391 
392 #ifdef EIGEN_CORE_H
393  model.IA[lambda].noalias()
394  += model.X_lambda[i].toMatrixTranspose()
395  * Ia * model.X_lambda[i].toMatrix();
396  model.pA[lambda].noalias()
397  += model.X_lambda[i].applyTranspose(pa);
398 #else
399  model.IA[lambda]
400  += model.X_lambda[i].toMatrixTranspose()
401  * Ia * model.X_lambda[i].toMatrix();
402 
403  model.pA[lambda] += model.X_lambda[i].applyTranspose(pa);
404 #endif
405  LOG << "pA[" << lambda << "] = "
406  << model.pA[lambda].transpose() << std::endl;
407  }
408  } else if (model.mJoints[i].mDoFCount == 3
409  && model.mJoints[i].mJointType != JointTypeCustom) {
410  model.multdof3_U[i] = model.IA[i] * model.multdof3_S[i];
411 #ifdef EIGEN_CORE_H
412  model.multdof3_Dinv[i] = (model.multdof3_S[i].transpose()
413  * model.multdof3_U[i]).inverse().eval();
414 #else
415  model.multdof3_Dinv[i] = (model.multdof3_S[i].transpose()
416  * model.multdof3_U[i]).inverse();
417 #endif
418  Vector3d tau_temp(Tau.block(q_index,0,3,1));
419  model.multdof3_u[i] = tau_temp
420  - model.multdof3_S[i].transpose() * model.pA[i];
421 
422  // LOG << "multdof3_u[" << i << "] = "
423  // << model.multdof3_u[i].transpose() << std::endl;
424  unsigned int lambda = model.lambda[i];
425  if (lambda != 0) {
426  SpatialMatrix Ia = model.IA[i]
427  - model.multdof3_U[i]
428  * model.multdof3_Dinv[i]
429  * model.multdof3_U[i].transpose();
430  SpatialVector pa = model.pA[i]
431  + Ia
432  * model.c[i]
433  + model.multdof3_U[i]
434  * model.multdof3_Dinv[i]
435  * model.multdof3_u[i];
436 #ifdef EIGEN_CORE_H
437  model.IA[lambda].noalias()
438  += model.X_lambda[i].toMatrixTranspose()
439  * Ia
440  * model.X_lambda[i].toMatrix();
441 
442  model.pA[lambda].noalias()
443  += model.X_lambda[i].applyTranspose(pa);
444 #else
445  model.IA[lambda]
446  += model.X_lambda[i].toMatrixTranspose()
447  * Ia
448  * model.X_lambda[i].toMatrix();
449 
450  model.pA[lambda] += model.X_lambda[i].applyTranspose(pa);
451 #endif
452  LOG << "pA[" << lambda << "] = "
453  << model.pA[lambda].transpose()
454  << std::endl;
455  }
456  } else if (model.mJoints[i].mJointType == JointTypeCustom) {
457  unsigned int kI = model.mJoints[i].custom_joint_index;
458  unsigned int dofI = model.mCustomJoints[kI]->mDoFCount;
459  model.mCustomJoints[kI]->U =
460  model.IA[i] * model.mCustomJoints[kI]->S;
461 
462 #ifdef EIGEN_CORE_H
463  model.mCustomJoints[kI]->Dinv
464  = (model.mCustomJoints[kI]->S.transpose()
465  * model.mCustomJoints[kI]->U).inverse().eval();
466 #else
467  model.mCustomJoints[kI]->Dinv
468  = (model.mCustomJoints[kI]->S.transpose()
469  * model.mCustomJoints[kI]->U).inverse();
470 #endif
471  VectorNd tau_temp(Tau.block(q_index,0,dofI,1));
472  model.mCustomJoints[kI]->u = tau_temp
473  - model.mCustomJoints[kI]->S.transpose() * model.pA[i];
474 
475  // LOG << "multdof3_u[" << i << "] = "
476  // << model.multdof3_u[i].transpose() << std::endl;
477  unsigned int lambda = model.lambda[i];
478  if (lambda != 0) {
479  SpatialMatrix Ia = model.IA[i]
480  - (model.mCustomJoints[kI]->U
481  * model.mCustomJoints[kI]->Dinv
482  * model.mCustomJoints[kI]->U.transpose());
483  SpatialVector pa = model.pA[i]
484  + Ia * model.c[i]
485  + (model.mCustomJoints[kI]->U
486  * model.mCustomJoints[kI]->Dinv
487  * model.mCustomJoints[kI]->u);
488 
489 #ifdef EIGEN_CORE_H
490  model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose()
491  * Ia
492  * model.X_lambda[i].toMatrix();
493  model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa);
494 #else
495  model.IA[lambda] += model.X_lambda[i].toMatrixTranspose()
496  * Ia
497  * model.X_lambda[i].toMatrix();
498  model.pA[lambda] += model.X_lambda[i].applyTranspose(pa);
499 #endif
500  LOG << "pA[" << lambda << "] = "
501  << model.pA[lambda].transpose()
502  << std::endl;
503  }
504  }
505  }
506 
507  // ClearLogOutput();
508 
509  model.a[0] = spatial_gravity * -1.;
510 
511  for (i = 1; i < model.mBodies.size(); i++) {
512  unsigned int q_index = model.mJoints[i].q_index;
513  unsigned int lambda = model.lambda[i];
514  SpatialTransform X_lambda = model.X_lambda[i];
515 
516  model.a[i] = X_lambda.apply(model.a[lambda]) + model.c[i];
517  LOG << "a'[" << i << "] = " << model.a[i].transpose() << std::endl;
518 
519  if (model.mJoints[i].mDoFCount == 1
520  && model.mJoints[i].mJointType != JointTypeCustom) {
521  QDDot[q_index] = (1./model.d[i]) * (model.u[i] - model.U[i].dot(model.a[i]));
522  model.a[i] = model.a[i] + model.S[i] * QDDot[q_index];
523  } else if (model.mJoints[i].mDoFCount == 3
524  && model.mJoints[i].mJointType != JointTypeCustom) {
525  Vector3d qdd_temp = model.multdof3_Dinv[i] * (model.multdof3_u[i] - model.multdof3_U[i].transpose() * model.a[i]);
526  QDDot[q_index] = qdd_temp[0];
527  QDDot[q_index + 1] = qdd_temp[1];
528  QDDot[q_index + 2] = qdd_temp[2];
529  model.a[i] = model.a[i] + model.multdof3_S[i] * qdd_temp;
530  } else if (model.mJoints[i].mJointType == JointTypeCustom) {
531  unsigned int kI = model.mJoints[i].custom_joint_index;
532  unsigned int dofI=model.mCustomJoints[kI]->mDoFCount;
533 
534  VectorNd qdd_temp = model.mCustomJoints[kI]->Dinv
535  * ( model.mCustomJoints[kI]->u
536  - model.mCustomJoints[kI]->U.transpose()
537  * model.a[i]);
538 
539  for(int z=0; z<dofI; ++z){
540  QDDot[q_index+z] = qdd_temp[z];
541  }
542 
543  model.a[i] = model.a[i]
544  + model.mCustomJoints[kI]->S * qdd_temp;
545  }
546  }
547 
548  LOG << "QDDot = " << QDDot.transpose() << std::endl;
549 }
550 
551 RBDL_DLLAPI void ForwardDynamicsLagrangian (
552  Model &model,
553  const VectorNd &Q,
554  const VectorNd &QDot,
555  const VectorNd &Tau,
556  VectorNd &QDDot,
557  Math::LinearSolver linear_solver,
558  std::vector<SpatialVector> *f_ext,
559  Math::MatrixNd *H,
560  Math::VectorNd *C) {
561  LOG << "-------- " << __func__ << " --------" << std::endl;
562 
563  bool free_H = false;
564  bool free_C = false;
565 
566  if (H == NULL) {
567  H = new MatrixNd (MatrixNd::Zero(model.dof_count, model.dof_count));
568  free_H = true;
569  }
570 
571  if (C == NULL) {
572  C = new VectorNd (VectorNd::Zero(model.dof_count));
573  free_C = true;
574  }
575 
576  // we set QDDot to zero to compute C properly with the InverseDynamics
577  // method.
578  QDDot.setZero();
579 
580  InverseDynamics (model, Q, QDot, QDDot, (*C), f_ext);
581  CompositeRigidBodyAlgorithm (model, Q, *H, false);
582 
583  LOG << "A = " << std::endl << *H << std::endl;
584  LOG << "b = " << std::endl << *C * -1. + Tau << std::endl;
585 
586 #ifndef RBDL_USE_SIMPLE_MATH
587  switch (linear_solver) {
588  case (LinearSolverPartialPivLU) :
589  QDDot = H->partialPivLu().solve (*C * -1. + Tau);
590  break;
592  QDDot = H->colPivHouseholderQr().solve (*C * -1. + Tau);
593  break;
595  QDDot = H->householderQr().solve (*C * -1. + Tau);
596  break;
597  case (LinearSolverLLT) :
598  QDDot = H->llt().solve (*C * -1. + Tau);
599  break;
600  default:
601  LOG << "Error: Invalid linear solver: " << linear_solver << std::endl;
602  assert (0);
603  break;
604  }
605 #else
606  bool solve_successful = LinSolveGaussElimPivot (*H, *C * -1. + Tau, QDDot);
607  assert (solve_successful);
608 #endif
609 
610  if (free_C) {
611  delete C;
612  }
613 
614  if (free_H) {
615  delete H;
616  }
617 
618  LOG << "x = " << QDDot << std::endl;
619 }
620 
621 RBDL_DLLAPI void CalcMInvTimesTau ( Model &model,
622  const VectorNd &Q,
623  const VectorNd &Tau,
624  VectorNd &QDDot,
625  bool update_kinematics) {
626 
627  LOG << "Q = " << Q.transpose() << std::endl;
628  LOG << "---" << std::endl;
629 
630  // Reset the velocity of the root body
631  model.v[0].setZero();
632  model.a[0].setZero();
633 
634  if (update_kinematics) {
635  for (unsigned int i = 1; i < model.mBodies.size(); i++) {
636  jcalc_X_lambda_S (model, model.mJointUpdateOrder[i], Q);
637 
638  model.v_J[i].setZero();
639  model.v[i].setZero();
640  model.c[i].setZero();
641  model.pA[i].setZero();
642  model.I[i].setSpatialMatrix (model.IA[i]);
643  }
644  }
645 
646  for (unsigned int i = 1; i < model.mBodies.size(); i++) {
647  model.pA[i].setZero();
648  }
649 
650  // ClearLogOutput();
651 
652  if (update_kinematics) {
653  // Compute Articulate Body Inertias
654  for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) {
655  unsigned int q_index = model.mJoints[i].q_index;
656 
657  if (model.mJoints[i].mDoFCount == 1
658  && model.mJoints[i].mJointType != JointTypeCustom) {
659  model.U[i] = model.IA[i] * model.S[i];
660  model.d[i] = model.S[i].dot(model.U[i]);
661  // LOG << "u[" << i << "] = " << model.u[i] << std::endl;
662  unsigned int lambda = model.lambda[i];
663 
664  if (lambda != 0) {
665  SpatialMatrix Ia = model.IA[i] -
666  model.U[i] * (model.U[i] / model.d[i]).transpose();
667 #ifdef EIGEN_CORE_H
668  model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose()
669  * Ia
670  * model.X_lambda[i].toMatrix();
671 #else
672  model.IA[lambda] += model.X_lambda[i].toMatrixTranspose()
673  * Ia
674  * model.X_lambda[i].toMatrix();
675 #endif
676  }
677  } else if (model.mJoints[i].mDoFCount == 3
678  && model.mJoints[i].mJointType != JointTypeCustom) {
679 
680  model.multdof3_U[i] = model.IA[i] * model.multdof3_S[i];
681 
682 #ifdef EIGEN_CORE_H
683  model.multdof3_Dinv[i] =
684  (model.multdof3_S[i].transpose()*model.multdof3_U[i]).inverse().eval();
685 #else
686  model.multdof3_Dinv[i] =
687  (model.multdof3_S[i].transpose() * model.multdof3_U[i]).inverse();
688 #endif
689  // LOG << "mCustomJoints[kI]->u[" << i << "] = "
690  //<< model.mCustomJoints[kI]->u[i].transpose() << std::endl;
691 
692  unsigned int lambda = model.lambda[i];
693 
694  if (lambda != 0) {
695  SpatialMatrix Ia = model.IA[i]
696  - ( model.multdof3_U[i]
697  * model.multdof3_Dinv[i]
698  * model.multdof3_U[i].transpose());
699 #ifdef EIGEN_CORE_H
700  model.IA[lambda].noalias() +=
701  model.X_lambda[i].toMatrixTranspose()
702  * Ia
703  * model.X_lambda[i].toMatrix();
704 #else
705  model.IA[lambda] +=
706  model.X_lambda[i].toMatrixTranspose()
707  * Ia * model.X_lambda[i].toMatrix();
708 #endif
709  }
710  } else if (model.mJoints[i].mJointType == JointTypeCustom) {
711  unsigned int kI = model.mJoints[i].custom_joint_index;
712  unsigned int dofI = model.mCustomJoints[kI]->mDoFCount;
713  model.mCustomJoints[kI]->U = model.IA[i] * model.mCustomJoints[kI]->S;
714 
715 #ifdef EIGEN_CORE_H
716  model.mCustomJoints[kI]->Dinv = (model.mCustomJoints[kI]->S.transpose()
717  * model.mCustomJoints[kI]->U
718  ).inverse().eval();
719 #else
720  model.mCustomJoints[kI]->Dinv=(model.mCustomJoints[kI]->S.transpose()
721  * model.mCustomJoints[kI]->U
722  ).inverse();
723 #endif
724  // LOG << "mCustomJoints[kI]->u[" << i << "] = "
725  //<< model.mCustomJoints[kI]->u.transpose() << std::endl;
726  unsigned int lambda = model.lambda[i];
727 
728  if (lambda != 0) {
729  SpatialMatrix Ia = model.IA[i]
730  - ( model.mCustomJoints[kI]->U
731  * model.mCustomJoints[kI]->Dinv
732  * model.mCustomJoints[kI]->U.transpose());
733 #ifdef EIGEN_CORE_H
734  model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose()
735  * Ia
736  * model.X_lambda[i].toMatrix();
737 #else
738  model.IA[lambda] += model.X_lambda[i].toMatrixTranspose()
739  * Ia * model.X_lambda[i].toMatrix();
740 #endif
741  }
742  }
743  }
744  }
745 
746  // compute articulated bias forces
747  for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) {
748  unsigned int q_index = model.mJoints[i].q_index;
749 
750  if (model.mJoints[i].mDoFCount == 1
751  && model.mJoints[i].mJointType != JointTypeCustom) {
752 
753  model.u[i] = Tau[q_index] - model.S[i].dot(model.pA[i]);
754  // LOG << "u[" << i << "] = " << model.u[i] << std::endl;
755  unsigned int lambda = model.lambda[i];
756  if (lambda != 0) {
757  SpatialVector pa = model.pA[i] + model.U[i] * model.u[i] / model.d[i];
758 
759 #ifdef EIGEN_CORE_H
760  model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa);
761 #else
762  model.pA[lambda] += model.X_lambda[i].applyTranspose(pa);
763 #endif
764  LOG << "pA[" << lambda << "] = "
765  << model.pA[lambda].transpose() << std::endl;
766  }
767  } else if (model.mJoints[i].mDoFCount == 3
768  && model.mJoints[i].mJointType != JointTypeCustom) {
769 
770  Vector3d tau_temp ( Tau[q_index],
771  Tau[q_index + 1],
772  Tau[q_index + 2]);
773  model.multdof3_u[i] = tau_temp
774  - model.multdof3_S[i].transpose()*model.pA[i];
775  // LOG << "multdof3_u[" << i << "] = "
776  // << model.multdof3_u[i].transpose() << std::endl;
777  unsigned int lambda = model.lambda[i];
778 
779  if (lambda != 0) {
780  SpatialVector pa = model.pA[i]
781  + model.multdof3_U[i]
782  * model.multdof3_Dinv[i]
783  * model.multdof3_u[i];
784 
785 #ifdef EIGEN_CORE_H
786  model.pA[lambda].noalias() +=
787  model.X_lambda[i].applyTranspose(pa);
788 #else
789  model.pA[lambda] += model.X_lambda[i].applyTranspose(pa);
790 #endif
791  LOG << "pA[" << lambda << "] = "
792  << model.pA[lambda].transpose() << std::endl;
793  }
794  } else if (model.mJoints[i].mJointType == JointTypeCustom) {
795  unsigned int kI = model.mJoints[i].custom_joint_index;
796  unsigned int dofI = model.mCustomJoints[kI]->mDoFCount;
797  VectorNd tau_temp(Tau.block(q_index,0,dofI,1));
798 
799  model.mCustomJoints[kI]->u =
800  tau_temp - ( model.mCustomJoints[kI]->S.transpose()* model.pA[i]);
801  // LOG << "mCustomJoints[kI]->u"
802  // << model.mCustomJoints[kI]->u.transpose() << std::endl;
803  unsigned int lambda = model.lambda[i];
804 
805  if (lambda != 0) {
806  SpatialVector pa = model.pA[i]
807  + ( model.mCustomJoints[kI]->U
808  * model.mCustomJoints[kI]->Dinv
809  * model.mCustomJoints[kI]->u);
810 
811 #ifdef EIGEN_CORE_H
812  model.pA[lambda].noalias() +=
813  model.X_lambda[i].applyTranspose(pa);
814 #else
815  model.pA[lambda] += model.X_lambda[i].applyTranspose(pa);
816 #endif
817  LOG << "pA[" << lambda << "] = "
818  << model.pA[lambda].transpose() << std::endl;
819  }
820  }
821  }
822 
823  // ClearLogOutput();
824 
825  for (unsigned int i = 1; i < model.mBodies.size(); i++) {
826  unsigned int q_index = model.mJoints[i].q_index;
827  unsigned int lambda = model.lambda[i];
828  SpatialTransform X_lambda = model.X_lambda[i];
829 
830  model.a[i] = X_lambda.apply(model.a[lambda]) + model.c[i];
831  LOG << "a'[" << i << "] = " << model.a[i].transpose() << std::endl;
832 
833  if (model.mJoints[i].mDoFCount == 1
834  && model.mJoints[i].mJointType != JointTypeCustom) {
835  QDDot[q_index] = (1./model.d[i])*(model.u[i]-model.U[i].dot(model.a[i]));
836  model.a[i] = model.a[i] + model.S[i] * QDDot[q_index];
837  } else if (model.mJoints[i].mDoFCount == 3
838  && model.mJoints[i].mJointType != JointTypeCustom) {
839  Vector3d qdd_temp =
840  model.multdof3_Dinv[i] * (model.multdof3_u[i]
841  - model.multdof3_U[i].transpose()*model.a[i]);
842 
843  QDDot[q_index] = qdd_temp[0];
844  QDDot[q_index + 1] = qdd_temp[1];
845  QDDot[q_index + 2] = qdd_temp[2];
846  model.a[i] = model.a[i] + model.multdof3_S[i] * qdd_temp;
847  } else if (model.mJoints[i].mJointType == JointTypeCustom) {
848  unsigned int kI = model.mJoints[i].custom_joint_index;
849  unsigned int dofI = model.mCustomJoints[kI]->mDoFCount;
850 
851  VectorNd qdd_temp = model.mCustomJoints[kI]->Dinv
852  * ( model.mCustomJoints[kI]->u
853  - model.mCustomJoints[kI]->U.transpose() * model.a[i]);
854 
855  for(unsigned z = 0; z < dofI; ++z){
856  QDDot[q_index+z] = qdd_temp[z];
857  }
858 
859  model.a[i] = model.a[i]
860  + model.mCustomJoints[kI]->S * qdd_temp;
861  }
862  }
863 
864  LOG << "QDDot = " << QDDot.transpose() << std::endl;
865 }
866 
867 } /* namespace RigidBodyDynamics */
SpatialMatrix crossf(const SpatialVector &v)
Compact representation of spatial transformations.
std::vector< unsigned int > lambda
The id of the parents body.
Definition: Model.h:127
RBDL_DLLAPI void NonlinearEffects(Model &model, const VectorNd &Q, const VectorNd &QDot, VectorNd &Tau, std::vector< Math::SpatialVector > *f_ext)
Computes the coriolis forces.
Definition: Dynamics.cc:107
std::vector< Math::SpatialVector > v_J
Definition: Model.h:181
RBDL_DLLAPI void jcalc_X_lambda_S(Model &model, unsigned int joint_id, const VectorNd &q)
Definition: Joint.cc:271
Contains all information about the rigid body model.
Definition: Model.h:121
std::vector< Math::Matrix63 > multdof3_U
Definition: Model.h:197
RBDL_DLLAPI void ForwardDynamics(Model &model, const VectorNd &Q, const VectorNd &QDot, const VectorNd &Tau, VectorNd &QDDot, std::vector< SpatialVector > *f_ext)
Computes forward dynamics with the Articulated Body Algorithm.
Definition: Dynamics.cc:315
LinearSolverPartialPivLU
LinearSolverHouseholderQR
SpatialMatrix crossm(const SpatialVector &v)
RBDL_DLLAPI void InverseDynamics(Model &model, const VectorNd &Q, const VectorNd &QDot, const VectorNd &QDDot, VectorNd &Tau, std::vector< SpatialVector > *f_ext)
Computes inverse dynamics with the Newton-Euler Algorithm.
Definition: Dynamics.cc:26
std::vector< Math::SpatialVector > v
The spatial velocity of the bodies.
Definition: Model.h:166
RBDL_DLLAPI void CompositeRigidBodyAlgorithm(Model &model, const VectorNd &Q, MatrixNd &H, bool update_kinematics)
Computes the joint space inertia matrix by using the Composite Rigid Body Algorithm.
Definition: Dynamics.cc:167
std::vector< Body > mBodies
All bodies 0 ... N_B, including the base.
Definition: Model.h:263
std::vector< CustomJoint * > mCustomJoints
Definition: Model.h:202
std::vector< Math::SpatialRigidBodyInertia > Ic
Definition: Model.h:224
RBDL_DLLAPI void ForwardDynamicsLagrangian(Model &model, const VectorNd &Q, const VectorNd &QDot, const VectorNd &Tau, VectorNd &QDDot, Math::LinearSolver linear_solver, std::vector< SpatialVector > *f_ext, Math::MatrixNd *H, Math::VectorNd *C)
Computes forward dynamics by building and solving the full Lagrangian equation.
Definition: Dynamics.cc:551
Namespace for all structures of the RigidBodyDynamics library.
Definition: Constraints.cc:23
std::vector< Math::SpatialVector > f
Internal forces on the body (used only InverseDynamics())
Definition: Model.h:220
Math::Vector3d gravity
the cartesian vector of the gravity
Definition: Model.h:162
std::vector< Math::SpatialRigidBodyInertia > I
The spatial inertia of body i (used only in CompositeRigidBodyAlgorithm())
Definition: Model.h:223
std::vector< Math::SpatialTransform > X_lambda
Transformation from the parent body to the current body .
Definition: Model.h:236
std::vector< unsigned int > mJointUpdateOrder
Definition: Model.h:184
Math::VectorNd d
Temporary variable D_i (RBDA p. 130)
Definition: Model.h:216
LinearSolverLLT
LinearSolverColPivHouseholderQR
SpatialVector apply(const SpatialVector &v_sp)
std::vector< Math::Vector3d > multdof3_u
Definition: Model.h:199
std::vector< Math::SpatialVector > c
The velocity dependent spatial acceleration.
Definition: Model.h:208
#define LOG
Definition: Logging.h:23
RBDL_DLLAPI void jcalc(Model &model, unsigned int joint_id, const VectorNd &q, const VectorNd &qdot)
Computes all variables for a joint model.
Definition: Joint.cc:21
std::vector< Math::SpatialVector > a
The spatial acceleration of the bodies.
Definition: Model.h:168
std::vector< Math::SpatialVector > pA
The spatial bias force.
Definition: Model.h:212
Math::VectorNd u
Temporary variable u (RBDA p. 130)
Definition: Model.h:218
unsigned int dof_count
number of degrees of freedoms of the model
Definition: Model.h:139
std::vector< Math::SpatialVector > S
The joint axis for joint i.
Definition: Model.h:177
RBDL_DLLAPI void CalcMInvTimesTau(Model &model, const VectorNd &Q, const VectorNd &Tau, VectorNd &QDDot, bool update_kinematics)
Computes the effect of multiplying the inverse of the joint space inertia matrix with a vector in lin...
Definition: Dynamics.cc:621
std::vector< Math::SpatialVector > U
Temporary variable U_i (RBDA p. 130)
Definition: Model.h:214
std::vector< Math::SpatialTransform > X_base
Transformation from the base to bodies reference frame.
Definition: Model.h:238
std::vector< Math::SpatialVector > c_J
Definition: Model.h:182
std::vector< Math::Matrix3d > multdof3_Dinv
Definition: Model.h:198
std::vector< Math::Matrix63 > multdof3_S
Motion subspace for joints with 3 degrees of freedom.
Definition: Model.h:196
std::vector< Math::SpatialMatrix > IA
The spatial inertia of the bodies.
Definition: Model.h:210
std::vector< Joint > mJoints
All joints.
Definition: Model.h:175
RBDL_DLLAPI bool LinSolveGaussElimPivot(MatrixNd A, VectorNd b, VectorNd &x)
Solves a linear system using gaussian elimination with pivoting.
User defined joints of varying size.
Definition: Joint.h:200