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 <string>
11#include <limits>
12#include <assert.h>
13//The ConstraintCache input to each function contains all of the working
14//memory necessary for this constraint. So nothing appears here.
15
16#include "rbdl/rbdl_mathutils.h"
17#include "rbdl/rbdl_errors.h"
18#include "rbdl/Logging.h"
19
20#include "rbdl/Model.h"
21#include "rbdl/Joint.h"
22#include "rbdl/Body.h"
23#include "rbdl/Constraints.h"
24#include "rbdl/Constraint_Contact.h"
25#include "rbdl/Dynamics.h"
26#include "rbdl/Kinematics.h"
27
28namespace RigidBodyDynamics
29{
30
31using namespace Math;
32
33void SolveLinearSystem (
34 const MatrixNd& A,
35 const VectorNd& b,
36 VectorNd& x,
37 LinearSolver ls
38);
39
40unsigned int GetMovableBodyId (Model& model, unsigned int id);
41
42//==============================================================================
43
44
45//==============================================================================
46unsigned int ConstraintSet::AddContactConstraint (
47 unsigned int body_id,
48 const Vector3d &body_point,
49 const Vector3d &world_normal,
50 const char *constraint_name,
51 unsigned int userDefinedId)
52{
53 assert (bound == false);
54
55
56 unsigned int insertAtRowInG = size();
57 unsigned int rowsInG = insertAtRowInG+1;
58
59 std::string nameStr;
60 if(constraint_name != NULL) {
61 nameStr = constraint_name;
62 }
63 //Go through all existing ContactConstraints,
64 //if there is a BodyToGroundPosition
65 //constraint at body_id with the identical body_point, then append the
66 //constraint.
67 //
68 // Why am I bothering to do this? To save on computation.
69 // Every individual ContactConstraint evaluates a point Jacobian.
70 // Thus 3 individual constraints evaluates a point Jacobian 3 times. If these
71 // are all grouped together then the point Jacobian is only evaluated once.
72 bool constraintAppended = false;
73
74 if(contactConstraints.size() > 0) {
75 unsigned int i = unsigned(contactConstraints.size()-1);
76 if(contactConstraints[i]->getBodyIds()[0] == body_id) {
77 Vector3d pointErr = body_point -
78 contactConstraints[i]->getBodyFrames()[0].r;
79#ifdef RBDL_USE_CASADI_MATH
80 if(pointErr.is_zero() && contactConstraints[i]->getUserDefinedId() == userDefinedId) {
81#else
82 if(pointErr.norm() < std::numeric_limits<double>::epsilon()*100 &&
83 contactConstraints[i]->getUserDefinedId() == userDefinedId) {
84#endif
85 constraintAppended = true;
86 contactConstraints[i]->appendNormalVector(world_normal);
87 }
88 }
89 }
90
91 if(constraintAppended == false) {
92
93 contactConstraints.push_back(
94 std::shared_ptr<ContactConstraint>(
95 new ContactConstraint(body_id,body_point, world_normal, constraint_name,userDefinedId)));
96 unsigned int idx = unsigned(contactConstraints.size()-1);
97 contactConstraints[idx]->addToConstraintSet(insertAtRowInG);
98 constraints.emplace_back(contactConstraints[idx]);
99
100 }
101
102 constraintType.push_back (ConstraintTypeContact);
103 name.push_back (nameStr);
104
105
106 err.conservativeResize(rowsInG);
107 err[insertAtRowInG] = 0.;
108 errd.conservativeResize(rowsInG);
109 errd[insertAtRowInG] = 0.;
110
111 force.conservativeResize (rowsInG);
112 force[insertAtRowInG] = 0.;
113
114 impulse.conservativeResize (rowsInG);
115 impulse[insertAtRowInG] = 0.;
116
117 v_plus.conservativeResize (rowsInG);
118 v_plus[insertAtRowInG] = 0.;
119
120 d_multdof3_u = std::vector<Math::Vector3d>( rowsInG,
121 Math::Vector3d::Zero());
122
123 //Set up access maps
124 if(nameStr.size() > 0) {
125 std::pair< std::map<std::string, unsigned int>::iterator, bool > iter;
126 iter = nameGroupMap.insert(std::pair<std::string, unsigned int>(
127 name[name.size()-1],
128 unsigned(constraints.size()-1)));
129 //if(iter.second == false){
130 // std::cerr << "Error: optional name is not unique."
131 // << std::endl;
132 // assert(0);
133 // abort();
134 //}
135
136 }
137 if(userDefinedId < std::numeric_limits<unsigned int>::max()) {
138 std::pair< std::map<unsigned int, unsigned int>::iterator, bool > iter;
139 iter =userDefinedIdGroupMap.insert( std::pair<unsigned int, unsigned int>(
140 userDefinedId,
141 unsigned(constraints.size()-1)));
142 //if(iter.second == false){
143 // std::cerr << "Error: optional userDefinedId is not unique."
144 // << std::endl;
145 // assert(0);
146 // abort();
147 //}
148 }
149
150 std::pair< std::map<unsigned int, unsigned int>::iterator, bool > iter;
151 iter = idGroupMap.insert(std::pair<unsigned int, unsigned int>(
152 unsigned(rowsInG-1),
153 unsigned(constraints.size()-1)));
154 if(iter.second == false) {
155 std::cerr << "Error: Constraint row entry in system is not unique."
156 << " (This should not be possible: contact the "
157 "maintainer of this code.)"
158 << std::endl;
159 assert(0);
160 abort();
161 }
162
163 return rowsInG-1;
164
165}
166
167//==============================================================================
168unsigned int ConstraintSet::AddLoopConstraint (
169 unsigned int idPredecessor,
170 unsigned int idSuccessor,
171 const Math::SpatialTransform &XPredecessor,
172 const Math::SpatialTransform &XSuccessor,
173 const Math::SpatialVector &constraintAxisInPredecessor,
174 bool enableBaumgarteStabilization,
175 double stabilizationTimeConstant,
176 const char *constraintName,
177 unsigned int userDefinedId)
178{
179 assert (bound == false);
180
181
182 unsigned int insertAtRowInG = unsigned(size());
183 unsigned int rowsInG = insertAtRowInG+1;
184
185 double tol = std::numeric_limits<double>::epsilon()*100.;
186 bool constraintAppended = false;
187 unsigned int idx = unsigned(loopConstraints.size());
188
189 if(loopConstraints.size() > 0) {
190 idx = idx-1;
191 if(loopConstraints[idx]->getBodyIds()[0] == idPredecessor &&
192 loopConstraints[idx]->getBodyIds()[1] == idSuccessor) {
193
194 bool framesNumericallyIdentical=true;
195 SpatialTransform frameErrorPre, frameErrorSuc;
196
197 frameErrorPre.r=XPredecessor.r-loopConstraints[idx]->getBodyFrames()[0].r;
198 frameErrorPre.E=XPredecessor.E-loopConstraints[idx]->getBodyFrames()[0].E;
199
200 frameErrorSuc.r=XSuccessor.r -loopConstraints[idx]->getBodyFrames()[1].r;
201 frameErrorSuc.E=XSuccessor.E -loopConstraints[idx]->getBodyFrames()[1].E;
202
203 //Using this awkward element by element comparison to maintain
204 //compatibility with SimpleMath.
205#ifndef RBDL_USE_CASADI_MATH
206 for(unsigned int i=0; i<frameErrorPre.r.size(); ++i) {
207 if(fabs(frameErrorPre.r[i]) > tol || fabs(frameErrorSuc.r[i]) > tol) {
208 framesNumericallyIdentical=false;
209 }
210 for(unsigned int j=0; j<frameErrorPre.E.cols(); ++j) {
211 if(fabs(frameErrorPre.E(i,j))>tol || fabs(frameErrorSuc.E(i,j))>tol) {
212 framesNumericallyIdentical=false;
213 }
214 }
215 }
216#endif
217
218 if(framesNumericallyIdentical
219 && loopConstraints[idx]->getUserDefinedId() == userDefinedId) {
220 constraintAppended = true;
221 loopConstraints[idx]->appendConstraintAxis(constraintAxisInPredecessor);
222 }
223 }
224 }
225
226 if(constraintAppended==false) {
227
228 loopConstraints.push_back(
229 std::shared_ptr<LoopConstraint>(
230 new LoopConstraint(
231 idPredecessor, idSuccessor,XPredecessor, XSuccessor,
232 constraintAxisInPredecessor, enableBaumgarteStabilization,
233 stabilizationTimeConstant,constraintName,userDefinedId)));
234 idx = unsigned(loopConstraints.size()-1);
235 loopConstraints[idx]->addToConstraintSet(insertAtRowInG);
236 constraints.emplace_back(loopConstraints[idx]);
237 }
238
239 constraintType.push_back(ConstraintTypeLoop);
240
241 //Update all of the struct arrays so that they have the correct number
242 //of elements
243 std::string nameStr;
244 if (constraintName != NULL) {
245 nameStr = constraintName;
246 }
247
248 name.push_back (nameStr);
249
250
251 err.conservativeResize(rowsInG);
252 err[insertAtRowInG] = 0.;
253 errd.conservativeResize(rowsInG);
254 errd[insertAtRowInG] = 0.;
255
256 force.conservativeResize (rowsInG);
257 force[insertAtRowInG] = 0.;
258
259 impulse.conservativeResize (rowsInG);
260 impulse[insertAtRowInG] = 0.;
261
262 v_plus.conservativeResize (rowsInG);
263 v_plus[insertAtRowInG] = 0.;
264
265 d_multdof3_u = std::vector<Math::Vector3d>(rowsInG, Math::Vector3d::Zero());
266
267 //Set up access maps
268 if(nameStr.size() > 0) {
269 std::pair< std::map<std::string, unsigned int>::iterator, bool > iter;
270 iter = nameGroupMap.insert(std::pair<std::string, unsigned int>(
271 name[name.size()-1],
272 unsigned(constraints.size()-1)));
273 //if(iter.second == false){
274 // std::cerr << "Error: optional name is not unique."
275 // << std::endl;
276 // assert(0);
277 // abort();
278 //}
279
280 }
281
282 if(userDefinedId < std::numeric_limits<unsigned int>::max()) {
283 std::pair< std::map<unsigned int, unsigned int>::iterator, bool > iter;
284 iter =userDefinedIdGroupMap.insert( std::pair<unsigned int, unsigned int>(
285 userDefinedId,
286 unsigned(constraints.size()-1)));
287 //if(iter.second == false){
288 // std::cerr << "Error: optional userDefinedId is not unique."
289 // << std::endl;
290 // assert(0);
291 // abort();
292 //}
293 }
294
295 std::pair< std::map<unsigned int, unsigned int>::iterator, bool > iter;
296 iter = idGroupMap.insert(std::pair<unsigned int, unsigned int>(
297 unsigned(rowsInG-1),
298 unsigned(constraints.size()-1)));
299 if(iter.second == false) {
300 std::stringstream errormsg;
301 errormsg << "Error: Constraint row entry in system is not unique."
302 << " (This should not be possible: contact the "
303 "maintainer of this code.)"
304 << std::endl;
305 throw Errors::RBDLError(errormsg.str());
306 }
307
308 return rowsInG-1;
309}
310
311
312
313//==============================================================================
314unsigned int ConstraintSet::AddCustomConstraint(
315 std::shared_ptr<Constraint> customConstraint)
316{
317 unsigned int insertAtRowInG = unsigned(size());
318 unsigned int rowsInG = insertAtRowInG+customConstraint->getConstraintSize();
319 unsigned int cIndex = constraints.size();
320
321 constraints.emplace_back(customConstraint);
322 constraints[cIndex]->addToConstraintSet(insertAtRowInG);
323
324 //Resize constraint set system variables
325 std::string nameStr("");
326 if(customConstraint->getName() != NULL) {
327 nameStr = customConstraint->getName();
328 }
329
330 err.conservativeResize( rowsInG);
331 errd.conservativeResize( rowsInG);
332 force.conservativeResize ( rowsInG);
333 impulse.conservativeResize (rowsInG);
334 v_plus.conservativeResize ( rowsInG);
335 d_multdof3_u = std::vector<Math::Vector3d>(rowsInG, Math::Vector3d::Zero());
336
337 for(unsigned int i=0; i<customConstraint->getConstraintSize(); ++i) {
338 //The list of names, constraint types, and ids must have the same
339 //number of entries as G has rows.
340 name.push_back (nameStr);
341 constraintType.push_back (ConstraintTypeCustom);
342
343
344 err[ insertAtRowInG+i ] = 0.;
345 errd[ insertAtRowInG+i ] = 0.;
346 force[ insertAtRowInG+i ] = 0.;
347 impulse[ insertAtRowInG+i ] = 0.;
348 v_plus[ insertAtRowInG+i ] = 0.;
349 }
350
351 //Set up access maps
352 if(nameStr.size() > 0) {
353 std::pair< std::map<std::string, unsigned int>::iterator, bool > iter;
354 iter = nameGroupMap.insert(std::pair<std::string, unsigned int>(
355 name[name.size()-1],
356 unsigned(constraints.size()-1)));
357 if(iter.second == false) {
358 throw Errors::RBDLError("Error: optional name is not unique.\n");
359 }
360
361 }
362 if(customConstraint->getUserDefinedId()
363 < std::numeric_limits<unsigned int>::max()) {
364 std::pair< std::map<unsigned int, unsigned int>::iterator, bool > iter;
365 iter =userDefinedIdGroupMap.insert( std::pair<unsigned int, unsigned int>(
366 customConstraint->getUserDefinedId(),
367 unsigned(constraints.size()-1)));
368 if(iter.second == false) {
369 throw Errors::RBDLError("Error: optional userDefinedId is not unique.\n");
370 }
371
372 }
373
374 std::pair< std::map<unsigned int, unsigned int>::iterator, bool > iter;
375 iter = idGroupMap.insert(std::pair<unsigned int, unsigned int>(
376 unsigned(rowsInG-1),
377 unsigned(constraints.size()-1)));
378 if(iter.second == false) {
379 std::stringstream errormsg;
380 errormsg << "Error: Constraint row entry into system is not unique."
381 << " (This should not be possible: contact the "
382 "maintainer of this code.)"
383 << std::endl;
384 throw Errors::RBDLError(errormsg.str());
385 }
386
387 return rowsInG-1;
388
389}
390
391
392//==============================================================================
393bool ConstraintSet::Bind (const Model &model)
394{
395 assert (bound == false);
396
397 if (bound) {
398 throw Errors::RBDLError("Error: binding an already bound constraint set!\n");
399 }
400 for(unsigned int i=0; i<constraints.size(); ++i) {
401 constraints[i]->bind(model);
402 }
403
404 cache.vecNZeros = VectorNd::Zero(model.qdot_size);
405 cache.vecNA.resize(model.qdot_size,1);
406 cache.vecNB.resize(model.qdot_size,1);
407 cache.vecNC.resize(model.qdot_size,1);
408 cache.vecND.resize(model.qdot_size,1);
409
410 cache.mat3NA.resize(3, model.qdot_size);
411 cache.mat3NB.resize(3, model.qdot_size);
412 cache.mat3NC.resize(3, model.qdot_size);
413 cache.mat3ND.resize(3, model.qdot_size);
414
415 cache.mat6NA.resize(6, model.qdot_size);
416 cache.mat6NB.resize(6, model.qdot_size);
417 cache.mat6NC.resize(6, model.qdot_size);
418 cache.mat6ND.resize(6, model.qdot_size);
419
420
421 unsigned int n_constr = size();
422
423 H.conservativeResize (model.dof_count, model.dof_count);
424 H.setZero();
425 C.conservativeResize (model.dof_count);
426 C.setZero();
427 gamma.conservativeResize (n_constr);
428 gamma.setZero();
429 G.conservativeResize (n_constr, model.dof_count);
430 G.setZero();
431 A.conservativeResize (model.dof_count + n_constr, model.dof_count + n_constr);
432 A.setZero();
433 b.conservativeResize (model.dof_count + n_constr);
434 b.setZero();
435 x.conservativeResize (model.dof_count + n_constr);
436 x.setZero();
437
438
439 S.conservativeResize(model.dof_count, model.dof_count);
440 S.setZero();
441 W.conservativeResize(model.dof_count, model.dof_count);
442 W.Identity(model.dof_count, model.dof_count);
443
444 // HouseHolderQR crashes if matrix G has more rows than columns.
445#ifndef RBDL_USE_CASADI_MATH
446 GT_qr = Eigen::HouseholderQR<Math::MatrixNd> (G.transpose());
447#endif
448 GT_qr_Q = MatrixNd::Zero (model.dof_count, model.dof_count);
449 Y = MatrixNd::Zero (model.dof_count, G.rows());
450 Z = MatrixNd::Zero (model.dof_count, model.dof_count - G.rows());
451 qddot_y = VectorNd::Zero (model.dof_count);
452 qddot_z = VectorNd::Zero (model.dof_count);
453
454 K.conservativeResize (n_constr, n_constr);
455 K.setZero();
456 a.conservativeResize (n_constr);
457 a.setZero();
458 QDDot_t.conservativeResize (model.dof_count);
459 QDDot_t.setZero();
460 f_t.resize (n_constr, SpatialVector::Zero());
461 point_accel_0.resize (n_constr, Vector3d::Zero());
462
463 QDDot_0.conservativeResize (model.dof_count);
464 QDDot_0.setZero();
465
466
467 f_ext_constraints.resize (model.mBodies.size(), SpatialVector::Zero());
468
469
470 d_pA =std::vector<SpatialVector> (model.mBodies.size(),SpatialVector::Zero());
471 d_a = std::vector<SpatialVector> (model.mBodies.size(),SpatialVector::Zero());
472 d_u = VectorNd::Zero (model.mBodies.size());
473
474 d_IA = std::vector<SpatialMatrix> (model.mBodies.size()
475 , SpatialMatrix::Identity());
476 d_U = std::vector<SpatialVector> (model.mBodies.size(),SpatialVector::Zero());
477 d_d = VectorNd::Zero (model.mBodies.size());
478
479 d_multdof3_u = std::vector<Math::Vector3d> (model.mBodies.size()
480 , Math::Vector3d::Zero());
481
482 bound = true;
483
484 return bound;
485}
486
487//==============================================================================
488void ConstraintSet::SetActuationMap(const Model &model,
489 const std::vector<bool> &actuatedDofUpd)
490{
491
492 assert(actuatedDofUpd.size() == model.dof_count);
493
494 unsigned int n = unsigned( int( model.dof_count ));
495 unsigned int nc = unsigned( int( name.size() ));
496 unsigned int na = 0; //actuated dofs
497 unsigned int nu = 0; //unactuated dofs
498
499 for(unsigned int i=0; i<actuatedDofUpd.size(); ++i) {
500 if(actuatedDofUpd[i]) {
501 ++na;
502 }
503 }
504 nu = n-na;
505
506 S.conservativeResize(na,model.dof_count);
507 S.setZero();
508 P.conservativeResize(n-na,model.dof_count);
509 P.setZero();
510 W.conservativeResize(na,na);
511 W.setZero();
512 Winv.conservativeResize(na,na);
513 Winv.setZero();
514 WinvSC.conservativeResize(na);
515 WinvSC.setZero();
516
517 u.resize(na);
518 v.resize(nu);
519
520 unsigned int j=0;
521 unsigned int k=0;
522 for(unsigned int i=0; i<model.dof_count; ++i) {
523 if(actuatedDofUpd[i]) {
524 S(j,i) = 1.;
525 ++j;
526 } else {
527 P(k,i) = 1.;
528 ++k;
529 }
530 }
531
532 unsigned int dim = n+n+nc+na;
533
534 //Null space method variable initialization
535 dim = na+nu;
536 F.conservativeResize(dim,dim);
537 F.setZero();
538
539 Ful.conservativeResize(na,na);
540 Ful.setZero();
541 Fur.conservativeResize(na,nu);
542 Fur.setZero();
543 Fll.conservativeResize(nu,na);
544 Fll.setZero();
545 Flr.conservativeResize(nu,nu);
546 Flr.setZero();
547
548 g.conservativeResize(n);
549
550 Ru.conservativeResize(nc,nc);
551 py.conservativeResize(nc);
552 pz.conservativeResize(n-nc);
553
554 GT.conservativeResize(n,nc);
555 GTu.conservativeResize(na,nc);
556 GTl.conservativeResize(nu,nc);
557
558 GPT.conservativeResize(nc,nu);
559
560}
561
562void ConstraintSet::clear()
563{
564 force.setZero();
565 impulse.setZero();
566
567 H.setZero();
568 C.setZero();
569 gamma.setZero();
570 G.setZero();
571 A.setZero();
572 b.setZero();
573 x.setZero();
574
575 //Constraint cache
576 cache.vecNZeros.setZero();
577
578 cache.vecNA.setZero();
579 cache.vecNB.setZero();
580 cache.vecNC.setZero();
581 cache.vecND.setZero();
582
583
584 cache.mat3NA.setZero();
585 cache.mat3NB.setZero();
586 cache.mat3NC.setZero();
587 cache.mat3ND.setZero();
588
589 cache.vec3A.setZero();
590 cache.vec3B.setZero();
591 cache.vec3C.setZero();
592 cache.vec3D.setZero();
593 cache.vec3E.setZero();
594 cache.vec3F.setZero();
595
596 cache.svecA.setZero();
597 cache.svecB.setZero();
598 cache.svecC.setZero();
599 cache.svecD.setZero();
600 cache.svecE.setZero();
601 cache.svecF.setZero();
602
603 cache.stA.E.Identity();
604 cache.stA.r.setZero();
605 cache.stB.E.Identity();
606 cache.stB.r.setZero();
607 cache.stC.E.Identity();
608 cache.stC.r.setZero();
609 cache.stD.E.Identity();
610 cache.stD.r.setZero();
611
612 cache.mat3A.setZero();
613 cache.mat3B.setZero();
614 cache.mat3C.setZero();
615 cache.mat3D.setZero();
616 cache.mat3E.setZero();
617 cache.mat3F.setZero();
618
619
620 //Kokkevis Cache
621 QDDot_t.setZero();
622 a.setZero();
623 K.setZero();
624 for(unsigned int i=0; i<point_accel_0.size(); ++i) {
625 point_accel_0[i].setZero();
626 }
627 for(unsigned int i=0; i<f_t.size(); ++i) {
628 f_t[i].setZero();
629 }
630
631 QDDot_0.setZero();
632
633 unsigned int i;
634 for (i = 0; i < f_t.size(); i++) {
635 f_t[i].setZero();
636 }
637
638 for (i = 0; i < f_ext_constraints.size(); i++) {
639 f_ext_constraints[i].setZero();
640 }
641
642 for (i = 0; i < point_accel_0.size(); i++) {
643 point_accel_0[i].setZero();
644 }
645
646 for (i = 0; i < d_pA.size(); i++) {
647 d_pA[i].setZero();
648 }
649
650 for (i = 0; i < d_a.size(); i++) {
651 d_a[i].setZero();
652 }
653
654 d_u.setZero();
655}
656
657
658//==============================================================================
659RBDL_DLLAPI
660void SolveConstrainedSystemDirect (
661 Math::MatrixNd &H,
662 const Math::MatrixNd &G,
663 const Math::VectorNd &c,
664 const Math::VectorNd &gamma,
665 Math::VectorNd &lambda,
666 Math::MatrixNd &A,
667 Math::VectorNd &b,
668 Math::VectorNd &x,
669 Math::LinearSolver &linear_solver
670)
671{
672 // Build the system: Copy H
673 A.block(0, 0, c.rows(), c.rows()) = H;
674
675 // Copy G and G^T
676 A.block(0, c.rows(), c.rows(), gamma.rows()) = G.transpose();
677 A.block(c.rows(), 0, gamma.rows(), c.rows()) = G;
678
679 // Build the system: Copy -C + \tau
680 b.block(0, 0, c.rows(), 1) = c;
681 b.block(c.rows(), 0, gamma.rows(), 1) = gamma;
682
683 LOG << "A = " << std::endl << A << std::endl;
684 LOG << "b = " << std::endl << b << std::endl;
685
686#ifdef RBDL_USE_CASADI_MATH
687 auto linsol = casadi::Linsol("linear_solver", "symbolicqr", A.sparsity());
688 x = linsol.solve(A, b);
689#else
690 switch (linear_solver) {
691 case (LinearSolverPartialPivLU) :
692 x = A.partialPivLu().solve(b);
693 break;
694 case (LinearSolverColPivHouseholderQR) :
695 x = A.colPivHouseholderQr().solve(b);
696 break;
697 case (LinearSolverHouseholderQR) :
698 x = A.householderQr().solve(b);
699 break;
700 default:
701 LOG << "Error: Invalid linear solver: " << linear_solver << std::endl;
702 assert (0);
703 break;
704 }
705#endif
706
707 LOG << "x = " << std::endl << x << std::endl;
708}
709
710//==============================================================================
711RBDL_DLLAPI
712void SolveConstrainedSystemRangeSpaceSparse (
713 Model &model,
714 Math::MatrixNd &H,
715 const Math::MatrixNd &G,
716 const Math::VectorNd &c,
717 const Math::VectorNd &gamma,
718 Math::VectorNd &qddot,
719 Math::VectorNd &lambda,
720 Math::MatrixNd &K,
721 Math::VectorNd &a,
722 Math::LinearSolver linear_solver
723)
724{
725 SparseFactorizeLTL (model, H);
726
727 MatrixNd Y (G.transpose());
728
729 for (unsigned int i = 0; i < Y.cols(); i++) {
730 VectorNd Y_col = Y.block(0,i,Y.rows(),1);
731 SparseSolveLTx (model, H, Y_col);
732 Y.block(0,i,Y.rows(),1) = Y_col;
733 }
734
735 VectorNd z (c);
736 SparseSolveLTx (model, H, z);
737
738 K = Y.transpose() * Y;
739
740 a = gamma - Y.transpose() * z;
741#ifdef RBDL_USE_CASADI_MATH
742 auto linsol = casadi::Linsol("linear_solver", "symbolicqr", K.sparsity());
743 lambda = linsol.solve(K, a);
744#else
745 lambda = K.llt().solve(a);
746#endif
747
748 qddot = c + G.transpose() * lambda;
749 SparseSolveLTx (model, H, qddot);
750 SparseSolveLx (model, H, qddot);
751}
752
753//==============================================================================
754RBDL_DLLAPI
755void SolveConstrainedSystemNullSpace (
756 Math::MatrixNd &H,
757 const Math::MatrixNd &G,
758 const Math::VectorNd &c,
759 const Math::VectorNd &gamma,
760 Math::VectorNd &qddot,
761 Math::VectorNd &lambda,
762 Math::MatrixNd &Y,
763 Math::MatrixNd &Z,
764 Math::VectorNd &qddot_y,
765 Math::VectorNd &qddot_z,
766 Math::LinearSolver &linear_solver
767)
768{
769
770#ifdef RBDL_USE_CASADI_MATH
771 auto GY = G * Y;
772 auto linsol = casadi::Linsol("linear_solver", "symbolicqr", GY.sparsity());
773 qddot_y = linsol.solve(GY, gamma);
774#else
775 switch (linear_solver) {
776 case (LinearSolverPartialPivLU) :
777 qddot_y = (G * Y).partialPivLu().solve (gamma);
778 break;
779 case (LinearSolverColPivHouseholderQR) :
780 qddot_y = (G * Y).colPivHouseholderQr().solve (gamma);
781 break;
782 case (LinearSolverHouseholderQR) :
783 qddot_y = (G * Y).householderQr().solve (gamma);
784 break;
785 default:
786 LOG << "Error: Invalid linear solver: " << linear_solver << std::endl;
787 assert (0);
788 break;
789 }
790#endif
791
792#ifdef RBDL_USE_CASADI_MATH
793 auto ZHZ = (Z.transpose()*H*Z);
794 linsol = casadi::Linsol("linear_solver", "symbolicqr", ZHZ.sparsity());
795 qddot_z = linsol.solve(ZHZ, Z.transpose()*(c - H*Y*qddot_y));
796#else
797 qddot_z = (Z.transpose()*H*Z).llt().solve(Z.transpose()*(c - H*Y*qddot_y));
798#endif
799 qddot = Y * qddot_y + Z * qddot_z;
800
801#ifdef RBDL_USE_CASADI_MATH
802 GY = G * Y;
803 linsol = casadi::Linsol("linear_solver", "symbolicqr", GY.sparsity());
804 lambda = linsol.solve(GY, Y.transpose() * (H * qddot - c));
805#else
806 switch (linear_solver) {
807 case (LinearSolverPartialPivLU) :
808 lambda = (G * Y).partialPivLu().solve (Y.transpose() * (H * qddot - c));
809 break;
810 case (LinearSolverColPivHouseholderQR) :
811 lambda = (G*Y).colPivHouseholderQr().solve (Y.transpose()*(H*qddot - c));
812 break;
813 case (LinearSolverHouseholderQR) :
814 lambda = (G * Y).householderQr().solve (Y.transpose() * (H * qddot - c));
815 break;
816 default:
817 LOG << "Error: Invalid linear solver: " << linear_solver << std::endl;
818 assert (0);
819 break;
820 }
821#endif
822}
823
824
825//==============================================================================
826RBDL_DLLAPI
827void CalcConstraintsPositionError (
828 Model& model,
829 const Math::VectorNd &Q,
830 ConstraintSet &CS,
831 Math::VectorNd& err,
832 bool update_kinematics
833)
834{
835 assert(err.size() == CS.size());
836
837 if(update_kinematics) {
838 UpdateKinematicsCustom (model, &Q, NULL, NULL);
839 }
840
841 for(unsigned int i=0; i<CS.constraints.size(); ++i) {
842 CS.constraints[i]->calcPositionError(model,0,Q,err, CS.cache,
843 update_kinematics);
844 }
845}
846
847//==============================================================================
848RBDL_DLLAPI
849void CalcConstraintsJacobian (
850 Model &model,
851 const Math::VectorNd &Q,
852 ConstraintSet &CS,
853 Math::MatrixNd &G,
854 bool update_kinematics
855)
856{
857 if (update_kinematics) {
858 UpdateKinematicsCustom (model, &Q, NULL, NULL);
859 }
860
861 for(unsigned int i=0; i<CS.constraints.size(); ++i) {
862 CS.constraints[i]->calcConstraintJacobian(model,0,Q,CS.cache.vecNZeros,G,
863 CS.cache,update_kinematics);
864 }
865}
866
867//==============================================================================
868RBDL_DLLAPI
869void CalcConstraintsVelocityError (
870 Model& model,
871 const Math::VectorNd &Q,
872 const Math::VectorNd &QDot,
873 ConstraintSet &CS,
874 Math::VectorNd& err,
875 bool update_kinematics
876)
877{
878
879
880 CalcConstraintsJacobian (model, Q, CS, CS.G, update_kinematics);
881
882 for(unsigned int i=0; i<CS.constraints.size(); ++i) {
883 CS.constraints[i]->calcVelocityError(model,0,Q,QDot,CS.G,err,CS.cache,
884 update_kinematics);
885 }
886
887}
888
889//==============================================================================
890RBDL_DLLAPI
891void CalcConstrainedSystemVariables (
892 Model &model,
893 const Math::VectorNd &Q,
894 const Math::VectorNd &QDot,
895 const Math::VectorNd &Tau,
896 ConstraintSet &CS,
897 bool update_kinematics,
898 std::vector<Math::SpatialVector> *f_ext
899)
900{
901 // Compute G
902 if(update_kinematics){
903 UpdateKinematicsCustom(model, &Q, NULL, NULL);
904 }
905
906 // Compute C
907 NonlinearEffects(model, Q, QDot, CS.C, f_ext);
908 assert(CS.H.cols() == model.dof_count && CS.H.rows() == model.dof_count);
909
910 // Compute H
911 CS.H.setZero();
912 CompositeRigidBodyAlgorithm(model, Q, CS.H, false);
913
914 CalcConstraintsJacobian (model, Q, CS, CS.G, false);
915
916 // Compute position error for Baumgarte Stabilization.
917 CalcConstraintsPositionError (model, Q, CS, CS.err, false);
918
919 // Compute velocity error for Baugarte stabilization.
920 CalcConstraintsVelocityError (model, Q, QDot, CS, CS.errd, false);
921 //CS.errd = CS.G * QDot;
922
923 // Compute gamma
924 unsigned int prev_body_id = 0;
925 Vector3d prev_body_point = Vector3d::Zero();
926 Vector3d gamma_i = Vector3d::Zero();
927
928 CS.QDDot_0.setZero();
929 UpdateKinematicsCustom(model, NULL, NULL, &CS.QDDot_0);
930
931
932 for(unsigned int i=0; i<CS.constraints.size(); ++i) {
933 CS.constraints[i]->calcGamma(model,0,Q,QDot,CS.G,CS.gamma,CS.cache);
934 if(CS.constraints[i]->isBaumgarteStabilizationEnabled()) {
935 CS.constraints[i]->addInBaumgarteStabilizationForces(
936 CS.err,CS.errd,CS.gamma);
937 }
938 }
939
940
941}
942
943//==============================================================================
944#ifndef RBDL_USE_CASADI_MATH
945RBDL_DLLAPI
946bool CalcAssemblyQ (
947 Model &model,
948 Math::VectorNd QInit, // Note: passed by value intentionally
949 ConstraintSet &cs,
950 Math::VectorNd &Q,
951 const Math::VectorNd &weights,
952 double tolerance,
953 unsigned int max_iter
954)
955{
956
957 if(Q.size() != model.q_size) {
958 throw Errors::RBDLDofMismatchError("Incorrect Q vector size.\n");
959 }
960 if(QInit.size() != model.q_size) {
961 throw Errors::RBDLDofMismatchError("Incorrect QInit vector size.\n");
962 }
963 if(weights.size() != model.dof_count) {
964 throw Errors::RBDLDofMismatchError("Incorrect weights vector size.\n");
965 }
966
967 // Initialize variables.
968 MatrixNd constraintJac (cs.size(), model.dof_count);
969 MatrixNd A = MatrixNd::Zero (cs.size() + model.dof_count, cs.size()
970 + model.dof_count);
971 VectorNd b = VectorNd::Zero (cs.size() + model.dof_count);
972 VectorNd x = VectorNd::Zero (cs.size() + model.dof_count);
973 VectorNd d = VectorNd::Zero (model.dof_count);
974 VectorNd e = VectorNd::Zero (cs.size());
975
976 // The top-left block is the weight matrix and is constant.
977 for(unsigned int i = 0; i < weights.size(); ++i) {
978 A(i,i) = weights[i];
979 }
980
981 // Check if the error is small enough already. If so, just return the initial
982 // guess as the solution.
983 CalcConstraintsPositionError (model, QInit, cs, e);
984 if (e.norm() < tolerance) {
985 Q = QInit;
986 return true;
987 }
988
989 // We solve the linearized problem iteratively.
990 // Iterations are stopped if the maximum is reached.
991 for(unsigned int it = 0; it < max_iter; ++it) {
992 // Compute the constraint jacobian and build A and b.
993 constraintJac.setZero();
994 CalcConstraintsJacobian (model, QInit, cs, constraintJac);
995 A.block (model.dof_count, 0, cs.size(), model.dof_count) = constraintJac;
996 A.block (0, model.dof_count, model.dof_count, cs.size())
997 = constraintJac.transpose();
998 b.block (model.dof_count, 0, cs.size(), 1) = -e;
999
1000 // Solve the sistem A*x = b.
1001 SolveLinearSystem (A, b, x, cs.linear_solver);
1002
1003 // Extract the d = (Q - QInit) vector from x.
1004 d = x.block (0, 0, model.dof_count, 1);
1005
1006 // Update solution.
1007 for (size_t i = 0; i < model.mJoints.size(); ++i) {
1008 // If the joint is spherical, translate the corresponding components
1009 // of d into a modification in the joint quaternion.
1010 if (model.mJoints[i].mJointType == JointTypeSpherical) {
1011 Quaternion quat = model.GetQuaternion(i, QInit);
1012 Vector3d omega = d.block<3,1>(model.mJoints[i].q_index,0);
1013 // Convert the 3d representation of the displacement to 4d and sum it
1014 // to the components of the quaternion.
1015 quat += quat.omegaToQDot(omega);
1016 // The quaternion needs to be normalized after the previous sum.
1017 quat /= quat.norm();
1018 model.SetQuaternion(i, quat, QInit);
1019 }
1020 // If the current joint is not spherical, simply add the corresponding
1021 // components of d to QInit.
1022 else {
1023 unsigned int qIdx = model.mJoints[i].q_index;
1024 for(size_t j = 0; j < model.mJoints[i].mDoFCount; ++j) {
1025 QInit[qIdx + j] += d[qIdx + j];
1026 }
1027 }
1028 }
1029
1030 // Update the errors.
1031 CalcConstraintsPositionError (model, QInit, cs, e);
1032
1033 // Check if the error and the step are small enough to end.
1034 if (e.norm() < tolerance && d.norm() < tolerance) {
1035 Q = QInit;
1036 return true;
1037 }
1038 }
1039
1040 // Return false if maximum number of iterations is exceeded.
1041 Q = QInit;
1042 return false;
1043}
1044#endif
1045
1046//==============================================================================
1047RBDL_DLLAPI
1048void CalcAssemblyQDot (
1049 Model &model,
1050 const Math::VectorNd &Q,
1051 const Math::VectorNd &QDotInit,
1052 ConstraintSet &cs,
1053 Math::VectorNd &QDot,
1054 const Math::VectorNd &weights
1055)
1056{
1057 if(QDot.size() != model.dof_count) {
1058 throw Errors::RBDLDofMismatchError("Incorrect QDot vector size.\n");
1059 }
1060 if(Q.size() != model.q_size) {
1061 throw Errors::RBDLDofMismatchError("Incorrect Q vector size.\n");
1062 }
1063 if(QDotInit.size() != QDot.size()) {
1064 throw Errors::RBDLDofMismatchError("Incorrect QDotInit vector size.\n");
1065 }
1066 if(weights.size() != QDot.size()) {
1067 throw Errors::RBDLDofMismatchError("Incorrect weight vector size.\n");
1068 }
1069
1070 // Initialize variables.
1071 MatrixNd constraintJac = MatrixNd::Zero(cs.size(), model.dof_count);
1072 MatrixNd A = MatrixNd::Zero(cs.size() + model.dof_count, cs.size()
1073 + model.dof_count);
1074 VectorNd b = VectorNd::Zero(cs.size() + model.dof_count);
1075 VectorNd x = VectorNd::Zero(cs.size() + model.dof_count);
1076
1077 // The top-left block is the weight matrix and is constant.
1078 for(unsigned int i = 0; i < weights.size(); ++i) {
1079 A(i,i) = weights[i];
1080 b[i] = weights[i] * QDotInit[i];
1081 }
1082 CalcConstraintsJacobian (model, Q, cs, constraintJac);
1083 A.block (model.dof_count, 0, cs.size(), model.dof_count) = constraintJac;
1084 A.block (0, model.dof_count, model.dof_count, cs.size())
1085 = constraintJac.transpose();
1086
1087 // Solve the sistem A*x = b.
1088 SolveLinearSystem (A, b, x, cs.linear_solver);
1089
1090 // Copy the result to the output variable.
1091 QDot = x.block (0, 0, model.dof_count, 1);
1092}
1093
1094//==============================================================================
1095RBDL_DLLAPI
1096void ForwardDynamicsConstraintsDirect (
1097 Model &model,
1098 const VectorNd &Q,
1099 const VectorNd &QDot,
1100 const VectorNd &Tau,
1101 ConstraintSet &CS,
1102 VectorNd &QDDot,
1103 bool update_kinematics,
1104 std::vector<Math::SpatialVector> *f_ext
1105)
1106{
1107 LOG << "-------- " << __func__ << " --------" << std::endl;
1108
1109 CalcConstrainedSystemVariables (model, Q, QDot, Tau, CS, update_kinematics,
1110 f_ext);
1111
1112 SolveConstrainedSystemDirect (CS.H, CS.G, Tau - CS.C, CS.gamma
1113 , CS.force, CS.A, CS.b, CS.x, CS.linear_solver);
1114
1115 // Copy back QDDot
1116 for (unsigned int i = 0; i < model.dof_count; i++) {
1117 QDDot[i] = CS.x[i];
1118 }
1119
1120 // Copy back contact forces
1121 for (unsigned int i = 0; i < CS.size(); i++) {
1122 CS.force[i] = -CS.x[model.dof_count + i];
1123 }
1124}
1125
1126//==============================================================================
1127RBDL_DLLAPI
1128void ForwardDynamicsConstraintsRangeSpaceSparse (
1129 Model &model,
1130 const Math::VectorNd &Q,
1131 const Math::VectorNd &QDot,
1132 const Math::VectorNd &Tau,
1133 ConstraintSet &CS,
1134 Math::VectorNd &QDDot,
1135 bool update_kinematics,
1136 std::vector<Math::SpatialVector> *f_ext)
1137{
1138
1139 CalcConstrainedSystemVariables (model, Q, QDot, Tau, CS, update_kinematics,
1140 f_ext);
1141
1142 SolveConstrainedSystemRangeSpaceSparse (model, CS.H, CS.G, Tau - CS.C
1143 , CS.gamma, QDDot, CS.force, CS.K, CS.a, CS.linear_solver);
1144}
1145
1146//==============================================================================
1147#ifndef RBDL_USE_CASADI_MATH
1148RBDL_DLLAPI
1149void ForwardDynamicsConstraintsNullSpace (
1150 Model &model,
1151 const VectorNd &Q,
1152 const VectorNd &QDot,
1153 const VectorNd &Tau,
1154 ConstraintSet &CS,
1155 VectorNd &QDDot,
1156 bool update_kinematics,
1157 std::vector<Math::SpatialVector> *f_ext
1158)
1159{
1160
1161 LOG << "-------- " << __func__ << " --------" << std::endl;
1162
1163 CalcConstrainedSystemVariables (model, Q, QDot, Tau, CS, update_kinematics,
1164 f_ext);
1165
1166 CS.GT_qr.compute (CS.G.transpose());
1167 CS.GT_qr.householderQ().evalTo (CS.GT_qr_Q);
1168
1169 CS.Y = CS.GT_qr_Q.block (0,0,QDot.rows(), CS.G.rows());
1170 CS.Z = CS.GT_qr_Q.block (0,CS.G.rows(),QDot.rows(), QDot.rows() - CS.G.rows());
1171
1172 SolveConstrainedSystemNullSpace (CS.H, CS.G, Tau - CS.C, CS.gamma, QDDot
1173 , CS.force, CS.Y, CS.Z, CS.qddot_y, CS.qddot_z, CS.linear_solver);
1174
1175}
1176#endif
1177
1178//==============================================================================
1179RBDL_DLLAPI
1180void ComputeConstraintImpulsesDirect (
1181 Model &model,
1182 const Math::VectorNd &Q,
1183 const Math::VectorNd &QDotMinus,
1184 ConstraintSet &CS,
1185 Math::VectorNd &QDotPlus
1186)
1187{
1188
1189 // Compute H
1190 UpdateKinematicsCustom (model, &Q, NULL, NULL);
1191 CompositeRigidBodyAlgorithm (model, Q, CS.H, false);
1192
1193 // Compute G
1194 CalcConstraintsJacobian (model, Q, CS, CS.G, false);
1195
1196 SolveConstrainedSystemDirect (CS.H, CS.G, CS.H * QDotMinus, CS.v_plus
1197 , CS.impulse, CS.A, CS.b, CS.x, CS.linear_solver);
1198
1199 // Copy back QDotPlus
1200 for (unsigned int i = 0; i < model.dof_count; i++) {
1201 QDotPlus[i] = CS.x[i];
1202 }
1203
1204 // Copy back constraint impulses
1205 for (unsigned int i = 0; i < CS.size(); i++) {
1206 CS.impulse[i] = CS.x[model.dof_count + i];
1207 }
1208
1209}
1210
1211//==============================================================================
1212RBDL_DLLAPI
1213void ComputeConstraintImpulsesRangeSpaceSparse (
1214 Model &model,
1215 const Math::VectorNd &Q,
1216 const Math::VectorNd &QDotMinus,
1217 ConstraintSet &CS,
1218 Math::VectorNd &QDotPlus
1219)
1220{
1221
1222 // Compute H
1223 UpdateKinematicsCustom (model, &Q, NULL, NULL);
1224 CompositeRigidBodyAlgorithm (model, Q, CS.H, false);
1225
1226 // Compute G
1227 CalcConstraintsJacobian (model, Q, CS, CS.G, false);
1228
1229 SolveConstrainedSystemRangeSpaceSparse (model, CS.H, CS.G, CS.H * QDotMinus
1230 , CS.v_plus, QDotPlus, CS.impulse, CS.K, CS.a, CS.linear_solver);
1231
1232}
1233
1234//==============================================================================
1235#ifndef RBDL_USE_CASADI_MATH
1236RBDL_DLLAPI
1237void ComputeConstraintImpulsesNullSpace (
1238 Model &model,
1239 const Math::VectorNd &Q,
1240 const Math::VectorNd &QDotMinus,
1241 ConstraintSet &CS,
1242 Math::VectorNd &QDotPlus
1243)
1244{
1245
1246 // Compute H
1247 UpdateKinematicsCustom (model, &Q, NULL, NULL);
1248 CompositeRigidBodyAlgorithm (model, Q, CS.H, false);
1249
1250 // Compute G
1251 CalcConstraintsJacobian (model, Q, CS, CS.G, false);
1252
1253 CS.GT_qr.compute(CS.G.transpose());
1254 CS.GT_qr_Q = CS.GT_qr.householderQ();
1255
1256 CS.Y = CS.GT_qr_Q.block (0,0,QDotMinus.rows(), CS.G.rows());
1257 CS.Z = CS.GT_qr_Q.block (0,CS.G.rows(),QDotMinus.rows(), QDotMinus.rows()
1258 - CS.G.rows());
1259
1260 SolveConstrainedSystemNullSpace (CS.H, CS.G, CS.H * QDotMinus, CS.v_plus
1261 , QDotPlus, CS.impulse, CS.Y, CS.Z, CS.qddot_y, CS.qddot_z
1262 , CS.linear_solver);
1263}
1264#endif
1265
1266//==============================================================================
1275RBDL_DLLAPI
1276void ForwardDynamicsApplyConstraintForces (
1277 Model &model,
1278 const VectorNd &Tau,
1279 ConstraintSet &CS,
1280 VectorNd &QDDot
1281)
1282{
1283 LOG << "-------- " << __func__ << " --------" << std::endl;
1284 assert (QDDot.size() == model.dof_count);
1285
1286 unsigned int i = 0;
1287
1288 for (i = 1; i < model.mBodies.size(); i++) {
1289 model.IA[i] = model.I[i].toMatrix();;
1290 model.pA[i] = crossf(model.v[i],model.I[i] * model.v[i]);
1291
1292#ifdef RBDL_USE_CASADI_MATH
1293 {
1294#else
1295 if (CS.f_ext_constraints[i] != SpatialVector::Zero()) {
1296#endif
1297 LOG << "External force (" << i << ") = "
1298 << model.X_base[i].toMatrixAdjoint() * CS.f_ext_constraints[i]
1299 << std::endl;
1300 model.pA[i] -= model.X_base[i].toMatrixAdjoint()*CS.f_ext_constraints[i];
1301 }
1302 }
1303
1304 // ClearLogOutput();
1305
1306 LOG << "--- first loop ---" << std::endl;
1307
1308 for (i = model.mBodies.size() - 1; i > 0; i--) {
1309 unsigned int q_index = model.mJoints[i].q_index;
1310
1311 if (model.mJoints[i].mDoFCount == 3
1312 && model.mJoints[i].mJointType != JointTypeCustom) {
1313 unsigned int lambda = model.lambda[i];
1314 model.multdof3_u[i] = Vector3d (Tau[q_index],
1315 Tau[q_index + 1],
1316 Tau[q_index + 2])
1317 - model.multdof3_S[i].transpose() * model.pA[i];
1318
1319 if (lambda != 0) {
1320 SpatialMatrix Ia = model.IA[i] - (model.multdof3_U[i]
1321 * model.multdof3_Dinv[i]
1322 * model.multdof3_U[i].transpose());
1323
1324 SpatialVector pa = model.pA[i] + Ia * model.c[i]
1325 + model.multdof3_U[i] * model.multdof3_Dinv[i] * model.multdof3_u[i];
1326
1327#ifdef RBDL_USE_CASADI_MATH
1328 model.IA[lambda] += (model.X_lambda[i].toMatrixTranspose()
1329 * Ia * model.X_lambda[i].toMatrix());
1330 model.pA[lambda] += model.X_lambda[i].applyTranspose(pa);
1331#else
1332 model.IA[lambda].noalias() += (model.X_lambda[i].toMatrixTranspose()
1333 * Ia * model.X_lambda[i].toMatrix());
1334 model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa);
1335#endif
1336
1337 LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose()
1338 << std::endl;
1339 }
1340 } else if (model.mJoints[i].mDoFCount == 1
1341 && model.mJoints[i].mJointType != JointTypeCustom) {
1342 model.u[i] = Tau[q_index] - model.S[i].dot(model.pA[i]);
1343
1344 unsigned int lambda = model.lambda[i];
1345 if (lambda != 0) {
1346 SpatialMatrix Ia = model.IA[i]
1347 - model.U[i] * (model.U[i] / model.d[i]).transpose();
1348 SpatialVector pa = model.pA[i] + Ia * model.c[i]
1349 + model.U[i] * model.u[i] / model.d[i];
1350#ifdef RBDL_USE_CASADI_MATH
1351 model.IA[lambda] += (model.X_lambda[i].toMatrixTranspose()
1352 * Ia * model.X_lambda[i].toMatrix());
1353 model.pA[lambda] += model.X_lambda[i].applyTranspose(pa);
1354#else
1355 model.IA[lambda].noalias() += (model.X_lambda[i].toMatrixTranspose()
1356 * Ia * model.X_lambda[i].toMatrix());
1357 model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa);
1358#endif
1359
1360 LOG << "pA[" << lambda << "] = "
1361 << model.pA[lambda].transpose() << std::endl;
1362 }
1363 } else if(model.mJoints[i].mJointType == JointTypeCustom) {
1364
1365 unsigned int kI = model.mJoints[i].custom_joint_index;
1366 unsigned int dofI = model.mCustomJoints[kI]->mDoFCount;
1367 unsigned int lambda = model.lambda[i];
1368 VectorNd tau_temp = VectorNd::Zero(dofI);
1369
1370 for(int z=0; z<dofI; ++z) {
1371 tau_temp[z] = Tau[q_index+z];
1372 }
1373
1374 model.mCustomJoints[kI]->u = tau_temp
1375 - (model.mCustomJoints[kI]->S.transpose()
1376 * model.pA[i]);
1377
1378 if (lambda != 0) {
1379 SpatialMatrix Ia = model.IA[i]
1380 - ( model.mCustomJoints[kI]->U
1381 * model.mCustomJoints[kI]->Dinv
1382 * model.mCustomJoints[kI]->U.transpose());
1383
1384 SpatialVector pa = model.pA[i] + Ia * model.c[i]
1385 + ( model.mCustomJoints[kI]->U
1386 * model.mCustomJoints[kI]->Dinv
1387 * model.mCustomJoints[kI]->u );
1388
1389#ifdef RBDL_USE_CASADI_MATH
1390 model.IA[lambda] += model.X_lambda[i].toMatrixTranspose()
1391 * Ia * model.X_lambda[i].toMatrix();
1392
1393 model.pA[lambda] += model.X_lambda[i].applyTranspose(pa);
1394#else
1395 model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose()
1396 * Ia * model.X_lambda[i].toMatrix();
1397
1398 model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa);
1399#endif
1400
1401 LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose()
1402 << std::endl;
1403 }
1404 }
1405 }
1406
1407 model.a[0] = SpatialVector (0., 0., 0.,
1408 -model.gravity[0],
1409 -model.gravity[1],
1410 -model.gravity[2]);
1411
1412 for (i = 1; i < model.mBodies.size(); i++) {
1413 unsigned int q_index = model.mJoints[i].q_index;
1414 unsigned int lambda = model.lambda[i];
1415 SpatialTransform X_lambda = model.X_lambda[i];
1416
1417 model.a[i] = X_lambda.apply(model.a[lambda]) + model.c[i];
1418 LOG << "a'[" << i << "] = " << model.a[i].transpose() << std::endl;
1419
1420 if (model.mJoints[i].mDoFCount == 3
1421 && model.mJoints[i].mJointType != JointTypeCustom) {
1422 Vector3d qdd_temp = model.multdof3_Dinv[i] *
1423 (model.multdof3_u[i]
1424 - model.multdof3_U[i].transpose() * model.a[i]);
1425
1426 QDDot[q_index] = qdd_temp[0];
1427 QDDot[q_index + 1] = qdd_temp[1];
1428 QDDot[q_index + 2] = qdd_temp[2];
1429 model.a[i] = model.a[i] + model.multdof3_S[i] * qdd_temp;
1430 } else if (model.mJoints[i].mDoFCount == 1
1431 && model.mJoints[i].mJointType != JointTypeCustom) {
1432 QDDot[q_index] = (1./model.d[i]) * (
1433 model.u[i] - model.U[i].dot(model.a[i]));
1434 model.a[i] = model.a[i] + model.S[i] * QDDot[q_index];
1435 } else if (model.mJoints[i].mJointType == JointTypeCustom) {
1436 unsigned int kI = model.mJoints[i].custom_joint_index;
1437 unsigned int dofI = model.mCustomJoints[kI]->mDoFCount;
1438 VectorNd qdd_temp = VectorNd::Zero(dofI);
1439
1440 qdd_temp = model.mCustomJoints[kI]->Dinv
1441 * (model.mCustomJoints[kI]->u
1442 - model.mCustomJoints[kI]->U.transpose()
1443 * model.a[i]);
1444
1445 for(int z=0; z<dofI; ++z) {
1446 QDDot[q_index+z] = qdd_temp[z];
1447 }
1448
1449 model.a[i] = model.a[i] + (model.mCustomJoints[kI]->S * qdd_temp);
1450 }
1451 }
1452
1453 LOG << "QDDot = " << QDDot.transpose() << std::endl;
1454}
1455
1456//==============================================================================
1464RBDL_DLLAPI
1465void ForwardDynamicsAccelerationDeltas (
1466 Model &model,
1467 ConstraintSet &CS,
1468 VectorNd &QDDot_t,
1469 const unsigned int body_id,
1470 const std::vector<SpatialVector> &f_t
1471)
1472{
1473 LOG << "-------- " << __func__ << " ------" << std::endl;
1474
1475 assert (CS.d_pA.size() == model.mBodies.size());
1476 assert (CS.d_a.size() == model.mBodies.size());
1477 assert (CS.d_u.size() == model.mBodies.size());
1478
1479 // TODO reset all values (debug)
1480 for (unsigned int i = 0; i < model.mBodies.size(); i++) {
1481 CS.d_pA[i].setZero();
1482 CS.d_a[i].setZero();
1483 CS.d_u[i] = 0.;
1484 CS.d_multdof3_u[i].setZero();
1485 }
1486 for(unsigned int i=0; i<model.mCustomJoints.size(); i++) {
1487 model.mCustomJoints[i]->d_u.setZero();
1488 }
1489
1490 for (unsigned int i = body_id; i > 0; i--) {
1491 if (i == body_id) {
1492 CS.d_pA[i] = -model.X_base[i].applyAdjoint(f_t[i]);
1493 }
1494
1495 if (model.mJoints[i].mDoFCount == 3
1496 && model.mJoints[i].mJointType != JointTypeCustom) {
1497 CS.d_multdof3_u[i] = - model.multdof3_S[i].transpose() * (CS.d_pA[i]);
1498
1499 unsigned int lambda = model.lambda[i];
1500 if (lambda != 0) {
1501 CS.d_pA[lambda] = CS.d_pA[lambda]
1502 + model.X_lambda[i].applyTranspose (
1503 CS.d_pA[i] + (model.multdof3_U[i]
1504 * model.multdof3_Dinv[i]
1505 * CS.d_multdof3_u[i]));
1506 }
1507 } else if(model.mJoints[i].mDoFCount == 1
1508 && model.mJoints[i].mJointType != JointTypeCustom) {
1509 CS.d_u[i] = - model.S[i].dot(CS.d_pA[i]);
1510 unsigned int lambda = model.lambda[i];
1511
1512 if (lambda != 0) {
1513 CS.d_pA[lambda] = CS.d_pA[lambda]
1514 + model.X_lambda[i].applyTranspose (
1515 CS.d_pA[i] + model.U[i] * CS.d_u[i] / model.d[i]);
1516 }
1517 } else if (model.mJoints[i].mJointType == JointTypeCustom) {
1518
1519 unsigned int kI = model.mJoints[i].custom_joint_index;
1520 unsigned int dofI = model.mCustomJoints[kI]->mDoFCount;
1521 //CS.
1522 model.mCustomJoints[kI]->d_u =
1523 - model.mCustomJoints[kI]->S.transpose() * (CS.d_pA[i]);
1524 unsigned int lambda = model.lambda[i];
1525 if (lambda != 0) {
1526 CS.d_pA[lambda] =
1527 CS.d_pA[lambda]
1528 + model.X_lambda[i].applyTranspose (
1529 CS.d_pA[i] + ( model.mCustomJoints[kI]->U
1530 * model.mCustomJoints[kI]->Dinv
1531 * model.mCustomJoints[kI]->d_u)
1532 );
1533 }
1534 }
1535 }
1536
1537 for (unsigned int i = 0; i < f_t.size(); i++) {
1538 LOG << "f_t[" << i << "] = " << f_t[i].transpose() << std::endl;
1539 }
1540
1541 for (unsigned int i = 0; i < model.mBodies.size(); i++) {
1542 LOG << "i = " << i << ": d_pA[i] " << CS.d_pA[i].transpose() << std::endl;
1543 }
1544 for (unsigned int i = 0; i < model.mBodies.size(); i++) {
1545 LOG << "i = " << i << ": d_u[i] = " << CS.d_u[i] << std::endl;
1546 }
1547
1548 QDDot_t[0] = 0.;
1549 CS.d_a[0] = model.a[0];
1550
1551 for (unsigned int i = 1; i < model.mBodies.size(); i++) {
1552 unsigned int q_index = model.mJoints[i].q_index;
1553 unsigned int lambda = model.lambda[i];
1554
1555 SpatialVector Xa = model.X_lambda[i].apply(CS.d_a[lambda]);
1556
1557 if (model.mJoints[i].mDoFCount == 3
1558 && model.mJoints[i].mJointType != JointTypeCustom) {
1559 Vector3d qdd_temp = model.multdof3_Dinv[i]
1560 * (CS.d_multdof3_u[i] - model.multdof3_U[i].transpose() * Xa);
1561
1562 QDDot_t[q_index] = qdd_temp[0];
1563 QDDot_t[q_index + 1] = qdd_temp[1];
1564 QDDot_t[q_index + 2] = qdd_temp[2];
1565 model.a[i] = model.a[i] + model.multdof3_S[i] * qdd_temp;
1566 CS.d_a[i] = Xa + model.multdof3_S[i] * qdd_temp;
1567 } else if (model.mJoints[i].mDoFCount == 1
1568 && model.mJoints[i].mJointType != JointTypeCustom) {
1569
1570 QDDot_t[q_index] = (CS.d_u[i] - model.U[i].dot(Xa) ) / model.d[i];
1571 CS.d_a[i] = Xa + model.S[i] * QDDot_t[q_index];
1572 } else if (model.mJoints[i].mJointType == JointTypeCustom) {
1573 unsigned int kI = model.mJoints[i].custom_joint_index;
1574 unsigned int dofI = model.mCustomJoints[kI]->mDoFCount;
1575 VectorNd qdd_temp = VectorNd::Zero(dofI);
1576
1577 qdd_temp = model.mCustomJoints[kI]->Dinv
1578 * (model.mCustomJoints[kI]->d_u
1579 - model.mCustomJoints[kI]->U.transpose() * Xa);
1580
1581 for(int z=0; z<dofI; ++z) {
1582 QDDot_t[q_index+z] = qdd_temp[z];
1583 }
1584
1585 model.a[i] = model.a[i] + model.mCustomJoints[kI]->S * qdd_temp;
1586 CS.d_a[i] = Xa + model.mCustomJoints[kI]->S * qdd_temp;
1587 }
1588
1589 LOG << "QDDot_t[" << i - 1 << "] = " << QDDot_t[i - 1] << std::endl;
1590 LOG << "d_a[i] = " << CS.d_a[i].transpose() << std::endl;
1591 }
1592}
1593
1594inline void set_zero (std::vector<SpatialVector> &spatial_values)
1595{
1596 for (unsigned int i = 0; i < spatial_values.size(); i++) {
1597 spatial_values[i].setZero();
1598 }
1599}
1600
1601//==============================================================================
1602RBDL_DLLAPI
1603void ForwardDynamicsContactsKokkevis (
1604 Model &model,
1605 const VectorNd &Q,
1606 const VectorNd &QDot,
1607 const VectorNd &Tau,
1608 ConstraintSet &CS,
1609 VectorNd &QDDot
1610)
1611{
1612 LOG << "-------- " << __func__ << " ------" << std::endl;
1613
1614 assert (CS.f_ext_constraints.size() == model.mBodies.size());
1615 assert (CS.QDDot_0.size() == model.dof_count);
1616 assert (CS.QDDot_t.size() == model.dof_count);
1617 assert (CS.f_t.size() == CS.size());
1618 assert (CS.point_accel_0.size() == CS.size());
1619 assert (CS.K.rows() == CS.size());
1620 assert (CS.K.cols() == CS.size());
1621 assert (CS.force.size() == CS.size());
1622 assert (CS.a.size() == CS.size());
1623
1624 if(CS.constraints.size() != CS.contactConstraints.size()) {
1625 std::stringstream errormsg;
1626 errormsg << "Incompatible constraint types: all constraints"
1627 << " must be ContactConstraints for the Kokkevis method"
1628 << std::endl;
1629 throw Errors::RBDLError(errormsg.str());
1630 }
1631
1632 Vector3d point_accel_t;
1633
1634 unsigned int ci = 0; //constraint index:
1635 // the row index in the constraint Jacobian.
1636
1637 // The default acceleration only needs to be computed once
1638 {
1639 SUPPRESS_LOGGING;
1640 ForwardDynamics(model, Q, QDot, Tau, CS.QDDot_0);
1641 }
1642
1643 LOG << "=== Initial Loop Start ===" << std::endl;
1644 // we have to compute the standard accelerations first as we use them to
1645 // compute the effects of each test force
1646 unsigned int bi = 0;
1647 for(bi =0; bi < CS.contactConstraints.size(); ++bi) {
1648 {
1649 SUPPRESS_LOGGING;
1650 UpdateKinematicsCustom(model, NULL, NULL, &CS.QDDot_0);
1651 }
1652 {
1653 LOG << "body_id = "
1654 << CS.contactConstraints[bi]->getBodyIds()[0]
1655 << std::endl;
1656 LOG << "point = "
1657 << CS.contactConstraints[bi]->getBodyFrames()[0].r
1658 << std::endl;
1659 LOG << "QDDot_0 = " << CS.QDDot_0.transpose() << std::endl;
1660 }
1661 {
1662 SUPPRESS_LOGGING;
1663 CS.contactConstraints[bi]->calcPointAccelerations(
1664 model,Q,QDot,CS.QDDot_0,CS.point_accel_0,false);
1665 CS.contactConstraints[bi]->calcPointAccelerationError(
1666 CS.point_accel_0,CS.a);
1667 }
1668 }
1669
1670 // K: ContactConstraints
1671 unsigned int cj=0;
1672 unsigned int movable_body_id = 0;
1673 Vector3d point_global;
1674
1675 for (bi = 0; bi < CS.contactConstraints.size(); bi++) {
1676
1677 LOG << "=== Testforce Loop Start ===" << std::endl;
1678
1679 ci = CS.contactConstraints[bi]->getConstraintIndex();
1680
1681 movable_body_id = GetMovableBodyId(model,
1682 CS.contactConstraints[bi]->getBodyIds()[0]);
1683
1684 // assemble the test force
1685 LOG << "point_global = " << point_global.transpose() << std::endl;
1686
1687 CS.contactConstraints[bi]->calcPointForceJacobian(
1688 model,Q,CS.cache,CS.f_t,false);
1689
1690 for(unsigned int j = 0; j<CS.contactConstraints[bi]
1691 ->getConstraintNormalVectors().size(); ++j) {
1692
1693 CS.f_ext_constraints[movable_body_id] = CS.f_t[ci+j];
1694
1695 LOG << "f_t[" << movable_body_id << "] = " << CS.f_t[ci+j].transpose()
1696 << std::endl;
1697 {
1698 ForwardDynamicsAccelerationDeltas(model, CS, CS.QDDot_t
1699 , movable_body_id, CS.f_ext_constraints);
1700
1701 LOG << "QDDot_0 = " << CS.QDDot_0.transpose() << std::endl;
1702 LOG << "QDDot_t = " << (CS.QDDot_t + CS.QDDot_0).transpose()
1703 << std::endl;
1704 LOG << "QDDot_t - QDDot_0 = " << (CS.QDDot_t).transpose() << std::endl;
1705 }
1706
1707 CS.f_ext_constraints[movable_body_id].setZero();
1708
1709 CS.QDDot_t += CS.QDDot_0;
1710 // compute the resulting acceleration
1711 {
1712 SUPPRESS_LOGGING;
1713 UpdateKinematicsCustom(model, NULL, NULL, &CS.QDDot_t);
1714 }
1715
1716 for(unsigned int dj = 0;
1717 dj < CS.contactConstraints.size(); dj++) {
1718
1719 cj = CS.contactConstraints[dj]->getConstraintIndex();
1720 {
1721 SUPPRESS_LOGGING;
1722 CS.contactConstraints[dj]->calcPointAccelerations(
1723 model,Q,QDot,CS.QDDot_t,point_accel_t,false);
1724 }
1725
1726 LOG << "point_accel_0 = " << CS.point_accel_0[ci+j].transpose()
1727 << std::endl;
1728 LOG << "point_accel_t = " << point_accel_t.transpose() << std::endl;
1729 for(unsigned int k=0;
1730 k < CS.contactConstraints[dj]
1731 ->getConstraintNormalVectors().size(); ++k) {
1732
1733 CS.K(ci+j,cj+k) = CS.contactConstraints[dj]
1734 ->getConstraintNormalVectors()[k].dot(
1735 point_accel_t - CS.point_accel_0[cj+k]);
1736 }
1737 }
1738 }
1739 }
1740
1741
1742
1743
1744 LOG << "K = " << std::endl << CS.K << std::endl;
1745 LOG << "a = " << std::endl << CS.a << std::endl;
1746
1747#ifdef RBDL_USE_CASADI_MATH
1748 auto linsol = casadi::Linsol("linear_solver", "symbolicqr", CS.K.sparsity());
1749 CS.force = linsol.solve(CS.K, CS.a);
1750#else
1751 switch (CS.linear_solver) {
1752 case (LinearSolverPartialPivLU) :
1753 CS.force = CS.K.partialPivLu().solve(CS.a);
1754 break;
1755 case (LinearSolverColPivHouseholderQR) :
1756 CS.force = CS.K.colPivHouseholderQr().solve(CS.a);
1757 break;
1758 case (LinearSolverHouseholderQR) :
1759 CS.force = CS.K.householderQr().solve(CS.a);
1760 break;
1761 default:
1762 LOG << "Error: Invalid linear solver: " << CS.linear_solver << std::endl;
1763 assert (0);
1764 break;
1765 }
1766#endif
1767
1768 LOG << "f = " << CS.force.transpose() << std::endl;
1769
1770
1771 for(bi=0; bi<CS.contactConstraints.size(); ++bi) {
1772 unsigned int body_id =
1773 CS.contactConstraints[bi]->getBodyIds()[0];
1774 unsigned int movable_body_id = body_id;
1775
1776 if (model.IsFixedBodyId(body_id)) {
1777 unsigned int fbody_id = body_id - model.fixed_body_discriminator;
1778 movable_body_id = model.mFixedBodies[fbody_id].mMovableParent;
1779 }
1780 ci = CS.contactConstraints[bi]->getConstraintIndex();
1781
1782 for(unsigned int k=0;
1783 k<CS.contactConstraints[bi]->getConstraintSize(); ++k) {
1784 CS.f_ext_constraints[movable_body_id] -= CS.f_t[ci+k] * CS.force[ci+k];
1785 LOG << "f_ext[" << movable_body_id << "] = "
1786 << CS.f_ext_constraints[movable_body_id].transpose() << std::endl;
1787 }
1788
1789 }
1790
1791
1792
1793 {
1794 SUPPRESS_LOGGING;
1795 ForwardDynamicsApplyConstraintForces (model, Tau, CS, QDDot);
1796 }
1797
1798 LOG << "QDDot after applying f_ext: " << QDDot.transpose() << std::endl;
1799}
1800
1801
1802//==============================================================================
1803#ifndef RBDL_USE_CASADI_MATH
1804RBDL_DLLAPI
1805bool isConstrainedSystemFullyActuated(
1806 Model &model,
1807 const Math::VectorNd &Q,
1808 const Math::VectorNd &QDot,
1809 ConstraintSet &CS,
1810 bool update_kinematics,
1811 std::vector<Math::SpatialVector> *f_ext)
1812{
1813
1814 LOG << "-------- " << __func__ << " ------" << std::endl;
1815
1816
1817 assert (CS.S.cols() == QDot.rows());
1818
1819 unsigned int n = unsigned( CS.H.rows());
1820 unsigned int nc = unsigned( CS.name.size());
1821 unsigned int na = unsigned( CS.S.rows());
1822 unsigned int nu = n-na;
1823
1824
1825 CalcConstrainedSystemVariables(model,Q,QDot,VectorNd::Zero(QDot.rows()),CS,
1826 update_kinematics, f_ext);
1827
1828 CS.GPT = CS.G*CS.P.transpose();
1829
1830 CS.GPT_full_qr.compute(CS.GPT);
1831 unsigned int r = unsigned(CS.GPT_full_qr.rank());
1832
1833 bool isCompatible = false;
1834 if(r == (n-na)) {
1835 isCompatible = true;
1836 } else {
1837 isCompatible = false;
1838 }
1839
1840 return isCompatible;
1841
1842}
1843#endif
1844
1845RBDL_DLLAPI
1846void InverseDynamicsConstraints(
1847 Model &model,
1848 const Math::VectorNd &Q,
1849 const Math::VectorNd &QDot,
1850 const Math::VectorNd &QDDotDesired,
1851 ConstraintSet &CS,
1852 Math::VectorNd &QDDotOutput,
1853 Math::VectorNd &TauOutput,
1854 bool update_kinematics,
1855 std::vector<Math::SpatialVector> *f_ext)
1856{
1857
1858 LOG << "-------- " << __func__ << " ------" << std::endl;
1859
1860 assert (QDot.size() == QDDotDesired.size());
1861 assert (QDDotDesired.size() == QDot.size());
1862 assert (QDDotOutput.size() == QDot.size());
1863 assert (TauOutput.size() == CS.H.rows());
1864
1865 assert (CS.S.cols() == QDDotDesired.rows());
1866
1867 unsigned int n = unsigned( CS.H.rows());
1868 unsigned int nc = unsigned( CS.name.size());
1869 unsigned int na = unsigned( CS.S.rows());
1870 unsigned int nu = n-na;
1871
1872 TauOutput.setZero();
1873 CalcConstrainedSystemVariables(model,Q,QDot,TauOutput,CS,update_kinematics,
1874 f_ext);
1875
1876 // This implementation follows the projected KKT system described in
1877 // Eqn. 5.20 of Henning Koch's thesis work. Note that this will fail
1878 // for under actuated systems
1879 // [ SMS' SMP' SJ' I][ u] [ -SC ]
1880 // [ PMS' PMP' PJ' ][ v] = [ -PC ]
1881 // [ JS' JP' 0 ][-lambda] [ -gamma ]
1882 // [ I ][ -tau] [ v* ]
1883 //double alpha = 0.1;
1884
1885 CS.Ful = CS.S*CS.H*CS.S.transpose();
1886 CS.Fur = CS.S*CS.H*CS.P.transpose();
1887 CS.Fll = CS.P*CS.H*CS.S.transpose();
1888 CS.Flr = CS.P*CS.H*CS.P.transpose();
1889
1890 CS.GTu = CS.S*(CS.G.transpose());
1891 CS.GTl = CS.P*(CS.G.transpose());
1892
1893 //Exploiting the block triangular structure
1894 //u:
1895 //I u = S*qdd*
1896 CS.u = CS.S*QDDotDesired;
1897 // v
1898 //(JP')v = -gamma - (JS')u
1899 //Using GT
1900
1901 //This fails using SimpleMath and I'm not sure how to fix it
1902 SolveLinearSystem( CS.GTl.transpose(),
1903 CS.gamma - CS.GTu.transpose()*CS.u,
1904 CS.v, CS.linear_solver);
1905
1906 // lambda
1907 SolveLinearSystem(CS.GTl,
1908 -CS.P*CS.C
1909 - CS.Fll*CS.u
1910 - CS.Flr*CS.v,
1911 CS.force,
1912 CS.linear_solver);
1913
1914 for(unsigned int i=0; i<CS.force.rows(); ++i) {
1915 CS.force[i] *= -1.0;
1916 }
1917
1918 //Evaluating qdd
1919 QDDotOutput = CS.S.transpose()*CS.u + CS.P.transpose()*CS.v;
1920
1921 //Evaluating tau
1922 TauOutput = -CS.S.transpose()*( -CS.S*CS.C
1923 -( CS.Ful*CS.u
1924 +CS.Fur*CS.v
1925 -CS.GTu*CS.force));
1926
1927
1928
1929
1930}
1931
1932#ifndef RBDL_USE_CASADI_MATH
1933RBDL_DLLAPI
1934void InverseDynamicsConstraintsRelaxed(
1935 Model &model,
1936 const Math::VectorNd &Q,
1937 const Math::VectorNd &QDot,
1938 const Math::VectorNd &QDDotControls,
1939 ConstraintSet &CS,
1940 Math::VectorNd &QDDotOutput,
1941 Math::VectorNd &TauOutput,
1942 bool update_kinematics,
1943 std::vector<Math::SpatialVector> *f_ext)
1944{
1945 LOG << "-------- " << __func__ << " --------" << std::endl;
1946
1947 //Check that the input vectors and matricies are sized appropriately
1948 assert(Q.size() == model.q_size);
1949 assert(QDot.size() == model.qdot_size);
1950 assert(QDDotControls.size() == model.dof_count);
1951 assert(CS.S.cols() == model.dof_count);
1952 assert(CS.W.rows() == CS.W.cols());
1953 assert(CS.W.rows() == CS.S.rows());
1954 assert(QDDotOutput.size() == model.dof_count);
1955
1956 TauOutput.setZero();
1957 CalcConstrainedSystemVariables(model,Q,QDot,TauOutput,CS,update_kinematics,
1958 f_ext);
1959
1960 unsigned int n = unsigned( CS.H.rows());
1961 unsigned int nc = unsigned( CS.name.size());
1962 unsigned int na = unsigned( CS.S.rows());
1963 unsigned int nu = n-na;
1964
1965 //MM 2020/5/29:
1966 // The updates I made to Henning's original formulation have
1967 // almost certainly made the sensitivity of the resulting qdd
1968 // w.r.t. the controls poorly scaled. At least this is a suspicion
1969 // of mine looking at how an OCP is behaving when using this operator.
1970 // Reverting to the original formulation.
1971 //MM: Update to Henning's formulation s.t. the relaxed IDC operator will
1972 // more closely satisfy QDDotControls if it is possible.
1973 //double diag = 0.;//100.*CS.H.maxCoeff();
1974 //double diagInv = 0.;
1975 //for(unsigned int i=0; i<CS.H.rows(); ++i) {
1976 // for(unsigned int j=0; j<CS.H.cols(); ++j) {
1977 // if(fabs(CS.H(i,j)) > diag) {
1978 // diag = fabs(CS.H(i,j));
1979 // }
1980 // }
1981 //}
1982 //diag = diag*100.;
1983 //diagInv = 1.0/diag;
1984 //for(unsigned int i=0; i<CS.W.rows(); ++i) {
1985 // CS.W(i,i) = diag;
1986 // CS.Winv(i,i) = diagInv;
1987 //}
1988
1989 CS.W = 100.0*CS.S*CS.H*CS.S.transpose();
1990 CS.Winv = CS.W.inverse();
1991 CS.WinvSC = CS.Winv * CS.S * CS.C;
1992
1993 //CS.W = CS.S*CS.H*CS.S.transpose();
1994
1995 CS.F.block( 0, 0, na, na) = CS.S*CS.H*CS.S.transpose() + CS.W;
1996 CS.F.block( 0, na, na, nu) = CS.S*CS.H*CS.P.transpose();
1997 CS.F.block( na, 0, nu, na) = CS.P*CS.H*CS.S.transpose();
1998 CS.F.block( na, na, nu, nu) = CS.P*CS.H*CS.P.transpose();
1999
2000 CS.GT.block( 0, 0,na, nc) = CS.S*(CS.G.transpose());
2001 CS.GT.block( na, 0,nu, nc) = CS.P*(CS.G.transpose());
2002
2003 CS.GT_qr.compute (CS.GT);
2004 CS.GT_qr.householderQ().evalTo (CS.GT_qr_Q);
2005
2006 //GT = [Y Z] * [ R ]
2007 // [ 0 ]
2008
2009 CS.R = CS.GT_qr_Q.transpose()*CS.GT;
2010 CS.Ru = CS.R.block(0,0,nc,nc);
2011
2012 CS.Y = CS.GT_qr_Q.block( 0, 0, n, nc );
2013 CS.Z = CS.GT_qr_Q.block( 0, nc, n, (n-nc));
2014
2015 //MM: Update to Henning's formulation s.t. the relaxed IDC operator will
2016 // exactly satisfy QDDotControls if it is possible.
2017 //
2018 //Modify QDDotControls so that SN is cancelled.
2019 //
2020 // +SC - WS(qdd*)
2021 //
2022 // Add a term to cancel off SN
2023 //
2024 // +SC - WS( qdd* + (S' W^-1 S)N )
2025 //
2026
2027 CS.u = CS.S*CS.C - CS.W*(CS.S*(QDDotControls
2028 +(CS.S.transpose()*CS.WinvSC)));
2029 //CS.u = CS.S*CS.C - CS.W*(CS.S*QDDotControls);
2030 CS.v = CS.P*CS.C;
2031
2032 for(unsigned int i=0; i<CS.S.rows(); ++i) {
2033 CS.g[i] = CS.u[i];
2034 }
2035 unsigned int j=CS.S.rows();
2036 for(unsigned int i=0; i<CS.P.rows(); ++i) {
2037 CS.g[j] = CS.v[i];
2038 ++j;
2039 }
2040
2041 //nc x nc system
2042 SolveLinearSystem(CS.Ru.transpose(), CS.gamma, CS.py, CS.linear_solver);
2043
2044 //(n-nc) x (n-nc) system
2045 SolveLinearSystem(CS.Z.transpose()*CS.F*CS.Z,
2046 CS.Z.transpose()*(-CS.F*CS.Y*CS.py-CS.g),
2047 CS.pz,
2048 CS.linear_solver);
2049
2050 //nc x nc system
2051 SolveLinearSystem(CS.Ru,
2052 CS.Y.transpose()*(CS.g + CS.F*CS.Y*CS.py + CS.F*CS.Z*CS.pz),
2053 CS.force, CS.linear_solver);
2054
2055 //Eqn. 32d, the equation for qdd, is in error. Instead
2056 // p = Ypy + Zpz = [v,w]
2057 // qdd = S'v + P'w
2058 QDDotOutput = CS.Y*CS.py + CS.Z*CS.pz;
2059 for(unsigned int i=0; i<CS.S.rows(); ++i) {
2060 CS.u[i] = QDDotOutput[i];
2061 }
2062 j = CS.S.rows();
2063 for(unsigned int i=0; i<CS.P.rows(); ++i) {
2064 CS.v[i] = QDDotOutput[j];
2065 ++j;
2066 }
2067
2068 QDDotOutput = CS.S.transpose()*CS.u
2069 +CS.P.transpose()*CS.v;
2070
2071 TauOutput = (CS.S.transpose()*CS.W*CS.S)*(
2072 QDDotControls+(CS.S.transpose()*CS.WinvSC)
2073 -QDDotOutput);
2074 //TauOutput = CS.S.transpose()*CS.W*CS.S*(QDDotControls - QDDotOutput);
2075
2076
2077}
2078#endif
2079
2080void SolveLinearSystem (
2081 const MatrixNd& A,
2082 const VectorNd& b,
2083 VectorNd& x,
2084 LinearSolver ls
2085)
2086{
2087 if(A.rows() != b.size() || A.cols() != x.size()) {
2088 throw Errors::RBDLSizeMismatchError("Mismatching sizes.\n");
2089 }
2090
2091#ifdef RBDL_USE_CASADI_MATH
2092 auto linsol = casadi::Linsol("linear_solver", "symbolicqr", A.sparsity());
2093 x = linsol.solve(A, b);
2094#else
2095 // Solve the system A*x = b.
2096 switch (ls) {
2097 case (LinearSolverPartialPivLU) :
2098 x = A.partialPivLu().solve(b);
2099 break;
2100 case (LinearSolverColPivHouseholderQR) :
2101 x = A.colPivHouseholderQr().solve(b);
2102 break;
2103 case (LinearSolverHouseholderQR) :
2104 x = A.householderQr().solve(b);
2105 break;
2106 default:
2107 std::ostringstream errormsg;
2108 errormsg << "Error: Invalid linear solver: " << ls << std::endl;
2109 throw Errors::RBDLError(errormsg.str());
2110 break;
2111 }
2112#endif
2113}
2114
2115//==============================================================================
2116unsigned int GetMovableBodyId (Model& model, unsigned int id)
2117{
2118 if(model.IsFixedBodyId(id)) {
2119 unsigned int fbody_id = id - model.fixed_body_discriminator;
2120 return model.mFixedBodies[fbody_id].mMovableParent;
2121 } else {
2122 return id;
2123 }
2124}
2125
2126//==============================================================================
2127
2128void ConstraintSet::calcForces(
2129 unsigned int groupIndex,
2130 Model& model,
2131 const Math::VectorNd &Q,
2132 const Math::VectorNd &QDot,
2133 std::vector< unsigned int > &constraintBodyIdsUpd,
2134 std::vector< Math::SpatialTransform > &constraintBodyFramesUpd,
2135 std::vector< Math::SpatialVector > &constraintForcesUpd,
2136 bool resolveAllInRootFrame,
2137 bool updKin)
2138{
2139 assert(groupIndex <= unsigned(constraints.size()-1));
2140
2141 if (updKin) {
2142 UpdateKinematicsCustom (model, &Q, &QDot, NULL);
2143 }
2144
2145 constraints[groupIndex]->calcConstraintForces(model,0.,Q,QDot,G,force,
2146 constraintBodyIdsUpd,
2147 constraintBodyFramesUpd,
2148 constraintForcesUpd,
2149 cache,
2150 resolveAllInRootFrame,
2151 updKin);
2152}
2153
2154//==============================================================================
2155
2156void ConstraintSet::calcImpulses(
2157 unsigned int groupIndex,
2158 Model& model,
2159 const Math::VectorNd &Q,
2160 const Math::VectorNd &QDot,
2161 std::vector< unsigned int > &constraintBodyIdsUpd,
2162 std::vector< Math::SpatialTransform > &constraintBodyFramesUpd,
2163 std::vector< Math::SpatialVector > &constraintImpulsesUpd,
2164 bool resolveAllInRootFrame,
2165 bool updKin)
2166{
2167 assert(groupIndex <= unsigned(constraints.size()-1));
2168
2169 if (updKin) {
2170 UpdateKinematicsCustom (model, &Q, &QDot, NULL);
2171 }
2172
2173 //The transformation is identical to resolve the impulses
2174 //to the desired frame
2175 constraints[groupIndex]->calcConstraintForces(model,0.,Q,QDot,G,impulse,
2176 constraintBodyIdsUpd,
2177 constraintBodyFramesUpd,
2178 constraintImpulsesUpd,
2179 cache,
2180 resolveAllInRootFrame,
2181 updKin);
2182
2183 //But due to Martin's choice of signs on the Lagrange multipliers vs
2184 //impulses the signs are opposite
2185 for(unsigned int i=0; i<constraintImpulsesUpd.size(); ++i) {
2186 constraintImpulsesUpd[i] *= -1.0;
2187 }
2188}
2189
2190//==============================================================================
2191
2192void ConstraintSet::calcPositionError(
2193 unsigned int groupIndex,
2194 Model& model,
2195 const Math::VectorNd &Q,
2196 Math::VectorNd &posErrUpd,
2197 bool updKin)
2198{
2199 assert(groupIndex <= unsigned(constraints.size()-1));
2200
2201 if (updKin) {
2202 UpdateKinematicsCustom (model, &Q, NULL, NULL);
2203 }
2204
2205 //Update the position errors for this constraint in the system level
2206 //error vector
2207 constraints[groupIndex]->calcPositionError(model,0.,Q,err,cache,updKin);
2208
2209 //Pick out the position errors for this constraint from the system level
2210 //error vector.
2211 constraints[groupIndex]->getPositionError(err,posErrUpd);
2212
2213}
2214//==============================================================================
2215
2216void ConstraintSet::calcVelocityError(
2217 unsigned int groupIndex,
2218 Model& model,
2219 const Math::VectorNd &Q,
2220 const Math::VectorNd &QDot,
2221 Math::VectorNd &velErrUpd,
2222 bool updKin)
2223{
2224 assert(groupIndex <= unsigned(constraints.size()-1));
2225
2226 if (updKin) {
2227 UpdateKinematicsCustom (model, &Q, &QDot, NULL);
2228 }
2229
2230 //Update the constraint Jacobian of this constraint
2231 constraints[groupIndex]->calcConstraintJacobian(model,0.,Q,QDot,G,cache,
2232 updKin);
2233
2234 //Update the velocity-level errors of this constraint
2235 constraints[groupIndex]->calcVelocityError(model,0.,Q,QDot,G,errd,cache,
2236 updKin);
2237
2238 //Pick out the sub vector of velocity errors for this constraint from
2239 //the system error vector.
2240 constraints[groupIndex]->getVelocityError(errd,velErrUpd);
2241
2242}
2243//==============================================================================
2244
2245void ConstraintSet::calcBaumgarteStabilizationForces(
2246 unsigned int groupIndex,
2247 Model& model,
2248 const Math::VectorNd &positionError,
2249 const Math::VectorNd &velocityError,
2250 Math::VectorNd &baumgarteForces)
2251{
2252 assert(groupIndex <= unsigned(constraints.size()-1));
2253 assert(positionError.rows() == constraints[groupIndex]->getConstraintSize());
2254 assert(velocityError.rows() == constraints[groupIndex]->getConstraintSize());
2255
2256 constraints[groupIndex]->getBaumgarteStabilizationForces(positionError,
2257 velocityError,
2258 baumgarteForces);
2259
2260}
2261
2262} /* namespace RigidBodyDynamics */
Body.h
Constraint_Contact.h
RBDLCasadiMath::MX_Xd_dynamic::transpose
MX_Xd_dynamic transpose() const
Definition: MX_Xd_dynamic.h:159
RBDLCasadiMath::MX_Xd_dynamic::rows
unsigned int rows() const
Definition: MX_Xd_dynamic.h:95
RBDLCasadiMath::MX_Xd_dynamic::setZero
MX_Xd_dynamic & setZero()
Definition: MX_Xd_dynamic.h:55
RBDLCasadiMath::MX_Xd_dynamic::cols
unsigned int cols() const
Definition: MX_Xd_dynamic.h:99
RBDLCasadiMath::MX_Xd_dynamic::resize
void resize(unsigned int newI, unsigned int newJ=1)
Definition: MX_Xd_dynamic.h:91
RBDLCasadiMath::MX_Xd_dynamic::size
unsigned int size() const
Definition: MX_Xd_dynamic.h:103
RBDLCasadiMath::MX_Xd_dynamic::Identity
static MX_Xd_dynamic Identity(unsigned int size, unsigned int ignoredSize=0)
Definition: MX_Xd_dynamic.h:68
RBDLCasadiMath::MX_Xd_dynamic::conservativeResize
void conservativeResize(unsigned int nrows, unsigned int ncols=1)
Definition: MX_Xd_dynamic.h:37
RBDLCasadiMath::MX_Xd_dynamic::Zero
static MX_Xd_dynamic Zero(unsigned int nrows, unsigned int ncols=1)
Definition: MX_Xd_dynamic.h:52
RBDLCasadiMath::MX_Xd_dynamic::block
MX_Xd_SubMatrix block(unsigned int row_start, unsigned int col_start)
Definition: MX_Xd_dynamic.h:108
RigidBodyDynamics::ConstraintSet::userDefinedIdGroupMap
std::map< unsigned int, unsigned int > userDefinedIdGroupMap
Definition: Constraints.h:867
RigidBodyDynamics::ComputeConstraintImpulsesNullSpace
RBDL_DLLAPI void ComputeConstraintImpulsesNullSpace(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotMinus, ConstraintSet &CS, Math::VectorNd &QDotPlusOutput)
Resolves contact gain using SolveContactSystemNullSpace()
RigidBodyDynamics::ConstraintSet::WinvSC
Math::VectorNd WinvSC
Definition: Constraints.h:930
RigidBodyDynamics::ConstraintSet::calcVelocityError
void calcVelocityError(unsigned int groupIndex, Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::VectorNd &velocityErrorOutput, bool updateKinematics=false)
calcVelocityError calculates the vector of position errors associated with this constraint....
RigidBodyDynamics::ConstraintSet::calcPositionError
void calcPositionError(unsigned int groupIndex, Model &model, const Math::VectorNd &Q, Math::VectorNd &positionErrorOutput, bool updateKinematics=false)
calcPositionError calculates the vector of position errors associated with this constraint....
RigidBodyDynamics::ConstraintSet::b
Math::VectorNd b
Workspace for the Lagrangian right-hand-side.
Definition: Constraints.h:917
RigidBodyDynamics::ConstraintSet::QDDot_t
Math::VectorNd QDDot_t
Workspace for the test accelerations.
Definition: Constraints.h:965
RigidBodyDynamics::ConstraintSet::f_ext_constraints
std::vector< Math::SpatialVector > f_ext_constraints
Workspace for the actual spatial forces.
Definition: Constraints.h:971
RigidBodyDynamics::ConstraintSet::g
Math::VectorNd g
Definition: Constraints.h:940
RigidBodyDynamics::ConstraintSet::d_multdof3_u
std::vector< Math::Vector3d > d_multdof3_u
Definition: Constraints.h:988
RigidBodyDynamics::ConstraintSet::W
Math::MatrixNd W
Definition: Constraints.h:928
RigidBodyDynamics::ConstraintSet::loopConstraints
std::vector< std::shared_ptr< LoopConstraint > > loopConstraints
Definition: Constraints.h:887
RigidBodyDynamics::ConstraintSet::impulse
Math::VectorNd impulse
Constraint impulses along the constraint directions.
Definition: Constraints.h:899
RigidBodyDynamics::ConstraintSet::size
size_t size() const
Returns the number of constraints.
Definition: Constraints.h:849
RigidBodyDynamics::ConstraintSet::GTl
Math::MatrixNd GTl
Definition: Constraints.h:939
RigidBodyDynamics::ConstraintSet::calcBaumgarteStabilizationForces
void calcBaumgarteStabilizationForces(unsigned int groupIndex, Model &model, const Math::VectorNd &positionError, const Math::VectorNd &velocityError, Math::VectorNd &baumgarteForcesOutput)
RigidBodyDynamics::ConstraintSet::contactConstraints
std::vector< std::shared_ptr< ContactConstraint > > contactConstraints
Definition: Constraints.h:885
RigidBodyDynamics::ConstraintSet::QDDot_0
Math::VectorNd QDDot_0
Workspace for the default accelerations.
Definition: Constraints.h:967
RigidBodyDynamics::CalcConstraintsPositionError
RBDL_DLLAPI void CalcConstraintsPositionError(Model &model, const Math::VectorNd &Q, ConstraintSet &CS, Math::VectorNd &errOutput, bool update_kinematics=true)
Computes the position errors for the given ConstraintSet.
RigidBodyDynamics::ConstraintSet::Winv
Math::MatrixNd Winv
Definition: Constraints.h:929
RigidBodyDynamics::ConstraintSet::errd
Math::VectorNd errd
Definition: Constraints.h:893
RigidBodyDynamics::ForwardDynamicsConstraintsNullSpace
RBDL_DLLAPI void ForwardDynamicsConstraintsNullSpace(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS, Math::VectorNd &QDDotOutput, bool update_kinematics=true, std::vector< Math::SpatialVector > *f_ext=NULL)
RigidBodyDynamics::CalcConstraintsVelocityError
RBDL_DLLAPI void CalcConstraintsVelocityError(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, ConstraintSet &CS, Math::VectorNd &errOutput, bool update_kinematics=true)
Computes the velocity errors for the given ConstraintSet.
RigidBodyDynamics::ConstraintSet::a
Math::VectorNd a
Workspace for the accelerations of due to the test forces.
Definition: Constraints.h:963
RigidBodyDynamics::InverseDynamicsConstraintsRelaxed
RBDL_DLLAPI void InverseDynamicsConstraintsRelaxed(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDotControls, ConstraintSet &CS, Math::VectorNd &QDDotOutput, Math::VectorNd &TauOutput, bool update_kinematics=true, std::vector< Math::SpatialVector > *f_ext=NULL)
A relaxed inverse-dynamics operator that can be applied to under-actuated or fully-actuated constrain...
RigidBodyDynamics::ConstraintSet::x
Math::VectorNd x
Workspace for the Lagrangian solution.
Definition: Constraints.h:919
RigidBodyDynamics::ForwardDynamicsConstraintsDirect
RBDL_DLLAPI void ForwardDynamicsConstraintsDirect(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS, Math::VectorNd &QDDotOutput, bool update_kinematics=true, std::vector< Math::SpatialVector > *f_ext=NULL)
Computes forward dynamics with contact by constructing and solving the full lagrangian equation.
RigidBodyDynamics::ConstraintSet::GPT
Math::MatrixNd GPT
Definition: Constraints.h:952
RigidBodyDynamics::ConstraintSet::d_d
Math::VectorNd d_d
Workspace when applying constraint forces.
Definition: Constraints.h:986
RigidBodyDynamics::ConstraintSet::GTu
Math::MatrixNd GTu
Definition: Constraints.h:939
RigidBodyDynamics::ConstraintSet::d_pA
std::vector< Math::SpatialVector > d_pA
Workspace for the bias force due to the test force.
Definition: Constraints.h:976
RigidBodyDynamics::ConstraintSet::S
Math::MatrixNd S
Definition: Constraints.h:923
RigidBodyDynamics::ConstraintSet::G
Math::MatrixNd G
Workspace for the constraint Jacobian.
Definition: Constraints.h:912
RigidBodyDynamics::ConstraintSet::SetActuationMap
void SetActuationMap(const Model &model, const std::vector< bool > &actuatedDof)
Initializes and allocates memory needed for InverseDynamicsConstraints and InverseDynamicsConstraints...
RigidBodyDynamics::SolveConstrainedSystemDirect
RBDL_DLLAPI void SolveConstrainedSystemDirect(Math::MatrixNd &H, const Math::MatrixNd &G, const Math::VectorNd &c, const Math::VectorNd &gamma, 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...
RigidBodyDynamics::ConstraintSet::constraints
std::vector< std::shared_ptr< Constraint > > constraints
Definition: Constraints.h:883
RigidBodyDynamics::ConstraintSet::py
Math::VectorNd py
Definition: Constraints.h:942
RigidBodyDynamics::CalcConstrainedSystemVariables
RBDL_DLLAPI void CalcConstrainedSystemVariables(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CSOutput, bool update_kinematics=true, std::vector< Math::SpatialVector > *f_ext=NULL)
Computes the terms , , and of the constrained dynamic problem and stores them in the ConstraintSet.
RigidBodyDynamics::InverseDynamicsConstraints
RBDL_DLLAPI void InverseDynamicsConstraints(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDotDesired, ConstraintSet &CS, Math::VectorNd &QDDotOutput, Math::VectorNd &TauOutput, bool update_kinematics=true, std::vector< Math::SpatialVector > *f_ext=NULL)
An inverse-dynamics operator that can be applied to fully-actuated constrained systems.
RigidBodyDynamics::ConstraintSet::cache
ConstraintCache cache
Definition: Constraints.h:990
RigidBodyDynamics::CalcAssemblyQ
RBDL_DLLAPI bool CalcAssemblyQ(Model &model, Math::VectorNd QInit, ConstraintSet &CS, Math::VectorNd &QOutput, const Math::VectorNd &weights, double tolerance=1e-12, unsigned int max_iter=100)
Computes a feasible initial value of the generalized joint positions.
RigidBodyDynamics::ConstraintSet::AddLoopConstraint
unsigned int AddLoopConstraint(unsigned int bodyIdPredecessor, unsigned int bodyIdSuccessor, const Math::SpatialTransform &XPredecessor, const Math::SpatialTransform &XSuccessor, const Math::SpatialVector &constraintAxisInPredecessor, bool enableBaumgarteStabilization=false, double stabilizationTimeConstant=0.1, const char *constraintName=NULL, unsigned int userDefinedId=std::numeric_limits< unsigned int >::max())
Adds a loop constraint to the constraint set.
RigidBodyDynamics::ConstraintSet::constraintType
std::vector< ConstraintType > constraintType
Definition: Constraints.h:862
RigidBodyDynamics::ComputeConstraintImpulsesDirect
RBDL_DLLAPI void ComputeConstraintImpulsesDirect(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotMinus, ConstraintSet &CS, Math::VectorNd &QDotPlusOutput)
Computes contact gain by constructing and solving the full lagrangian equation.
RigidBodyDynamics::ConstraintSet::d_IA
std::vector< Math::SpatialMatrix > d_IA
Workspace for the inertia when applying constraint forces.
Definition: Constraints.h:982
RigidBodyDynamics::ConstraintSet::name
std::vector< std::string > name
Definition: Constraints.h:864
RigidBodyDynamics::CalcAssemblyQDot
RBDL_DLLAPI void CalcAssemblyQDot(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotInit, ConstraintSet &CS, Math::VectorNd &QDotOutput, const Math::VectorNd &weights)
Computes a feasible initial value of the generalized joint velocities.
RigidBodyDynamics::ConstraintSet::AddCustomConstraint
unsigned int AddCustomConstraint(std::shared_ptr< Constraint > customConstraint)
Adds a custom constraint to the constraint set.
RigidBodyDynamics::ConstraintSet::Y
Math::MatrixNd Y
Definition: Constraints.h:953
RigidBodyDynamics::ConstraintSet::Flr
Math::MatrixNd Flr
Definition: Constraints.h:936
RigidBodyDynamics::ConstraintSet::qddot_z
Math::VectorNd qddot_z
Definition: Constraints.h:957
RigidBodyDynamics::ConstraintSet::H
Math::MatrixNd H
Workspace for the joint space inertia matrix.
Definition: Constraints.h:906
RigidBodyDynamics::ConstraintSet::gamma
Math::VectorNd gamma
Workspace of the right hand side of the acceleration equation.
Definition: Constraints.h:910
RigidBodyDynamics::ConstraintSet::P
Math::MatrixNd P
Selection matrix for the non-actuated parts of the model.
Definition: Constraints.h:925
RigidBodyDynamics::ConstraintSet::d_U
std::vector< Math::SpatialVector > d_U
Workspace when applying constraint forces.
Definition: Constraints.h:984
RigidBodyDynamics::ConstraintSet::force
Math::VectorNd force
Definition: Constraints.h:897
RigidBodyDynamics::ConstraintSet::d_u
Math::VectorNd d_u
Definition: Constraints.h:979
RigidBodyDynamics::ConstraintSet::Z
Math::MatrixNd Z
Definition: Constraints.h:954
RigidBodyDynamics::ConstraintSet::v_plus
Math::VectorNd v_plus
The velocities we want to have along the constraint directions.
Definition: Constraints.h:901
RigidBodyDynamics::ConstraintSet::v
Math::VectorNd v
Definition: Constraints.h:933
RigidBodyDynamics::ConstraintSet::bound
bool bound
Whether the constraint set was bound to a model (mandatory!).
Definition: Constraints.h:859
RigidBodyDynamics::ConstraintSet::linear_solver
Math::LinearSolver linear_solver
Method that should be used to solve internal linear systems.
Definition: Constraints.h:857
RigidBodyDynamics::ConstraintSet::d_a
std::vector< Math::SpatialVector > d_a
Workspace for the acceleration due to the test force.
Definition: Constraints.h:978
RigidBodyDynamics::SolveConstrainedSystemNullSpace
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...
RigidBodyDynamics::ConstraintSet::GT_qr_Q
Math::MatrixNd GT_qr_Q
Definition: Constraints.h:951
RigidBodyDynamics::ConstraintSet::Ru
Math::MatrixNd Ru
Definition: Constraints.h:941
RigidBodyDynamics::ConstraintSet::calcImpulses
void calcImpulses(unsigned int groupIndex, Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, std::vector< unsigned int > &constraintBodyIdsOutput, std::vector< Math::SpatialTransform > &constraintBodyFramesOutput, std::vector< Math::SpatialVector > &constraintImpulsesOutput, bool resolveAllInRootFrame=false, bool updateKinematics=false)
calcImpulses resolves the generalized impluses generated by this constraint into equivalent spatial i...
RigidBodyDynamics::ConstraintSet::u
Math::VectorNd u
Definition: Constraints.h:932
RigidBodyDynamics::ConstraintSet::err
Math::VectorNd err
Definition: Constraints.h:891
RigidBodyDynamics::ConstraintSet::GT
Math::MatrixNd GT
Definition: Constraints.h:938
RigidBodyDynamics::ConstraintSet::ConstraintSet
ConstraintSet()
Definition: Constraints.h:282
RigidBodyDynamics::ConstraintSet::Ful
Math::MatrixNd Ful
Definition: Constraints.h:936
RigidBodyDynamics::ConstraintSet::idGroupMap
std::map< unsigned int, unsigned int > idGroupMap
Definition: Constraints.h:868
RigidBodyDynamics::ConstraintSet::AddContactConstraint
unsigned int AddContactConstraint(unsigned int bodyId, const Math::Vector3d &bodyPoint, const Math::Vector3d &worldNormal, const char *constraintName=NULL, unsigned int userDefinedId=std::numeric_limits< unsigned int >::max())
Adds a single contact constraint (point-ground) to the constraint set.
RigidBodyDynamics::ConstraintSet::C
Math::VectorNd C
Workspace for the coriolis forces.
Definition: Constraints.h:908
RigidBodyDynamics::isConstrainedSystemFullyActuated
RBDL_DLLAPI bool isConstrainedSystemFullyActuated(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, ConstraintSet &CS, bool update_kinematics=true, std::vector< Math::SpatialVector > *f_ext=NULL)
A method to evaluate if the constrained system is fully actuated.
RigidBodyDynamics::CalcConstraintsJacobian
RBDL_DLLAPI void CalcConstraintsJacobian(Model &model, const Math::VectorNd &Q, ConstraintSet &CS, Math::MatrixNd &GOutput, bool update_kinematics=true)
Computes the Jacobian for the given ConstraintSet.
RigidBodyDynamics::ConstraintSet::enableBaumgarteStabilization
void enableBaumgarteStabilization(unsigned int groupIndex)
Definition: Constraints.h:672
RigidBodyDynamics::SolveConstrainedSystemRangeSpaceSparse
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...
RigidBodyDynamics::ConstraintSet::clear
void clear()
Clears all variables in the constraint set.
RigidBodyDynamics::ConstraintSet::f_t
std::vector< Math::SpatialVector > f_t
Workspace for the test forces.
Definition: Constraints.h:969
RigidBodyDynamics::ComputeConstraintImpulsesRangeSpaceSparse
RBDL_DLLAPI void ComputeConstraintImpulsesRangeSpaceSparse(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotMinus, ConstraintSet &CS, Math::VectorNd &QDotPlusOutput)
Resolves contact gain using SolveContactSystemRangeSpaceSparse()
RigidBodyDynamics::ForwardDynamicsConstraintsRangeSpaceSparse
RBDL_DLLAPI void ForwardDynamicsConstraintsRangeSpaceSparse(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS, Math::VectorNd &QDDotOutput, bool update_kinematics=true, std::vector< Math::SpatialVector > *f_ext=NULL)
RigidBodyDynamics::ConstraintSet::F
Math::MatrixNd F
Definition: Constraints.h:935
RigidBodyDynamics::ForwardDynamicsContactsKokkevis
RBDL_DLLAPI void ForwardDynamicsContactsKokkevis(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS, Math::VectorNd &QDDotOutput)
Computes forward dynamics that accounts for active contacts in ConstraintSet.
RigidBodyDynamics::ConstraintSet::GT_qr
Eigen::HouseholderQR< Math::MatrixNd > GT_qr
Workspace for the QR decomposition of the null-space method.
Definition: Constraints.h:947
RigidBodyDynamics::ConstraintSet::calcForces
void calcForces(unsigned int groupIndex, Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, std::vector< unsigned int > &updConstraintBodyIdsOutput, std::vector< Math::SpatialTransform > &updConstraintBodyFramesOutput, std::vector< Math::SpatialVector > &updConstraintForcesOutput, bool resolveAllInRootFrame=false, bool updateKinematics=false)
calcForces resolves the generalized forces generated by this constraint into equivalent spatial force...
RigidBodyDynamics::ConstraintSet::Bind
bool Bind(const Model &model)
Initializes and allocates memory for the constraint set.
RigidBodyDynamics::ConstraintSet::point_accel_0
std::vector< Math::Vector3d > point_accel_0
Workspace for the default point accelerations.
Definition: Constraints.h:973
RigidBodyDynamics::ConstraintSet::pz
Math::VectorNd pz
Definition: Constraints.h:943
RigidBodyDynamics::ConstraintSet::K
Math::MatrixNd K
Workspace for the Inverse Articulated-Body Inertia.
Definition: Constraints.h:961
RigidBodyDynamics::ConstraintSet::qddot_y
Math::VectorNd qddot_y
Definition: Constraints.h:956
RigidBodyDynamics::ConstraintSet::nameGroupMap
std::map< std::string, unsigned int > nameGroupMap
Definition: Constraints.h:866
RigidBodyDynamics::ConstraintSet::A
Math::MatrixNd A
Workspace for the Lagrangian left-hand-side matrix.
Definition: Constraints.h:915
RigidBodyDynamics::ConstraintSet::Fll
Math::MatrixNd Fll
Definition: Constraints.h:936
RigidBodyDynamics::ConstraintSet::Fur
Math::MatrixNd Fur
Definition: Constraints.h:936
RigidBodyDynamics::NonlinearEffects
RBDL_DLLAPI void NonlinearEffects(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::VectorNd &Tau, std::vector< Math::SpatialVector > *f_ext=NULL)
Computes the coriolis forces.
RigidBodyDynamics::ForwardDynamics
RBDL_DLLAPI void ForwardDynamics(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, Math::VectorNd &QDDot, std::vector< Math::SpatialVector > *f_ext=NULL)
Computes forward dynamics with the Articulated Body Algorithm.
RigidBodyDynamics::CompositeRigidBodyAlgorithm
RBDL_DLLAPI void CompositeRigidBodyAlgorithm(Model &model, const Math::VectorNd &Q, Math::MatrixNd &H, bool update_kinematics=true)
Computes the joint space inertia matrix by using the Composite Rigid Body Algorithm.
RigidBodyDynamics::UpdateKinematicsCustom
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
RigidBodyDynamics::Math::SpatialMatrix
SpatialMatrix_t SpatialMatrix
Definition: rbdl_math.h:62
RigidBodyDynamics::Math::VectorNd
VectorN_t VectorNd
Definition: rbdl_math.h:66
RigidBodyDynamics::Math::crossf
SpatialMatrix crossf(const SpatialVector &v)
Definition: SpatialAlgebraOperators.h:425
RigidBodyDynamics::Math::SpatialVector
SpatialVector_t SpatialVector
Definition: rbdl_math.h:61
RigidBodyDynamics::Math::SparseFactorizeLTL
RBDL_DLLAPI void SparseFactorizeLTL(Model &model, Math::MatrixNd &H)
Definition: rbdl_mathutils.cc:261
RigidBodyDynamics::Math::SparseSolveLx
RBDL_DLLAPI void SparseSolveLx(Model &model, Math::MatrixNd &L, Math::VectorNd &x)
Definition: rbdl_mathutils.cc:300
RigidBodyDynamics::Math::Vector3d
Vector3_t Vector3d
Definition: rbdl_math.h:57
RigidBodyDynamics::Math::SparseSolveLTx
RBDL_DLLAPI void SparseSolveLTx(Model &model, Math::MatrixNd &L, Math::VectorNd &x)
Definition: rbdl_mathutils.cc:311
RigidBodyDynamics::Math::MatrixNd
MatrixN_t MatrixNd
Definition: rbdl_math.h:67
RigidBodyDynamics
Definition: Joint.cc:45
RigidBodyDynamics::ConstraintTypeContact
@ ConstraintTypeContact
Definition: Constraint.h:19
RigidBodyDynamics::ConstraintTypeCustom
@ ConstraintTypeCustom
Definition: Constraint.h:21
RigidBodyDynamics::ConstraintTypeLoop
@ ConstraintTypeLoop
Definition: Constraint.h:20
std::fabs
MX_Xd_dynamic fabs(const MX_Xd_dynamic &m)
Definition: MX_Xd_utils.h:371
rbdl_errors.h
rbdl_mathutils.h
LinearSolverPartialPivLU
LinearSolverPartialPivLU
Definition: rbdl_mathutils.h:29
LinearSolverColPivHouseholderQR
LinearSolverColPivHouseholderQR
Definition: rbdl_mathutils.h:30
LinearSolverHouseholderQR
LinearSolverHouseholderQR
Definition: rbdl_mathutils.h:31
RigidBodyDynamics::ConstraintCache::vecND
Math::VectorNd vecND
Definition: Constraint.h:40
RigidBodyDynamics::ConstraintCache::vec3C
Math::Vector3d vec3C
Definition: Constraint.h:43
RigidBodyDynamics::ConstraintCache::mat3NC
Math::MatrixNd mat3NC
Definition: Constraint.h:50
RigidBodyDynamics::ConstraintCache::mat3E
Math::Matrix3d mat3E
Definition: Constraint.h:46
RigidBodyDynamics::ConstraintCache::mat3C
Math::Matrix3d mat3C
Definition: Constraint.h:46
RigidBodyDynamics::ConstraintCache::svecB
Math::SpatialVector svecB
Definition: Constraint.h:57
RigidBodyDynamics::ConstraintCache::svecA
Math::SpatialVector svecA
Working SpatialVectors.
Definition: Constraint.h:57
RigidBodyDynamics::ConstraintCache::mat3NB
Math::MatrixNd mat3NB
Definition: Constraint.h:50
RigidBodyDynamics::ConstraintCache::stB
Math::SpatialTransform stB
Definition: Constraint.h:60
RigidBodyDynamics::ConstraintCache::vec3E
Math::Vector3d vec3E
Definition: Constraint.h:43
RigidBodyDynamics::ConstraintCache::stA
Math::SpatialTransform stA
Working SpatialTransforms.
Definition: Constraint.h:60
RigidBodyDynamics::ConstraintCache::vecNA
Math::VectorNd vecNA
Working vectors that are sized to match the length of qdot.
Definition: Constraint.h:40
RigidBodyDynamics::ConstraintCache::mat6NC
Math::MatrixNd mat6NC
Definition: Constraint.h:54
RigidBodyDynamics::ConstraintCache::svecD
Math::SpatialVector svecD
Definition: Constraint.h:57
RigidBodyDynamics::ConstraintCache::svecE
Math::SpatialVector svecE
Definition: Constraint.h:57
RigidBodyDynamics::ConstraintCache::mat3F
Math::Matrix3d mat3F
Definition: Constraint.h:46
RigidBodyDynamics::ConstraintCache::svecF
Math::SpatialVector svecF
Definition: Constraint.h:57
RigidBodyDynamics::ConstraintCache::stD
Math::SpatialTransform stD
Definition: Constraint.h:60
RigidBodyDynamics::ConstraintCache::mat3NA
Math::MatrixNd mat3NA
Definition: Constraint.h:50
RigidBodyDynamics::ConstraintCache::mat3ND
Math::MatrixNd mat3ND
Definition: Constraint.h:50
RigidBodyDynamics::ConstraintCache::vecNC
Math::VectorNd vecNC
Definition: Constraint.h:40
RigidBodyDynamics::ConstraintCache::vecNZeros
Math::VectorNd vecNZeros
Here N is taken to mean the number of elements in QDot.
Definition: Constraint.h:37
RigidBodyDynamics::ConstraintCache::vec3F
Math::Vector3d vec3F
Definition: Constraint.h:43
RigidBodyDynamics::ConstraintCache::vec3A
Math::Vector3d vec3A
Working Vector3d entries.
Definition: Constraint.h:43
RigidBodyDynamics::ConstraintCache::vecNB
Math::VectorNd vecNB
Definition: Constraint.h:40
RigidBodyDynamics::ConstraintCache::mat3A
Math::Matrix3d mat3A
Working Matrix3d entries.
Definition: Constraint.h:46
RigidBodyDynamics::ConstraintCache::mat6ND
Math::MatrixNd mat6ND
Definition: Constraint.h:54
RigidBodyDynamics::ConstraintCache::vec3D
Math::Vector3d vec3D
Definition: Constraint.h:43
RigidBodyDynamics::ConstraintCache::vec3B
Math::Vector3d vec3B
Definition: Constraint.h:43
RigidBodyDynamics::ConstraintCache::mat6NA
Math::MatrixNd mat6NA
Definition: Constraint.h:54
RigidBodyDynamics::ConstraintCache::mat3B
Math::Matrix3d mat3B
Definition: Constraint.h:46
RigidBodyDynamics::ConstraintCache::svecC
Math::SpatialVector svecC
Definition: Constraint.h:57
RigidBodyDynamics::ConstraintCache::mat6NB
Math::MatrixNd mat6NB
Definition: Constraint.h:54
RigidBodyDynamics::ConstraintCache::mat3D
Math::Matrix3d mat3D
Definition: Constraint.h:46
RigidBodyDynamics::ConstraintCache::stC
Math::SpatialTransform stC
Definition: Constraint.h:60
RigidBodyDynamics::Math::SpatialTransform::E
Matrix3d E
Definition: SpatialAlgebraOperators.h:311
RigidBodyDynamics::Math::SpatialTransform::r
Vector3d r
Definition: SpatialAlgebraOperators.h:312