Rigid Body Dynamics Library
Kinematics.cc
Go to the documentation of this file.
1 /*
2  * RBDL - Rigid Body Dynamics Library
3  * Copyright (c) 2011-2016 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
4  *
5  * Licensed under the zlib license. See LICENSE for more details.
6  */
7 
8 #include <iostream>
9 #include <limits>
10 #include <cstring>
11 #include <assert.h>
12 
13 #include "rbdl/rbdl_mathutils.h"
14 #include "rbdl/Logging.h"
15 
16 #include "rbdl/Model.h"
17 #include "rbdl/Kinematics.h"
18 
19 namespace RigidBodyDynamics {
20 
21 using namespace Math;
22 
23 RBDL_DLLAPI void UpdateKinematics(
24  Model &model,
25  const VectorNd &Q,
26  const VectorNd &QDot,
27  const VectorNd &QDDot) {
28  LOG << "-------- " << __func__ << " --------" << std::endl;
29 
30  unsigned int i;
31 
32  SpatialVector spatial_gravity (0.,
33  0.,
34  0.,
35  model.gravity[0],
36  model.gravity[1],
37  model.gravity[2]);
38 
39  model.a[0].setZero();
40  //model.a[0] = spatial_gravity;
41 
42  for (i = 1; i < model.mBodies.size(); i++) {
43  unsigned int q_index = model.mJoints[i].q_index;
44 
45  Joint joint = model.mJoints[i];
46  unsigned int lambda = model.lambda[i];
47 
48  jcalc (model, i, Q, QDot);
49 
50  model.X_lambda[i] = model.X_J[i] * model.X_T[i];
51 
52  if (lambda != 0) {
53  model.X_base[i] = model.X_lambda[i] * model.X_base[lambda];
54  model.v[i] = model.X_lambda[i].apply(model.v[lambda]) + model.v_J[i];
55  } else {
56  model.X_base[i] = model.X_lambda[i];
57  model.v[i] = model.v_J[i];
58  }
59 
60  model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]);
61  model.a[i] = model.X_lambda[i].apply(model.a[lambda]) + model.c[i];
62 
63  if(model.mJoints[i].mJointType != JointTypeCustom){
64  if (model.mJoints[i].mDoFCount == 1) {
65  model.a[i] = model.a[i] + model.S[i] * QDDot[q_index];
66  } else if (model.mJoints[i].mDoFCount == 3) {
67  Vector3d omegadot_temp (QDDot[q_index],
68  QDDot[q_index + 1],
69  QDDot[q_index + 2]);
70  model.a[i] = model.a[i] + model.multdof3_S[i] * omegadot_temp;
71  }
72  } else {
73  unsigned int custom_index = model.mJoints[i].custom_joint_index;
74  const CustomJoint* custom_joint = model.mCustomJoints[custom_index];
75  unsigned int joint_dof_count = custom_joint->mDoFCount;
76 
77  model.a[i] = model.a[i]
78  + ( model.mCustomJoints[custom_index]->S
79  * QDDot.block(q_index, 0, joint_dof_count, 1));
80  }
81  }
82 
83  for (i = 1; i < model.mBodies.size(); i++) {
84  LOG << "a[" << i << "] = " << model.a[i].transpose() << std::endl;
85  }
86 }
87 
88 RBDL_DLLAPI void UpdateKinematicsCustom(
89  Model &model,
90  const VectorNd *Q,
91  const VectorNd *QDot,
92  const VectorNd *QDDot) {
93  LOG << "-------- " << __func__ << " --------" << std::endl;
94 
95  unsigned int i;
96 
97  if (Q) {
98  for (i = 1; i < model.mBodies.size(); i++) {
99  unsigned int lambda = model.lambda[i];
100 
101  VectorNd QDot_zero (VectorNd::Zero (model.q_size));
102 
103  jcalc (model, i, (*Q), QDot_zero);
104 
105  model.X_lambda[i] = model.X_J[i] * model.X_T[i];
106 
107  if (lambda != 0) {
108  model.X_base[i] = model.X_lambda[i] * model.X_base[lambda];
109  } else {
110  model.X_base[i] = model.X_lambda[i];
111  }
112  }
113  }
114 
115  if (QDot) {
116  for (i = 1; i < model.mBodies.size(); i++) {
117  unsigned int lambda = model.lambda[i];
118 
119  jcalc (model, i, *Q, *QDot);
120 
121  if (lambda != 0) {
122  model.v[i] = model.X_lambda[i].apply(model.v[lambda]) + model.v_J[i];
123  model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]);
124  } else {
125  model.v[i] = model.v_J[i];
126  model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]);
127  }
128  // LOG << "v[" << i << "] = " << model.v[i].transpose() << std::endl;
129  }
130  }
131 
132  if (QDDot) {
133  for (i = 1; i < model.mBodies.size(); i++) {
134  unsigned int q_index = model.mJoints[i].q_index;
135 
136  unsigned int lambda = model.lambda[i];
137 
138  if (lambda != 0) {
139  model.a[i] = model.X_lambda[i].apply(model.a[lambda]) + model.c[i];
140  } else {
141  model.a[i] = model.c[i];
142  }
143 
144  if( model.mJoints[i].mJointType != JointTypeCustom){
145  if (model.mJoints[i].mDoFCount == 1) {
146  model.a[i] = model.a[i] + model.S[i] * (*QDDot)[q_index];
147  } else if (model.mJoints[i].mDoFCount == 3) {
148  Vector3d omegadot_temp ((*QDDot)[q_index],
149  (*QDDot)[q_index + 1],
150  (*QDDot)[q_index + 2]);
151  model.a[i] = model.a[i]
152  + model.multdof3_S[i] * omegadot_temp;
153  }
154  } else {
155  unsigned int k = model.mJoints[i].custom_joint_index;
156 
157  const CustomJoint* custom_joint = model.mCustomJoints[k];
158  unsigned int joint_dof_count = custom_joint->mDoFCount;
159 
160  model.a[i] = model.a[i]
161  + ( (model.mCustomJoints[k]->S)
162  *(QDDot->block(q_index, 0, joint_dof_count, 1)));
163  }
164  }
165  }
166 }
167 
169  Model &model,
170  const VectorNd &Q,
171  unsigned int body_id,
172  const Vector3d &point_body_coordinates,
173  bool update_kinematics) {
174  // update the Kinematics if necessary
175  if (update_kinematics) {
176  UpdateKinematicsCustom (model, &Q, NULL, NULL);
177  }
178 
179  if (body_id >= model.fixed_body_discriminator) {
180  unsigned int fbody_id = body_id - model.fixed_body_discriminator;
181  unsigned int parent_id = model.mFixedBodies[fbody_id].mMovableParent;
182 
183  Matrix3d fixed_rotation =
184  model.mFixedBodies[fbody_id].mParentTransform.E.transpose();
185  Vector3d fixed_position = model.mFixedBodies[fbody_id].mParentTransform.r;
186 
187  Matrix3d parent_body_rotation = model.X_base[parent_id].E.transpose();
188  Vector3d parent_body_position = model.X_base[parent_id].r;
189 
190  return (parent_body_position
191  + (parent_body_rotation
192  * (fixed_position + fixed_rotation * (point_body_coordinates))) );
193  }
194 
195  Matrix3d body_rotation = model.X_base[body_id].E.transpose();
196  Vector3d body_position = model.X_base[body_id].r;
197 
198  return body_position + body_rotation * point_body_coordinates;
199 }
200 
202  Model &model,
203  const VectorNd &Q,
204  unsigned int body_id,
205  const Vector3d &point_base_coordinates,
206  bool update_kinematics) {
207  if (update_kinematics) {
208  UpdateKinematicsCustom (model, &Q, NULL, NULL);
209  }
210 
211  if (body_id >= model.fixed_body_discriminator) {
212  unsigned int fbody_id = body_id - model.fixed_body_discriminator;
213  unsigned int parent_id = model.mFixedBodies[fbody_id].mMovableParent;
214 
215  Matrix3d fixed_rotation = model.mFixedBodies[fbody_id].mParentTransform.E;
216  Vector3d fixed_position = model.mFixedBodies[fbody_id].mParentTransform.r;
217 
218  Matrix3d parent_body_rotation = model.X_base[parent_id].E;
219  Vector3d parent_body_position = model.X_base[parent_id].r;
220 
221  return (fixed_rotation
222  * ( - fixed_position
223  - parent_body_rotation
224  * (parent_body_position - point_base_coordinates)));
225  }
226 
227  Matrix3d body_rotation = model.X_base[body_id].E;
228  Vector3d body_position = model.X_base[body_id].r;
229 
230  return body_rotation * (point_base_coordinates - body_position);
231 }
232 
234  Model &model,
235  const VectorNd &Q,
236  const unsigned int body_id,
237  bool update_kinematics) {
238  // update the Kinematics if necessary
239  if (update_kinematics) {
240  UpdateKinematicsCustom (model, &Q, NULL, NULL);
241  }
242 
243  if (body_id >= model.fixed_body_discriminator) {
244  unsigned int fbody_id = body_id - model.fixed_body_discriminator;
245  model.mFixedBodies[fbody_id].mBaseTransform =
246  model.mFixedBodies[fbody_id].mParentTransform
247  * model.X_base[model.mFixedBodies[fbody_id].mMovableParent];
248 
249  return model.mFixedBodies[fbody_id].mBaseTransform.E;
250  }
251 
252  return model.X_base[body_id].E;
253 }
254 
255 RBDL_DLLAPI void CalcPointJacobian (
256  Model &model,
257  const VectorNd &Q,
258  unsigned int body_id,
259  const Vector3d &point_position,
260  MatrixNd &G,
261  bool update_kinematics) {
262  LOG << "-------- " << __func__ << " --------" << std::endl;
263 
264  // update the Kinematics if necessary
265  if (update_kinematics) {
266  UpdateKinematicsCustom (model, &Q, NULL, NULL);
267  }
268 
269  SpatialTransform point_trans =
270  SpatialTransform (Matrix3d::Identity(),
271  CalcBodyToBaseCoordinates ( model,
272  Q,
273  body_id,
274  point_position,
275  false));
276 
277  assert (G.rows() == 3 && G.cols() == model.qdot_size );
278 
279  unsigned int reference_body_id = body_id;
280 
281  if (model.IsFixedBodyId(body_id)) {
282  unsigned int fbody_id = body_id - model.fixed_body_discriminator;
283  reference_body_id = model.mFixedBodies[fbody_id].mMovableParent;
284  }
285 
286  unsigned int j = reference_body_id;
287 
288  // e[j] is set to 1 if joint j contributes to the jacobian that we are
289  // computing. For all other joints the column will be zero.
290  while (j != 0) {
291  unsigned int q_index = model.mJoints[j].q_index;
292 
293  if(model.mJoints[j].mJointType != JointTypeCustom){
294  if (model.mJoints[j].mDoFCount == 1) {
295  G.block(0,q_index, 3, 1) =
296  point_trans.apply(
297  model.X_base[j].inverse().apply(
298  model.S[j])).block(3,0,3,1);
299  } else if (model.mJoints[j].mDoFCount == 3) {
300  G.block(0, q_index, 3, 3) =
301  ((point_trans
302  * model.X_base[j].inverse()).toMatrix()
303  * model.multdof3_S[j]).block(3,0,3,3);
304  }
305  } else {
306  unsigned int k = model.mJoints[j].custom_joint_index;
307 
308  G.block(0, q_index, 3, model.mCustomJoints[k]->mDoFCount) =
309  ((point_trans
310  * model.X_base[j].inverse()).toMatrix()
311  * model.mCustomJoints[k]->S).block(
312  3,0,3,model.mCustomJoints[k]->mDoFCount);
313  }
314 
315  j = model.lambda[j];
316  }
317 }
318 
319 RBDL_DLLAPI void CalcPointJacobian6D (
320  Model &model,
321  const VectorNd &Q,
322  unsigned int body_id,
323  const Vector3d &point_position,
324  MatrixNd &G,
325  bool update_kinematics) {
326  LOG << "-------- " << __func__ << " --------" << std::endl;
327 
328  // update the Kinematics if necessary
329  if (update_kinematics) {
330  UpdateKinematicsCustom (model, &Q, NULL, NULL);
331  }
332 
333  SpatialTransform point_trans =
334  SpatialTransform (Matrix3d::Identity(),
336  Q,
337  body_id,
338  point_position,
339  false));
340 
341  assert (G.rows() == 6 && G.cols() == model.qdot_size );
342 
343  unsigned int reference_body_id = body_id;
344 
345  if (model.IsFixedBodyId(body_id)) {
346  unsigned int fbody_id = body_id - model.fixed_body_discriminator;
347  reference_body_id = model.mFixedBodies[fbody_id].mMovableParent;
348  }
349 
350  unsigned int j = reference_body_id;
351 
352  while (j != 0) {
353  unsigned int q_index = model.mJoints[j].q_index;
354 
355  if(model.mJoints[j].mJointType != JointTypeCustom){
356  if (model.mJoints[j].mDoFCount == 1) {
357  G.block(0,q_index, 6, 1)
358  = point_trans.apply(
359  model.X_base[j].inverse().apply(
360  model.S[j])).block(0,0,6,1);
361  } else if (model.mJoints[j].mDoFCount == 3) {
362  G.block(0, q_index, 6, 3)
363  = ((point_trans
364  * model.X_base[j].inverse()).toMatrix()
365  * model.multdof3_S[j]).block(0,0,6,3);
366  }
367  } else {
368  unsigned int k = model.mJoints[j].custom_joint_index;
369 
370  G.block(0, q_index, 6, model.mCustomJoints[k]->mDoFCount)
371  = ((point_trans
372  * model.X_base[j].inverse()).toMatrix()
373  * model.mCustomJoints[k]->S).block(
374  0,0,6,model.mCustomJoints[k]->mDoFCount);
375  }
376 
377  j = model.lambda[j];
378  }
379 }
380 
381 RBDL_DLLAPI void CalcBodySpatialJacobian (
382  Model &model,
383  const VectorNd &Q,
384  unsigned int body_id,
385  MatrixNd &G,
386  bool update_kinematics) {
387  LOG << "-------- " << __func__ << " --------" << std::endl;
388 
389  // update the Kinematics if necessary
390  if (update_kinematics) {
391  UpdateKinematicsCustom (model, &Q, NULL, NULL);
392  }
393 
394  assert (G.rows() == 6 && G.cols() == model.qdot_size );
395 
396  unsigned int reference_body_id = body_id;
397 
398  SpatialTransform base_to_body;
399 
400  if (model.IsFixedBodyId(body_id)) {
401  unsigned int fbody_id = body_id
402  - model.fixed_body_discriminator;
403 
404  reference_body_id = model
405  .mFixedBodies[fbody_id]
406  .mMovableParent;
407 
408  base_to_body = model.mFixedBodies[fbody_id]
409  .mParentTransform
410  * model.X_base[reference_body_id];
411  } else {
412  base_to_body = model.X_base[reference_body_id];
413  }
414 
415  unsigned int j = reference_body_id;
416 
417  while (j != 0) {
418  unsigned int q_index = model.mJoints[j].q_index;
419 
420  if(model.mJoints[j].mJointType != JointTypeCustom){
421  if (model.mJoints[j].mDoFCount == 1) {
422  G.block(0,q_index,6,1) =
423  base_to_body.apply(
424  model.X_base[j]
425  .inverse()
426  .apply(model.S[j])
427  );
428  } else if (model.mJoints[j].mDoFCount == 3) {
429  G.block(0,q_index,6,3) =
430  (base_to_body * model.X_base[j].inverse()
431  ).toMatrix() * model.multdof3_S[j];
432  }
433  }else if(model.mJoints[j].mJointType == JointTypeCustom) {
434  unsigned int k = model.mJoints[j].custom_joint_index;
435 
436  G.block(0,q_index,6,model.mCustomJoints[k]->mDoFCount ) =
437  (base_to_body * model.X_base[j].inverse()
438  ).toMatrix() * model.mCustomJoints[k]->S;
439  }
440 
441  j = model.lambda[j];
442  }
443 }
444 
446  Model &model,
447  const VectorNd &Q,
448  const VectorNd &QDot,
449  unsigned int body_id,
450  const Vector3d &point_position,
451  bool update_kinematics) {
452  LOG << "-------- " << __func__ << " --------" << std::endl;
453  assert (model.IsBodyId(body_id));
454  assert (model.q_size == Q.size());
455  assert (model.qdot_size == QDot.size());
456 
457  // Reset the velocity of the root body
458  model.v[0].setZero();
459 
460  // update the Kinematics with zero acceleration
461  if (update_kinematics) {
462  UpdateKinematicsCustom (model, &Q, &QDot, NULL);
463  }
464 
465  unsigned int reference_body_id = body_id;
466  Vector3d reference_point = point_position;
467 
468  if (model.IsFixedBodyId(body_id)) {
469  unsigned int fbody_id = body_id - model.fixed_body_discriminator;
470  reference_body_id = model.mFixedBodies[fbody_id].mMovableParent;
471  Vector3d base_coords =
472  CalcBodyToBaseCoordinates(model, Q, body_id, point_position, false);
473  reference_point =
474  CalcBaseToBodyCoordinates(model, Q, reference_body_id, base_coords,false);
475  }
476 
477  SpatialVector point_spatial_velocity =
479  CalcBodyWorldOrientation (model, Q, reference_body_id, false).transpose(),
480  reference_point).apply(model.v[reference_body_id]);
481 
482  return Vector3d (
483  point_spatial_velocity[3],
484  point_spatial_velocity[4],
485  point_spatial_velocity[5]
486  );
487 }
488 
490  Model &model,
491  const Math::VectorNd &Q,
492  const Math::VectorNd &QDot,
493  unsigned int body_id,
494  const Math::Vector3d &point_position,
495  bool update_kinematics) {
496  LOG << "-------- " << __func__ << " --------" << std::endl;
497  assert (model.IsBodyId(body_id));
498  assert (model.q_size == Q.size());
499  assert (model.qdot_size == QDot.size());
500 
501  // Reset the velocity of the root body
502  model.v[0].setZero();
503 
504  // update the Kinematics with zero acceleration
505  if (update_kinematics) {
506  UpdateKinematicsCustom (model, &Q, &QDot, NULL);
507  }
508 
509  unsigned int reference_body_id = body_id;
510  Vector3d reference_point = point_position;
511 
512  if (model.IsFixedBodyId(body_id)) {
513  unsigned int fbody_id = body_id - model.fixed_body_discriminator;
514  reference_body_id = model.mFixedBodies[fbody_id].mMovableParent;
515  Vector3d base_coords =
516  CalcBodyToBaseCoordinates(model, Q, body_id, point_position, false);
517  reference_point =
518  CalcBaseToBodyCoordinates(model, Q, reference_body_id, base_coords,false);
519  }
520 
521  return SpatialTransform (
522  CalcBodyWorldOrientation (model, Q, reference_body_id, false).transpose(),
523  reference_point).apply(model.v[reference_body_id]);
524 }
525 
527  Model &model,
528  const VectorNd &Q,
529  const VectorNd &QDot,
530  const VectorNd &QDDot,
531  unsigned int body_id,
532  const Vector3d &point_position,
533  bool update_kinematics) {
534  LOG << "-------- " << __func__ << " --------" << std::endl;
535 
536  // Reset the velocity of the root body
537  model.v[0].setZero();
538  model.a[0].setZero();
539 
540  if (update_kinematics)
541  UpdateKinematics (model, Q, QDot, QDDot);
542 
543  LOG << std::endl;
544 
545  unsigned int reference_body_id = body_id;
546  Vector3d reference_point = point_position;
547 
548  if (model.IsFixedBodyId(body_id)) {
549  unsigned int fbody_id = body_id - model.fixed_body_discriminator;
550  reference_body_id = model.mFixedBodies[fbody_id].mMovableParent;
551  Vector3d base_coords =
552  CalcBodyToBaseCoordinates (model, Q, body_id, point_position, false);
553  reference_point =
554  CalcBaseToBodyCoordinates (model, Q, reference_body_id,base_coords,false);
555  }
556 
557  SpatialTransform p_X_i (
558  CalcBodyWorldOrientation (model, Q, reference_body_id, false).transpose(),
559  reference_point);
560 
561  SpatialVector p_v_i = p_X_i.apply(model.v[reference_body_id]);
562  Vector3d a_dash = Vector3d (p_v_i[0], p_v_i[1], p_v_i[2]
563  ).cross(Vector3d (p_v_i[3], p_v_i[4], p_v_i[5]));
564  SpatialVector p_a_i = p_X_i.apply(model.a[reference_body_id]);
565 
566  return Vector3d (
567  p_a_i[3] + a_dash[0],
568  p_a_i[4] + a_dash[1],
569  p_a_i[5] + a_dash[2]
570  );
571 }
572 
574  Model &model,
575  const VectorNd &Q,
576  const VectorNd &QDot,
577  const VectorNd &QDDot,
578  unsigned int body_id,
579  const Vector3d &point_position,
580  bool update_kinematics) {
581  LOG << "-------- " << __func__ << " --------" << std::endl;
582 
583  // Reset the velocity of the root body
584  model.v[0].setZero();
585  model.a[0].setZero();
586 
587  if (update_kinematics)
588  UpdateKinematics (model, Q, QDot, QDDot);
589 
590  LOG << std::endl;
591 
592  unsigned int reference_body_id = body_id;
593  Vector3d reference_point = point_position;
594 
595  if (model.IsFixedBodyId(body_id)) {
596  unsigned int fbody_id = body_id - model.fixed_body_discriminator;
597  reference_body_id = model.mFixedBodies[fbody_id].mMovableParent;
598  Vector3d base_coords =
599  CalcBodyToBaseCoordinates (model, Q, body_id, point_position, false);
600  reference_point =
601  CalcBaseToBodyCoordinates (model, Q, reference_body_id,base_coords,false);
602  }
603 
604  SpatialTransform p_X_i (
605  CalcBodyWorldOrientation (model, Q, reference_body_id, false).transpose(),
606  reference_point);
607 
608  SpatialVector p_v_i = p_X_i.apply(model.v[reference_body_id]);
609  Vector3d a_dash = Vector3d (p_v_i[0], p_v_i[1], p_v_i[2]
610  ).cross(Vector3d (p_v_i[3], p_v_i[4], p_v_i[5]));
611  return (p_X_i.apply(model.a[reference_body_id])
612  + SpatialVector (0, 0, 0, a_dash[0], a_dash[1], a_dash[2]));
613 }
614 
615 RBDL_DLLAPI bool InverseKinematics (
616  Model &model,
617  const VectorNd &Qinit,
618  const std::vector<unsigned int>& body_id,
619  const std::vector<Vector3d>& body_point,
620  const std::vector<Vector3d>& target_pos,
621  VectorNd &Qres,
622  double step_tol,
623  double lambda,
624  unsigned int max_iter) {
625  assert (Qinit.size() == model.q_size);
626  assert (body_id.size() == body_point.size());
627  assert (body_id.size() == target_pos.size());
628 
629  MatrixNd J = MatrixNd::Zero(3 * body_id.size(), model.qdot_size);
630  VectorNd e = VectorNd::Zero(3 * body_id.size());
631 
632  Qres = Qinit;
633 
634  for (unsigned int ik_iter = 0; ik_iter < max_iter; ik_iter++) {
635  UpdateKinematicsCustom (model, &Qres, NULL, NULL);
636 
637  for (unsigned int k = 0; k < body_id.size(); k++) {
638  MatrixNd G (MatrixNd::Zero(3, model.qdot_size));
639  CalcPointJacobian (model, Qres, body_id[k], body_point[k], G, false);
640  Vector3d point_base =
641  CalcBodyToBaseCoordinates (model, Qres, body_id[k], body_point[k], false);
642  LOG << "current_pos = " << point_base.transpose() << std::endl;
643 
644  for (unsigned int i = 0; i < 3; i++) {
645  for (unsigned int j = 0; j < model.qdot_size; j++) {
646  unsigned int row = k * 3 + i;
647  LOG << "i = " << i << " j = " << j << " k = " << k << " row = "
648  << row << " col = " << j << std::endl;
649  J(row, j) = G (i,j);
650  }
651 
652  e[k * 3 + i] = target_pos[k][i] - point_base[i];
653  }
654  }
655 
656  LOG << "J = " << J << std::endl;
657  LOG << "e = " << e.transpose() << std::endl;
658 
659  // abort if we are getting "close"
660  if (e.norm() < step_tol) {
661  LOG << "Reached target close enough after " << ik_iter << " steps" << std::endl;
662  return true;
663  }
664 
665  MatrixNd JJTe_lambda2_I =
666  J * J.transpose()
667  + lambda*lambda * MatrixNd::Identity(e.size(), e.size());
668 
669  VectorNd z (body_id.size() * 3);
670 #ifndef RBDL_USE_SIMPLE_MATH
671  z = JJTe_lambda2_I.colPivHouseholderQr().solve (e);
672 #else
673  bool solve_successful = LinSolveGaussElimPivot (JJTe_lambda2_I, e, z);
674  assert (solve_successful);
675 #endif
676 
677  LOG << "z = " << z << std::endl;
678 
679  VectorNd delta_theta = J.transpose() * z;
680  LOG << "change = " << delta_theta << std::endl;
681 
682  Qres = Qres + delta_theta;
683  LOG << "Qres = " << Qres.transpose() << std::endl;
684 
685  if (delta_theta.norm() < step_tol) {
686  LOG << "reached convergence after " << ik_iter << " steps" << std::endl;
687  return true;
688  }
689 
690  VectorNd test_1 (z.size());
691  VectorNd test_res (z.size());
692 
693  test_1.setZero();
694 
695  for (unsigned int i = 0; i < z.size(); i++) {
696  test_1[i] = 1.;
697 
698  VectorNd test_delta = J.transpose() * test_1;
699 
700  test_res[i] = test_delta.squaredNorm();
701 
702  test_1[i] = 0.;
703  }
704 
705  LOG << "test_res = " << test_res.transpose() << std::endl;
706  }
707 
708  return false;
709 }
710 
711 }
Compact representation of spatial transformations.
std::vector< unsigned int > lambda
The id of the parents body.
Definition: Model.h:127
RBDL_DLLAPI void UpdateKinematicsCustom(Model &model, const VectorNd *Q, const VectorNd *QDot, const VectorNd *QDDot)
Selectively updates model internal states of body positions, velocities and/or accelerations.
Definition: Kinematics.cc:88
unsigned int q_size
The size of the -vector. For models without spherical joints the value is the same as Model::dof_coun...
Definition: Model.h:148
bool IsFixedBodyId(unsigned int body_id)
Checks whether the body is rigidly attached to another body.
Definition: Model.h:396
std::vector< Math::SpatialVector > v_J
Definition: Model.h:181
Contains all information about the rigid body model.
Definition: Model.h:121
SpatialVector_t SpatialVector
Definition: rbdl_math.h:54
RBDL_DLLAPI bool InverseKinematics(Model &model, const VectorNd &Qinit, const std::vector< unsigned int > &body_id, const std::vector< Vector3d > &body_point, const std::vector< Vector3d > &target_pos, VectorNd &Qres, double step_tol, double lambda, unsigned int max_iter)
Computes the inverse kinematics iteratively using a damped Levenberg-Marquardt method (also known as ...
Definition: Kinematics.cc:615
SpatialMatrix crossm(const SpatialVector &v)
std::vector< Math::SpatialVector > v
The spatial velocity of the bodies.
Definition: Model.h:166
std::vector< Body > mBodies
All bodies 0 ... N_B, including the base.
Definition: Model.h:262
RBDL_DLLAPI void CalcPointJacobian(Model &model, const VectorNd &Q, unsigned int body_id, const Vector3d &point_position, MatrixNd &G, bool update_kinematics)
Computes the point jacobian for a point on a body.
Definition: Kinematics.cc:255
std::vector< CustomJoint * > mCustomJoints
Definition: Model.h:202
RBDL_DLLAPI Vector3d CalcBodyToBaseCoordinates(Model &model, const VectorNd &Q, unsigned int body_id, const Vector3d &point_body_coordinates, bool update_kinematics)
Returns the base coordinates of a point given in body coordinates.
Definition: Kinematics.cc:168
RBDL_DLLAPI SpatialVector CalcPointAcceleration6D(Model &model, const VectorNd &Q, const VectorNd &QDot, const VectorNd &QDDot, unsigned int body_id, const Vector3d &point_position, bool update_kinematics)
Computes linear and angular acceleration of a point on a body.
Definition: Kinematics.cc:573
Namespace for all structures of the RigidBodyDynamics library.
Definition: Contacts.cc:22
RBDL_DLLAPI void CalcBodySpatialJacobian(Model &model, const VectorNd &Q, unsigned int body_id, MatrixNd &G, bool update_kinematics)
Computes the spatial jacobian for a body.
Definition: Kinematics.cc:381
Math::Vector3d gravity
the cartesian vector of the gravity
Definition: Model.h:162
std::vector< Math::SpatialTransform > X_lambda
Transformation from the parent body to the current body .
Definition: Model.h:235
RBDL_DLLAPI Vector3d CalcPointVelocity(Model &model, const VectorNd &Q, const VectorNd &QDot, unsigned int body_id, const Vector3d &point_position, bool update_kinematics)
Computes the velocity of a point on a body.
Definition: Kinematics.cc:445
std::vector< FixedBody > mFixedBodies
All bodies that are attached to a body via a fixed joint.
Definition: Model.h:240
RBDL_DLLAPI Math::SpatialVector CalcPointVelocity6D(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, unsigned int body_id, const Math::Vector3d &point_position, bool update_kinematics)
Computes angular and linear velocity of a point that is fixed on a body.
Definition: Kinematics.cc:489
RBDL_DLLAPI Matrix3d CalcBodyWorldOrientation(Model &model, const VectorNd &Q, const unsigned int body_id, bool update_kinematics)
Returns the orientation of a given body as 3x3 matrix.
Definition: Kinematics.cc:233
unsigned int qdot_size
The size of the.
Definition: Model.h:156
SpatialVector apply(const SpatialVector &v_sp)
RBDL_DLLAPI Vector3d CalcBaseToBodyCoordinates(Model &model, const VectorNd &Q, unsigned int body_id, const Vector3d &point_base_coordinates, bool update_kinematics)
Returns the body coordinates of a point given in base coordinates.
Definition: Kinematics.cc:201
std::vector< Math::SpatialVector > c
The velocity dependent spatial acceleration.
Definition: Model.h:208
#define LOG
Definition: Logging.h:24
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::SpatialTransform > X_J
Definition: Model.h:180
std::vector< Math::SpatialVector > a
The spatial acceleration of the bodies.
Definition: Model.h:168
unsigned int fixed_body_discriminator
Value that is used to discriminate between fixed and movable bodies.
Definition: Model.h:253
std::vector< Math::SpatialVector > S
The joint axis for joint i.
Definition: Model.h:177
RBDL_DLLAPI void CalcPointJacobian6D(Model &model, const VectorNd &Q, unsigned int body_id, const Vector3d &point_position, MatrixNd &G, bool update_kinematics)
Computes a 6-D Jacobian for a point on a body.
Definition: Kinematics.cc:319
std::vector< Math::SpatialTransform > X_base
Transformation from the base to bodies reference frame.
Definition: Model.h:237
std::vector< Math::SpatialVector > c_J
Definition: Model.h:182
RBDL_DLLAPI void UpdateKinematics(Model &model, const VectorNd &Q, const VectorNd &QDot, const VectorNd &QDDot)
Updates and computes velocities and accelerations of the bodies.
Definition: Kinematics.cc:23
Describes a joint relative to the predecessor body.
Definition: Joint.h:208
RBDL_DLLAPI Vector3d CalcPointAcceleration(Model &model, const VectorNd &Q, const VectorNd &QDot, const VectorNd &QDDot, unsigned int body_id, const Vector3d &point_position, bool update_kinematics)
Computes the linear acceleration of a point on a body.
Definition: Kinematics.cc:526
bool IsBodyId(unsigned int id)
Definition: Model.h:405
std::vector< Math::Matrix63 > multdof3_S
Motion subspace for joints with 3 degrees of freedom.
Definition: Model.h:196
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:199
std::vector< Math::SpatialTransform > X_T
Transformations from the parent body to the frame of the joint.
Definition: Model.h:188