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