Dynamics.cc
Go to the documentation of this file.
1 /*
2  * Original Copyright (c) 2011-2016 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
3  *
4  *
5  * RDL - Robot Dynamics Library
6  * Modifications Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
7  *
8  * Licensed under the zlib license. See LICENSE for more details.
9  */
10 
11 #include <limits>
12 #include <string.h>
13 
14 #include "rdl_dynamics/Dynamics.h"
15 #include "rdl_dynamics/Kinematics.h"
16 #include "rdl_dynamics/SpatialMomentum.hpp"
17 #include "rdl_dynamics/SpatialMotion.hpp"
18 
19 namespace RobotDynamics
20 {
21 using namespace Math;
22 
23 void inverseDynamics(Model& model, const VectorNd& Q, const VectorNd& QDot, const VectorNd& QDDot, VectorNd& Tau, SpatialForceV* f_ext, bool update_kinematics)
24 {
25  // Reset the velocity of the root body
26  model.v[0].setZero();
27  model.a[0].set(-model.gravity);
28 
29  for (unsigned int i = 1; i < model.mBodies.size(); i++)
30  {
31  unsigned int q_index = model.mJoints[i].q_index;
32  unsigned int lambda = model.lambda[i];
33  ReferenceFramePtr bodyFrame = model.bodyFrames[i];
34 
35  if (update_kinematics)
36  {
37  jcalc(model, i, Q, QDot);
38 
39  model.v[i].set(model.v[lambda].transform_copy(bodyFrame->getTransformFromParent()) + model.v_J[i]);
40  model.c[i] = model.c_J[i] + model.v[i] % model.v_J[i];
41  }
42 
43  if (model.mJoints[i].mJointType != JointTypeCustom)
44  {
45  if (model.mJoints[i].mDoFCount == 1)
46  {
47  model.a[i].set(model.a[lambda].transform_copy(bodyFrame->getTransformFromParent()) + model.c[i] + model.S[i] * QDDot[q_index]);
48  }
49  else if (model.mJoints[i].mDoFCount == 3)
50  {
51  model.a[i].set(bodyFrame->getTransformFromParent().apply(model.a[lambda]) + model.c[i] +
52  model.multdof3_S[i] * Vector3d(QDDot[q_index], QDDot[q_index + 1], QDDot[q_index + 2]));
53  }
54  }
55  else
56  {
57  unsigned int k = model.mJoints[i].custom_joint_index;
58  model.a[i].set(bodyFrame->getTransformFromParent().apply(model.a[lambda]) + model.c[i] +
59  model.mCustomJoints[k]->S * QDDot.block(q_index, 0, model.mCustomJoints[k]->mDoFCount, 1));
60  }
61 
62  if (!model.mBodies[i].mIsVirtual)
63  {
64  model.f_b[i] = f_ext == nullptr ? RobotDynamics::Math::SpatialForce(bodyFrame, model.I[i] * model.a[i] + model.v[i] % (model.I[i] * model.v[i])) :
65  RobotDynamics::Math::SpatialForce(bodyFrame, model.I[i] * model.a[i] + model.v[i] % (model.I[i] * model.v[i]) -
66  (*f_ext)[i].changeFrameAndCopy(bodyFrame));
67  }
68  else
69  {
70  model.f_b[i].setIncludingFrame(bodyFrame, 0., 0., 0., 0., 0., 0.);
71  }
72  }
73 
74  for (unsigned int i = model.mBodies.size() - 1; i > 0; i--)
75  {
76  if (model.mJoints[i].mJointType != JointTypeCustom)
77  {
78  if (model.mJoints[i].mDoFCount == 1)
79  {
80  Tau[model.mJoints[i].q_index] = model.S[i].dot(model.f_b[i]);
81  }
82  else if (model.mJoints[i].mDoFCount == 3)
83  {
84  Tau.block<3, 1>(model.mJoints[i].q_index, 0) = model.multdof3_S[i].transpose() * model.f_b[i];
85  }
86  }
87  else
88  {
89  unsigned int k = model.mJoints[i].custom_joint_index;
90  Tau.block(model.mJoints[i].q_index, 0, model.mCustomJoints[k]->mDoFCount, 1) = model.mCustomJoints[k]->S.transpose() * model.f_b[i];
91  }
92 
93  if (model.lambda[i] != 0)
94  {
95  model.f_b[model.lambda[i]] += model.f_b[i].changeFrameAndCopy(model.bodyFrames[model.lambda[i]]);
96  }
97  }
98 }
99 
100 void gravityEffects(Model& model, Math::VectorNd& Tau)
101 {
102  for (unsigned int i = 1; i < model.mBodies.size(); i++)
103  {
104  RobotDynamics::ReferenceFramePtr frame = model.bodyFrames[i];
105  if (!model.mBodies[i].mIsVirtual)
106  {
107  model.f_b[i].setIncludingFrame(frame, -(model.I[i] * model.gravity.transform_copy(model.worldFrame->getTransformToDesiredFrame(frame))));
108  }
109  else
110  {
111  model.f_b[i].setIncludingFrame(frame, 0., 0., 0., 0., 0., 0.);
112  }
113  }
114 
115  for (unsigned int i = model.mBodies.size() - 1; i > 0; i--)
116  {
117  if (model.mJoints[i].mJointType != JointTypeCustom)
118  {
119  if (model.mJoints[i].mDoFCount == 1)
120  {
121  Tau[model.mJoints[i].q_index] = model.S[i].dot(model.f_b[i]);
122  }
123  else if (model.mJoints[i].mDoFCount == 3)
124  {
125  Tau.block<3, 1>(model.mJoints[i].q_index, 0) = model.multdof3_S[i].transpose() * model.f_b[i];
126  }
127  }
128  else
129  {
130  unsigned int k = model.mJoints[i].custom_joint_index;
131  Tau.block(model.mJoints[i].q_index, 0, model.mCustomJoints[k]->mDoFCount, 1) = model.mCustomJoints[k]->S.transpose() * model.f_b[i];
132  }
133 
134  if (model.lambda[i] != 0)
135  {
136  model.f_b[model.lambda[i]] += model.f_b[i].changeFrameAndCopy(model.bodyFrames[model.lambda[i]]);
137  }
138  }
139 }
140 
141 void coriolisEffects(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, Math::VectorNd& Tau, bool update_kinematics)
142 {
143  model.v[0].setZero();
144  model.a[0].setZero();
145 
146  for (unsigned int i = 1; i < model.mBodies.size(); i++)
147  {
148  ReferenceFramePtr bodyFrame = model.bodyFrames[i];
149  if (model.lambda[i] == 0)
150  {
151  if (update_kinematics)
152  {
153  jcalc(model, i, Q, QDot);
154  model.v[i].set(model.v_J[i]);
155  }
156 
157  model.a[i].setZero();
158  }
159  else
160  {
161  if (update_kinematics)
162  {
163  jcalc(model, i, Q, QDot);
164  model.v[i].set(model.v[model.lambda[i]].transform_copy(model.bodyFrames[i]->getTransformFromParent()) + model.v_J[i]);
165  model.c[i] = model.c_J[i] + model.v[i] % model.v_J[i];
166  }
167 
168  model.a[i].set(model.a[model.lambda[i]].transform_copy(bodyFrame->getTransformFromParent()) + model.c[i]);
169  }
170 
171  if (!model.mBodies[i].mIsVirtual)
172  {
173  model.f_b[i].setIncludingFrame(bodyFrame, model.I[i] * model.a[i] + model.v[i] % (Math::Momentum(model.I[i], model.v[i])));
174  }
175  else
176  {
177  model.f_b[i].setIncludingFrame(bodyFrame, 0., 0., 0., 0., 0., 0.);
178  }
179  }
180 
181  for (unsigned int i = model.mBodies.size() - 1; i > 0; i--)
182  {
183  if (model.mJoints[i].mJointType != JointTypeCustom)
184  {
185  if (model.mJoints[i].mDoFCount == 1)
186  {
187  Tau[model.mJoints[i].q_index] = model.S[i].dot(model.f_b[i]);
188  }
189  else if (model.mJoints[i].mDoFCount == 3)
190  {
191  Tau.block<3, 1>(model.mJoints[i].q_index, 0) = model.multdof3_S[i].transpose() * model.f_b[i];
192  }
193  }
194  else
195  {
196  unsigned int k = model.mJoints[i].custom_joint_index;
197  Tau.block(model.mJoints[i].q_index, 0, model.mCustomJoints[k]->mDoFCount, 1) = model.mCustomJoints[k]->S.transpose() * model.f_b[i];
198  }
199 
200  if (model.lambda[i] != 0)
201  {
202  model.f_b[model.lambda[i]] += model.f_b[i].changeFrameAndCopy(model.bodyFrames[model.lambda[i]]);
203  }
204  }
205 }
206 
207 void calcBodyGravityWrench(Model& model, unsigned int body_id, RobotDynamics::Math::SpatialForce& gravity_wrench)
208 {
209  assert(model.IsBodyId(body_id));
210  assert(body_id > 0);
211 
212  for (unsigned int i = 1; i < model.mBodies.size(); i++)
213  {
214  RobotDynamics::ReferenceFramePtr frame = model.bodyFrames[i];
215  if (!model.mBodies[i].mIsVirtual)
216  {
217  model.f_b[i].setIncludingFrame(frame, (model.I[i] * model.gravity.transform_copy(model.worldFrame->getTransformToDesiredFrame(frame))));
218  }
219  else
220  {
221  model.f_b[i].setIncludingFrame(frame, 0., 0., 0., 0., 0., 0.);
222  }
223  }
224 
225  unsigned int i = model.mBodies.size() - 1;
226  while (i != body_id)
227  {
228  // This is very inefficient, need to only go down the chain of the branch the body is on
229  model.f_b[model.lambda[i]] += model.f_b[i].changeFrameAndCopy(model.bodyFrames[model.lambda[i]]);
230  i--;
231  }
232 
233  gravity_wrench = model.f_b[body_id];
234 }
235 
236 void nonlinearEffects(Model& model, const VectorNd& Q, const VectorNd& QDot, VectorNd& Tau, bool update_kinematics)
237 {
238  model.v[0].setZero();
239  model.a[0].set(-model.gravity);
240 
241  for (unsigned int i = 1; i < model.mBodies.size(); i++)
242  {
243  RobotDynamics::ReferenceFramePtr frame = model.bodyFrames[i];
244  if (model.lambda[i] == 0)
245  {
246  if (update_kinematics)
247  {
248  jcalc(model, i, Q, QDot);
249  model.v[i].set(model.v_J[i]);
250  }
251 
252  model.a[i].set(-model.gravity.transform_copy(frame->getTransformFromParent()));
253  }
254  else
255  {
256  if (update_kinematics)
257  {
258  jcalc(model, i, Q, QDot);
259  model.v[i].set(model.v[model.lambda[i]].transform_copy(frame->getTransformFromParent()) + model.v_J[i]);
260  model.c[i] = model.c_J[i] + model.v[i] % model.v_J[i];
261  }
262 
263  model.a[i].set(model.a[model.lambda[i]].transform_copy(frame->getTransformFromParent()) + model.c[i]);
264  }
265 
266  if (!model.mBodies[i].mIsVirtual)
267  {
268  model.f_b[i].setIncludingFrame(frame, model.I[i] * model.a[i] + model.v[i] % (model.I[i] * model.v[i]));
269  }
270  else
271  {
272  model.f_b[i].setIncludingFrame(frame, 0., 0., 0., 0., 0., 0.);
273  }
274  }
275 
276  for (unsigned int i = model.mBodies.size() - 1; i > 0; i--)
277  {
278  if (model.mJoints[i].mJointType != JointTypeCustom)
279  {
280  if (model.mJoints[i].mDoFCount == 1)
281  {
282  Tau[model.mJoints[i].q_index] = model.S[i].dot(model.f_b[i]);
283  }
284  else if (model.mJoints[i].mDoFCount == 3)
285  {
286  Tau.block<3, 1>(model.mJoints[i].q_index, 0) = model.multdof3_S[i].transpose() * model.f_b[i];
287  }
288  }
289  else
290  {
291  unsigned int k = model.mJoints[i].custom_joint_index;
292  Tau.block(model.mJoints[i].q_index, 0, model.mCustomJoints[k]->mDoFCount, 1) = model.mCustomJoints[k]->S.transpose() * model.f_b[i];
293  }
294 
295  if (model.lambda[i] != 0)
296  {
297  model.f_b[model.lambda[i]] += model.f_b[i].changeFrameAndCopy(model.bodyFrames[model.lambda[i]]);
298  }
299  }
300 }
301 
302 void compositeRigidBodyAlgorithm(Model& model, const VectorNd& Q, MatrixNd& H, bool update_kinematics)
303 {
304  assert(H.rows() == model.dof_count && H.cols() == model.dof_count);
305 
306  for (unsigned int i = 1; i < model.mBodies.size(); i++)
307  {
308  if (update_kinematics)
309  {
310  jcalc_X_lambda_S(model, i, Q);
311  }
312  model.Ic[i] = model.I[i];
313  }
314 
315  for (unsigned int i = model.mBodies.size() - 1; i > 0; i--)
316  {
317  if (model.lambda[i] != 0)
318  {
319  model.Ic[model.lambda[i]] = model.Ic[model.lambda[i]] + model.Ic[i].transform_copy(model.bodyFrames[i]->getTransformToParent());
320  }
321 
322  unsigned int dof_index_i = model.mJoints[i].q_index;
323 
324  if (model.mJoints[i].mDoFCount == 1 && model.mJoints[i].mJointType != JointTypeCustom)
325  {
326  Momentum F(model.Ic[i], model.S[i]);
327  H(dof_index_i, dof_index_i) = model.S[i].dot(F);
328 
329  unsigned int j = i;
330  unsigned int dof_index_j = dof_index_i;
331 
332  while (model.lambda[j] != 0)
333  {
334  F.transform(model.bodyFrames[j]->getTransformToParent());
335  j = model.lambda[j];
336  dof_index_j = model.mJoints[j].q_index;
337 
338  if (model.mJoints[j].mJointType != JointTypeCustom)
339  {
340  if (model.mJoints[j].mDoFCount == 1)
341  {
342  H(dof_index_i, dof_index_j) = F.dot(model.S[j]);
343  H(dof_index_j, dof_index_i) = H(dof_index_i, dof_index_j);
344  }
345  else if (model.mJoints[j].mDoFCount == 3)
346  {
347  Vector3d H_temp2 = (F.transpose() * model.multdof3_S[j]).transpose();
348 
349  H.block<1, 3>(dof_index_i, dof_index_j) = H_temp2.transpose();
350  H.block<3, 1>(dof_index_j, dof_index_i) = H_temp2;
351  }
352  }
353  else
354  {
355  unsigned int k = model.mJoints[j].custom_joint_index;
356  unsigned int dof = model.mCustomJoints[k]->mDoFCount;
357  VectorNd H_temp2 = (F.transpose() * model.mCustomJoints[k]->S).transpose();
358 
359  H.block(dof_index_i, dof_index_j, 1, dof) = H_temp2.transpose();
360  H.block(dof_index_j, dof_index_i, dof, 1) = H_temp2;
361  }
362  }
363  }
364  else if (model.mJoints[i].mDoFCount == 3 && model.mJoints[i].mJointType != JointTypeCustom)
365  {
366  Matrix63 F_63 = model.Ic[i].toMatrix() * model.multdof3_S[i];
367  H.block<3, 3>(dof_index_i, dof_index_i) = model.multdof3_S[i].transpose() * F_63;
368 
369  unsigned int j = i;
370  unsigned int dof_index_j = dof_index_i;
371 
372  while (model.lambda[j] != 0)
373  {
374  F_63 = model.bodyFrames[j]->getTransformFromParent().toMatrixTranspose() * (F_63);
375  j = model.lambda[j];
376  dof_index_j = model.mJoints[j].q_index;
377 
378  if (model.mJoints[j].mJointType != JointTypeCustom)
379  {
380  if (model.mJoints[j].mDoFCount == 1)
381  {
382  Vector3d H_temp2 = F_63.transpose() * (model.S[j]);
383 
384  H.block<3, 1>(dof_index_i, dof_index_j) = H_temp2;
385  H.block<1, 3>(dof_index_j, dof_index_i) = H_temp2.transpose();
386  }
387  else if (model.mJoints[j].mDoFCount == 3)
388  {
389  Matrix3d H_temp2 = F_63.transpose() * (model.multdof3_S[j]);
390 
391  H.block<3, 3>(dof_index_i, dof_index_j) = H_temp2;
392  H.block<3, 3>(dof_index_j, dof_index_i) = H_temp2.transpose();
393  }
394  }
395  else
396  {
397  unsigned int k = model.mJoints[j].custom_joint_index;
398  unsigned int dof = model.mCustomJoints[k]->mDoFCount;
399 
400  MatrixNd H_temp2 = F_63.transpose() * (model.mCustomJoints[k]->S);
401 
402  H.block(dof_index_i, dof_index_j, 3, dof) = H_temp2;
403  H.block(dof_index_j, dof_index_i, dof, 3) = H_temp2.transpose();
404  }
405  }
406  }
407  else if (model.mJoints[i].mJointType == JointTypeCustom)
408  {
409  unsigned int kI = model.mJoints[i].custom_joint_index;
410  unsigned int dofI = model.mCustomJoints[kI]->mDoFCount;
411 
412  MatrixNd F_Nd = model.Ic[i].toMatrix() * model.mCustomJoints[kI]->S;
413 
414  H.block(dof_index_i, dof_index_i, dofI, dofI) = model.mCustomJoints[kI]->S.transpose() * F_Nd;
415 
416  unsigned int j = i;
417  unsigned int dof_index_j = dof_index_i;
418 
419  while (model.lambda[j] != 0)
420  {
421  F_Nd = model.bodyFrames[j]->getTransformFromParent().toMatrixTranspose() * (F_Nd);
422  j = model.lambda[j];
423  dof_index_j = model.mJoints[j].q_index;
424 
425  if (model.mJoints[j].mJointType != JointTypeCustom)
426  {
427  if (model.mJoints[j].mDoFCount == 1)
428  {
429  MatrixNd H_temp2 = F_Nd.transpose() * (model.S[j]);
430  H.block(dof_index_i, dof_index_j, H_temp2.rows(), H_temp2.cols()) = H_temp2;
431  H.block(dof_index_j, dof_index_i, H_temp2.cols(), H_temp2.rows()) = H_temp2.transpose();
432  }
433  else if (model.mJoints[j].mDoFCount == 3)
434  {
435  MatrixNd H_temp2 = F_Nd.transpose() * (model.multdof3_S[j]);
436  H.block(dof_index_i, dof_index_j, H_temp2.rows(), H_temp2.cols()) = H_temp2;
437  H.block(dof_index_j, dof_index_i, H_temp2.cols(), H_temp2.rows()) = H_temp2.transpose();
438  }
439  }
440  else
441  {
442  unsigned int k = model.mJoints[j].custom_joint_index;
443  unsigned int dof = model.mCustomJoints[k]->mDoFCount;
444 
445  MatrixNd H_temp2 = F_Nd.transpose() * (model.mCustomJoints[k]->S);
446 
447  H.block(dof_index_i, dof_index_j, 3, dof) = H_temp2;
448  H.block(dof_index_j, dof_index_i, dof, 3) = H_temp2.transpose();
449  }
450  }
451  }
452  }
453 }
454 
455 void forwardDynamics(Model& model, const VectorNd& Q, const VectorNd& QDot, const VectorNd& Tau, VectorNd& QDDot, SpatialForceV* f_ext, bool update_kinematics)
456 {
457  // Reset the velocity of the root body
458  model.v[0].setZero();
459 
460  for (unsigned int i = 1; i < model.mBodies.size(); i++)
461  {
462  unsigned int lambda = model.lambda[i];
463  ReferenceFramePtr bodyFrame = model.bodyFrames[i];
464 
465  if (update_kinematics)
466  {
467  jcalc(model, i, Q, QDot);
468  model.v[i].set(model.v[lambda].transform_copy(bodyFrame->getTransformFromParent()) + model.v_J[i]);
469  model.c[i] = model.c_J[i] + SpatialVector(model.v[i] % model.v_J[i]);
470  }
471 
472  model.I[i].setSpatialMatrix(model.IA[i]);
473  model.pA[i].setIncludingFrame(bodyFrame, f_ext == nullptr ? model.v[i].cross(model.I[i] * model.v[i]) :
474  model.v[i].cross(model.I[i] * model.v[i]) - (*f_ext)[i].changeFrameAndCopy(bodyFrame));
475  }
476 
477  for (unsigned int i = model.mBodies.size() - 1; i > 0; i--)
478  {
479  unsigned int q_index = model.mJoints[i].q_index;
480  ReferenceFramePtr bodyFrame = model.bodyFrames[i];
481 
482  if (model.mJoints[i].mDoFCount == 1 && model.mJoints[i].mJointType != JointTypeCustom)
483  {
484  model.U[i] = model.IA[i] * model.S[i];
485  model.d[i] = model.S[i].dot(model.U[i]);
486  model.u[i] = Tau[q_index] - model.S[i].dot(model.pA[i]);
487 
488  unsigned int lambda = model.lambda[i];
489  if (lambda != 0)
490  {
491  SpatialMatrix Ia = model.IA[i] - model.U[i] * (model.U[i] / model.d[i]).transpose();
492  RobotDynamics::Math::SpatialForce pa(RobotDynamics::Math::SpatialForce(bodyFrame, model.pA[i] + Ia * model.c[i] + model.U[i] * model.u[i] / model.d[i]));
493 
494  model.IA[lambda].noalias() += bodyFrame->getTransformFromParent().toMatrixTranspose() * Ia * bodyFrame->getTransformFromParent().toMatrix();
495  model.pA[lambda] += pa.changeFrameAndCopy(model.bodyFrames[model.lambda[i]]);
496  }
497  }
498  else if (model.mJoints[i].mDoFCount == 3 && model.mJoints[i].mJointType != JointTypeCustom)
499  {
500  model.multdof3_U[i] = model.IA[i] * model.multdof3_S[i];
501 
502  model.multdof3_Dinv[i] = (model.multdof3_S[i].transpose() * model.multdof3_U[i]).inverse().eval();
503 
504  Vector3d tau_temp(Tau[q_index], Tau[q_index + 1], Tau[q_index + 2]);
505  model.multdof3_u[i] = tau_temp - model.multdof3_S[i].transpose() * model.pA[i];
506 
507  unsigned int lambda = model.lambda[i];
508  if (lambda != 0)
509  {
510  SpatialMatrix Ia = model.IA[i] - model.multdof3_U[i] * model.multdof3_Dinv[i] * model.multdof3_U[i].transpose();
511  RobotDynamics::Math::SpatialForce pa(
512  model.pA[i] + RobotDynamics::Math::SpatialForce(bodyFrame, Ia * model.c[i] + model.multdof3_U[i] * model.multdof3_Dinv[i] * model.multdof3_u[i]));
513 
514  model.IA[lambda].noalias() += bodyFrame->getTransformFromParent().toMatrixTranspose() * Ia * bodyFrame->getTransformFromParent().toMatrix();
515  model.pA[lambda] += pa.changeFrameAndCopy(model.bodyFrames[model.lambda[i]]);
516  }
517  }
518  else if (model.mJoints[i].mJointType == JointTypeCustom)
519  {
520  unsigned int kI = model.mJoints[i].custom_joint_index;
521  model.mCustomJoints[kI]->U = model.IA[i] * model.mCustomJoints[kI]->S;
522 
523  model.mCustomJoints[kI]->Dinv = (model.mCustomJoints[kI]->S.transpose() * model.mCustomJoints[kI]->U).inverse().eval();
524 
525  model.mCustomJoints[kI]->u = Tau.block(q_index, 0, model.mCustomJoints[kI]->mDoFCount, 1) - model.mCustomJoints[kI]->S.transpose() * model.pA[i];
526 
527  unsigned int lambda = model.lambda[i];
528  if (lambda != 0)
529  {
530  SpatialMatrix Ia = model.IA[i] - (model.mCustomJoints[kI]->U * model.mCustomJoints[kI]->Dinv * model.mCustomJoints[kI]->U.transpose());
531  RobotDynamics::Math::SpatialForce pa(
532  model.pA[i] + RobotDynamics::Math::SpatialForce(
533  bodyFrame, Ia * model.c[i] + (model.mCustomJoints[kI]->U * model.mCustomJoints[kI]->Dinv * model.mCustomJoints[kI]->u)));
534 
535  model.IA[lambda].noalias() += bodyFrame->getTransformFromParent().toMatrixTranspose() * Ia * bodyFrame->getTransformFromParent().toMatrix();
536  model.pA[lambda] += pa.changeFrameAndCopy(model.bodyFrames[model.lambda[i]]);
537  }
538  }
539  }
540 
541  model.a[0].set(model.gravity * -1.);
542 
543  for (unsigned int i = 1; i < model.mBodies.size(); i++)
544  {
545  unsigned int q_index = model.mJoints[i].q_index;
546 
547  model.a[i].set(model.a[model.lambda[i]].transform_copy(model.bodyFrames[i]->getTransformFromParent()) + model.c[i]);
548 
549  if (model.mJoints[i].mDoFCount == 1 && model.mJoints[i].mJointType != JointTypeCustom)
550  {
551  QDDot[q_index] = (1. / model.d[i]) * (model.u[i] - model.U[i].dot(model.a[i]));
552  model.a[i].set(model.a[i] + model.S[i] * QDDot[q_index]);
553  }
554  else if (model.mJoints[i].mDoFCount == 3 && model.mJoints[i].mJointType != JointTypeCustom)
555  {
556  Vector3d qdd_temp = model.multdof3_Dinv[i] * (model.multdof3_u[i] - model.multdof3_U[i].transpose() * model.a[i]);
557  QDDot[q_index] = qdd_temp[0];
558  QDDot[q_index + 1] = qdd_temp[1];
559  QDDot[q_index + 2] = qdd_temp[2];
560  model.a[i].set(model.a[i] + model.multdof3_S[i] * qdd_temp);
561  }
562  else if (model.mJoints[i].mJointType == JointTypeCustom)
563  {
564  unsigned int kI = model.mJoints[i].custom_joint_index;
565  unsigned int dofI = model.mCustomJoints[kI]->mDoFCount;
566 
567  VectorNd qdd_temp = model.mCustomJoints[kI]->Dinv * (model.mCustomJoints[kI]->u - model.mCustomJoints[kI]->U.transpose() * model.a[i]);
568 
569  for (unsigned int z = 0; z < dofI; ++z)
570  {
571  QDDot[q_index + z] = qdd_temp[z];
572  }
573 
574  model.a[i].set(model.a[i] + model.mCustomJoints[kI]->S * qdd_temp);
575  }
576  }
577 }
578 
579 void forwardDynamicsLagrangian(Model& model, const VectorNd& Q, const VectorNd& QDot, const VectorNd& Tau, VectorNd& QDDot, Math::MatrixNd& H, Math::VectorNd& C,
580  Math::LinearSolver linear_solver, SpatialForceV* f_ext, bool update_kinematics)
581 {
582  assert(H.rows() == model.dof_count && H.cols() == model.dof_count);
583  assert(C.rows() == model.dof_count);
584 
585  // we set QDDot to zero to compute C properly with the InverseDynamics
586  // method.
587  QDDot.setZero();
588 
589  inverseDynamics(model, Q, QDot, QDDot, C, f_ext, update_kinematics);
590  compositeRigidBodyAlgorithm(model, Q, H, false);
591 
592  bool solve_successful = linSolveGaussElimPivot(H, C * -1. + Tau, QDDot);
593  assert(solve_successful);
594 }
595 
596 void calcMInvTimesTau(Model& model, const VectorNd& Q, const VectorNd& Tau, VectorNd& QDDot, bool update_kinematics)
597 {
598  model.v[0].setZero();
599  model.a[0].setZero();
600 
601  for (unsigned int i = 1; i < model.mBodies.size(); i++)
602  {
603  if (update_kinematics)
604  {
605  jcalc_X_lambda_S(model, i, Q);
606  }
607 
608  model.pA[i].setIncludingFrame(model.bodyFrames[i], 0., 0., 0., 0., 0., 0.);
609  model.I[i].setSpatialMatrix(model.IA[i]);
610  }
611 
612  // Compute Articulate Body Inertias
613  for (unsigned int i = model.mBodies.size() - 1; i > 0; i--)
614  {
615  RobotDynamics::ReferenceFramePtr frame = model.bodyFrames[i];
616  if (model.mJoints[i].mDoFCount == 1 && model.mJoints[i].mJointType != JointTypeCustom)
617  {
618  model.U[i] = model.IA[i] * model.S[i];
619  model.d[i] = model.S[i].dot(model.U[i]);
620  unsigned int lambda = model.lambda[i];
621 
622  if (lambda != 0)
623  {
624  SpatialMatrix Ia = model.IA[i] - model.U[i] * (model.U[i] / model.d[i]).transpose();
625  model.IA[lambda].noalias() += frame->getTransformFromParent().toMatrixTranspose() * Ia * frame->getTransformFromParent().toMatrix();
626  }
627  }
628  else if (model.mJoints[i].mDoFCount == 3 && model.mJoints[i].mJointType != JointTypeCustom)
629  {
630  model.multdof3_U[i] = model.IA[i] * model.multdof3_S[i];
631  model.multdof3_Dinv[i] = (model.multdof3_S[i].transpose() * model.multdof3_U[i]).inverse().eval();
632 
633  unsigned int lambda = model.lambda[i];
634 
635  if (lambda != 0)
636  {
637  SpatialMatrix Ia = model.IA[i] - (model.multdof3_U[i] * model.multdof3_Dinv[i] * model.multdof3_U[i].transpose());
638  model.IA[lambda].noalias() += frame->getTransformFromParent().toMatrixTranspose() * Ia * frame->getTransformFromParent().toMatrix();
639  }
640  }
641  else if (model.mJoints[i].mJointType == JointTypeCustom)
642  {
643  unsigned int kI = model.mJoints[i].custom_joint_index;
644  model.mCustomJoints[kI]->U = model.IA[i] * model.mCustomJoints[kI]->S;
645  model.mCustomJoints[kI]->Dinv = (model.mCustomJoints[kI]->S.transpose() * model.mCustomJoints[kI]->U).inverse().eval();
646 
647  unsigned int lambda = model.lambda[i];
648 
649  if (lambda != 0)
650  {
651  SpatialMatrix Ia = model.IA[i] - (model.mCustomJoints[kI]->U * model.mCustomJoints[kI]->Dinv * model.mCustomJoints[kI]->U.transpose());
652  model.IA[lambda].noalias() += frame->getTransformFromParent().toMatrixTranspose() * Ia * frame->getTransformFromParent().toMatrix();
653  }
654  }
655  }
656 
657  // compute articulated bias forces
658  for (unsigned int i = model.mBodies.size() - 1; i > 0; i--)
659  {
660  unsigned int q_index = model.mJoints[i].q_index;
661  RobotDynamics::ReferenceFramePtr frame = model.bodyFrames[i];
662 
663  if (model.mJoints[i].mDoFCount == 1 && model.mJoints[i].mJointType != JointTypeCustom)
664  {
665  model.u[i] = Tau[q_index] - model.S[i].dot(model.pA[i]);
666  unsigned int lambda = model.lambda[i];
667  if (lambda != 0)
668  {
669  RobotDynamics::Math::SpatialForce pa = model.pA[i] + RobotDynamics::Math::SpatialForce(frame, model.U[i] * model.u[i] / model.d[i]);
670  model.pA[lambda] += pa.changeFrameAndCopy(model.bodyFrames[lambda]);
671  }
672  }
673  else if (model.mJoints[i].mDoFCount == 3 && model.mJoints[i].mJointType != JointTypeCustom)
674  {
675  Vector3d tau_temp(Tau[q_index], Tau[q_index + 1], Tau[q_index + 2]);
676  model.multdof3_u[i] = tau_temp - model.multdof3_S[i].transpose() * model.pA[i];
677  unsigned int lambda = model.lambda[i];
678 
679  if (lambda != 0)
680  {
681  RobotDynamics::Math::SpatialForce pa =
682  model.pA[i] + RobotDynamics::Math::SpatialForce(frame, model.multdof3_U[i] * model.multdof3_Dinv[i] * model.multdof3_u[i]);
683  model.pA[lambda] += pa.changeFrameAndCopy(model.bodyFrames[lambda]);
684  }
685  }
686  else if (model.mJoints[i].mJointType == JointTypeCustom)
687  {
688  unsigned int kI = model.mJoints[i].custom_joint_index;
689  unsigned int dofI = model.mCustomJoints[kI]->mDoFCount;
690  VectorNd tau_temp = model.mCustomJoints[kI]->ndof0_vec;
691 
692  for (unsigned int z = 0; z < dofI; ++z)
693  {
694  tau_temp(z) = Tau[q_index + z];
695  }
696  model.mCustomJoints[kI]->u = tau_temp - (model.mCustomJoints[kI]->S.transpose() * model.pA[i]);
697 
698  unsigned int lambda = model.lambda[i];
699 
700  if (lambda != 0)
701  {
702  RobotDynamics::Math::SpatialForce pa =
703  model.pA[i] + SpatialForce(frame, model.mCustomJoints[kI]->U * model.mCustomJoints[kI]->Dinv * model.mCustomJoints[kI]->u);
704  model.pA[lambda] += pa.changeFrameAndCopy(model.bodyFrames[lambda]);
705  }
706  }
707  }
708 
709  for (unsigned int i = 1; i < model.mBodies.size(); i++)
710  {
711  unsigned int q_index = model.mJoints[i].q_index;
712  unsigned int lambda = model.lambda[i];
713 
714  model.a[i].set(model.bodyFrames[i]->getTransformFromParent().apply(model.a[lambda]));
715 
716  if (model.mJoints[i].mDoFCount == 1 && model.mJoints[i].mJointType != JointTypeCustom)
717  {
718  QDDot[q_index] = (1. / model.d[i]) * (model.u[i] - model.U[i].dot(model.a[i]));
719  model.a[i].set(model.a[i] + model.S[i] * QDDot[q_index]);
720  }
721  else if (model.mJoints[i].mDoFCount == 3 && model.mJoints[i].mJointType != JointTypeCustom)
722  {
723  Vector3d qdd_temp = model.multdof3_Dinv[i] * (model.multdof3_u[i] - model.multdof3_U[i].transpose() * model.a[i]);
724 
725  QDDot[q_index] = qdd_temp[0];
726  QDDot[q_index + 1] = qdd_temp[1];
727  QDDot[q_index + 2] = qdd_temp[2];
728  model.a[i].set(model.a[i] + model.multdof3_S[i] * qdd_temp);
729  }
730  else if (model.mJoints[i].mJointType == JointTypeCustom)
731  {
732  unsigned int kI = model.mJoints[i].custom_joint_index;
733  unsigned int dofI = model.mCustomJoints[kI]->mDoFCount;
734 
735  VectorNd qdd_temp = model.mCustomJoints[kI]->Dinv * (model.mCustomJoints[kI]->u - model.mCustomJoints[kI]->U.transpose() * model.a[i]);
736 
737  for (unsigned int z = 0; z < dofI; ++z)
738  {
739  QDDot[q_index + z] = qdd_temp[z];
740  }
741 
742  model.a[i].set(model.a[i] + model.mCustomJoints[kI]->S * qdd_temp);
743  }
744  }
745 }
746 
747 } /* namespace RobotDynamics */
RobotDynamics::Model::IA
std::vector< Math::SpatialMatrix > IA
The spatial inertia of the bodies.
Definition: Model.h:251
RobotDynamics::compositeRigidBodyAlgorithm
void compositeRigidBodyAlgorithm(Model &model, const Math::VectorNd &Q, Math::MatrixNd &H, bool update_kinematics=true)
Computes the joint space inertia matrix by using the Composite Rigid Body Algorithm.
Definition: Dynamics.cc:302
RobotDynamics::Math::SpatialForceV
std::vector< SpatialForce, Eigen::aligned_allocator< SpatialForce > > SpatialForceV
Definition: SpatialForce.hpp:248
RobotDynamics::Model::mJoints
std::vector< Joint > mJoints
All joints.
Definition: Model.h:209
RobotDynamics::Model::u
Math::VectorNd u
Temporary variable u (RBDA p. 130)
Definition: Model.h:263
Tau
VectorNd Tau
Definition: RdlTwoLegModelTests.cpp:32
RobotDynamics::Model::c_J
std::vector< Math::SpatialVector > c_J
Definition: Model.h:217
RobotDynamics::JointTypeCustom
User defined joints of varying size.
Definition: Joint.h:213
QDDot
VectorNd QDDot
Definition: RdlTwoLegModelTests.cpp:31
RobotDynamics::Model::multdof3_U
std::vector< Math::Matrix63 > multdof3_U
Definition: Model.h:237
RobotDynamics::Model::lambda
std::vector< unsigned int > lambda
Definition: Model.h:156
RobotDynamics::Math::linSolveGaussElimPivot
bool linSolveGaussElimPivot(MatrixNd A, VectorNd b, VectorNd &x)
Solves a linear system using gaussian elimination with pivoting.
Definition: rdl_mathutils.cc:36
QDot
VectorNd QDot
Definition: RdlTwoLegModelTests.cpp:30
RobotDynamics::coriolisEffects
void coriolisEffects(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::VectorNd &Tau, bool update_kinematics=true)
Computes the coriolis forces.
Definition: Dynamics.cc:141
Q
VectorNd Q
Definition: RdlTwoLegModelTests.cpp:29
RobotDynamics::jcalc
void jcalc(Model &model, unsigned int joint_id, const Math::VectorNd &q, const Math::VectorNd &qdot)
Computes all variables for a joint model and updates the body frame as well as the body&#39;s center of m...
Definition: Joint.cc:23
RobotDynamics::Model::gravity
Math::MotionVector gravity
the cartesian vector of the gravity
Definition: Model.h:196
RobotDynamics::Math::ForceVector::transform
void transform(const SpatialTransform &X)
Performs the following in place transform .
Definition: ForceVector.hpp:211
RobotDynamics::Model::bodyFrames
std::vector< ReferenceFramePtr > bodyFrames
Definition: Model.h:145
RobotDynamics::Model::worldFrame
ReferenceFramePtr worldFrame
Definition: Model.h:141
RobotDynamics::Math::LinearSolver
LinearSolver
Available solver methods for the linear systems.
Definition: rdl_mathutils.h:34
RobotDynamics::Model::v
Math::SpatialMotionV v
Definition: Model.h:202
RobotDynamics::ReferenceFramePtr
std::shared_ptr< ReferenceFrame > ReferenceFramePtr
Definition: ReferenceFrame.hpp:72
RobotDynamics::Model::IsBodyId
bool IsBodyId(unsigned int id) const
Definition: Model.h:492
RobotDynamics::Math::Momentum
Momentum is mass/inertia multiplied by velocity.
Definition: Momentum.hpp:27
RobotDynamics::Model::pA
Math::SpatialForceV pA
The spatial bias force.
Definition: Model.h:254
RobotDynamics::Model::mBodies
std::vector< Body > mBodies
All bodies 0 ... N_B, including the base.
Definition: Model.h:321
RobotDynamics::calcBodyGravityWrench
void calcBodyGravityWrench(Model &model, unsigned int body_id, RobotDynamics::Math::SpatialForce &gravity_wrench)
Calculate the wrench due to gravity on a body.
Definition: Dynamics.cc:207
RobotDynamics::Model::d
Math::VectorNd d
Temporary variable D_i (RBDA p. 130)
Definition: Model.h:260
RobotDynamics::Math::VectorNd
Eigen::VectorXd VectorNd
Definition: rdl_eigenmath.h:25
RobotDynamics::Model::a
Math::SpatialAccelerationV a
The spatial velocity of the bodies.
Definition: Model.h:200
RobotDynamics::Model::mCustomJoints
std::vector< CustomJoint * > mCustomJoints
Definition: Model.h:242
RobotDynamics::Model::multdof3_Dinv
std::vector< Math::Matrix3d > multdof3_Dinv
Definition: Model.h:238
RobotDynamics::calcMInvTimesTau
void calcMInvTimesTau(Model &model, const Math::VectorNd &Q, const Math::VectorNd &Tau, Math::VectorNd &QDDot, bool update_kinematics=true)
Computes the effect of multiplying the inverse of the joint space inertia matrix with a vector in lin...
Definition: Dynamics.cc:596
RobotDynamics::Model
Contains all information about the rigid body model.
Definition: Model.h:121
RobotDynamics::Model::U
std::vector< Math::SpatialVector > U
Temporary variable U_i (RBDA p. 130)
Definition: Model.h:257
RobotDynamics::jcalc_X_lambda_S
void jcalc_X_lambda_S(Model &model, unsigned int joint_id, const Math::VectorNd &q)
Definition: Joint.cc:242
RobotDynamics::Model::I
Math::SpatialInertiaV I
Definition: Model.h:269
RobotDynamics::Math::MatrixNd
Eigen::MatrixXd MatrixNd
Definition: rdl_eigenmath.h:26
RobotDynamics::Model::c
std::vector< Math::SpatialVector > c
The velocity dependent spatial acceleration.
Definition: Model.h:248
RobotDynamics::Math::Matrix63
Eigen::Matrix< double, 6, 3 > Matrix63
Definition: rdl_eigenmath.h:24
RobotDynamics::Model::Ic
Math::RigidBodyInertiaV Ic
Definition: Model.h:270
RobotDynamics::Math::SpatialForce
A SpatialForce is a spatial vector with the angular part being three moments and the linear part bein...
Definition: SpatialForce.hpp:32
RobotDynamics::gravityEffects
void gravityEffects(Model &model, Math::VectorNd &Tau)
Computes the gravity vector.
Definition: Dynamics.cc:100
RobotDynamics::Model::v_J
Math::SpatialMotionV v_J
Definition: Model.h:219
RobotDynamics::Model::multdof3_S
std::vector< Math::Matrix63 > multdof3_S
Motion subspace for joints with 3 degrees of freedom.
Definition: Model.h:235
RobotDynamics::inverseDynamics
void inverseDynamics(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, Math::VectorNd &Tau, Math::SpatialForceV *f_ext=nullptr, bool update_kinematics=true)
Definition: Dynamics.cc:23
RobotDynamics::forwardDynamics
void forwardDynamics(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, Math::VectorNd &QDDot, Math::SpatialForceV *f_ext=nullptr, bool update_kinematics=true)
Computes forward dynamics with the Articulated Body Algorithm.
Definition: Dynamics.cc:455
RobotDynamics::Math::MotionVector::transform_copy
MotionVector transform_copy(const SpatialTransform &X) const
Copies, transforms, and returns a MotionVector. Performs .
Definition: MotionVector.hpp:217
RobotDynamics::Model::multdof3_u
std::vector< Math::Vector3d > multdof3_u
Definition: Model.h:239
RobotDynamics::forwardDynamicsLagrangian
void forwardDynamicsLagrangian(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, Math::VectorNd &QDDot, Math::MatrixNd &H, Math::VectorNd &C, Math::LinearSolver linear_solver=Math::LinearSolverColPivHouseholderQR, Math::SpatialForceV *f_ext=nullptr, bool update_kinematics=true)
Computes forward dynamics by building and solving the full Lagrangian equation.
Definition: Dynamics.cc:579
RobotDynamics::Math::SpatialForce::changeFrameAndCopy
SpatialForce changeFrameAndCopy(ReferenceFramePtr referenceFrame) const
Copy and change frame.
Definition: SpatialForce.hpp:86
RobotDynamics::nonlinearEffects
void nonlinearEffects(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::VectorNd &Tau, bool update_kinematics=true)
Computes the coriolis forces.
Definition: Dynamics.cc:236
RobotDynamics::Model::dof_count
unsigned int dof_count
number of degrees of freedoms of the model
Definition: Model.h:169
RobotDynamics
Namespace for all structures of the RobotDynamics library.
Definition: Body.h:21
RobotDynamics::Model::f_b
Math::SpatialForceV f_b
Definition: Model.h:267
RobotDynamics::Model::S
Math::MotionVectorV S
Definition: Model.h:211

rdl_dynamics
Author(s):
autogenerated on Tue Apr 20 2021 02:25:27