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
22namespace RigidBodyDynamics {
23
24using namespace Math;
25
26RBDL_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
107RBDL_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#ifdef RBDL_USE_CASADI_MATH
138 if (f_ext != NULL)
139#else
140 if (f_ext != NULL && (*f_ext)[i] != SpatialVector::Zero())
141#endif
142 {
143 model.f[i] -= model.X_base[i].toMatrixAdjoint() * (*f_ext)[i];
144 }
145 } else {
146 model.f[i].setZero();
147 }
148 }
149
150 for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) {
151 if(model.mJoints[i].mJointType != JointTypeCustom){
152 if (model.mJoints[i].mDoFCount == 1) {
153 Tau[model.mJoints[i].q_index]
154 = model.S[i].dot(model.f[i]);
155 } else if (model.mJoints[i].mDoFCount == 3) {
156 Tau.block<3,1>(model.mJoints[i].q_index, 0)
157 = model.multdof3_S[i].transpose() * model.f[i];
158 }
159 } else if(model.mJoints[i].mJointType == JointTypeCustom) {
160 unsigned int k = model.mJoints[i].custom_joint_index;
161 Tau.block(model.mJoints[i].q_index,0,
162 model.mCustomJoints[k]->mDoFCount, 1)
163 = model.mCustomJoints[k]->S.transpose() * model.f[i];
164 }
165
166 if (model.lambda[i] != 0) {
167 model.f[model.lambda[i]] = model.f[model.lambda[i]] + model.X_lambda[i].applyTranspose(model.f[i]);
168 }
169 }
170}
171
172RBDL_DLLAPI void CompositeRigidBodyAlgorithm (
173 Model& model,
174 const VectorNd &Q,
175 MatrixNd &H,
176 bool update_kinematics) {
177 LOG << "-------- " << __func__ << " --------" << std::endl;
178
179 assert (H.rows() == model.dof_count && H.cols() == model.dof_count);
180
181 for (unsigned int i = 1; i < model.mBodies.size(); i++) {
182 if (update_kinematics) {
183 jcalc_X_lambda_S (model, i, Q);
184 }
185 model.Ic[i] = model.I[i];
186 }
187
188 for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) {
189 if (model.lambda[i] != 0) {
190 model.Ic[model.lambda[i]] = model.Ic[model.lambda[i]] + model.X_lambda[i].applyTranspose(model.Ic[i]);
191 }
192
193 unsigned int dof_index_i = model.mJoints[i].q_index;
194
195 if (model.mJoints[i].mDoFCount == 1
196 && model.mJoints[i].mJointType != JointTypeCustom) {
197
198 SpatialVector F = model.Ic[i] * model.S[i];
199 H(dof_index_i, dof_index_i) = model.S[i].dot(F);
200
201 unsigned int j = i;
202 unsigned int dof_index_j = dof_index_i;
203
204 while (model.lambda[j] != 0) {
205 F = model.X_lambda[j].applyTranspose(F);
206 j = model.lambda[j];
207 dof_index_j = model.mJoints[j].q_index;
208
209 if(model.mJoints[j].mJointType != JointTypeCustom) {
210 if (model.mJoints[j].mDoFCount == 1) {
211 H(dof_index_i,dof_index_j) = F.dot(model.S[j]);
212 H(dof_index_j,dof_index_i) = H(dof_index_i,dof_index_j);
213 } else if (model.mJoints[j].mDoFCount == 3) {
214 Vector3d H_temp2 =
215 (F.transpose() * model.multdof3_S[j]).transpose();
216 LOG << F.transpose() << std::endl
217 << model.multdof3_S[j] << std::endl;
218 LOG << H_temp2.transpose() << std::endl;
219
220 H.block<1,3>(dof_index_i,dof_index_j) = H_temp2.transpose();
221 H.block<3,1>(dof_index_j,dof_index_i) = H_temp2;
222 }
223 } else if (model.mJoints[j].mJointType == JointTypeCustom){
224 unsigned int k = model.mJoints[j].custom_joint_index;
225 unsigned int dof = model.mCustomJoints[k]->mDoFCount;
226 VectorNd H_temp2 =
227 (F.transpose() * model.mCustomJoints[k]->S).transpose();
228
229 LOG << F.transpose()
230 << std::endl
231 << model.mCustomJoints[j]->S << std::endl;
232
233 LOG << H_temp2.transpose() << std::endl;
234
235 H.block(dof_index_i,dof_index_j,1,dof) = H_temp2.transpose();
236 H.block(dof_index_j,dof_index_i,dof,1) = H_temp2;
237 }
238 }
239 } else if (model.mJoints[i].mDoFCount == 3
240 && model.mJoints[i].mJointType != JointTypeCustom) {
241 Matrix63 F_63 = model.Ic[i].toMatrix() * model.multdof3_S[i];
242 H.block<3,3>(dof_index_i, dof_index_i) = model.multdof3_S[i].transpose() * F_63;
243
244 unsigned int j = i;
245 unsigned int dof_index_j = dof_index_i;
246
247 while (model.lambda[j] != 0) {
248 F_63 = model.X_lambda[j].toMatrixTranspose() * (F_63);
249 j = model.lambda[j];
250 dof_index_j = model.mJoints[j].q_index;
251
252 if(model.mJoints[j].mJointType != JointTypeCustom){
253 if (model.mJoints[j].mDoFCount == 1) {
254 Vector3d H_temp2 = F_63.transpose() * (model.S[j]);
255
256 H.block<3,1>(dof_index_i,dof_index_j) = H_temp2;
257 H.block<1,3>(dof_index_j,dof_index_i) = H_temp2.transpose();
258 } else if (model.mJoints[j].mDoFCount == 3) {
259 Matrix3d H_temp2 = F_63.transpose() * (model.multdof3_S[j]);
260
261 H.block<3,3>(dof_index_i,dof_index_j) = H_temp2;
262 H.block<3,3>(dof_index_j,dof_index_i) = H_temp2.transpose();
263 }
264 } else if (model.mJoints[j].mJointType == JointTypeCustom){
265 unsigned int k = model.mJoints[j].custom_joint_index;
266 unsigned int dof = model.mCustomJoints[k]->mDoFCount;
267
268 MatrixNd H_temp2 = F_63.transpose() * (model.mCustomJoints[k]->S);
269
270 H.block(dof_index_i,dof_index_j,3,dof) = H_temp2;
271 H.block(dof_index_j,dof_index_i,dof,3) = H_temp2.transpose();
272 }
273 }
274 } else if (model.mJoints[i].mJointType == JointTypeCustom) {
275 unsigned int kI = model.mJoints[i].custom_joint_index;
276 unsigned int dofI = model.mCustomJoints[kI]->mDoFCount;
277
278 MatrixNd F_Nd = model.Ic[i].toMatrix()
279 * model.mCustomJoints[kI]->S;
280
281 H.block(dof_index_i, dof_index_i,dofI,dofI)
282 = model.mCustomJoints[kI]->S.transpose() * F_Nd;
283
284 unsigned int j = i;
285 unsigned int dof_index_j = dof_index_i;
286
287 while (model.lambda[j] != 0) {
288 F_Nd = model.X_lambda[j].toMatrixTranspose() * (F_Nd);
289 j = model.lambda[j];
290 dof_index_j = model.mJoints[j].q_index;
291
292 if(model.mJoints[j].mJointType != JointTypeCustom){
293 if (model.mJoints[j].mDoFCount == 1) {
294 MatrixNd H_temp2 = F_Nd.transpose() * (model.S[j]);
295 H.block( dof_index_i, dof_index_j,
296 H_temp2.rows(),H_temp2.cols()) = H_temp2;
297 H.block(dof_index_j,dof_index_i,
298 H_temp2.cols(),H_temp2.rows()) = H_temp2.transpose();
299 } else if (model.mJoints[j].mDoFCount == 3) {
300 MatrixNd H_temp2 = F_Nd.transpose() * (model.multdof3_S[j]);
301 H.block(dof_index_i, dof_index_j,
302 H_temp2.rows(),H_temp2.cols()) = H_temp2;
303 H.block(dof_index_j, dof_index_i,
304 H_temp2.cols(),H_temp2.rows()) = H_temp2.transpose();
305 }
306 } else if (model.mJoints[j].mJointType == JointTypeCustom){
307 unsigned int k = model.mJoints[j].custom_joint_index;
308 unsigned int dof = model.mCustomJoints[k]->mDoFCount;
309
310 MatrixNd H_temp2 = F_Nd.transpose() * (model.mCustomJoints[k]->S);
311
312 H.block(dof_index_i,dof_index_j,3,dof) = H_temp2;
313 H.block(dof_index_j,dof_index_i,dof,3) = H_temp2.transpose();
314 }
315 }
316 }
317 }
318}
319
320RBDL_DLLAPI void ForwardDynamics (
321 Model &model,
322 const VectorNd &Q,
323 const VectorNd &QDot,
324 const VectorNd &Tau,
325 VectorNd &QDDot,
326 std::vector<SpatialVector> *f_ext) {
327 LOG << "-------- " << __func__ << " --------" << std::endl;
328
329 SpatialVector spatial_gravity (0., 0., 0., model.gravity[0], model.gravity[1], model.gravity[2]);
330
331 unsigned int i = 0;
332
333 LOG << "Q = " << Q.transpose() << std::endl;
334 LOG << "QDot = " << QDot.transpose() << std::endl;
335 LOG << "Tau = " << Tau.transpose() << std::endl;
336 LOG << "---" << std::endl;
337
338 // Reset the velocity of the root body
339 model.v[0].setZero();
340
341 for (i = 1; i < model.mBodies.size(); i++) {
342 unsigned int lambda = model.lambda[i];
343
344 jcalc (model, i, Q, QDot);
345
346 if (lambda != 0)
347 model.X_base[i] = model.X_lambda[i] * model.X_base[lambda];
348 else
349 model.X_base[i] = model.X_lambda[i];
350
351 model.v[i] = model.X_lambda[i].apply( model.v[lambda]) + model.v_J[i];
352
353 /*
354 LOG << "X_J (" << i << "):" << std::endl << X_J << std::endl;
355 LOG << "v_J (" << i << "):" << std::endl << v_J << std::endl;
356 LOG << "v_lambda" << i << ":" << std::endl << model.v.at(lambda) << std::endl;
357 LOG << "X_base (" << i << "):" << std::endl << model.X_base[i] << std::endl;
358 LOG << "X_lambda (" << i << "):" << std::endl << model.X_lambda[i] << std::endl;
359 LOG << "SpatialVelocity (" << i << "): " << model.v[i] << std::endl;
360 */
361 model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]);
362 model.I[i].setSpatialMatrix (model.IA[i]);
363
364 model.pA[i] = crossf(model.v[i],model.I[i] * model.v[i]);
365
366#ifdef RBDL_USE_CASADI_MATH
367 if (f_ext != NULL) {
368#else
369 if (f_ext != NULL && (*f_ext)[i] != SpatialVector::Zero()) {
370#endif
371 LOG << "External force (" << i << ") = " << model.X_base[i].toMatrixAdjoint() * (*f_ext)[i] << std::endl;
372 model.pA[i] -= model.X_base[i].toMatrixAdjoint() * (*f_ext)[i];
373 }
374 }
375
376 // ClearLogOutput();
377
378 LOG << "--- first loop ---" << std::endl;
379
380 for (i = model.mBodies.size() - 1; i > 0; i--) {
381 unsigned int q_index = model.mJoints[i].q_index;
382
383 if (model.mJoints[i].mDoFCount == 1
384 && model.mJoints[i].mJointType != JointTypeCustom) {
385
386 model.U[i] = model.IA[i] * model.S[i];
387 model.d[i] = model.S[i].dot(model.U[i]);
388 model.u[i] = Tau[q_index] - model.S[i].dot(model.pA[i]);
389 // LOG << "u[" << i << "] = " << model.u[i] << std::endl;
390
391 unsigned int lambda = model.lambda[i];
392 if (lambda != 0) {
393 SpatialMatrix Ia = model.IA[i]
394 - model.U[i]
395 * (model.U[i] / model.d[i]).transpose();
396
397 SpatialVector pa = model.pA[i]
398 + Ia * model.c[i]
399 + model.U[i] * model.u[i] / model.d[i];
400
401#ifdef RBDL_USE_CASADI_MATH
402 model.IA[lambda]
403 += model.X_lambda[i].toMatrixTranspose()
404 * Ia * model.X_lambda[i].toMatrix();
405
406 model.pA[lambda] += model.X_lambda[i].applyTranspose(pa);
407#else
408 model.IA[lambda].noalias()
409 += model.X_lambda[i].toMatrixTranspose()
410 * Ia * model.X_lambda[i].toMatrix();
411 model.pA[lambda].noalias()
412 += model.X_lambda[i].applyTranspose(pa);
413#endif
414
415 LOG << "pA[" << lambda << "] = "
416 << model.pA[lambda].transpose() << std::endl;
417 }
418 } else if (model.mJoints[i].mDoFCount == 3
419 && model.mJoints[i].mJointType != JointTypeCustom) {
420 model.multdof3_U[i] = model.IA[i] * model.multdof3_S[i];
421
422#ifdef RBDL_USE_CASADI_MATH
423 model.multdof3_Dinv[i] = (model.multdof3_S[i].transpose()
424 * model.multdof3_U[i]).inverse();
425#else
426 model.multdof3_Dinv[i] = (model.multdof3_S[i].transpose()
427 * model.multdof3_U[i]).inverse().eval();
428#endif
429 Vector3d tau_temp(Tau.block(q_index,0,3,1));
430 model.multdof3_u[i] = tau_temp
431 - model.multdof3_S[i].transpose() * model.pA[i];
432
433 // LOG << "multdof3_u[" << i << "] = "
434 // << model.multdof3_u[i].transpose() << std::endl;
435 unsigned int lambda = model.lambda[i];
436 if (lambda != 0) {
437 SpatialMatrix Ia = model.IA[i]
438 - model.multdof3_U[i]
439 * model.multdof3_Dinv[i]
440 * model.multdof3_U[i].transpose();
441 SpatialVector pa = model.pA[i]
442 + Ia
443 * model.c[i]
444 + model.multdof3_U[i]
445 * model.multdof3_Dinv[i]
446 * model.multdof3_u[i];
447#ifdef RBDL_USE_CASADI_MATH
448 model.IA[lambda]
449 += model.X_lambda[i].toMatrixTranspose()
450 * Ia
451 * model.X_lambda[i].toMatrix();
452
453 model.pA[lambda] += model.X_lambda[i].applyTranspose(pa);
454#else
455 model.IA[lambda].noalias()
456 += model.X_lambda[i].toMatrixTranspose()
457 * Ia
458 * model.X_lambda[i].toMatrix();
459
460 model.pA[lambda].noalias()
461 += model.X_lambda[i].applyTranspose(pa);
462#endif
463 LOG << "pA[" << lambda << "] = "
464 << model.pA[lambda].transpose()
465 << std::endl;
466 }
467 } else if (model.mJoints[i].mJointType == JointTypeCustom) {
468 unsigned int kI = model.mJoints[i].custom_joint_index;
469 unsigned int dofI = model.mCustomJoints[kI]->mDoFCount;
470 model.mCustomJoints[kI]->U =
471 model.IA[i] * model.mCustomJoints[kI]->S;
472
473#ifdef RBDL_USE_CASADI_MATH
474 model.mCustomJoints[kI]->Dinv
475 = (model.mCustomJoints[kI]->S.transpose()
476 * model.mCustomJoints[kI]->U).inverse();
477#else
478 model.mCustomJoints[kI]->Dinv
479 = (model.mCustomJoints[kI]->S.transpose()
480 * model.mCustomJoints[kI]->U).inverse().eval();
481#endif
482 VectorNd tau_temp(Tau.block(q_index,0,dofI,1));
483 model.mCustomJoints[kI]->u = tau_temp
484 - model.mCustomJoints[kI]->S.transpose() * model.pA[i];
485
486 // LOG << "multdof3_u[" << i << "] = "
487 // << model.multdof3_u[i].transpose() << std::endl;
488 unsigned int lambda = model.lambda[i];
489 if (lambda != 0) {
490 SpatialMatrix Ia = model.IA[i]
491 - (model.mCustomJoints[kI]->U
492 * model.mCustomJoints[kI]->Dinv
493 * model.mCustomJoints[kI]->U.transpose());
494 SpatialVector pa = model.pA[i]
495 + Ia * model.c[i]
496 + (model.mCustomJoints[kI]->U
497 * model.mCustomJoints[kI]->Dinv
498 * model.mCustomJoints[kI]->u);
499
500#ifdef RBDL_USE_CASADI_MATH
501 model.IA[lambda] += model.X_lambda[i].toMatrixTranspose()
502 * Ia
503 * model.X_lambda[i].toMatrix();
504 model.pA[lambda] += model.X_lambda[i].applyTranspose(pa);
505#else
506 model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose()
507 * Ia
508 * model.X_lambda[i].toMatrix();
509 model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa);
510#endif
511 LOG << "pA[" << lambda << "] = "
512 << model.pA[lambda].transpose()
513 << std::endl;
514 }
515 }
516 }
517
518 // ClearLogOutput();
519
520 model.a[0] = spatial_gravity * -1.;
521
522 for (i = 1; i < model.mBodies.size(); i++) {
523 unsigned int q_index = model.mJoints[i].q_index;
524 unsigned int lambda = model.lambda[i];
525 SpatialTransform X_lambda = model.X_lambda[i];
526
527 model.a[i] = X_lambda.apply(model.a[lambda]) + model.c[i];
528 LOG << "a'[" << i << "] = " << model.a[i].transpose() << std::endl;
529
530 if (model.mJoints[i].mDoFCount == 1
531 && model.mJoints[i].mJointType != JointTypeCustom) {
532 QDDot[q_index] = (1./model.d[i]) * (model.u[i] - model.U[i].dot(model.a[i]));
533 model.a[i] = model.a[i] + model.S[i] * QDDot[q_index];
534 } else if (model.mJoints[i].mDoFCount == 3
535 && model.mJoints[i].mJointType != JointTypeCustom) {
536 Vector3d qdd_temp = model.multdof3_Dinv[i] * (model.multdof3_u[i] - model.multdof3_U[i].transpose() * model.a[i]);
537 QDDot[q_index] = qdd_temp[0];
538 QDDot[q_index + 1] = qdd_temp[1];
539 QDDot[q_index + 2] = qdd_temp[2];
540 model.a[i] = model.a[i] + model.multdof3_S[i] * qdd_temp;
541 } else if (model.mJoints[i].mJointType == JointTypeCustom) {
542 unsigned int kI = model.mJoints[i].custom_joint_index;
543 unsigned int dofI=model.mCustomJoints[kI]->mDoFCount;
544
545 VectorNd qdd_temp = model.mCustomJoints[kI]->Dinv
546 * ( model.mCustomJoints[kI]->u
547 - model.mCustomJoints[kI]->U.transpose()
548 * model.a[i]);
549
550 for(int z=0; z<dofI; ++z){
551 QDDot[q_index+z] = qdd_temp[z];
552 }
553
554 model.a[i] = model.a[i]
555 + model.mCustomJoints[kI]->S * qdd_temp;
556 }
557 }
558
559 LOG << "QDDot = " << QDDot.transpose() << std::endl;
560}
561
562RBDL_DLLAPI void ForwardDynamicsLagrangian (
563 Model &model,
564 const VectorNd &Q,
565 const VectorNd &QDot,
566 const VectorNd &Tau,
567 VectorNd &QDDot,
568 Math::LinearSolver linear_solver,
569 std::vector<SpatialVector> *f_ext,
570 Math::MatrixNd *H,
571 Math::VectorNd *C) {
572 LOG << "-------- " << __func__ << " --------" << std::endl;
573
574 bool free_H = false;
575 bool free_C = false;
576
577 if (H == NULL) {
578 H = new MatrixNd (MatrixNd::Zero(model.dof_count, model.dof_count));
579 free_H = true;
580 }
581
582 if (C == NULL) {
583 C = new VectorNd (VectorNd::Zero(model.dof_count));
584 free_C = true;
585 }
586
587 // we set QDDot to zero to compute C properly with the InverseDynamics
588 // method.
589 QDDot.setZero();
590
591 InverseDynamics (model, Q, QDot, QDDot, (*C), f_ext);
592 CompositeRigidBodyAlgorithm (model, Q, *H, false);
593
594 LOG << "A = " << std::endl << *H << std::endl;
595 LOG << "b = " << std::endl << *C * -1. + Tau << std::endl;
596
597#ifdef RBDL_USE_CASADI_MATH
598 QDDot = H->inverse() * (*C * -1. + Tau);
599#else
600 switch (linear_solver) {
601 case (LinearSolverPartialPivLU) :
602 QDDot = H->partialPivLu().solve (*C * -1. + Tau);
603 break;
604 case (LinearSolverColPivHouseholderQR) :
605 QDDot = H->colPivHouseholderQr().solve (*C * -1. + Tau);
606 break;
607 case (LinearSolverHouseholderQR) :
608 QDDot = H->householderQr().solve (*C * -1. + Tau);
609 break;
610 case (LinearSolverLLT) :
611 QDDot = H->llt().solve (*C * -1. + Tau);
612 break;
613 default:
614 LOG << "Error: Invalid linear solver: " << linear_solver << std::endl;
615 assert (0);
616 break;
617 }
618#endif
619
620 if (free_C) {
621 delete C;
622 }
623
624 if (free_H) {
625 delete H;
626 }
627
628 LOG << "x = " << QDDot << std::endl;
629}
630
631RBDL_DLLAPI void CalcMInvTimesTau ( Model &model,
632 const VectorNd &Q,
633 const VectorNd &Tau,
634 VectorNd &QDDot,
635 bool update_kinematics) {
636
637 LOG << "Q = " << Q.transpose() << std::endl;
638 LOG << "---" << std::endl;
639
640 // Reset the velocity of the root body
641 model.v[0].setZero();
642 model.a[0].setZero();
643
644 if (update_kinematics) {
645 for (unsigned int i = 1; i < model.mBodies.size(); i++) {
646 jcalc_X_lambda_S (model, model.mJointUpdateOrder[i], Q);
647
648 model.v_J[i].setZero();
649 model.v[i].setZero();
650 model.c[i].setZero();
651 model.pA[i].setZero();
652 model.I[i].setSpatialMatrix (model.IA[i]);
653 }
654 }
655
656 for (unsigned int i = 1; i < model.mBodies.size(); i++) {
657 model.pA[i].setZero();
658 }
659
660 // ClearLogOutput();
661
662 if (update_kinematics) {
663 // Compute Articulate Body Inertias
664 for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) {
665 unsigned int q_index = model.mJoints[i].q_index;
666
667 if (model.mJoints[i].mDoFCount == 1
668 && model.mJoints[i].mJointType != JointTypeCustom) {
669 model.U[i] = model.IA[i] * model.S[i];
670 model.d[i] = model.S[i].dot(model.U[i]);
671 // LOG << "u[" << i << "] = " << model.u[i] << std::endl;
672 unsigned int lambda = model.lambda[i];
673
674 if (lambda != 0) {
675 SpatialMatrix Ia = model.IA[i] -
676 model.U[i] * (model.U[i] / model.d[i]).transpose();
677
678#ifdef RBDL_USE_CASADI_MATH
679 model.IA[lambda] += model.X_lambda[i].toMatrixTranspose()
680 * Ia
681 * model.X_lambda[i].toMatrix();
682#else
683 model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose()
684 * Ia
685 * model.X_lambda[i].toMatrix();
686#endif
687 }
688 } else if (model.mJoints[i].mDoFCount == 3
689 && model.mJoints[i].mJointType != JointTypeCustom) {
690
691 model.multdof3_U[i] = model.IA[i] * model.multdof3_S[i];
692
693#ifdef RBDL_USE_CASADI_MATH
694 model.multdof3_Dinv[i] =
695 (model.multdof3_S[i].transpose() * model.multdof3_U[i]).inverse();
696#else
697 model.multdof3_Dinv[i] =
698 (model.multdof3_S[i].transpose()*model.multdof3_U[i]).inverse().eval();
699#endif
700 // LOG << "mCustomJoints[kI]->u[" << i << "] = "
701 //<< model.mCustomJoints[kI]->u[i].transpose() << std::endl;
702
703 unsigned int lambda = model.lambda[i];
704
705 if (lambda != 0) {
706 SpatialMatrix Ia = model.IA[i]
707 - ( model.multdof3_U[i]
708 * model.multdof3_Dinv[i]
709 * model.multdof3_U[i].transpose());
710
711#ifdef RBDL_USE_CASADI_MATH
712 model.IA[lambda] +=
713 model.X_lambda[i].toMatrixTranspose()
714 * Ia * model.X_lambda[i].toMatrix();
715#else
716 model.IA[lambda].noalias() +=
717 model.X_lambda[i].toMatrixTranspose()
718 * Ia
719 * model.X_lambda[i].toMatrix();
720#endif
721 }
722 } else if (model.mJoints[i].mJointType == JointTypeCustom) {
723 unsigned int kI = model.mJoints[i].custom_joint_index;
724 unsigned int dofI = model.mCustomJoints[kI]->mDoFCount;
725 model.mCustomJoints[kI]->U = model.IA[i] * model.mCustomJoints[kI]->S;
726
727#ifdef RBDL_USE_CASADI_MATH
728 model.mCustomJoints[kI]->Dinv=(model.mCustomJoints[kI]->S.transpose()
729 * model.mCustomJoints[kI]->U
730 ).inverse();
731#else
732 model.mCustomJoints[kI]->Dinv = (model.mCustomJoints[kI]->S.transpose()
733 * model.mCustomJoints[kI]->U
734 ).inverse().eval();
735#endif
736 // LOG << "mCustomJoints[kI]->u[" << i << "] = "
737 //<< model.mCustomJoints[kI]->u.transpose() << std::endl;
738 unsigned int lambda = model.lambda[i];
739
740 if (lambda != 0) {
741 SpatialMatrix Ia = model.IA[i]
742 - ( model.mCustomJoints[kI]->U
743 * model.mCustomJoints[kI]->Dinv
744 * model.mCustomJoints[kI]->U.transpose());
745#ifdef RBDL_USE_CASADI_MATH
746 model.IA[lambda] += model.X_lambda[i].toMatrixTranspose()
747 * Ia * model.X_lambda[i].toMatrix();
748#else
749 model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose()
750 * Ia
751 * model.X_lambda[i].toMatrix();
752#endif
753 }
754 }
755 }
756 }
757
758 // compute articulated bias forces
759 for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) {
760 unsigned int q_index = model.mJoints[i].q_index;
761
762 if (model.mJoints[i].mDoFCount == 1
763 && model.mJoints[i].mJointType != JointTypeCustom) {
764
765 model.u[i] = Tau[q_index] - model.S[i].dot(model.pA[i]);
766 // LOG << "u[" << i << "] = " << model.u[i] << std::endl;
767 unsigned int lambda = model.lambda[i];
768 if (lambda != 0) {
769 SpatialVector pa = model.pA[i] + model.U[i] * model.u[i] / model.d[i];
770
771#ifdef RBDL_USE_CASADI_MATH
772 model.pA[lambda] += model.X_lambda[i].applyTranspose(pa);
773#else
774 model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa);
775#endif
776 LOG << "pA[" << lambda << "] = "
777 << model.pA[lambda].transpose() << std::endl;
778 }
779 } else if (model.mJoints[i].mDoFCount == 3
780 && model.mJoints[i].mJointType != JointTypeCustom) {
781
782 Vector3d tau_temp ( Tau[q_index],
783 Tau[q_index + 1],
784 Tau[q_index + 2]);
785 model.multdof3_u[i] = tau_temp
786 - model.multdof3_S[i].transpose()*model.pA[i];
787 // LOG << "multdof3_u[" << i << "] = "
788 // << model.multdof3_u[i].transpose() << std::endl;
789 unsigned int lambda = model.lambda[i];
790
791 if (lambda != 0) {
792 SpatialVector pa = model.pA[i]
793 + model.multdof3_U[i]
794 * model.multdof3_Dinv[i]
795 * model.multdof3_u[i];
796
797#ifdef RBDL_USE_CASADI_MATH
798 model.pA[lambda] += model.X_lambda[i].applyTranspose(pa);
799#else
800 model.pA[lambda].noalias() +=
801 model.X_lambda[i].applyTranspose(pa);
802#endif
803 LOG << "pA[" << lambda << "] = "
804 << model.pA[lambda].transpose() << std::endl;
805 }
806 } else if (model.mJoints[i].mJointType == JointTypeCustom) {
807 unsigned int kI = model.mJoints[i].custom_joint_index;
808 unsigned int dofI = model.mCustomJoints[kI]->mDoFCount;
809 VectorNd tau_temp(Tau.block(q_index,0,dofI,1));
810
811 model.mCustomJoints[kI]->u =
812 tau_temp - ( model.mCustomJoints[kI]->S.transpose()* model.pA[i]);
813 // LOG << "mCustomJoints[kI]->u"
814 // << model.mCustomJoints[kI]->u.transpose() << std::endl;
815 unsigned int lambda = model.lambda[i];
816
817 if (lambda != 0) {
818 SpatialVector pa = model.pA[i]
819 + ( model.mCustomJoints[kI]->U
820 * model.mCustomJoints[kI]->Dinv
821 * model.mCustomJoints[kI]->u);
822
823#ifdef RBDL_USE_CASADI_MATH
824 model.pA[lambda] += model.X_lambda[i].applyTranspose(pa);
825#else
826 model.pA[lambda].noalias() +=
827 model.X_lambda[i].applyTranspose(pa);
828#endif
829 LOG << "pA[" << lambda << "] = "
830 << model.pA[lambda].transpose() << std::endl;
831 }
832 }
833 }
834
835 // ClearLogOutput();
836
837 for (unsigned int i = 1; i < model.mBodies.size(); i++) {
838 unsigned int q_index = model.mJoints[i].q_index;
839 unsigned int lambda = model.lambda[i];
840 SpatialTransform X_lambda = model.X_lambda[i];
841
842 model.a[i] = X_lambda.apply(model.a[lambda]) + model.c[i];
843 LOG << "a'[" << i << "] = " << model.a[i].transpose() << std::endl;
844
845 if (model.mJoints[i].mDoFCount == 1
846 && model.mJoints[i].mJointType != JointTypeCustom) {
847 QDDot[q_index] = (1./model.d[i])*(model.u[i]-model.U[i].dot(model.a[i]));
848 model.a[i] = model.a[i] + model.S[i] * QDDot[q_index];
849 } else if (model.mJoints[i].mDoFCount == 3
850 && model.mJoints[i].mJointType != JointTypeCustom) {
851 Vector3d qdd_temp =
852 model.multdof3_Dinv[i] * (model.multdof3_u[i]
853 - model.multdof3_U[i].transpose()*model.a[i]);
854
855 QDDot[q_index] = qdd_temp[0];
856 QDDot[q_index + 1] = qdd_temp[1];
857 QDDot[q_index + 2] = qdd_temp[2];
858 model.a[i] = model.a[i] + model.multdof3_S[i] * qdd_temp;
859 } else if (model.mJoints[i].mJointType == JointTypeCustom) {
860 unsigned int kI = model.mJoints[i].custom_joint_index;
861 unsigned int dofI = model.mCustomJoints[kI]->mDoFCount;
862
863 VectorNd qdd_temp = model.mCustomJoints[kI]->Dinv
864 * ( model.mCustomJoints[kI]->u
865 - model.mCustomJoints[kI]->U.transpose() * model.a[i]);
866
867 for(unsigned z = 0; z < dofI; ++z){
868 QDDot[q_index+z] = qdd_temp[z];
869 }
870
871 model.a[i] = model.a[i]
872 + model.mCustomJoints[kI]->S * qdd_temp;
873 }
874 }
875
876 LOG << "QDDot = " << QDDot.transpose() << std::endl;
877}
878
879} /* namespace RigidBodyDynamics */
Body.h
RBDLCasadiMath::MX_Xd_dynamic::transpose
MX_Xd_dynamic transpose() const
Definition: MX_Xd_dynamic.h:159
RBDLCasadiMath::MX_Xd_dynamic::Zero
static MX_Xd_dynamic Zero(unsigned int nrows, unsigned int ncols=1)
Definition: MX_Xd_dynamic.h:52
RBDLCasadiMath::MX_Xd_dynamic::block
MX_Xd_SubMatrix block(unsigned int row_start, unsigned int col_start)
Definition: MX_Xd_dynamic.h:108
RBDLCasadiMath::MX_Xd_static::block
MX_Xd_SubMatrix block(unsigned int row_start, unsigned int col_start)
Definition: MX_Xd_static.h:291
RigidBodyDynamics::InverseDynamics
RBDL_DLLAPI void InverseDynamics(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, Math::VectorNd &Tau, std::vector< Math::SpatialVector > *f_ext=NULL)
Computes inverse dynamics with the Newton-Euler Algorithm.
RigidBodyDynamics::CalcMInvTimesTau
RBDL_DLLAPI 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...
RigidBodyDynamics::NonlinearEffects
RBDL_DLLAPI void NonlinearEffects(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::VectorNd &Tau, std::vector< Math::SpatialVector > *f_ext=NULL)
Computes the coriolis forces.
RigidBodyDynamics::ForwardDynamicsLagrangian
RBDL_DLLAPI void ForwardDynamicsLagrangian(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, Math::VectorNd &QDDot, Math::LinearSolver linear_solver=Math::LinearSolverColPivHouseholderQR, std::vector< Math::SpatialVector > *f_ext=NULL, Math::MatrixNd *H=NULL, Math::VectorNd *C=NULL)
Computes forward dynamics by building and solving the full Lagrangian equation.
RigidBodyDynamics::ForwardDynamics
RBDL_DLLAPI void ForwardDynamics(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, Math::VectorNd &QDDot, std::vector< Math::SpatialVector > *f_ext=NULL)
Computes forward dynamics with the Articulated Body Algorithm.
RigidBodyDynamics::CompositeRigidBodyAlgorithm
RBDL_DLLAPI 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.
RigidBodyDynamics::Math::SpatialMatrix
SpatialMatrix_t SpatialMatrix
Definition: rbdl_math.h:62
RigidBodyDynamics::Math::Matrix3d
Matrix3_t Matrix3d
Definition: rbdl_math.h:60
RigidBodyDynamics::Math::VectorNd
VectorN_t VectorNd
Definition: rbdl_math.h:66
RigidBodyDynamics::Math::crossf
SpatialMatrix crossf(const SpatialVector &v)
Definition: SpatialAlgebraOperators.h:425
RigidBodyDynamics::Math::SpatialVector
SpatialVector_t SpatialVector
Definition: rbdl_math.h:61
RigidBodyDynamics::Math::crossm
SpatialMatrix crossm(const SpatialVector &v)
Definition: SpatialAlgebraOperators.h:403
RigidBodyDynamics::Math::Matrix63
Matrix63_t Matrix63
Definition: rbdl_math.h:63
RigidBodyDynamics::Math::Vector3d
Vector3_t Vector3d
Definition: rbdl_math.h:57
RigidBodyDynamics::Math::MatrixNd
MatrixN_t MatrixNd
Definition: rbdl_math.h:67
RigidBodyDynamics
Definition: Joint.cc:45
RigidBodyDynamics::jcalc
RBDL_DLLAPI void jcalc(Model &model, unsigned int joint_id, const VectorNd &q, const VectorNd &qdot)
Definition: Joint.cc:49
RigidBodyDynamics::jcalc_X_lambda_S
RBDL_DLLAPI void jcalc_X_lambda_S(Model &model, unsigned int joint_id, const VectorNd &q)
Definition: Joint.cc:403
rbdl_mathutils.h
LinearSolverPartialPivLU
LinearSolverPartialPivLU
Definition: rbdl_mathutils.h:29
LinearSolverColPivHouseholderQR
LinearSolverColPivHouseholderQR
Definition: rbdl_mathutils.h:30
LinearSolverHouseholderQR
LinearSolverHouseholderQR
Definition: rbdl_mathutils.h:31
LinearSolverLLT
LinearSolverLLT
Definition: rbdl_mathutils.h:32