Rigid Body Dynamics Library
Constraints.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 <sstream>
10 #include <limits>
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/Joint.h"
18 #include "rbdl/Body.h"
19 #include "rbdl/Constraints.h"
20 #include "rbdl/Dynamics.h"
21 #include "rbdl/Kinematics.h"
22 
23 namespace RigidBodyDynamics {
24 
25 using namespace Math;
26 
27 void SolveLinearSystem (
28  const MatrixNd& A,
29  const VectorNd& b,
30  VectorNd& x,
31  LinearSolver ls
32 );
33 
34 unsigned int GetMovableBodyId (Model& model, unsigned int id);
35 
37  unsigned int body_id,
38  const Vector3d &body_point,
39  const Vector3d &world_normal,
40  const char *constraint_name,
41  double normal_acceleration
42  ) {
43  assert (bound == false);
44 
45  unsigned int n_constr = size() + 1;
46 
47  std::string name_str;
48  if (constraint_name != NULL) {
49  name_str = constraint_name;
50  }
51 
52  constraintType.push_back (ContactConstraint);
53  name.push_back (name_str);
54  mContactConstraintIndices.push_back(size());
55 
56  // These variables will be used for this type of constraint.
57  body.push_back (body_id);
58  point.push_back (body_point);
59  normal.push_back (world_normal);
60 
61  // These variables will not be used.
62  body_p.push_back (0);
63  body_s.push_back (0);
64  X_p.push_back (SpatialTransform());
65  X_s.push_back (SpatialTransform());
66  constraintAxis.push_back (SpatialVector::Zero());
67  baumgarteParameters.push_back(Vector2d(0.0, 0.0));
68  err.conservativeResize(n_constr);
69  err[n_constr - 1] = 0.;
70  errd.conservativeResize(n_constr);
71  errd[n_constr - 1] = 0.;
72 
73  acceleration.conservativeResize (n_constr);
74  acceleration[n_constr - 1] = normal_acceleration;
75 
76  force.conservativeResize (n_constr);
77  force[n_constr - 1] = 0.;
78 
79  impulse.conservativeResize (n_constr);
80  impulse[n_constr - 1] = 0.;
81 
82  v_plus.conservativeResize (n_constr);
83  v_plus[n_constr - 1] = 0.;
84 
85  d_multdof3_u = std::vector<Math::Vector3d>(n_constr, Math::Vector3d::Zero());
86 
87  return n_constr - 1;
88 }
89 
91  unsigned int id_predecessor,
92  unsigned int id_successor,
93  const Math::SpatialTransform &X_predecessor,
94  const Math::SpatialTransform &X_successor,
95  const Math::SpatialVector &axis,
96  bool enable_stabilization,
97  const double stabilization_param,
98  const char *constraint_name
99  ) {
100  assert (bound == false);
101 
102  unsigned int n_constr = size() + 1;
103 
104  std::string name_str;
105  if (constraint_name != NULL) {
106  name_str = constraint_name;
107  }
108 
109  constraintType.push_back(LoopConstraint);
110  name.push_back (name_str);
111  mLoopConstraintIndices.push_back(size());
112 
113  // These variables will be used for this kind of constraint.
114  body_p.push_back (id_predecessor);
115  body_s.push_back (id_successor);
116  X_p.push_back (X_predecessor);
117  X_s.push_back (X_successor);
118  constraintAxis.push_back (axis);
119 
120  // Set up constraint stabilization
121  double baumgarte_coefficient = 0.0;
122  if (enable_stabilization) {
123  if (stabilization_param == 0.0) {
124  std::cerr << "Error: Baumgarte stabilization is enabled but the stabilization parameter is 0.0" << std::endl;
125  abort();
126  }
127  baumgarte_coefficient = 1.0 / stabilization_param;
128  }
129  baumgarteParameters.push_back(
130  Vector2d(baumgarte_coefficient, baumgarte_coefficient));
131 
132  err.conservativeResize(n_constr);
133  err[n_constr - 1] = 0.;
134  errd.conservativeResize(n_constr);
135  errd[n_constr - 1] = 0.;
136 
137  // These variables will not be used by loop constraints but are necessary
138  // for point constraints.
139  body.push_back (0);
140  point.push_back (Vector3d::Zero());
141  normal.push_back (Vector3d::Zero());
142 
143  acceleration.conservativeResize (n_constr);
144  acceleration[n_constr - 1] = 0.;
145 
146  force.conservativeResize (n_constr);
147  force[n_constr - 1] = 0.;
148 
149  impulse.conservativeResize (n_constr);
150  impulse[n_constr - 1] = 0.;
151 
152  v_plus.conservativeResize (n_constr);
153  v_plus[n_constr - 1] = 0.;
154 
155  d_multdof3_u = std::vector<Math::Vector3d>(n_constr, Math::Vector3d::Zero());
156 
157  return n_constr - 1;
158 }
159 
161  CustomConstraint *customConstraint,
162  unsigned int id_predecessor,
163  unsigned int id_successor,
164  const Math::SpatialTransform &X_predecessor,
165  const Math::SpatialTransform &X_successor,
166  bool enable_stabilization,
167  const double stabilization_param,
168  const char *constraint_name) {
169  assert (bound == false);
170 
171  unsigned int n_constr_start_idx = size();
172  unsigned int n_constr_size = size() + customConstraint->mConstraintCount;
173 
174  std::string name_str;
175  if (constraint_name != NULL) {
176  name_str = constraint_name;
177  }
178 
179  SpatialVector axis = SpatialVector::Zero();
180  std::stringstream nameConstraintIndex;
181 
182  for(unsigned int i = 0; i < customConstraint->mConstraintCount; ++i ){
183  constraintType.push_back( ConstraintTypeCustom );
184  nameConstraintIndex << name_str << "_" << i;
185  name.push_back (nameConstraintIndex.str());
186  nameConstraintIndex.str(std::string());
187  if(i==0){
188  mCustomConstraintIndices.push_back(n_constr_start_idx);
189  }
190  // These variables will be used for each CustomConstraint
191  body_p.push_back (id_predecessor);
192  body_s.push_back (id_successor);
193  X_p.push_back (X_predecessor);
194  X_s.push_back (X_successor);
195  constraintAxis.push_back (axis);
196 
197  // Set up constraint stabilization
198  double baumgarte_coefficient = 0.0;
199  if (enable_stabilization) {
200  if (stabilization_param == 0.0) {
201  std::cerr << "Error: Baumgarte stabilization is enabled but the stabilization parameter is 0.0" << std::endl;
202  abort();
203  }
204  baumgarte_coefficient = 1.0 / stabilization_param;
205  }
206  baumgarteParameters.push_back(
207  Vector2d(baumgarte_coefficient, baumgarte_coefficient));
208 
209  // These variables will not be used in CustomConstraints but are kept
210  // so that the indexing across all of the ConstraintSet variables
211  // remains preserved.
212  body.push_back (0);
213  point.push_back (Vector3d::Zero());
214  normal.push_back (Vector3d::Zero());
215  }
216 
217  err.conservativeResize( n_constr_size);
218  errd.conservativeResize(n_constr_size);
219 
220  acceleration.conservativeResize ( n_constr_size);
221  force.conservativeResize ( n_constr_size);
222  impulse.conservativeResize ( n_constr_size);
223  v_plus.conservativeResize ( n_constr_size);
224  d_multdof3_u = std::vector<Math::Vector3d>(
225  n_constr_size, Math::Vector3d::Zero());
226 
227  for(unsigned int i = n_constr_start_idx; i < n_constr_size; i++){
228  err[i] = 0.;
229  errd[i] = 0.;
230  acceleration[i] = 0.;
231  force[i] = 0.;
232  impulse[i] = 0.;
233  v_plus[i] = 0.;
234  }
235 
236  mCustomConstraints.push_back(customConstraint);
237 
238  return n_constr_size - 1;
239 }
240 
241 bool ConstraintSet::Bind (const Model &model) {
242  assert (bound == false);
243 
244  if (bound) {
245  std::cerr << "Error: binding an already bound constraint set!" << std::endl;
246  abort();
247  }
248  unsigned int n_constr = size();
249 
250  H.conservativeResize (model.dof_count, model.dof_count);
251  H.setZero();
252  C.conservativeResize (model.dof_count);
253  C.setZero();
254  gamma.conservativeResize (n_constr);
255  gamma.setZero();
256  G.conservativeResize (n_constr, model.dof_count);
257  G.setZero();
258  A.conservativeResize (model.dof_count + n_constr, model.dof_count + n_constr);
259  A.setZero();
260  b.conservativeResize (model.dof_count + n_constr);
261  b.setZero();
262  x.conservativeResize (model.dof_count + n_constr);
263  x.setZero();
264 
265  Gi.conservativeResize (3, model.qdot_size);
266  GSpi.conservativeResize (6, model.qdot_size);
267  GSsi.conservativeResize (6, model.qdot_size);
268  GSJ.conservativeResize (6, model.qdot_size);
269 
270  // HouseHolderQR crashes if matrix G has more rows than columns.
271 #ifdef RBDL_USE_SIMPLE_MATH
272  GT_qr = SimpleMath::HouseholderQR<Math::MatrixNd> (G.transpose());
273 #else
274  GT_qr = Eigen::HouseholderQR<Math::MatrixNd> (G.transpose());
275 #endif
276  GT_qr_Q = MatrixNd::Zero (model.dof_count, model.dof_count);
277  Y = MatrixNd::Zero (model.dof_count, G.rows());
278  Z = MatrixNd::Zero (model.dof_count, model.dof_count - G.rows());
279  qddot_y = VectorNd::Zero (model.dof_count);
280  qddot_z = VectorNd::Zero (model.dof_count);
281 
282  K.conservativeResize (n_constr, n_constr);
283  K.setZero();
284  a.conservativeResize (n_constr);
285  a.setZero();
286  QDDot_t.conservativeResize (model.dof_count);
287  QDDot_t.setZero();
288  QDDot_0.conservativeResize (model.dof_count);
289  QDDot_0.setZero();
290  f_t.resize (n_constr, SpatialVector::Zero());
291  f_ext_constraints.resize (model.mBodies.size(), SpatialVector::Zero());
292  point_accel_0.resize (n_constr, Vector3d::Zero());
293 
294  d_pA =std::vector<SpatialVector> (model.mBodies.size(),SpatialVector::Zero());
295  d_a = std::vector<SpatialVector> (model.mBodies.size(),SpatialVector::Zero());
296  d_u = VectorNd::Zero (model.mBodies.size());
297 
298  d_IA = std::vector<SpatialMatrix> (model.mBodies.size()
299  , SpatialMatrix::Identity());
300  d_U = std::vector<SpatialVector> (model.mBodies.size(),SpatialVector::Zero());
301  d_d = VectorNd::Zero (model.mBodies.size());
302 
303  d_multdof3_u = std::vector<Math::Vector3d> (model.mBodies.size()
304  , Math::Vector3d::Zero());
305 
306  bound = true;
307 
308  return bound;
309 }
310 
312  acceleration.setZero();
313  force.setZero();
314  impulse.setZero();
315 
316  H.setZero();
317  C.setZero();
318  gamma.setZero();
319  G.setZero();
320  A.setZero();
321  b.setZero();
322  x.setZero();
323 
324  K.setZero();
325  a.setZero();
326  QDDot_t.setZero();
327  QDDot_0.setZero();
328 
329  unsigned int i;
330  for (i = 0; i < f_t.size(); i++)
331  f_t[i].setZero();
332 
333  for (i = 0; i < f_ext_constraints.size(); i++)
334  f_ext_constraints[i].setZero();
335 
336  for (i = 0; i < point_accel_0.size(); i++)
337  point_accel_0[i].setZero();
338 
339  for (i = 0; i < d_pA.size(); i++)
340  d_pA[i].setZero();
341 
342  for (i = 0; i < d_a.size(); i++)
343  d_a[i].setZero();
344 
345  d_u.setZero();
346 }
347 
348 RBDL_DLLAPI
350  Math::MatrixNd &H,
351  const Math::MatrixNd &G,
352  const Math::VectorNd &c,
353  const Math::VectorNd &gamma,
354  Math::VectorNd &qddot,
355  Math::VectorNd &lambda,
356  Math::MatrixNd &A,
357  Math::VectorNd &b,
358  Math::VectorNd &x,
359  Math::LinearSolver &linear_solver
360  ) {
361  // Build the system: Copy H
362  A.block(0, 0, c.rows(), c.rows()) = H;
363 
364  // Copy G and G^T
365  A.block(0, c.rows(), c.rows(), gamma.rows()) = G.transpose();
366  A.block(c.rows(), 0, gamma.rows(), c.rows()) = G;
367 
368  // Build the system: Copy -C + \tau
369  b.block(0, 0, c.rows(), 1) = c;
370  b.block(c.rows(), 0, gamma.rows(), 1) = gamma;
371 
372  LOG << "A = " << std::endl << A << std::endl;
373  LOG << "b = " << std::endl << b << std::endl;
374 
375  switch (linear_solver) {
376  case (LinearSolverPartialPivLU) :
377 #ifdef RBDL_USE_SIMPLE_MATH
378  // SimpleMath does not have a LU solver so just use its QR solver
379  x = A.householderQr().solve(b);
380 #else
381  x = A.partialPivLu().solve(b);
382 #endif
383  break;
385  x = A.colPivHouseholderQr().solve(b);
386  break;
388  x = A.householderQr().solve(b);
389  break;
390  default:
391  LOG << "Error: Invalid linear solver: " << linear_solver << std::endl;
392  assert (0);
393  break;
394  }
395 
396  LOG << "x = " << std::endl << x << std::endl;
397 }
398 
399 RBDL_DLLAPI
401  Model &model,
402  Math::MatrixNd &H,
403  const Math::MatrixNd &G,
404  const Math::VectorNd &c,
405  const Math::VectorNd &gamma,
406  Math::VectorNd &qddot,
407  Math::VectorNd &lambda,
408  Math::MatrixNd &K,
409  Math::VectorNd &a,
410  Math::LinearSolver linear_solver
411  ) {
412  SparseFactorizeLTL (model, H);
413 
414  MatrixNd Y (G.transpose());
415 
416  for (unsigned int i = 0; i < Y.cols(); i++) {
417  VectorNd Y_col = Y.block(0,i,Y.rows(),1);
418  SparseSolveLTx (model, H, Y_col);
419  Y.block(0,i,Y.rows(),1) = Y_col;
420  }
421 
422  VectorNd z (c);
423  SparseSolveLTx (model, H, z);
424 
425  K = Y.transpose() * Y;
426 
427  a = gamma - Y.transpose() * z;
428 
429  lambda = K.llt().solve(a);
430 
431  qddot = c + G.transpose() * lambda;
432  SparseSolveLTx (model, H, qddot);
433  SparseSolveLx (model, H, qddot);
434 }
435 
436 RBDL_DLLAPI
438  Math::MatrixNd &H,
439  const Math::MatrixNd &G,
440  const Math::VectorNd &c,
441  const Math::VectorNd &gamma,
442  Math::VectorNd &qddot,
443  Math::VectorNd &lambda,
444  Math::MatrixNd &Y,
445  Math::MatrixNd &Z,
446  Math::VectorNd &qddot_y,
447  Math::VectorNd &qddot_z,
448  Math::LinearSolver &linear_solver
449  ) {
450  switch (linear_solver) {
451  case (LinearSolverPartialPivLU) :
452 #ifdef RBDL_USE_SIMPLE_MATH
453  // SimpleMath does not have a LU solver so just use its QR solver
454  qddot_y = (G * Y).householderQr().solve (gamma);
455 #else
456  qddot_y = (G * Y).partialPivLu().solve (gamma);
457 #endif
458  break;
460  qddot_y = (G * Y).colPivHouseholderQr().solve (gamma);
461  break;
463  qddot_y = (G * Y).householderQr().solve (gamma);
464  break;
465  default:
466  LOG << "Error: Invalid linear solver: " << linear_solver << std::endl;
467  assert (0);
468  break;
469  }
470 
471  qddot_z = (Z.transpose() * H * Z).llt().solve(Z.transpose() * (c - H * Y * qddot_y));
472 
473  qddot = Y * qddot_y + Z * qddot_z;
474 
475  switch (linear_solver) {
476  case (LinearSolverPartialPivLU) :
477 #ifdef RBDL_USE_SIMPLE_MATH
478  // SimpleMath does not have a LU solver so just use its QR solver
479  qddot_y = (G * Y).householderQr().solve (gamma);
480 #else
481  lambda = (G * Y).partialPivLu().solve (Y.transpose() * (H * qddot - c));
482 #endif
483  break;
485  lambda = (G * Y).colPivHouseholderQr().solve (Y.transpose() * (H * qddot - c));
486  break;
488  lambda = (G * Y).householderQr().solve (Y.transpose() * (H * qddot - c));
489  break;
490  default:
491  LOG << "Error: Invalid linear solver: " << linear_solver << std::endl;
492  assert (0);
493  break;
494  }
495 }
496 
497 RBDL_DLLAPI
499  Model& model,
500  const Math::VectorNd &Q,
501  ConstraintSet &CS,
502  Math::VectorNd& err,
503  bool update_kinematics
504  ) {
505  assert(err.size() == CS.size());
506 
507  if(update_kinematics) {
508  UpdateKinematicsCustom (model, &Q, NULL, NULL);
509  }
510 
511  for (unsigned int i = 0; i < CS.mContactConstraintIndices.size(); i++) {
512  const unsigned int c = CS.mContactConstraintIndices[i];
513  err[c] = 0.;
514  }
515 
516  for (unsigned int i = 0; i < CS.mLoopConstraintIndices.size(); i++) {
517  const unsigned int lci = CS.mLoopConstraintIndices[i];
518 
519  // Variables used for computations.
520  Vector3d pos_p;
521  Vector3d pos_s;
522  Matrix3d rot_p;
523  Matrix3d rot_s;
524  Matrix3d rot_ps;
525  SpatialVector d;
526 
527  // Constraints computed in the predecessor body frame.
528 
529  // Compute the orientation of the two constraint frames.
530  rot_p = CalcBodyWorldOrientation (model, Q, CS.body_p[lci], false).transpose()
531  * CS.X_p[lci].E;
532  rot_s = CalcBodyWorldOrientation (model, Q, CS.body_s[lci], false).transpose()
533  * CS.X_s[lci].E;
534 
535  // Compute the orientation from the predecessor to the successor frame.
536  rot_ps = rot_p.transpose() * rot_s;
537 
538  // Compute the position of the two contact points.
539  pos_p = CalcBodyToBaseCoordinates (model, Q, CS.body_p[lci], CS.X_p[lci].r
540  , false);
541  pos_s = CalcBodyToBaseCoordinates (model, Q, CS.body_s[lci], CS.X_s[lci].r
542  , false);
543 
544  // The first three elements represent the rotation error.
545  // This formulation is equivalent to u * sin(theta), where u and theta are
546  // the angle-axis of rotation from the predecessor to the successor frame.
547  // These quantities are expressed in the predecessor frame.
548  d[0] = -0.5 * (rot_ps(1,2) - rot_ps(2,1));
549  d[1] = -0.5 * (rot_ps(2,0) - rot_ps(0,2));
550  d[2] = -0.5 * (rot_ps(0,1) - rot_ps(1,0));
551 
552  // The last three elements represent the position error.
553  // It is equivalent to the difference in the position of the two
554  // constraint points.
555  // The distance is projected on the predecessor frame to be consistent
556  // with the rotation.
557  d.block<3,1>(3,0) = rot_p.transpose() * (pos_s - pos_p);
558 
559  // Project the error on the constraint axis to find the actual error.
560  err[lci] = CS.constraintAxis[lci].transpose() * d;
561  }
562 
563  for (unsigned int i = 0; i < CS.mCustomConstraintIndices.size(); i++) {
564  const unsigned int cci = CS.mCustomConstraintIndices[i];
565  CS.mCustomConstraints[i]->CalcPositionError(model,cci,Q,CS,err, cci);
566  }
567 }
568 
569 RBDL_DLLAPI
571  Model &model,
572  const Math::VectorNd &Q,
573  ConstraintSet &CS,
574  Math::MatrixNd &G,
575  bool update_kinematics
576  ) {
577  if (update_kinematics) {
578  UpdateKinematicsCustom (model, &Q, NULL, NULL);
579  }
580 
581  // variables to check whether we need to recompute G.
582  unsigned int prev_body_id_1 = 0;
583  unsigned int prev_body_id_2 = 0;
584  SpatialTransform prev_body_X_1;
585  SpatialTransform prev_body_X_2;
586 
587  for (unsigned int i = 0; i < CS.mContactConstraintIndices.size(); i++) {
588  const unsigned int c = CS.mContactConstraintIndices[i];
589 
590  // only compute the matrix Gi if actually needed
591  if (prev_body_id_1 != CS.body[c]
592  || prev_body_X_1.r != CS.point[c]) {
593 
594  // Compute the jacobian for the point.
595  CS.Gi.setZero();
596  CalcPointJacobian (model, Q, CS.body[c], CS.point[c], CS.Gi, false);
597 
598  // Update variables for optimization check.
599  prev_body_id_1 = CS.body[c];
600  prev_body_X_1 = Xtrans(CS.point[c]);
601  }
602 
603  for(unsigned int j = 0; j < model.dof_count; j++) {
604  Vector3d gaxis (CS.Gi(0,j), CS.Gi(1,j), CS.Gi(2,j));
605  G(c,j) = gaxis.transpose() * CS.normal[c];
606  }
607  }
608 
609  // Variables used for computations.
610  Vector3d normal;
611  SpatialVector axis;
612  Vector3d pos_p;
613  Matrix3d rot_p;
614  SpatialTransform X_0p;
615 
616  for (unsigned int i = 0; i < CS.mLoopConstraintIndices.size(); i++) {
617  const unsigned int c = CS.mLoopConstraintIndices[i];
618 
619  // Only recompute variables if necessary.
620  if( prev_body_id_1 != CS.body_p[c]
621  || prev_body_id_2 != CS.body_s[c]
622  || prev_body_X_1.r != CS.X_p[c].r
623  || prev_body_X_2.r != CS.X_s[c].r
624  || prev_body_X_1.E != CS.X_p[c].E
625  || prev_body_X_2.E != CS.X_s[c].E) {
626 
627  // Compute the 6D jacobians of the two contact points.
628  CS.GSpi.setZero();
629  CS.GSsi.setZero();
630  CalcPointJacobian6D(model, Q, CS.body_p[c], CS.X_p[c].r, CS.GSpi, false);
631  CalcPointJacobian6D(model, Q, CS.body_s[c], CS.X_s[c].r, CS.GSsi, false);
632  CS.GSJ = CS.GSsi - CS.GSpi;
633 
634  // Compute position and rotation matrix from predecessor body to base.
635  pos_p = CalcBodyToBaseCoordinates (model, Q, CS.body_p[c], CS.X_p[c].r
636  , false);
637  rot_p = CalcBodyWorldOrientation (model, Q, CS.body_p[c]
638  , false).transpose()* CS.X_p[c].E;
639  X_0p = SpatialTransform (rot_p, pos_p);
640 
641  // Update variables for optimization check.
642  prev_body_id_1 = CS.body_p[c];
643  prev_body_id_2 = CS.body_s[c];
644  prev_body_X_1 = CS.X_p[c];
645  prev_body_X_2 = CS.X_s[c];
646  }
647 
648  // Express the constraint axis in the base frame.
649  axis = X_0p.apply(CS.constraintAxis[c]);
650 
651  // Compute the constraint Jacobian row.
652  G.block(c, 0, 1, model.dof_count) = axis.transpose() * CS.GSJ;
653  }
654 
655  // Go and get the CustomConstraint Jacobians
656  for (unsigned int i = 0; i < CS.mCustomConstraintIndices.size(); i++) {
657  const unsigned int cci = CS.mCustomConstraintIndices[i];
658  const unsigned int rows= CS.mCustomConstraints[i]->mConstraintCount;
659  const unsigned int cols= CS.G.cols();
660  CS.mCustomConstraints[i]->CalcConstraintsJacobianAndConstraintAxis(
661  model,cci,Q,CS,G, cci,0);
662  }
663 }
664 
665 RBDL_DLLAPI
667  Model& model,
668  const Math::VectorNd &Q,
669  const Math::VectorNd &QDot,
670  ConstraintSet &CS,
671  Math::VectorNd& err,
672  bool update_kinematics
673  ) {
674 
675  //This works for the contact and loop constraints because they are
676  //time invariant. But this does not necessarily work for the CustomConstraints
677  //which can be time-varying. And thus the parts of err associated with the
678  //custom constraints must be updated.
679  //MatrixNd G(MatrixNd::Zero(CS.size(), model.dof_count));
680  CalcConstraintsJacobian (model, Q, CS, CS.G, update_kinematics);
681  err = CS.G * QDot;
682 
683  unsigned int cci, rows, cols;
684  for (unsigned int i = 0; i < CS.mCustomConstraintIndices.size(); i++) {
685  cci = CS.mCustomConstraintIndices[i];
686  rows= CS.mCustomConstraints[i]->mConstraintCount;
687  cols= CS.G.cols();
688  CS.mCustomConstraints[i]->CalcVelocityError(model,cci,Q,QDot,CS,
689  CS.G.block(cci,0,rows,cols),err, cci);
690  }
691 
692 
693 }
694 
695 RBDL_DLLAPI
697  Model &model,
698  const Math::VectorNd &Q,
699  const Math::VectorNd &QDot,
700  const Math::VectorNd &Tau,
701  ConstraintSet &CS,
702  std::vector<Math::SpatialVector> *f_ext
703  ) {
704  // Compute C
705  NonlinearEffects(model, Q, QDot, CS.C, f_ext);
706  assert(CS.H.cols() == model.dof_count && CS.H.rows() == model.dof_count);
707 
708  // Compute H
709  CS.H.setZero();
710  CompositeRigidBodyAlgorithm(model, Q, CS.H, false);
711 
712  // Compute G
713  // We have to update model.X_base as they are not automatically computed
714  // by NonlinearEffects()
715  for (unsigned int i = 1; i < model.mBodies.size(); i++) {
716  model.X_base[i] = model.X_lambda[i] * model.X_base[model.lambda[i]];
717  }
718  CalcConstraintsJacobian (model, Q, CS, CS.G, false);
719 
720  // Compute position error for Baumgarte Stabilization.
721  CalcConstraintsPositionError (model, Q, CS, CS.err, false);
722 
723  // Compute velocity error for Baugarte stabilization.
724  CalcConstraintsVelocityError (model, Q, QDot, CS, CS.errd, false);
725  //CS.errd = CS.G * QDot;
726 
727  // Compute gamma
728  unsigned int prev_body_id = 0;
729  Vector3d prev_body_point = Vector3d::Zero();
730  Vector3d gamma_i = Vector3d::Zero();
731 
732  CS.QDDot_0.setZero();
733  UpdateKinematicsCustom(model, NULL, NULL, &CS.QDDot_0);
734 
735  for (unsigned int i = 0; i < CS.mContactConstraintIndices.size(); i++) {
736  const unsigned int c = CS.mContactConstraintIndices[i];
737 
738  // only compute point accelerations when necessary
739  if (prev_body_id != CS.body[c] || prev_body_point != CS.point[c]) {
740  gamma_i = CalcPointAcceleration (model, Q, QDot, CS.QDDot_0, CS.body[c]
741  , CS.point[c], false);
742  prev_body_id = CS.body[c];
743  prev_body_point = CS.point[c];
744  }
745 
746  // we also substract ContactData[c].acceleration such that the contact
747  // point will have the desired acceleration
748  CS.gamma[c] = CS.acceleration[c] - CS.normal[c].dot(gamma_i);
749  }
750 
751 
752 
753  for (unsigned int i = 0; i < CS.mLoopConstraintIndices.size(); i++) {
754  const unsigned int c = CS.mLoopConstraintIndices[i];
755 
756  // Variables used for computations.
757  Vector3d pos_p;
758  Matrix3d rot_p;
759  SpatialVector vel_p;
760  SpatialVector vel_s;
761  SpatialVector axis;
762  unsigned int id_p;
763  unsigned int id_s;
764 
765 
766  // Express the constraint axis in the base frame.
767  pos_p = CalcBodyToBaseCoordinates (model, Q, CS.body_p[c],
768  CS.X_p[c].r,false);
769  rot_p = CalcBodyWorldOrientation (model, Q, CS.body_p[c], false
770  ).transpose() * CS.X_p[c].E;
771  axis = SpatialTransform (rot_p, pos_p).apply(CS.constraintAxis[c]);
772 
773  // Compute the spatial velocities of the two constrained bodies.
774  vel_p = CalcPointVelocity6D (model, Q, QDot, CS.body_p[c],
775  CS.X_p[c].r, false);
776  vel_s = CalcPointVelocity6D (model, Q, QDot, CS.body_s[c],
777  CS.X_s[c].r, false);
778 
779  // Compute the derivative of the axis wrt the base frame.
780  SpatialVector axis_dot = crossm(vel_p, axis);
781 
782  // Compute the velocity product accelerations. These correspond to the
783  // accelerations that the bodies would have if q ddot were 0.
784  SpatialVector acc_p = CalcPointAcceleration6D (model, Q, QDot
785  , VectorNd::Zero(model.dof_count), CS.body_p[c], CS.X_p[c].r, false);
786  SpatialVector acc_s = CalcPointAcceleration6D (model, Q, QDot
787  , VectorNd::Zero(model.dof_count), CS.body_s[c], CS.X_s[c].r, false);
788 
789  // Problem here if one of the bodies is fixed...
790  // Compute the value of gamma.
791  CS.gamma[c]
792  // Right hand side term.
793  = - axis.dot(acc_s - acc_p) - axis_dot.dot(vel_s - vel_p)
794  // Baumgarte stabilization term.
795  - 2. * CS.baumgarteParameters[c][0] * CS.errd[c]
796  - CS.baumgarteParameters[c][1] * CS.baumgarteParameters[c][1] * CS.err[c];
797  }
798 
799  unsigned int ccid,rows,cols,z;
800  for(unsigned int i=0; i< CS.mCustomConstraintIndices.size(); i++){
801  ccid = CS.mCustomConstraintIndices[i];
802  rows = CS.mCustomConstraints[i]->mConstraintCount;
803  cols = CS.G.cols();
804  CS.mCustomConstraints[i]->CalcGamma(model,ccid,Q,QDot,CS,
805  CS.G.block(ccid,0,rows,cols),
806  CS.gamma,ccid);
807  for(unsigned int j=0; j<CS.mCustomConstraints[i]->mConstraintCount;j++){
808  z = ccid+j;
809  CS.gamma[z] += (- 2. * CS.baumgarteParameters[z][0] * CS.errd[z]
810  - CS.baumgarteParameters[z][1]
811  * CS.baumgarteParameters[z][1] * CS.err[z]);
812  }
813 
814  }
815 
816 }
817 
818 RBDL_DLLAPI
820  Model &model,
821  Math::VectorNd QInit, // Note: passed by value intentionally
822  ConstraintSet &cs,
823  Math::VectorNd &Q,
824  const Math::VectorNd &weights,
825  double tolerance,
826  unsigned int max_iter
827  ) {
828 
829  if(Q.size() != model.q_size) {
830  std::cerr << "Incorrect Q vector size." << std::endl;
831  assert(false);
832  abort();
833  }
834  if(QInit.size() != model.q_size) {
835  std::cerr << "Incorrect QInit vector size." << std::endl;
836  assert(false);
837  abort();
838  }
839  if(weights.size() != model.dof_count) {
840  std::cerr << "Incorrect weights vector size." << std::endl;
841  assert(false);
842  abort();
843  }
844 
845  // Initialize variables.
846  MatrixNd constraintJac (cs.size(), model.dof_count);
847  MatrixNd A = MatrixNd::Zero (cs.size() + model.dof_count, cs.size()
848  + model.dof_count);
849  VectorNd b = VectorNd::Zero (cs.size() + model.dof_count);
850  VectorNd x = VectorNd::Zero (cs.size() + model.dof_count);
851  VectorNd d = VectorNd::Zero (model.dof_count);
852  VectorNd e = VectorNd::Zero (cs.size());
853 
854  // The top-left block is the weight matrix and is constant.
855  for(unsigned int i = 0; i < weights.size(); ++i) {
856  A(i,i) = weights[i];
857  }
858 
859  // Check if the error is small enough already. If so, just return the initial
860  // guess as the solution.
861  CalcConstraintsPositionError (model, QInit, cs, e);
862  if (e.norm() < tolerance) {
863  Q = QInit;
864  return true;
865  }
866 
867  // We solve the linearized problem iteratively.
868  // Iterations are stopped if the maximum is reached.
869  for(unsigned int it = 0; it < max_iter; ++it) {
870  // Compute the constraint jacobian and build A and b.
871  constraintJac.setZero();
872  CalcConstraintsJacobian (model, QInit, cs, constraintJac);
873  A.block (model.dof_count, 0, cs.size(), model.dof_count) = constraintJac;
874  A.block (0, model.dof_count, model.dof_count, cs.size())
875  = constraintJac.transpose();
876  b.block (model.dof_count, 0, cs.size(), 1) = -e;
877 
878  // Solve the sistem A*x = b.
879  SolveLinearSystem (A, b, x, cs.linear_solver);
880 
881  // Extract the d = (Q - QInit) vector from x.
882  d = x.block (0, 0, model.dof_count, 1);
883 
884  // Update solution.
885  for (size_t i = 0; i < model.mJoints.size(); ++i) {
886  // If the joint is spherical, translate the corresponding components
887  // of d into a modification in the joint quaternion.
888  if (model.mJoints[i].mJointType == JointTypeSpherical) {
889  Quaternion quat = model.GetQuaternion(i, QInit);
890  Vector3d omega = d.block<3,1>(model.mJoints[i].q_index,0);
891  // Convert the 3d representation of the displacement to 4d and sum it
892  // to the components of the quaternion.
893  quat += quat.omegaToQDot(omega);
894  // The quaternion needs to be normalized after the previous sum.
895  quat /= quat.norm();
896  model.SetQuaternion(i, quat, QInit);
897  }
898  // If the current joint is not spherical, simply add the corresponding
899  // components of d to QInit.
900  else {
901  unsigned int qIdx = model.mJoints[i].q_index;
902  for(size_t j = 0; j < model.mJoints[i].mDoFCount; ++j) {
903  QInit[qIdx + j] += d[qIdx + j];
904  }
905  // QInit.block(qIdx, 0, model.mJoints[i].mDoFCount, 1)
906  // += d.block(model.mJoints[i].q_index, 0, model.mJoints[i].mDoFCount, 1);
907  }
908  }
909 
910  // Update the errors.
911  CalcConstraintsPositionError (model, QInit, cs, e);
912 
913  // Check if the error and the step are small enough to end.
914  if (e.norm() < tolerance && d.norm() < tolerance){
915  Q = QInit;
916  return true;
917  }
918  }
919 
920  // Return false if maximum number of iterations is exceeded.
921  Q = QInit;
922  return false;
923 }
924 
925 RBDL_DLLAPI
927  Model &model,
928  const Math::VectorNd &Q,
929  const Math::VectorNd &QDotInit,
930  ConstraintSet &cs,
931  Math::VectorNd &QDot,
932  const Math::VectorNd &weights
933  ) {
934  if(QDot.size() != model.dof_count) {
935  std::cerr << "Incorrect QDot vector size." << std::endl;
936  assert(false);
937  abort();
938  }
939  if(Q.size() != model.q_size) {
940  std::cerr << "Incorrect Q vector size." << std::endl;
941  assert(false);
942  abort();
943  }
944  if(QDotInit.size() != QDot.size()) {
945  std::cerr << "Incorrect QDotInit vector size." << std::endl;
946  assert(false);
947  abort();
948  }
949  if(weights.size() != QDot.size()) {
950  std::cerr << "Incorrect weight vector size." << std::endl;
951  assert(false);
952  abort();
953  }
954 
955  // Initialize variables.
956  MatrixNd constraintJac = MatrixNd::Zero(cs.size(), model.dof_count);
957  MatrixNd A = MatrixNd::Zero(cs.size() + model.dof_count, cs.size()
958  + model.dof_count);
959  VectorNd b = VectorNd::Zero(cs.size() + model.dof_count);
960  VectorNd x = VectorNd::Zero(cs.size() + model.dof_count);
961 
962  // The top-left block is the weight matrix and is constant.
963  for(unsigned int i = 0; i < weights.size(); ++i) {
964  A(i,i) = weights[i];
965  b[i] = weights[i] * QDotInit[i];
966  }
967  CalcConstraintsJacobian (model, Q, cs, constraintJac);
968  A.block (model.dof_count, 0, cs.size(), model.dof_count) = constraintJac;
969  A.block (0, model.dof_count, model.dof_count, cs.size())
970  = constraintJac.transpose();
971 
972  // Solve the sistem A*x = b.
973  SolveLinearSystem (A, b, x, cs.linear_solver);
974 
975  // Copy the result to the output variable.
976  QDot = x.block (0, 0, model.dof_count, 1);
977 }
978 
979 RBDL_DLLAPI
981  Model &model,
982  const VectorNd &Q,
983  const VectorNd &QDot,
984  const VectorNd &Tau,
985  ConstraintSet &CS,
986  VectorNd &QDDot,
987  std::vector<Math::SpatialVector> *f_ext
988  ) {
989  LOG << "-------- " << __func__ << " --------" << std::endl;
990 
991  CalcConstrainedSystemVariables (model, Q, QDot, Tau, CS, f_ext);
992 
993  SolveConstrainedSystemDirect (CS.H, CS.G, Tau - CS.C, CS.gamma, QDDot
994  , CS.force, CS.A, CS.b, CS.x, CS.linear_solver);
995 
996  // Copy back QDDot
997  for (unsigned int i = 0; i < model.dof_count; i++)
998  QDDot[i] = CS.x[i];
999 
1000  // Copy back contact forces
1001  for (unsigned int i = 0; i < CS.size(); i++) {
1002  CS.force[i] = -CS.x[model.dof_count + i];
1003  }
1004 }
1005 
1006 RBDL_DLLAPI
1008  Model &model,
1009  const Math::VectorNd &Q,
1010  const Math::VectorNd &QDot,
1011  const Math::VectorNd &Tau,
1012  ConstraintSet &CS,
1013  Math::VectorNd &QDDot,
1014  std::vector<Math::SpatialVector> *f_ext) {
1015 
1016  CalcConstrainedSystemVariables (model, Q, QDot, Tau, CS, f_ext);
1017 
1018  SolveConstrainedSystemRangeSpaceSparse (model, CS.H, CS.G, Tau - CS.C
1019  , CS.gamma, QDDot, CS.force, CS.K, CS.a, CS.linear_solver);
1020 }
1021 
1022 RBDL_DLLAPI
1024  Model &model,
1025  const VectorNd &Q,
1026  const VectorNd &QDot,
1027  const VectorNd &Tau,
1028  ConstraintSet &CS,
1029  VectorNd &QDDot,
1030  std::vector<Math::SpatialVector> *f_ext
1031  ) {
1032 
1033  LOG << "-------- " << __func__ << " --------" << std::endl;
1034 
1035  CalcConstrainedSystemVariables (model, Q, QDot, Tau, CS, f_ext);
1036 
1037  CS.GT_qr.compute (CS.G.transpose());
1038 #ifdef RBDL_USE_SIMPLE_MATH
1039  CS.GT_qr_Q = CS.GT_qr.householderQ();
1040 #else
1041  CS.GT_qr.householderQ().evalTo (CS.GT_qr_Q);
1042 #endif
1043 
1044  CS.Y = CS.GT_qr_Q.block (0,0,QDot.rows(), CS.G.rows());
1045  CS.Z = CS.GT_qr_Q.block (0,CS.G.rows(),QDot.rows(), QDot.rows() - CS.G.rows());
1046 
1047  SolveConstrainedSystemNullSpace (CS.H, CS.G, Tau - CS.C, CS.gamma, QDDot
1048  , CS.force, CS.Y, CS.Z, CS.qddot_y, CS.qddot_z, CS.linear_solver);
1049 
1050 }
1051 
1052 RBDL_DLLAPI
1054  Model &model,
1055  const Math::VectorNd &Q,
1056  const Math::VectorNd &QDotMinus,
1057  ConstraintSet &CS,
1058  Math::VectorNd &QDotPlus
1059  ) {
1060 
1061  // Compute H
1062  UpdateKinematicsCustom (model, &Q, NULL, NULL);
1063  CompositeRigidBodyAlgorithm (model, Q, CS.H, false);
1064 
1065  // Compute G
1066  CalcConstraintsJacobian (model, Q, CS, CS.G, false);
1067 
1068  SolveConstrainedSystemDirect (CS.H, CS.G, CS.H * QDotMinus, CS.v_plus
1069  , QDotPlus, CS.impulse, CS.A, CS.b, CS.x, CS.linear_solver);
1070 
1071  // Copy back QDotPlus
1072  for (unsigned int i = 0; i < model.dof_count; i++)
1073  QDotPlus[i] = CS.x[i];
1074 
1075  // Copy back constraint impulses
1076  for (unsigned int i = 0; i < CS.size(); i++) {
1077  CS.impulse[i] = CS.x[model.dof_count + i];
1078  }
1079 
1080 }
1081 
1082 RBDL_DLLAPI
1084  Model &model,
1085  const Math::VectorNd &Q,
1086  const Math::VectorNd &QDotMinus,
1087  ConstraintSet &CS,
1088  Math::VectorNd &QDotPlus
1089  ) {
1090 
1091  // Compute H
1092  UpdateKinematicsCustom (model, &Q, NULL, NULL);
1093  CompositeRigidBodyAlgorithm (model, Q, CS.H, false);
1094 
1095  // Compute G
1096  CalcConstraintsJacobian (model, Q, CS, CS.G, false);
1097 
1098  SolveConstrainedSystemRangeSpaceSparse (model, CS.H, CS.G, CS.H * QDotMinus
1099  , CS.v_plus, QDotPlus, CS.impulse, CS.K, CS.a, CS.linear_solver);
1100 
1101 }
1102 
1103 RBDL_DLLAPI
1105  Model &model,
1106  const Math::VectorNd &Q,
1107  const Math::VectorNd &QDotMinus,
1108  ConstraintSet &CS,
1109  Math::VectorNd &QDotPlus
1110  ) {
1111 
1112  // Compute H
1113  UpdateKinematicsCustom (model, &Q, NULL, NULL);
1114  CompositeRigidBodyAlgorithm (model, Q, CS.H, false);
1115 
1116  // Compute G
1117  CalcConstraintsJacobian (model, Q, CS, CS.G, false);
1118 
1119  CS.GT_qr.compute(CS.G.transpose());
1120  CS.GT_qr_Q = CS.GT_qr.householderQ();
1121 
1122  CS.Y = CS.GT_qr_Q.block (0,0,QDotMinus.rows(), CS.G.rows());
1123  CS.Z = CS.GT_qr_Q.block (0,CS.G.rows(),QDotMinus.rows(), QDotMinus.rows()
1124  - CS.G.rows());
1125 
1126  SolveConstrainedSystemNullSpace (CS.H, CS.G, CS.H * QDotMinus, CS.v_plus
1127  , QDotPlus, CS.impulse, CS.Y, CS.Z, CS.qddot_y, CS.qddot_z
1128  , CS.linear_solver);
1129 }
1130 
1138 RBDL_DLLAPI
1140  Model &model,
1141  const VectorNd &Tau,
1142  ConstraintSet &CS,
1143  VectorNd &QDDot
1144  ) {
1145  LOG << "-------- " << __func__ << " --------" << std::endl;
1146  assert (QDDot.size() == model.dof_count);
1147 
1148  unsigned int i = 0;
1149 
1150  for (i = 1; i < model.mBodies.size(); i++) {
1151  model.IA[i] = model.I[i].toMatrix();;
1152  model.pA[i] = crossf(model.v[i],model.I[i] * model.v[i]);
1153 
1154  if (CS.f_ext_constraints[i] != SpatialVector::Zero()) {
1155  LOG << "External force (" << i << ") = " << model.X_base[i].toMatrixAdjoint() * CS.f_ext_constraints[i] << std::endl;
1156  model.pA[i] -= model.X_base[i].toMatrixAdjoint() * CS.f_ext_constraints[i];
1157  }
1158  }
1159 
1160  // ClearLogOutput();
1161 
1162  LOG << "--- first loop ---" << std::endl;
1163 
1164  for (i = model.mBodies.size() - 1; i > 0; i--) {
1165  unsigned int q_index = model.mJoints[i].q_index;
1166 
1167  if (model.mJoints[i].mDoFCount == 3
1168  && model.mJoints[i].mJointType != JointTypeCustom) {
1169  unsigned int lambda = model.lambda[i];
1170  model.multdof3_u[i] = Vector3d (Tau[q_index],
1171  Tau[q_index + 1],
1172  Tau[q_index + 2])
1173  - model.multdof3_S[i].transpose() * model.pA[i];
1174 
1175  if (lambda != 0) {
1176  SpatialMatrix Ia = model.IA[i] - (model.multdof3_U[i]
1177  * model.multdof3_Dinv[i]
1178  * model.multdof3_U[i].transpose());
1179 
1180  SpatialVector pa = model.pA[i] + Ia * model.c[i]
1181  + model.multdof3_U[i] * model.multdof3_Dinv[i] * model.multdof3_u[i];
1182 
1183 #ifdef EIGEN_CORE_H
1184  model.IA[lambda].noalias() += (model.X_lambda[i].toMatrixTranspose()
1185  * Ia * model.X_lambda[i].toMatrix());
1186  model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa);
1187 #else
1188  model.IA[lambda] += (model.X_lambda[i].toMatrixTranspose()
1189  * Ia * model.X_lambda[i].toMatrix());
1190  model.pA[lambda] += model.X_lambda[i].applyTranspose(pa);
1191 #endif
1192  LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose()
1193  << std::endl;
1194  }
1195  } else if (model.mJoints[i].mDoFCount == 1
1196  && model.mJoints[i].mJointType != JointTypeCustom) {
1197  model.u[i] = Tau[q_index] - model.S[i].dot(model.pA[i]);
1198 
1199  unsigned int lambda = model.lambda[i];
1200  if (lambda != 0) {
1201  SpatialMatrix Ia = model.IA[i]
1202  - model.U[i] * (model.U[i] / model.d[i]).transpose();
1203  SpatialVector pa = model.pA[i] + Ia * model.c[i]
1204  + model.U[i] * model.u[i] / model.d[i];
1205 #ifdef EIGEN_CORE_H
1206  model.IA[lambda].noalias() += (model.X_lambda[i].toMatrixTranspose()
1207  * Ia * model.X_lambda[i].toMatrix());
1208  model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa);
1209 #else
1210  model.IA[lambda] += (model.X_lambda[i].toMatrixTranspose()
1211  * Ia * model.X_lambda[i].toMatrix());
1212  model.pA[lambda] += model.X_lambda[i].applyTranspose(pa);
1213 #endif
1214  LOG << "pA[" << lambda << "] = "
1215  << model.pA[lambda].transpose() << std::endl;
1216  }
1217  } else if(model.mJoints[i].mJointType == JointTypeCustom) {
1218 
1219  unsigned int kI = model.mJoints[i].custom_joint_index;
1220  unsigned int dofI = model.mCustomJoints[kI]->mDoFCount;
1221  unsigned int lambda = model.lambda[i];
1222  VectorNd tau_temp = VectorNd::Zero(dofI);
1223 
1224  for(int z=0; z<dofI;++z){
1225  tau_temp[z] = Tau[q_index+z];
1226  }
1227 
1228  model.mCustomJoints[kI]->u = tau_temp
1229  - (model.mCustomJoints[kI]->S.transpose()
1230  * model.pA[i]);
1231 
1232  if (lambda != 0) {
1233  SpatialMatrix Ia = model.IA[i]
1234  - ( model.mCustomJoints[kI]->U
1235  * model.mCustomJoints[kI]->Dinv
1236  * model.mCustomJoints[kI]->U.transpose());
1237 
1238  SpatialVector pa = model.pA[i] + Ia * model.c[i]
1239  + ( model.mCustomJoints[kI]->U
1240  * model.mCustomJoints[kI]->Dinv
1241  * model.mCustomJoints[kI]->u);
1242 #ifdef EIGEN_CORE_H
1243  model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose()
1244  * Ia * model.X_lambda[i].toMatrix();
1245 
1246  model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa);
1247 #else
1248  model.IA[lambda] += model.X_lambda[i].toMatrixTranspose()
1249  * Ia * model.X_lambda[i].toMatrix();
1250 
1251  model.pA[lambda] += model.X_lambda[i].applyTranspose(pa);
1252 #endif
1253  LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose()
1254  << std::endl;
1255  }
1256  }
1257  }
1258 
1259  model.a[0] = SpatialVector (0., 0., 0., -model.gravity[0], -model.gravity[1], -model.gravity[2]);
1260 
1261  for (i = 1; i < model.mBodies.size(); i++) {
1262  unsigned int q_index = model.mJoints[i].q_index;
1263  unsigned int lambda = model.lambda[i];
1264  SpatialTransform X_lambda = model.X_lambda[i];
1265 
1266  model.a[i] = X_lambda.apply(model.a[lambda]) + model.c[i];
1267  LOG << "a'[" << i << "] = " << model.a[i].transpose() << std::endl;
1268 
1269  if (model.mJoints[i].mDoFCount == 3
1270  && model.mJoints[i].mJointType != JointTypeCustom) {
1271  Vector3d qdd_temp = model.multdof3_Dinv[i] *
1272  (model.multdof3_u[i]
1273  - model.multdof3_U[i].transpose() * model.a[i]);
1274 
1275  QDDot[q_index] = qdd_temp[0];
1276  QDDot[q_index + 1] = qdd_temp[1];
1277  QDDot[q_index + 2] = qdd_temp[2];
1278  model.a[i] = model.a[i] + model.multdof3_S[i] * qdd_temp;
1279  } else if (model.mJoints[i].mDoFCount == 1
1280  && model.mJoints[i].mJointType != JointTypeCustom) {
1281  QDDot[q_index] = (1./model.d[i]) * (model.u[i] - model.U[i].dot(model.a[i]));
1282  model.a[i] = model.a[i] + model.S[i] * QDDot[q_index];
1283  } else if (model.mJoints[i].mJointType == JointTypeCustom){
1284  unsigned int kI = model.mJoints[i].custom_joint_index;
1285  unsigned int dofI = model.mCustomJoints[kI]->mDoFCount;
1286  VectorNd qdd_temp = VectorNd::Zero(dofI);
1287 
1288  qdd_temp = model.mCustomJoints[kI]->Dinv
1289  * (model.mCustomJoints[kI]->u
1290  - model.mCustomJoints[kI]->U.transpose()
1291  * model.a[i]);
1292 
1293  for(int z=0; z<dofI;++z){
1294  QDDot[q_index+z] = qdd_temp[z];
1295  }
1296 
1297  model.a[i] = model.a[i] + (model.mCustomJoints[kI]->S * qdd_temp);
1298  }
1299  }
1300 
1301  LOG << "QDDot = " << QDDot.transpose() << std::endl;
1302 }
1303 
1310 RBDL_DLLAPI
1312  Model &model,
1313  ConstraintSet &CS,
1314  VectorNd &QDDot_t,
1315  const unsigned int body_id,
1316  const std::vector<SpatialVector> &f_t
1317  ) {
1318  LOG << "-------- " << __func__ << " ------" << std::endl;
1319 
1320  assert (CS.d_pA.size() == model.mBodies.size());
1321  assert (CS.d_a.size() == model.mBodies.size());
1322  assert (CS.d_u.size() == model.mBodies.size());
1323 
1324  // TODO reset all values (debug)
1325  for (unsigned int i = 0; i < model.mBodies.size(); i++) {
1326  CS.d_pA[i].setZero();
1327  CS.d_a[i].setZero();
1328  CS.d_u[i] = 0.;
1329  CS.d_multdof3_u[i].setZero();
1330  }
1331  for(unsigned int i=0; i<model.mCustomJoints.size();i++){
1332  model.mCustomJoints[i]->d_u.setZero();
1333  }
1334 
1335  for (unsigned int i = body_id; i > 0; i--) {
1336  if (i == body_id) {
1337  CS.d_pA[i] = -model.X_base[i].applyAdjoint(f_t[i]);
1338  }
1339 
1340  if (model.mJoints[i].mDoFCount == 3
1341  && model.mJoints[i].mJointType != JointTypeCustom) {
1342  CS.d_multdof3_u[i] = - model.multdof3_S[i].transpose() * (CS.d_pA[i]);
1343 
1344  unsigned int lambda = model.lambda[i];
1345  if (lambda != 0) {
1346  CS.d_pA[lambda] = CS.d_pA[lambda]
1347  + model.X_lambda[i].applyTranspose (
1348  CS.d_pA[i] + (model.multdof3_U[i]
1349  * model.multdof3_Dinv[i]
1350  * CS.d_multdof3_u[i]));
1351  }
1352  } else if(model.mJoints[i].mDoFCount == 1
1353  && model.mJoints[i].mJointType != JointTypeCustom) {
1354  CS.d_u[i] = - model.S[i].dot(CS.d_pA[i]);
1355  unsigned int lambda = model.lambda[i];
1356 
1357  if (lambda != 0) {
1358  CS.d_pA[lambda] = CS.d_pA[lambda]
1359  + model.X_lambda[i].applyTranspose (
1360  CS.d_pA[i] + model.U[i] * CS.d_u[i] / model.d[i]);
1361  }
1362  } else if (model.mJoints[i].mJointType == JointTypeCustom){
1363 
1364  unsigned int kI = model.mJoints[i].custom_joint_index;
1365  unsigned int dofI = model.mCustomJoints[kI]->mDoFCount;
1366  //CS.
1367  model.mCustomJoints[kI]->d_u =
1368  - model.mCustomJoints[kI]->S.transpose() * (CS.d_pA[i]);
1369  unsigned int lambda = model.lambda[i];
1370  if (lambda != 0) {
1371  CS.d_pA[lambda] =
1372  CS.d_pA[lambda]
1373  + model.X_lambda[i].applyTranspose (
1374  CS.d_pA[i] + ( model.mCustomJoints[kI]->U
1375  * model.mCustomJoints[kI]->Dinv
1376  * model.mCustomJoints[kI]->d_u)
1377  );
1378  }
1379  }
1380  }
1381 
1382  for (unsigned int i = 0; i < f_t.size(); i++) {
1383  LOG << "f_t[" << i << "] = " << f_t[i].transpose() << std::endl;
1384  }
1385 
1386  for (unsigned int i = 0; i < model.mBodies.size(); i++) {
1387  LOG << "i = " << i << ": d_pA[i] " << CS.d_pA[i].transpose() << std::endl;
1388  }
1389  for (unsigned int i = 0; i < model.mBodies.size(); i++) {
1390  LOG << "i = " << i << ": d_u[i] = " << CS.d_u[i] << std::endl;
1391  }
1392 
1393  QDDot_t[0] = 0.;
1394  CS.d_a[0] = model.a[0];
1395 
1396  for (unsigned int i = 1; i < model.mBodies.size(); i++) {
1397  unsigned int q_index = model.mJoints[i].q_index;
1398  unsigned int lambda = model.lambda[i];
1399 
1400  SpatialVector Xa = model.X_lambda[i].apply(CS.d_a[lambda]);
1401 
1402  if (model.mJoints[i].mDoFCount == 3
1403  && model.mJoints[i].mJointType != JointTypeCustom) {
1404  Vector3d qdd_temp = model.multdof3_Dinv[i]
1405  * (CS.d_multdof3_u[i] - model.multdof3_U[i].transpose() * Xa);
1406 
1407  QDDot_t[q_index] = qdd_temp[0];
1408  QDDot_t[q_index + 1] = qdd_temp[1];
1409  QDDot_t[q_index + 2] = qdd_temp[2];
1410  model.a[i] = model.a[i] + model.multdof3_S[i] * qdd_temp;
1411  CS.d_a[i] = Xa + model.multdof3_S[i] * qdd_temp;
1412  } else if (model.mJoints[i].mDoFCount == 1
1413  && model.mJoints[i].mJointType != JointTypeCustom){
1414 
1415  QDDot_t[q_index] = (CS.d_u[i] - model.U[i].dot(Xa) ) / model.d[i];
1416  CS.d_a[i] = Xa + model.S[i] * QDDot_t[q_index];
1417  } else if (model.mJoints[i].mJointType == JointTypeCustom){
1418  unsigned int kI = model.mJoints[i].custom_joint_index;
1419  unsigned int dofI = model.mCustomJoints[kI]->mDoFCount;
1420  VectorNd qdd_temp = VectorNd::Zero(dofI);
1421 
1422  qdd_temp = model.mCustomJoints[kI]->Dinv
1423  * (model.mCustomJoints[kI]->d_u
1424  - model.mCustomJoints[kI]->U.transpose() * Xa);
1425 
1426  for(int z=0; z<dofI;++z){
1427  QDDot_t[q_index+z] = qdd_temp[z];
1428  }
1429 
1430  model.a[i] = model.a[i] + model.mCustomJoints[kI]->S * qdd_temp;
1431  CS.d_a[i] = Xa + model.mCustomJoints[kI]->S * qdd_temp;
1432  }
1433 
1434  LOG << "QDDot_t[" << i - 1 << "] = " << QDDot_t[i - 1] << std::endl;
1435  LOG << "d_a[i] = " << CS.d_a[i].transpose() << std::endl;
1436  }
1437 }
1438 
1439 inline void set_zero (std::vector<SpatialVector> &spatial_values) {
1440  for (unsigned int i = 0; i < spatial_values.size(); i++)
1441  spatial_values[i].setZero();
1442 }
1443 
1444 RBDL_DLLAPI
1446  Model &model,
1447  const VectorNd &Q,
1448  const VectorNd &QDot,
1449  const VectorNd &Tau,
1450  ConstraintSet &CS,
1451  VectorNd &QDDot
1452  ) {
1453  LOG << "-------- " << __func__ << " ------" << std::endl;
1454 
1455  assert (CS.f_ext_constraints.size() == model.mBodies.size());
1456  assert (CS.QDDot_0.size() == model.dof_count);
1457  assert (CS.QDDot_t.size() == model.dof_count);
1458  assert (CS.f_t.size() == CS.size());
1459  assert (CS.point_accel_0.size() == CS.size());
1460  assert (CS.K.rows() == CS.size());
1461  assert (CS.K.cols() == CS.size());
1462  assert (CS.force.size() == CS.size());
1463  assert (CS.a.size() == CS.size());
1464 
1465  Vector3d point_accel_t;
1466 
1467  unsigned int ci = 0;
1468 
1469  // The default acceleration only needs to be computed once
1470  {
1472  ForwardDynamics(model, Q, QDot, Tau, CS.QDDot_0);
1473  }
1474 
1475  LOG << "=== Initial Loop Start ===" << std::endl;
1476  // we have to compute the standard accelerations first as we use them to
1477  // compute the effects of each test force
1478  for(ci = 0; ci < CS.size(); ci++) {
1479 
1480  {
1482  UpdateKinematicsCustom(model, NULL, NULL, &CS.QDDot_0);
1483  }
1484 
1486  {
1487  LOG << "body_id = " << CS.body[ci] << std::endl;
1488  LOG << "point = " << CS.point[ci] << std::endl;
1489  LOG << "normal = " << CS.normal[ci] << std::endl;
1490  LOG << "QDDot_0 = " << CS.QDDot_0.transpose() << std::endl;
1491  {
1493  CS.point_accel_0[ci] = CalcPointAcceleration (model, Q, QDot
1494  , CS.QDDot_0, CS.body[ci], CS.point[ci], false);
1495  CS.a[ci] = - CS.acceleration[ci]
1496  + CS.normal[ci].dot(CS.point_accel_0[ci]);
1497  }
1498  LOG << "point_accel_0 = " << CS.point_accel_0[ci].transpose();
1499  }
1500  else
1501  {
1502  std::cerr << "Forward Dynamic Contact Kokkevis: unsupported constraint \
1503  type." << std::endl;
1504  assert(false);
1505  abort();
1506  }
1507  }
1508 
1509  // Now we can compute and apply the test forces and use their net effect
1510  // to compute the inverse articlated inertia to fill K.
1511  for (ci = 0; ci < CS.size(); ci++) {
1512 
1513  LOG << "=== Testforce Loop Start ===" << std::endl;
1514 
1515  unsigned int movable_body_id = 0;
1516  Vector3d point_global;
1517 
1518  switch (CS.constraintType[ci]) {
1519 
1521 
1522  movable_body_id = GetMovableBodyId(model, CS.body[ci]);
1523 
1524  // assemble the test force
1525  LOG << "normal = " << CS.normal[ci].transpose() << std::endl;
1526 
1527  point_global = CalcBodyToBaseCoordinates(model, Q, CS.body[ci]
1528  , CS.point[ci], false);
1529 
1530  LOG << "point_global = " << point_global.transpose() << std::endl;
1531 
1532  CS.f_t[ci] = SpatialTransform(Matrix3d::Identity(), -point_global)
1533  .applyAdjoint(SpatialVector (0., 0., 0.
1534  , -CS.normal[ci][0], -CS.normal[ci][1], -CS.normal[ci][2]));
1535  CS.f_ext_constraints[movable_body_id] = CS.f_t[ci];
1536 
1537  LOG << "f_t[" << movable_body_id << "] = " << CS.f_t[ci].transpose()
1538  << std::endl;
1539 
1540  {
1542  , movable_body_id, CS.f_ext_constraints);
1543 
1544  LOG << "QDDot_0 = " << CS.QDDot_0.transpose() << std::endl;
1545  LOG << "QDDot_t = " << (CS.QDDot_t + CS.QDDot_0).transpose()
1546  << std::endl;
1547  LOG << "QDDot_t - QDDot_0 = " << (CS.QDDot_t).transpose() << std::endl;
1548  }
1549 
1550  CS.f_ext_constraints[movable_body_id].setZero();
1551 
1552  CS.QDDot_t += CS.QDDot_0;
1553 
1554  // compute the resulting acceleration
1555  {
1557  UpdateKinematicsCustom(model, NULL, NULL, &CS.QDDot_t);
1558  }
1559 
1560  for(unsigned int cj = 0; cj < CS.size(); cj++) {
1561  {
1563 
1564  point_accel_t = CalcPointAcceleration(model, Q, QDot, CS.QDDot_t
1565  , CS.body[cj], CS.point[cj], false);
1566  }
1567 
1568  LOG << "point_accel_0 = " << CS.point_accel_0[ci].transpose()
1569  << std::endl;
1570  LOG << "point_accel_t = " << point_accel_t.transpose() << std::endl;
1571 
1572  CS.K(ci,cj) = CS.normal[cj].dot(point_accel_t - CS.point_accel_0[cj]);
1573 
1574  }
1575 
1576  break;
1577 
1578  default:
1579 
1580  std::cerr << "Forward Dynamic Contact Kokkevis: unsupported constraint \
1581  type." << std::endl;
1582  assert(false);
1583  abort();
1584 
1585  break;
1586 
1587  }
1588 
1589  }
1590 
1591  LOG << "K = " << std::endl << CS.K << std::endl;
1592  LOG << "a = " << std::endl << CS.a << std::endl;
1593 
1594 #ifndef RBDL_USE_SIMPLE_MATH
1595  switch (CS.linear_solver) {
1596  case (LinearSolverPartialPivLU) :
1597  CS.force = CS.K.partialPivLu().solve(CS.a);
1598  break;
1600  CS.force = CS.K.colPivHouseholderQr().solve(CS.a);
1601  break;
1602  case (LinearSolverHouseholderQR) :
1603  CS.force = CS.K.householderQr().solve(CS.a);
1604  break;
1605  default:
1606  LOG << "Error: Invalid linear solver: " << CS.linear_solver << std::endl;
1607  assert (0);
1608  break;
1609  }
1610 #else
1611  bool solve_successful = LinSolveGaussElimPivot (CS.K, CS.a, CS.force);
1612  assert (solve_successful);
1613 #endif
1614 
1615  LOG << "f = " << CS.force.transpose() << std::endl;
1616 
1617  for (ci = 0; ci < CS.size(); ci++) {
1618  unsigned int body_id = CS.body[ci];
1619  unsigned int movable_body_id = body_id;
1620 
1621  if (model.IsFixedBodyId(body_id)) {
1622  unsigned int fbody_id = body_id - model.fixed_body_discriminator;
1623  movable_body_id = model.mFixedBodies[fbody_id].mMovableParent;
1624  }
1625 
1626  CS.f_ext_constraints[movable_body_id] -= CS.f_t[ci] * CS.force[ci];
1627  LOG << "f_ext[" << movable_body_id << "] = " << CS.f_ext_constraints[movable_body_id].transpose() << std::endl;
1628  }
1629 
1630  {
1632  ForwardDynamicsApplyConstraintForces (model, Tau, CS, QDDot);
1633  }
1634 
1635  LOG << "QDDot after applying f_ext: " << QDDot.transpose() << std::endl;
1636 }
1637 
1639  const MatrixNd& A,
1640  const VectorNd& b,
1641  VectorNd& x,
1642  LinearSolver ls
1643  ) {
1644  if(A.rows() != b.size() || A.cols() != x.size()) {
1645  std::cerr << "Mismatching sizes." << std::endl;
1646  assert(false);
1647  abort();
1648  }
1649 
1650  // Solve the sistem A*x = b.
1651  switch (ls) {
1652  case (LinearSolverPartialPivLU) :
1653  #ifdef RBDL_USE_SIMPLE_MATH
1654  // SimpleMath does not have a LU solver so just use its QR solver
1655  x = A.householderQr().solve(b);
1656  #else
1657  x = A.partialPivLu().solve(b);
1658  #endif
1659  break;
1661  x = A.colPivHouseholderQr().solve(b);
1662  break;
1663  case (LinearSolverHouseholderQR) :
1664  x = A.householderQr().solve(b);
1665  break;
1666  default:
1667  std::cerr << "Error: Invalid linear solver: " << ls << std::endl;
1668  assert(false);
1669  abort();
1670  break;
1671  }
1672 }
1673 
1674 unsigned int GetMovableBodyId (Model& model, unsigned int id) {
1675  if(model.IsFixedBodyId(id)) {
1676  unsigned int fbody_id = id - model.fixed_body_discriminator;
1677  return model.mFixedBodies[fbody_id].mMovableParent;
1678  }
1679  else {
1680  return id;
1681  }
1682 }
1683 
1684 } /* namespace RigidBodyDynamics */
SpatialMatrix crossf(const SpatialVector &v)
Compact representation of spatial transformations.
std::vector< unsigned int > lambda
The id of the parents body.
Definition: Model.h:127
RBDL_DLLAPI void 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
Math::MatrixNd K
Workspace for the Inverse Articulated-Body Inertia.
Definition: Constraints.h:488
Math::VectorNd b
Workspace for the Lagrangian right-hand-side.
Definition: Constraints.h:459
SpatialVector applyAdjoint(const SpatialVector &f_sp)
Vector4d omegaToQDot(const Vector3d &omega) const
Converts a 3d angular velocity vector into a 4d derivative of the components of the quaternion...
Definition: Quaternion.h:196
unsigned int AddCustomConstraint(CustomConstraint *custom_constraint, unsigned int id_predecessor, unsigned int id_successor, const Math::SpatialTransform &X_predecessor, const Math::SpatialTransform &X_successor, bool enable_stabilization=false, const double stabilization_param=0.1, const char *constraint_name=NULL)
Adds a custom constraint to the constraint set.
Definition: Constraints.cc:160
bool Bind(const Model &model)
Initializes and allocates memory for the constraint set.
Definition: Constraints.cc:241
RBDL_DLLAPI void NonlinearEffects(Model &model, const VectorNd &Q, const VectorNd &QDot, VectorNd &Tau, std::vector< Math::SpatialVector > *f_ext)
Computes the coriolis forces.
Definition: Dynamics.cc:107
Quaternion that are used for singularity free joints.
Definition: Quaternion.h:21
bool IsFixedBodyId(unsigned int body_id)
Checks whether the body is rigidly attached to another body.
Definition: Model.h:371
Math::VectorNd a
Workspace for the accelerations of due to the test forces.
Definition: Constraints.h:490
std::vector< unsigned int > mContactConstraintIndices
Definition: Constraints.h:414
Math::VectorNd C
Workspace for the coriolis forces.
Definition: Constraints.h:452
Math::MatrixNd GSsi
Workspace when evaluating loop/CustomConstraint Jacobians.
Definition: Constraints.h:468
std::vector< ConstraintType > constraintType
Definition: Constraints.h:412
RBDL_DLLAPI void SparseFactorizeLTL(Model &model, Math::MatrixNd &H)
Math::VectorNd x
Workspace for the Lagrangian solution.
Definition: Constraints.h:461
Contains all information about the rigid body model.
Definition: Model.h:121
std::vector< Math::Matrix63 > multdof3_U
Definition: Model.h:197
RBDL_DLLAPI void ForwardDynamics(Model &model, const VectorNd &Q, const VectorNd &QDot, const VectorNd &Tau, VectorNd &QDDot, std::vector< SpatialVector > *f_ext)
Computes forward dynamics with the Articulated Body Algorithm.
Definition: Dynamics.cc:315
std::vector< CustomConstraint *> mCustomConstraints
Definition: Constraints.h:417
Math::VectorNd QDDot_0
Workspace for the default accelerations.
Definition: Constraints.h:494
LinearSolverPartialPivLU
SpatialVector_t SpatialVector
Definition: rbdl_math.h:59
std::vector< unsigned int > mCustomConstraintIndices
Definition: Constraints.h:416
LinearSolverHouseholderQR
SpatialMatrix crossm(const SpatialVector &v)
std::vector< Math::SpatialVector > v
The spatial velocity of the bodies.
Definition: Model.h:166
Math::MatrixNd Gi
Workspace when evaluating contact Jacobians.
Definition: Constraints.h:464
RBDL_DLLAPI void CompositeRigidBodyAlgorithm(Model &model, const VectorNd &Q, MatrixNd &H, bool update_kinematics)
Computes the joint space inertia matrix by using the Composite Rigid Body Algorithm.
Definition: Dynamics.cc:167
std::vector< Body > mBodies
All bodies 0 ... N_B, including the base.
Definition: Model.h:263
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
Math::MatrixNd H
Workspace for the joint space inertia matrix.
Definition: Constraints.h:450
RBDL_DLLAPI void CalcConstraintsPositionError(Model &model, const Math::VectorNd &Q, ConstraintSet &CS, Math::VectorNd &err, bool update_kinematics)
Computes the position errors for the given ConstraintSet.
Definition: Constraints.cc:498
std::vector< Math::SpatialVector > constraintAxis
Definition: Constraints.h:429
RBDL_DLLAPI void ComputeConstraintImpulsesDirect(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotMinus, ConstraintSet &CS, Math::VectorNd &QDotPlus)
Computes contact gain by constructing and solving the full lagrangian equation.
void SetQuaternion(unsigned int i, const Math::Quaternion &quat, Math::VectorNd &Q) const
Definition: Model.h:476
#define SUPPRESS_LOGGING
Definition: Logging.h:24
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
Math::Vector3d gravity
the cartesian vector of the gravity
Definition: Model.h:162
std::vector< Math::SpatialRigidBodyInertia > I
The spatial inertia of body i (used only in CompositeRigidBodyAlgorithm())
Definition: Model.h:223
RBDL_DLLAPI void ForwardDynamicsConstraintsNullSpace(Model &model, const VectorNd &Q, const VectorNd &QDot, const VectorNd &Tau, ConstraintSet &CS, VectorNd &QDDot, std::vector< Math::SpatialVector > *f_ext)
RBDL_DLLAPI void ComputeConstraintImpulsesRangeSpaceSparse(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotMinus, ConstraintSet &CS, Math::VectorNd &QDotPlus)
Resolves contact gain using SolveContactSystemRangeSpaceSparse()
void set_zero(std::vector< SpatialVector > &spatial_values)
unsigned int AddLoopConstraint(unsigned int id_predecessor, unsigned int id_successor, const Math::SpatialTransform &X_predecessor, const Math::SpatialTransform &X_successor, const Math::SpatialVector &axis, bool enable_stabilization=false, const double stabilization_param=0.1, const char *constraint_name=NULL)
Adds a loop constraint to the constraint set.
Definition: Constraints.cc:90
RBDL_DLLAPI void ForwardDynamicsApplyConstraintForces(Model &model, const VectorNd &Tau, ConstraintSet &CS, VectorNd &QDDot)
Compute only the effects of external forces on the generalized accelerations.
std::vector< Math::SpatialTransform > X_lambda
Transformation from the parent body to the current body .
Definition: Model.h:236
unsigned int GetMovableBodyId(Model &model, unsigned int id)
SpatialTransform Xtrans(const Vector3d &r)
RBDL_DLLAPI void CalcConstraintsVelocityError(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, ConstraintSet &CS, Math::VectorNd &err, bool update_kinematics)
Computes the velocity errors for the given ConstraintSet.
Definition: Constraints.cc:666
std::vector< FixedBody > mFixedBodies
All bodies that are attached to a body via a fixed joint.
Definition: Model.h:241
Math::VectorNd d
Temporary variable D_i (RBDA p. 130)
Definition: Model.h:216
RBDL_DLLAPI void ForwardDynamicsConstraintsDirect(Model &model, const VectorNd &Q, const VectorNd &QDot, const VectorNd &Tau, ConstraintSet &CS, VectorNd &QDDot, std::vector< Math::SpatialVector > *f_ext)
Computes forward dynamics with contact by constructing and solving the full lagrangian equation...
Definition: Constraints.cc:980
unsigned int AddContactConstraint(unsigned int body_id, const Math::Vector3d &body_point, const Math::Vector3d &world_normal, const char *constraint_name=NULL, double normal_acceleration=0.)
Adds a contact constraint to the constraint set.
Definition: Constraints.cc:36
RBDL_DLLAPI bool CalcAssemblyQ(Model &model, Math::VectorNd QInit, ConstraintSet &cs, Math::VectorNd &Q, const Math::VectorNd &weights, double tolerance, unsigned int max_iter)
Computes a feasible initial value of the generalized joint positions.
Definition: Constraints.cc:819
std::vector< Math::SpatialVector > f_t
Workspace for the test forces.
Definition: Constraints.h:496
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
Math::VectorNd gamma
Workspace of the lower part of b.
Definition: Constraints.h:454
LinearSolverColPivHouseholderQR
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
Math::Quaternion GetQuaternion(unsigned int i, const Math::VectorNd &Q) const
Definition: Model.h:461
RBDL_DLLAPI void CalcConstraintsJacobian(Model &model, const Math::VectorNd &Q, ConstraintSet &CS, Math::MatrixNd &G, bool update_kinematics)
Computes the Jacobian for the given ConstraintSet.
Definition: Constraints.cc:570
unsigned int qdot_size
The size of the.
Definition: Model.h:156
Math::MatrixNd A
Workspace for the Lagrangian left-hand-side matrix.
Definition: Constraints.h:457
Math::VectorNd QDDot_t
Workspace for the test accelerations.
Definition: Constraints.h:492
RBDL_DLLAPI void ForwardDynamicsContactsKokkevis(Model &model, const VectorNd &Q, const VectorNd &QDot, const VectorNd &Tau, ConstraintSet &CS, VectorNd &QDDot)
Computes forward dynamics that accounts for active contacts in ConstraintSet.
SpatialVector apply(const SpatialVector &v_sp)
RBDL_DLLAPI void SolveConstrainedSystemNullSpace(Math::MatrixNd &H, const Math::MatrixNd &G, const Math::VectorNd &c, const Math::VectorNd &gamma, Math::VectorNd &qddot, Math::VectorNd &lambda, Math::MatrixNd &Y, Math::MatrixNd &Z, Math::VectorNd &qddot_y, Math::VectorNd &qddot_z, Math::LinearSolver &linear_solver)
Solves the contact system by first solving for the joint accelerations and then for the constraint fo...
Definition: Constraints.cc:437
Interface to define general-purpose constraints.
Definition: Constraints.h:1048
Structure that contains both constraint information and workspace memory.
Definition: Constraints.h:266
std::vector< Math::Vector3d > multdof3_u
Definition: Model.h:199
RBDL_DLLAPI void ForwardDynamicsConstraintsRangeSpaceSparse(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS, Math::VectorNd &QDDot, std::vector< Math::SpatialVector > *f_ext)
std::vector< unsigned int > mLoopConstraintIndices
Definition: Constraints.h:415
std::vector< Math::SpatialVector > c
The velocity dependent spatial acceleration.
Definition: Model.h:208
std::vector< Math::SpatialTransform > X_p
Definition: Constraints.h:427
void clear()
Clears all variables in the constraint set.
Definition: Constraints.cc:311
#define LOG
Definition: Logging.h:23
void SolveLinearSystem(const MatrixNd &A, const VectorNd &b, VectorNd &x, LinearSolver ls)
std::vector< unsigned int > body_p
Definition: Constraints.h:425
RBDL_DLLAPI void SolveConstrainedSystemRangeSpaceSparse(Model &model, Math::MatrixNd &H, const Math::MatrixNd &G, const Math::VectorNd &c, const Math::VectorNd &gamma, Math::VectorNd &qddot, Math::VectorNd &lambda, Math::MatrixNd &K, Math::VectorNd &a, Math::LinearSolver linear_solver)
Solves the contact system by first solving for the the joint accelerations and then the contact force...
Definition: Constraints.cc:400
std::vector< Math::SpatialVector > a
The spatial acceleration of the bodies.
Definition: Model.h:168
std::vector< Math::Vector3d > point_accel_0
Workspace for the default point accelerations.
Definition: Constraints.h:500
std::vector< unsigned int > body_s
Definition: Constraints.h:426
unsigned int fixed_body_discriminator
Value that is used to discriminate between fixed and movable bodies.
Definition: Model.h:254
std::vector< Math::SpatialTransform > X_s
Definition: Constraints.h:428
std::vector< Math::SpatialVector > pA
The spatial bias force.
Definition: Model.h:212
RBDL_DLLAPI void ForwardDynamicsAccelerationDeltas(Model &model, ConstraintSet &CS, VectorNd &QDDot_t, const unsigned int body_id, const std::vector< SpatialVector > &f_t)
Computes the effect of external forces on the generalized accelerations.
RBDL_DLLAPI void CalcAssemblyQDot(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotInit, ConstraintSet &cs, Math::VectorNd &QDot, const Math::VectorNd &weights)
Computes a feasible initial value of the generalized joint velocities.
Definition: Constraints.cc:926
RBDL_DLLAPI void SparseSolveLx(Model &model, Math::MatrixNd &L, Math::VectorNd &x)
Math::VectorNd u
Temporary variable u (RBDA p. 130)
Definition: Model.h:218
RBDL_DLLAPI void CalcConstrainedSystemVariables(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS, std::vector< Math::SpatialVector > *f_ext)
Computes the terms , , and of the constrained dynamic problem and stores them in the ConstraintSet...
Definition: Constraints.cc:696
std::vector< Math::SpatialVector > d_a
Workspace for the acceleration due to the test force.
Definition: Constraints.h:505
unsigned int dof_count
number of degrees of freedoms of the model
Definition: Model.h:139
std::vector< Math::SpatialVector > S
The joint axis for joint i.
Definition: Model.h:177
std::vector< Math::SpatialVector > U
Temporary variable U_i (RBDA p. 130)
Definition: Model.h:214
std::vector< Math::Vector2d > baumgarteParameters
Definition: Constraints.h:431
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
RBDL_DLLAPI void ComputeConstraintImpulsesNullSpace(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotMinus, ConstraintSet &CS, Math::VectorNd &QDotPlus)
Resolves contact gain using SolveContactSystemNullSpace()
3 DoF joint using Quaternions for joint positional variables and angular velocity for joint velocity ...
Definition: Joint.h:186
std::vector< Math::Matrix3d > multdof3_Dinv
Definition: Model.h:198
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
std::vector< Math::Vector3d > normal
Definition: Constraints.h:422
std::vector< Math::Vector3d > d_multdof3_u
Definition: Constraints.h:515
std::vector< Math::SpatialVector > f_ext_constraints
Workspace for the actual spatial forces.
Definition: Constraints.h:498
Eigen::HouseholderQR< Math::MatrixNd > GT_qr
Workspace for the QR decomposition of the null-space method.
Definition: Constraints.h:476
size_t size() const
Returns the number of constraints.
Definition: Constraints.h:399
std::vector< Math::Vector3d > point
Definition: Constraints.h:421
std::vector< Math::Matrix63 > multdof3_S
Motion subspace for joints with 3 degrees of freedom.
Definition: Model.h:196
std::vector< Math::SpatialMatrix > IA
The spatial inertia of the bodies.
Definition: Model.h:210
Math::LinearSolver linear_solver
Method that should be used to solve internal linear systems.
Definition: Constraints.h:407
std::vector< Joint > mJoints
All joints.
Definition: Model.h:175
Math::MatrixNd GSpi
Workspace when evaluating loop/CustomConstraint Jacobians.
Definition: Constraints.h:466
std::vector< unsigned int > body
Definition: Constraints.h:420
RBDL_DLLAPI bool LinSolveGaussElimPivot(MatrixNd A, VectorNd b, VectorNd &x)
Solves a linear system using gaussian elimination with pivoting.
std::vector< Math::SpatialVector > d_pA
Workspace for the bias force due to the test force.
Definition: Constraints.h:503
Math::MatrixNd GSJ
Workspace when evaluating loop/CustomConstraint Jacobians.
Definition: Constraints.h:470
RBDL_DLLAPI void SolveConstrainedSystemDirect(Math::MatrixNd &H, const Math::MatrixNd &G, const Math::VectorNd &c, const Math::VectorNd &gamma, Math::VectorNd &qddot, Math::VectorNd &lambda, Math::MatrixNd &A, Math::VectorNd &b, Math::VectorNd &x, Math::LinearSolver &linear_solver)
Solves the full contact system directly, i.e. simultaneously for contact forces and joint acceleratio...
Definition: Constraints.cc:349
RBDL_DLLAPI void SparseSolveLTx(Model &model, Math::MatrixNd &L, Math::VectorNd &x)
User defined joints of varying size.
Definition: Joint.h:200