Biorbd Release_1.13.0
Loading...
Searching...
No Matches
Joints.cpp
1#define BIORBD_API_EXPORTS
2#include "RigidBody/Joints.h"
3
4#include <rbdl/Dynamics.h>
5#include <rbdl/Kinematics.h>
6#include <rbdl/rbdl_utils.h>
7
8#include "BiorbdModel.h"
9#include "RigidBody/Contacts.h"
10#include "RigidBody/ExternalForceSet.h"
11#include "RigidBody/GeneralizedAcceleration.h"
12#include "RigidBody/GeneralizedCoordinates.h"
13#include "RigidBody/GeneralizedTorque.h"
14#include "RigidBody/GeneralizedVelocity.h"
15#include "RigidBody/Markers.h"
16#include "RigidBody/Mesh.h"
17#include "RigidBody/MeshFace.h"
18#include "RigidBody/NodeSegment.h"
19#include "RigidBody/Segment.h"
20#include "RigidBody/SegmentCharacteristics.h"
21#include "RigidBody/SoftContacts.h"
22#include "Utils/Error.h"
23#include "Utils/Matrix.h"
24#include "Utils/Matrix3d.h"
25#include "Utils/Quaternion.h"
26#include "Utils/Rotation.h"
27#include "Utils/RotoTrans.h"
28#include "Utils/Scalar.h"
29#include "Utils/SpatialTransform.h"
30#include "Utils/SpatialVector.h"
31#include "Utils/String.h"
32
33using namespace BIORBD_NAMESPACE;
34
36 : RigidBodyDynamics::Model(),
37 m_segments(std::make_shared<std::vector<rigidbody::Segment>>()),
38 m_nbRoot(std::make_shared<size_t>(0)),
39 m_nbDof(std::make_shared<size_t>(0)),
40 m_nbQ(std::make_shared<size_t>(0)),
41 m_nbQdot(std::make_shared<size_t>(0)),
42 m_nbQddot(std::make_shared<size_t>(0)),
43 m_nRotAQuat(std::make_shared<size_t>(0)),
44 m_isKinematicsComputed(std::make_shared<bool>(false)),
45 m_totalMass(std::make_shared<utils::Scalar>(0)) {
46 // Redefining gravity so it is on z by default
47 this->gravity = utils::Vector3d(0, 0, -9.81);
48}
49
51 : RigidBodyDynamics::Model(other),
53 m_nbRoot(other.m_nbRoot),
54 m_nbDof(other.m_nbDof),
55 m_nbQ(other.m_nbQ),
56 m_nbQdot(other.m_nbQdot),
57 m_nbQddot(other.m_nbQddot),
60 m_totalMass(other.m_totalMass) {}
61
63
66 copy.DeepCopy(*this);
67 return copy;
68}
69
71 static_cast<RigidBodyDynamics::Model &>(*this) = other;
72 m_segments->resize(other.m_segments->size());
73 for (size_t i = 0; i < other.m_segments->size(); ++i) {
74 (*m_segments)[i] = (*other.m_segments)[i].DeepCopy();
75 }
76 *m_nbRoot = *other.m_nbRoot;
77 *m_nbDof = *other.m_nbDof;
78 *m_nbQ = *other.m_nbQ;
79 *m_nbQdot = *other.m_nbQdot;
80 *m_nbQddot = *other.m_nbQddot;
81 *m_nRotAQuat = *other.m_nRotAQuat;
83 *m_totalMass = *other.m_totalMass;
84}
85
87size_t rigidbody::Joints::nbDof() const { return *m_nbDof; }
88
89std::vector<utils::String> rigidbody::Joints::nameDof() const {
90 std::vector<utils::String> names;
91 for (size_t i = 0; i < nbSegment(); ++i) {
92 for (size_t j = 0; j < segment(i).nbDof(); ++j) {
93 names.push_back(segment(i).name() + "_" + segment(i).nameDof(j));
94 }
95 }
96 // Append Quaternion Q
97 for (size_t i = 0; i < nbSegment(); ++i) {
98 if (segment(i).isRotationAQuaternion()) {
99 names.push_back(segment(i).name() + "_" + segment(i).nameDof(3));
100 }
101 }
102 return names;
103}
104
105size_t rigidbody::Joints::nbQ() const { return *m_nbQ; }
106size_t rigidbody::Joints::nbQdot() const { return *m_nbQdot; }
107size_t rigidbody::Joints::nbQddot() const { return *m_nbQddot; }
108size_t rigidbody::Joints::nbRoot() const { return *m_nbRoot; }
109
110utils::Scalar rigidbody::Joints::mass() const { return *m_totalMass; }
111
113 const utils::String &segmentName,
114 const utils::String &parentName,
115 const utils::String &translationSequence,
116 const utils::String &rotationSequence,
117 const std::vector<utils::Range> &QRanges,
118 const std::vector<utils::Range> &QdotRanges,
119 const std::vector<utils::Range> &QddotRanges,
120 const std::vector<utils::Scalar> &jointDampings,
121 const rigidbody::SegmentCharacteristics &characteristics,
122 const utils::RotoTrans &referenceFrame) {
124 *this,
125 segmentName,
126 parentName,
127 translationSequence,
128 rotationSequence,
129 QRanges,
130 QdotRanges,
131 QddotRanges,
132 jointDampings,
133 characteristics,
135 referenceFrame.rot().transpose(), referenceFrame.trans()));
136 if (this->GetBodyId(parentName.c_str()) ==
137 std::numeric_limits<unsigned int>::max()) {
138 *m_nbRoot += tp.nbDof(); // If the segment name is "Root", add the
139 // number of DoF of root
140 }
141 *m_nbDof += tp.nbDof();
142 *m_nbQ += tp.nbQ();
143 *m_nbQdot += tp.nbQdot();
144 *m_nbQddot += tp.nbQddot();
145
146 if (tp.isRotationAQuaternion()) {
147 ++*m_nRotAQuat;
148 }
149
150 *m_totalMass +=
151 characteristics.mMass; // Add the segment mass to the total body mass
152 m_segments->push_back(tp);
153 return 0;
154}
155
157 const utils::String &segmentName,
158 const utils::String &parentName,
159 const utils::String &seqR,
160 const std::vector<utils::Range> &QRanges,
161 const std::vector<utils::Range> &QdotRanges,
162 const std::vector<utils::Range> &QddotRanges,
163 const std::vector<utils::Scalar> &jointDampings,
164 const rigidbody::SegmentCharacteristics &characteristics,
165 const utils::RotoTrans &referenceFrame) {
167 *this,
168 segmentName,
169 parentName,
170 seqR,
171 QRanges,
172 QdotRanges,
173 QddotRanges,
174 jointDampings,
175 characteristics,
177 referenceFrame.rot().transpose(), referenceFrame.trans()));
178 if (this->GetBodyId(parentName.c_str()) ==
179 std::numeric_limits<unsigned int>::max()) {
180 *m_nbRoot += tp.nbDof(); // If the name of the segment is "Root", add
181 // the number of DoF of root
182 }
183 *m_nbDof += tp.nbDof();
184
185 *m_totalMass +=
186 characteristics.mMass; // Add the segment mass to the total body mass
187 m_segments->push_back(tp);
188 return 0;
189}
190
192
194 gravity = newGravity;
195}
196
198 size_t idx,
199 const rigidbody::SegmentCharacteristics &characteristics) {
201 idx < m_segments->size(), "Asked for a wrong segment (out of range)");
202 (*m_segments)[idx].updateCharacteristics(*this, characteristics);
203}
204
207 idx < m_segments->size(), "Asked for a wrong segment (out of range)");
208 return (*m_segments)[idx];
209}
210
212 return segment(static_cast<size_t>(getBodyBiorbdId(name.c_str())));
213}
214
217 idx < m_segments->size(), "Asked for a wrong segment (out of range)");
218 return (*m_segments)[idx];
219}
220
222 const utils::String &name) const {
223 return segment(static_cast<size_t>(getBodyBiorbdId(name.c_str())));
224}
225
226std::vector<rigidbody::Segment> &rigidbody::Joints::segments() const {
227 return *m_segments;
228}
229
231 const rigidbody::GeneralizedVelocity &Qdot) const {
232 rigidbody::GeneralizedTorque dampings(*this);
233
234 size_t count(0);
235 for (auto &segment : *m_segments) {
236 for (auto &damping : segment.jointDampings()) {
237 dampings[count] = damping * Qdot[count];
238 count++;
239 }
240 }
241 return dampings;
242}
243
244size_t rigidbody::Joints::nbSegment() const { return m_segments->size(); }
245
247 for (size_t i = 0; i < m_segments->size(); ++i)
248 if (!(*m_segments)[i].name().compare(segmentName)) {
249 return static_cast<int>(i);
250 }
251 return -1;
252}
253
254int rigidbody::Joints::getBodyRbdlId(const utils::String &segmentName) const {
255 return GetBodyId(segmentName.c_str());
256}
257
259 // Assuming that this is also a joint type (via BiorbdModel)
260 const rigidbody::Joints &model =
261 dynamic_cast<const rigidbody::Joints &>(*this);
262 std::string bodyName = model.GetBodyName(idx);
263 return model.getBodyBiorbdId(bodyName);
264}
265
267 return (*m_segments)[idx].id();
268}
269
270std::vector<std::vector<size_t>> rigidbody::Joints::getDofSubTrees() {
271 // initialize subTrees
272 std::vector<std::vector<size_t>> subTrees;
273 std::vector<size_t> subTree_empty;
274 for (size_t j = 0; j < this->mu.size(); ++j) {
275 subTrees.push_back(subTree_empty);
276 }
277
278 // Get all dof without parent
279 std::vector<size_t> dof_with_no_parent_id;
280 for (size_t i = 1; i < this->mu.size();
281 ++i) { // begin at 1 because 0 is its own parent in rbdl.
282 if (this->lambda[i] == 0) {
283 dof_with_no_parent_id.push_back(i);
284 }
285 }
286
287 // Get all subtrees of dofs without parents
288 for (size_t i = 0; i < dof_with_no_parent_id.size(); ++i) {
289 size_t dof_id = dof_with_no_parent_id[i];
290
291 // initialize subTrees_temp
292 std::vector<std::vector<size_t>> subTrees_temp;
293 for (size_t j = 0; j < this->mu.size(); ++j) {
294 subTrees_temp.push_back(subTree_empty);
295 }
296
297 std::vector<std::vector<size_t>> subTrees_temp_filled =
298 recursiveDofSubTrees(subTrees_temp, dof_id);
299 for (size_t j = 0; j < subTrees_temp.size(); ++j) {
300 if (subTrees_temp_filled[j].empty()) {
301 continue;
302 } else {
303 subTrees[j].insert(
304 subTrees[j].end(),
305 subTrees_temp_filled[j].begin(),
306 subTrees_temp_filled[j].end());
307 }
308 }
309 }
310
311 subTrees.erase(subTrees.begin());
312
313 return subTrees;
314}
315
316std::vector<std::vector<size_t>> rigidbody::Joints::recursiveDofSubTrees(
317 std::vector<std::vector<size_t>> subTrees,
318 size_t idx) {
319 size_t q_index_i = this->mJoints[idx].q_index;
320 subTrees[idx].push_back(q_index_i);
321
322 std::vector<std::vector<size_t>> subTrees_filled;
323 subTrees_filled = subTrees;
324
325 std::vector<unsigned int> child_idx = this->mu[idx];
326
327 if (child_idx.size() > 0) {
328 for (size_t i = 0; i < child_idx.size(); ++i) {
329 size_t cur_child_id = child_idx[i];
330 subTrees_filled = recursiveDofSubTrees(subTrees_filled, cur_child_id);
331 std::vector<size_t> subTree_child = subTrees_filled[cur_child_id];
332
333 subTrees_filled[idx].insert(
334 subTrees_filled[idx].end(),
335 subTree_child.begin(),
336 subTree_child.end());
337 }
338 }
339
340 return subTrees_filled;
341}
342
343std::vector<utils::RotoTrans> rigidbody::Joints::allGlobalJCS(
345 bool updateKin) {
346#ifdef BIORBD_USE_CASADI_MATH
348#else
350#endif
351 model = this->UpdateKinematicsCustom(updateKin ? &Q : nullptr);
352
353 std::vector<utils::RotoTrans> out;
354 for (size_t i = 0; i < m_segments->size(); ++i) {
355 out.push_back(model.globalJCS(Q, i, false));
356 }
357 return out;
358}
359
362 const utils::String &name,
363 bool updateKin) {
364 return globalJCS(Q, static_cast<size_t>(getBodyBiorbdId(name)), updateKin);
365}
366
369 size_t idx,
370 bool updateKin) {
371 return this->CalcBodyWorldTransformation(
372 Q, (*m_segments)[idx].id(), updateKin);
373}
374
375std::vector<utils::RotoTrans> rigidbody::Joints::localJCS() const {
376 std::vector<utils::RotoTrans> out;
377 for (size_t i = 0; i < m_segments->size(); ++i) {
378 out.push_back(localJCS(i));
379 }
380 return out;
381}
383 return localJCS(static_cast<size_t>(getBodyBiorbdId(name.c_str())));
384}
385
387 return (*m_segments)[idx].localJCS();
388}
389
392 utils::String segmentName,
393 const utils::Vector3d &pointInLocal,
394 bool updateKin) {
395 return this->CalcBodyToBaseCoordinates(
396 Q, this->GetBodyId(segmentName.c_str()), pointInLocal, updateKin);
397}
398
401 unsigned int bodyId,
402 const utils::Vector3d &pointInLocal,
403 bool updateKin) {
404#ifdef BIORBD_USE_CASADI_MATH
406#else
408#endif
409 model = this->UpdateKinematicsCustom(updateKin ? &Q : nullptr);
410
411 return RigidBodyDynamics::CalcBodyToBaseCoordinates(
412 model, Q, bodyId, pointInLocal, false);
413}
414
418 utils::String segmentName,
419 const utils::Vector3d &pointInLocal,
420 bool updateKin) {
421 return this->CalcPointVelocity(
422 Q, Qdot, this->GetBodyId(segmentName.c_str()), pointInLocal, updateKin);
423}
424
428 unsigned int bodyId,
429 const utils::Vector3d &pointInLocal,
430 bool updateKin) {
431#ifdef BIORBD_USE_CASADI_MATH
433#else
435#endif
436 model = this->UpdateKinematicsCustom(
437 updateKin ? &Q : nullptr, updateKin ? &Qdot : nullptr);
438
439 return RigidBodyDynamics::CalcPointVelocity(
440 model, Q, Qdot, bodyId, pointInLocal, false);
441}
442
446 utils::String segmentName,
447 const utils::Vector3d &pointInLocal,
448 bool updateKin) {
449 return this->CalcPointVelocity6D(
450 Q, Qdot, this->GetBodyId(segmentName.c_str()), pointInLocal, updateKin);
451}
452
456 unsigned int bodyId,
457 const utils::Vector3d &pointInLocal,
458 bool updateKin) {
459#ifdef BIORBD_USE_CASADI_MATH
461#else
463#endif
464 model = this->UpdateKinematicsCustom(
465 updateKin ? &Q : nullptr, updateKin ? &Qdot : nullptr);
466
467 return RigidBodyDynamics::CalcPointVelocity6D(
468 model, Q, Qdot, bodyId, pointInLocal, false);
469}
470
475 utils::String segmentName,
476 const utils::Vector3d &pointInLocal,
477 bool updateKin) {
478 return this->CalcPointAcceleration(
479 Q,
480 Qdot,
481 Qddot,
482 this->GetBodyId(segmentName.c_str()),
483 pointInLocal,
484 updateKin);
485}
486
491 unsigned int bodyId,
492 const utils::Vector3d &pointInLocal,
493 bool updateKin) {
494#ifdef BIORBD_USE_CASADI_MATH
496#else
498#endif
499 model = this->UpdateKinematicsCustom(
500 updateKin ? &Q : nullptr,
501 updateKin ? &Qdot : nullptr,
502 updateKin ? &Qddot : nullptr);
503
504 return RigidBodyDynamics::CalcPointAcceleration(
505 model, Q, Qdot, Qddot, bodyId, pointInLocal, false);
506}
507
510 utils::String segmentName,
511 const utils::Vector3d &pointInLocal,
512 bool updateKin) {
513 return this->CalcPointJacobian(
514 Q, this->GetBodyId(segmentName.c_str()), pointInLocal, updateKin);
515}
516
519 unsigned int bodyId,
520 const utils::Vector3d &pointInLocal,
521 bool updateKin) {
522#ifdef BIORBD_USE_CASADI_MATH
524#else
526#endif
527 model = this->UpdateKinematicsCustom(updateKin ? &Q : nullptr);
528
529 utils::Matrix out(3, this->nbQ());
530 out.setZero();
531 RigidBodyDynamics::CalcPointJacobian(
532 model, Q, bodyId, pointInLocal, out, false);
533 return out;
534}
535
536std::vector<rigidbody::NodeSegment> rigidbody::Joints::projectPoint(
538 const std::vector<NodeSegment> &v,
539 bool updateKin) {
540 // Assuming that this is also a marker type (via BiorbdModel)
541 const rigidbody::Markers &marks = dynamic_cast<rigidbody::Markers &>(*this);
542
543 // Security check
545 marks.nbMarkers() == v.size(),
546 "Number of marker must be equal to number of Vector3d");
547
548#ifdef BIORBD_USE_CASADI_MATH
550#else
552#endif
553 updatedModel = this->UpdateKinematicsCustom(updateKin ? &Q : nullptr);
554
555 std::vector<rigidbody::NodeSegment> out;
556 for (size_t i = 0; i < marks.nbMarkers(); ++i) {
557 rigidbody::NodeSegment tp(marks.marker(i));
558 if (tp.nbAxesToRemove() != 0) {
559 // Project the marker by removing an axis by using the current pose
560 // of the model
561 utils::Vector3d pos = v[i].applyRT(
562 updatedModel.globalJCS(Q, tp.parent(), false).transpose());
563
564 out.push_back(updatedModel.projectPoint(Q, pos, false));
565 } else {
566 // If there isn't any projection to perform, just use the actual
567 // marker
568 out.push_back(v[i]);
569 }
570 }
571 return out;
572}
573
576 const utils::Vector3d &v,
577 int segmentIdx,
578 const utils::String &axesToRemove,
579 bool updateKin) {
580#ifdef BIORBD_USE_CASADI_MATH
582#else
584#endif
585 updatedModel = this->UpdateKinematicsCustom(updateKin ? &Q : nullptr);
586
587 // Create a marker
588 const utils::String &segmentName(
589 segment(static_cast<size_t>(segmentIdx)).name());
591 v.applyRT(
592 updatedModel.globalJCS(Q, static_cast<size_t>(segmentIdx), false)
593 .transpose()),
594 "tp",
595 segmentName,
596 true,
597 true,
598 axesToRemove,
599 static_cast<int>(GetBodyId(segmentName.c_str())));
600
601 // Project and then reset in global
602 return updatedModel.projectPoint(Q, node, false);
603}
604
607 const rigidbody::NodeSegment &n,
608 bool updateKin) {
609 // Assuming that this is also a Marker type (via BiorbdModel)
610 return dynamic_cast<rigidbody::Markers &>(*this).marker(
611 Q, n, updateKin, true);
612}
613
617 bool updateKin) {
618 // If the point has not been projected, there is no effect
619 if (node.nbAxesToRemove() == 0) {
620 // Return the value
621 return utils::Matrix::Zero(3, static_cast<unsigned int>(nbQ()));
622 }
623
624#ifdef BIORBD_USE_CASADI_MATH
626#else
628#endif
629 updatedModel = this->UpdateKinematicsCustom(updateKin ? &Q : nullptr);
630
631 // Assuming that this is also a Marker type (via BiorbdModel)
632 rigidbody::Markers &marks = dynamic_cast<rigidbody::Markers &>(*this);
633
634 // Jacobian of the marker
635 node.applyRT(updatedModel.globalJCS(Q, node.parent(), false).transpose());
636
637 utils::Matrix G_tp(
638 marks.markersJacobian(Q, node.parent(), utils::Vector3d(0, 0, 0), false));
639 utils::Matrix JCor(utils::Matrix::Zero(9, static_cast<unsigned int>(nbQ())));
640 updatedModel.CalcMatRotJacobian(
641 Q,
642 GetBodyId(node.parent().c_str()),
643 utils::Matrix3d::Identity(),
644 JCor,
645 false);
646 for (size_t n = 0; n < 3; ++n) {
647 if (node.isAxisKept(n)) {
648 G_tp += JCor.block(
649 static_cast<unsigned int>(n) * 3,
650 0,
651 3,
652 static_cast<unsigned int>(nbQ())) *
653 node(static_cast<unsigned int>(n));
654 }
655 }
656
657 return G_tp;
658}
659
662 const utils::Vector3d &v,
663 int segmentIdx,
664 const utils::String &axesToRemove,
665 bool updateKin) {
666#ifdef BIORBD_USE_CASADI_MATH
668#else
670#endif
671 updatedModel = this->UpdateKinematicsCustom(updateKin ? &Q : nullptr);
672
673 // Find the point
674 const rigidbody::NodeSegment &p(
675 updatedModel.projectPoint(Q, v, segmentIdx, axesToRemove, false));
676 updateKin = false;
677
678 // Return the value
679 return updatedModel.projectPointJacobian(Q, p, false);
680}
681
684 const std::vector<NodeSegment> &v,
685 bool updateKin) {
686#ifdef BIORBD_USE_CASADI_MATH
688#else
690#endif
691 updatedModel = this->UpdateKinematicsCustom(updateKin ? &Q : nullptr);
692
693 // Gather the points
694 const std::vector<rigidbody::NodeSegment> &tp(
695 updatedModel.projectPoint(Q, v, false));
696 updateKin = false;
697
698 // Calculate the Jacobian if the point is not projected
699 std::vector<utils::Matrix> G;
700
701 for (size_t i = 0; i < tp.size(); ++i) {
702 // Actual marker
703 G.push_back(updatedModel.projectPointJacobian(
704 Q, rigidbody::NodeSegment(v[i]), false));
705 }
706 return G;
707}
708
711 const size_t segmentIdx,
712 bool updateKin) {
713#ifdef BIORBD_USE_CASADI_MATH
715#else
717#endif
718 updatedModel = this->UpdateKinematicsCustom(updateKin ? &Q : nullptr);
719
720 if (segmentIdx >= updatedModel.fixed_body_discriminator) {
721 size_t fbody_id =
722 segmentIdx - static_cast<size_t>(updatedModel.fixed_body_discriminator);
723 size_t parent_id =
724 static_cast<size_t>(updatedModel.mFixedBodies[fbody_id].mMovableParent);
725 utils::RotoTrans parentRT(
726 updatedModel.X_base[parent_id].E.transpose(),
727 updatedModel.X_base[parent_id].r);
730 updatedModel.mFixedBodies[fbody_id].mParentTransform.E.transpose(),
731 updatedModel.mFixedBodies[fbody_id].mParentTransform.r),
732 "",
733 "");
734 const utils::RotoTrans &transfo_tp = parentRT * bodyRT;
735 return utils::SpatialTransform(transfo_tp.rot(), transfo_tp.trans());
736 }
737
739 updatedModel.X_base[segmentIdx].E.transpose(),
740 updatedModel.X_base[segmentIdx].r);
741}
742
743// Get a segment's angular velocity
747 size_t idx,
748 bool updateKin) {
749 // Calculate the velocity of the point (0 0 0)
750 return this
751 ->CalcPointVelocity6D(
752 Q, Qdot, segment(idx).name(), utils::Vector3d(0, 0, 0), updateKin)
753 .block(0, 0, 3, 1);
754}
755
758 bool updateKin) {
759 // For each segment, find the CoM (CoM = sum(segment_mass * pos_com_seg) /
760 // total mass)
761 const std::vector<rigidbody::NodeSegment> &com_segment(
762 CoMbySegment(Q, updateKin));
763 utils::Vector3d com(0, 0, 0);
764 for (size_t i = 0; i < com_segment.size(); ++i) {
765 com += (*m_segments)[i].characteristics().mMass * com_segment[i];
766 }
767
768 // Divide by total mass
769 com = com / this->mass();
770
771 // Return the CoM
772 return com;
773}
774
778 bool updateKin) {
779 return CalcAngularMomentum(Q, Qdot, updateKin);
780}
781
784 bool updateKin) {
785#ifdef BIORBD_USE_CASADI_MATH
787#else
789#endif
790 updatedModel = this->UpdateKinematicsCustom(updateKin ? &Q : nullptr);
791
793 static_cast<unsigned int>(nbQ()), static_cast<unsigned int>(nbQ()));
794 massMatrix.setZero();
795 RigidBodyDynamics::CompositeRigidBodyAlgorithm(
796 updatedModel, Q, massMatrix, false);
797 return massMatrix;
798}
799
802 bool updateKin) {
803#ifdef BIORBD_USE_CASADI_MATH
805#else
807#endif
808 updatedModel = this->UpdateKinematicsCustom(updateKin ? &Q : nullptr);
809
810 int i = 0; // for loop purpose
811 int j = 0; // for loop purpose
812 utils::Matrix Minv(updatedModel.dof_count, updatedModel.dof_count);
813 Minv.setZero();
814
815 // First Forward Pass
816 for (i = 1; i < updatedModel.mBodies.size(); i++) {
817 updatedModel.I[i].setSpatialMatrix(updatedModel.IA[i]);
818 }
819 // End First Forward Pass
820
821 // set F (n x 6 x n)
822 utils::Matrix F_i(6, updatedModel.dof_count);
823 F_i.setZero();
824 // Fill a vector of matrix (6 x n)
825 std::vector<utils::Matrix> F;
826 for (i = 1; i < updatedModel.mBodies.size(); i++) {
827 F.push_back(F_i);
828 }
829
830 // Backward Pass
831 std::vector<std::vector<size_t>> subTrees = getDofSubTrees();
832 for (i = static_cast<int>(updatedModel.mBodies.size() - 1); i > 0; i--) {
833 unsigned int q_index_i =
834 static_cast<size_t>(updatedModel.mJoints[i].q_index);
835 const std::vector<size_t> &sub_tree = subTrees[q_index_i];
836
837 updatedModel.U[i] = updatedModel.IA[i] * updatedModel.S[i];
838 updatedModel.d[i] = updatedModel.S[i].dot(updatedModel.U[i]);
839
840 Minv(q_index_i, q_index_i) = 1.0 / (updatedModel.d[i]);
841
842 for (j = 0; j < sub_tree.size(); j++) {
844 F[q_index_i].block(0, static_cast<unsigned int>(sub_tree[j]), 6, 1));
845 Minv(q_index_i, static_cast<unsigned int>(sub_tree[j])) -=
846 (1.0 / updatedModel.d[i]) * updatedModel.S[i].transpose() * Ftemp;
847 }
848
849 size_t lambda = static_cast<size_t>(updatedModel.lambda[i]);
850 size_t lambda_q_i =
851 static_cast<size_t>(updatedModel.mJoints[lambda].q_index);
852 if (lambda != 0) {
853 for (j = 0; j < sub_tree.size(); j++) {
854 F[q_index_i].block(0, static_cast<unsigned int>(sub_tree[j]), 6, 1) +=
855 updatedModel.U[i] *
856 Minv.block(q_index_i, static_cast<unsigned int>(sub_tree[j]), 1, 1);
857
858 F[lambda_q_i].block(0, static_cast<unsigned int>(sub_tree[j]), 6, 1) +=
859 updatedModel.X_lambda[i].toMatrixTranspose() *
860 F[q_index_i].block(0, static_cast<unsigned int>(sub_tree[j]), 6, 1);
861 }
862
863 RigidBodyDynamics::Math::SpatialMatrix Ia =
864 updatedModel.IA[i] -
865 updatedModel.U[i] *
866 (updatedModel.U[i] / updatedModel.d[i]).transpose();
867
868#ifdef BIORBD_USE_CASADI_MATH
869 updatedModel.IA[lambda] += updatedModel.X_lambda[i].toMatrixTranspose() *
870 Ia * updatedModel.X_lambda[i].toMatrix();
871
872#else
873 updatedModel.IA[lambda].noalias() +=
874 updatedModel.X_lambda[i].toMatrixTranspose() * Ia *
875 updatedModel.X_lambda[i].toMatrix();
876#endif
877 }
878 }
879 // End Backward Pass
880
881 // Second Forward Pass
882 for (i = 1; i < updatedModel.mBodies.size(); i++) {
883 unsigned int q_index_i = updatedModel.mJoints[i].q_index;
884 unsigned int lambda = updatedModel.lambda[i];
885 unsigned int lambda_q_i = updatedModel.mJoints[lambda].q_index;
886
887 utils::SpatialTransform X_lambda = updatedModel.X_lambda[i];
888
889 if (lambda != 0) {
890 for (j = q_index_i; j < static_cast<int>(updatedModel.dof_count); j++) {
891 utils::SpatialVector Ftemp(F[lambda_q_i].block(0, j, 6, 1));
892 Minv(q_index_i, j) -=
893 (1.0 / updatedModel.d[i]) *
894 (updatedModel.U[i].transpose() * X_lambda.toMatrix()) * Ftemp;
895 }
896 }
897 // F[i,:,i:] = np.outer(S,Minv[i,i:]) // could be simplified (S *
898 // M[q_index_i,q_index_i:]^T)
899 for (j = q_index_i; j < static_cast<int>(updatedModel.dof_count); j++) {
900 F[q_index_i].block(0, j, 6, 1) =
901 updatedModel.S[i] * Minv.block(q_index_i, j, 1, 1); // outer product
902 }
903
904 if (lambda != 0) {
905 // F[i,:,i:] += Xmat.transpose() * F[lambda,:,i:]
906 for (j = q_index_i; j < static_cast<int>(updatedModel.dof_count); j++) {
907 F[q_index_i].block(0, j, 6, 1) +=
908 X_lambda.toMatrix() * F[lambda_q_i].block(0, j, 6, 1);
909 }
910 }
911 }
912 // End of Second Forward Pass
913 // Fill in full matrix (currently only upper triangular)
914 for (j = 0; j < static_cast<int>(updatedModel.dof_count); j++) {
915 for (i = 0; i < static_cast<int>(updatedModel.dof_count); i++) {
916 if (j < i) {
917 Minv(i, j) = Minv(j, i);
918 }
919 }
920 }
921
922 return Minv;
923}
924
928 bool updateKin) {
929#ifdef BIORBD_USE_CASADI_MATH
931#else
933#endif
934 updatedModel = this->UpdateKinematicsCustom(
935 updateKin ? &Q : nullptr, updateKin ? &Qdot : nullptr);
936
937 // For each segment, find the CoM
938 utils::Vector3d com_dot(0, 0, 0);
939
940 // CoMdot = sum(mass_seg * Jacobian * qdot)/total_mass
941 for (const auto &segment : *m_segments) {
942 utils::Matrix Jac = updatedModel.CalcPointJacobian(
943 Q, segment.name(), segment.characteristics().mCenterOfMass, false);
944 com_dot += ((Jac * Qdot) * segment.characteristics().mMass);
945 }
946 // Divide by total mass
947 com_dot = com_dot / mass();
948
949 // Return the velocity of CoM
950 return com_dot;
951}
952
957 bool updateKin) {
958 utils::Scalar mass;
959 utils::Vector3d com, com_ddot;
960 this->CalcCenterOfMass(
961 Q,
962 Qdot,
963 &Qddot,
964 mass,
965 com,
966 nullptr,
967 &com_ddot,
968 nullptr,
969 nullptr,
970 updateKin);
971
972 // Return the acceleration of CoM
973 return com_ddot;
974}
975
978 bool updateKin) {
979#ifdef BIORBD_USE_CASADI_MATH
981#else
983#endif
984 updatedModel = this->UpdateKinematicsCustom(updateKin ? &Q : nullptr);
985
986 // Total jacobian
987 utils::Matrix JacTotal(utils::Matrix::Zero(3, this->dof_count));
988
989 // CoMdot = sum(mass_seg * Jacobian * qdot)/total_mass
990 for (auto segment : *m_segments) {
991 utils::Matrix Jac = updatedModel.CalcPointJacobian(
992 Q, segment.name(), segment.characteristics().mCenterOfMass, false);
993 JacTotal += segment.characteristics().mMass * Jac;
994 }
995
996 // Divide by total mass
997 JacTotal /= this->mass();
998
999 // Return the Jacobian of CoM
1000 return JacTotal;
1001}
1002
1007 utils::Scalar &mass,
1008 utils::Vector3d &com,
1009 utils::Vector3d *comVelocity,
1010 utils::Vector3d *comAcceleration,
1012 utils::Vector3d *changeOfAngularMomentum,
1013 bool updateKin) {
1014#ifdef BIORBD_USE_CASADI_MATH
1016#else
1018#endif
1019 updatedModel = this->UpdateKinematicsCustom(
1020 updateKin ? &Q : nullptr,
1021 updateKin ? &Qdot : nullptr,
1022 updateKin ? Qddot : nullptr);
1023
1024 RigidBodyDynamics::Utils::CalcCenterOfMass(
1025 updatedModel,
1026 Q,
1027 Qdot,
1028 Qddot,
1029 mass,
1030 com,
1031 comVelocity,
1032 comAcceleration,
1034 changeOfAngularMomentum,
1035 false);
1036}
1037
1042 bool updateKin) {
1043 return CalcZeroMomentPoint(
1044 Q,
1045 Qdot,
1046 Qddot,
1047 utils::Vector3d(0., 0., 1.),
1048 utils::Vector3d(0., 0., 0.),
1049 updateKin);
1050}
1051
1056 const utils::Vector3d &normal,
1057 const utils::Vector3d &point,
1058 bool updateKin) {
1059#ifdef BIORBD_USE_CASADI_MATH
1061#else
1063#endif
1064 updatedModel = this->UpdateKinematicsCustom(
1065 updateKin ? &Q : nullptr,
1066 updateKin ? &Qdot : nullptr,
1067 updateKin ? &Qddot : nullptr);
1068
1069 utils::Vector3d zmp;
1070 RigidBodyDynamics::Utils::CalcZeroMomentPoint(
1071 updatedModel, Q, Qdot, Qddot, &zmp, normal, point, false);
1072
1073 zmp += normal * ((point - zmp).dot(normal) / normal.dot(normal));
1074
1075 return zmp;
1076}
1077
1078std::vector<rigidbody::NodeSegment> rigidbody::Joints::CoMbySegment(
1080 bool updateKin) {
1081#ifdef BIORBD_USE_CASADI_MATH
1083#else
1085#endif
1086 updatedModel = this->UpdateKinematicsCustom(updateKin ? &Q : nullptr);
1087
1088 std::vector<rigidbody::NodeSegment> out;
1089 for (size_t i = 0; i < m_segments->size(); ++i) {
1090 out.push_back(updatedModel.CoMbySegment(Q, i, false));
1091 }
1092 return out;
1093}
1094
1097 bool updateKin) {
1098 std::vector<rigidbody::NodeSegment> allCoM(CoMbySegment(Q, updateKin));
1099
1100 utils::Matrix CoMs(3, static_cast<int>(allCoM.size()));
1101 for (size_t i = 0; i < allCoM.size(); ++i) {
1102 CoMs.block(0, static_cast<unsigned int>(i), 3, 1) = allCoM[i];
1103 }
1104 return CoMs;
1105}
1106
1109 const size_t idx,
1110 bool updateKin) {
1112 idx < m_segments->size(), "Choosen segment doesn't exist");
1113
1114 return this->CalcBodyToBaseCoordinates(
1115 Q,
1116 static_cast<unsigned int>((*m_segments)[idx].id()),
1117 (*m_segments)[idx].characteristics().mCenterOfMass,
1118 updateKin);
1119}
1120
1121std::vector<utils::Vector3d> rigidbody::Joints::CoMdotBySegment(
1124 bool updateKin) {
1125#ifdef BIORBD_USE_CASADI_MATH
1127#else
1129#endif
1130 updatedModel = this->UpdateKinematicsCustom(
1131 updateKin ? &Q : nullptr, updateKin ? &Qdot : nullptr);
1132
1133 std::vector<utils::Vector3d> out;
1134 for (size_t i = 0; i < m_segments->size(); ++i) {
1135 out.push_back(updatedModel.CoMdotBySegment(Q, Qdot, i, false));
1136 }
1137 return out;
1138}
1139
1143 const size_t idx,
1144 bool updateKin) {
1145 // Position of the center of mass of segment i
1147 idx < m_segments->size(), "Choosen segment doesn't exist");
1148
1149 return this->CalcPointVelocity(
1150 Q,
1151 Qdot,
1152 static_cast<unsigned int>((*m_segments)[idx].id()),
1153 (*m_segments)[idx].characteristics().mCenterOfMass,
1154 updateKin);
1155}
1156
1157std::vector<utils::Vector3d> rigidbody::Joints::CoMddotBySegment(
1161 bool updateKin) {
1162#ifdef BIORBD_USE_CASADI_MATH
1164#else
1166#endif
1167 updatedModel = this->UpdateKinematicsCustom(
1168 updateKin ? &Q : nullptr,
1169 updateKin ? &Qdot : nullptr,
1170 updateKin ? &Qddot : nullptr);
1171
1172 std::vector<utils::Vector3d> out;
1173 for (size_t i = 0; i < m_segments->size(); ++i) {
1174 out.push_back(updatedModel.CoMddotBySegment(Q, Qdot, Qddot, i, false));
1175 }
1176 return out;
1177}
1178
1183 const size_t idx,
1184 bool updateKin) {
1186 idx < m_segments->size(), "Choosen segment doesn't exist");
1187
1188 return this->CalcPointAcceleration(
1189 Q,
1190 Qdot,
1191 Qddot,
1192 static_cast<unsigned int>((*m_segments)[idx].id()),
1193 (*m_segments)[idx].characteristics().mCenterOfMass,
1194 updateKin);
1195}
1196
1197std::vector<std::vector<utils::Vector3d>> rigidbody::Joints::meshPoints(
1199 bool updateKin) {
1200#ifdef BIORBD_USE_CASADI_MATH
1202#else
1204#endif
1205 updatedModel = this->UpdateKinematicsCustom(updateKin ? &Q : nullptr);
1206
1207 // Find the position of the segments
1208 const std::vector<utils::RotoTrans> &RT(updatedModel.allGlobalJCS(Q, false));
1209
1210 // For all the segments
1211 std::vector<std::vector<utils::Vector3d>> out;
1212 for (size_t i = 0; i < nbSegment(); ++i) {
1213 out.push_back(updatedModel.meshPoints(RT, i));
1214 }
1215 return out;
1216}
1217std::vector<utils::Vector3d> rigidbody::Joints::meshPoints(
1219 size_t i,
1220 bool updateKin) {
1221 // Find the position of the segments
1222 const std::vector<utils::RotoTrans> &RT(allGlobalJCS(Q, updateKin));
1223
1224 return meshPoints(RT, i);
1225}
1226
1227std::vector<utils::Matrix> rigidbody::Joints::meshPointsInMatrix(
1229 bool updateKin) {
1230 const std::vector<utils::RotoTrans> &RT(allGlobalJCS(Q, updateKin));
1231
1232 std::vector<utils::Matrix> all_points;
1233 for (size_t i = 0; i < m_segments->size(); ++i) {
1234 utils::Matrix mat(3, mesh(i).nbVertex());
1235 for (size_t j = 0; j < mesh(i).nbVertex(); ++j) {
1236 utils::Vector3d tp(mesh(i).point(j));
1237 tp.applyRT(RT[i]);
1238 mat.block(0, static_cast<unsigned int>(j), 3, 1) = tp;
1239 }
1240 all_points.push_back(mat);
1241 }
1242 return all_points;
1243}
1244std::vector<utils::Vector3d> rigidbody::Joints::meshPoints(
1245 const std::vector<utils::RotoTrans> &RT,
1246 size_t i) const {
1247 // Gather the position of the meshings
1248 std::vector<utils::Vector3d> v;
1249 for (size_t j = 0; j < mesh(i).nbVertex(); ++j) {
1250 utils::Vector3d tp(mesh(i).point(j));
1251 tp.applyRT(RT[i]);
1252 v.push_back(tp);
1253 }
1254
1255 return v;
1256}
1257
1258std::vector<std::vector<rigidbody::MeshFace>> rigidbody::Joints::meshFaces()
1259 const {
1260 // Gather the position of the meshings for all the segments
1261 std::vector<std::vector<rigidbody::MeshFace>> v_all;
1262 for (size_t j = 0; j < nbSegment(); ++j) {
1263 v_all.push_back(meshFaces(j));
1264 }
1265 return v_all;
1266}
1267const std::vector<rigidbody::MeshFace> &rigidbody::Joints::meshFaces(
1268 size_t idx) const {
1269 // Find the position of the meshings for a segment i
1270 return mesh(idx).faces();
1271}
1272
1273std::vector<rigidbody::Mesh> rigidbody::Joints::mesh() const {
1274 std::vector<rigidbody::Mesh> segmentOut;
1275 for (size_t i = 0; i < nbSegment(); ++i) {
1276 segmentOut.push_back(mesh(i));
1277 }
1278 return segmentOut;
1279}
1280
1282 return segment(idx).characteristics().mesh();
1283}
1284
1286 for (size_t i = 0; i < nbSegment(); ++i) {
1287 segment(i).characteristics().mesh().path().setFolder(folder);
1288 }
1289}
1290
1294 bool updateKin) {
1296 utils::Scalar mass;
1297 this->CalcCenterOfMass(
1298 Q,
1299 Qdot,
1300 nullptr,
1301 mass,
1302 com,
1303 nullptr,
1304 nullptr,
1306 nullptr,
1307 updateKin);
1308 return angularMomentum;
1309}
1310
1315 bool updateKin) {
1317 utils::Scalar mass;
1318 this->CalcCenterOfMass(
1319 Q,
1320 Qdot,
1321 &Qddot,
1322 mass,
1323 com,
1324 nullptr,
1325 nullptr,
1327 nullptr,
1328 updateKin);
1329 return angularMomentum;
1330}
1331
1335 bool updateKin) {
1336#ifdef BIORBD_USE_CASADI_MATH
1338#else
1340#endif
1341 updatedModel = this->UpdateKinematicsCustom(
1342 updateKin ? &Q : nullptr, updateKin ? &Qdot : nullptr, nullptr);
1343
1344 utils::Scalar mass;
1345 utils::Vector3d com;
1346 RigidBodyDynamics::Utils::CalcCenterOfMass(
1347 updatedModel,
1348 Q,
1349 Qdot,
1350 nullptr,
1351 mass,
1352 com,
1353 nullptr,
1354 nullptr,
1355 nullptr,
1356 nullptr,
1357 false);
1358 utils::SpatialTransform X_to_COM(RigidBodyDynamics::Math::Xtrans(com));
1359
1360 std::vector<utils::Vector3d> h_segment;
1361 for (size_t i = 1; i < updatedModel.mBodies.size(); i++) {
1362 updatedModel.Ic[i] = updatedModel.I[i];
1363 updatedModel.hc[i] = updatedModel.Ic[i].toMatrix() * updatedModel.v[i];
1364
1366 updatedModel.X_lambda[i].applyTranspose(updatedModel.hc[i]);
1367 if (updatedModel.lambda[i] != 0) {
1368 size_t j(i);
1369 do {
1370 j = updatedModel.lambda[j];
1371 h = updatedModel.X_lambda[j].applyTranspose(h);
1372 } while (updatedModel.lambda[j] != 0);
1373 }
1374 h = X_to_COM.applyAdjoint(h);
1375 h_segment.push_back(utils::Vector3d(h[0], h[1], h[2]));
1376 }
1377
1378 return h_segment;
1379}
1380
1385 bool updateKin) {
1386#ifdef BIORBD_USE_CASADI_MATH
1388#else
1390#endif
1391 updatedModel = this->UpdateKinematicsCustom(
1392 updateKin ? &Q : nullptr, updateKin ? &Qdot : nullptr, nullptr);
1393
1394 utils::Scalar mass;
1395 utils::Vector3d com;
1396 RigidBodyDynamics::Utils::CalcCenterOfMass(
1397 updatedModel,
1398 Q,
1399 Qdot,
1400 &Qddot,
1401 mass,
1402 com,
1403 nullptr,
1404 nullptr,
1405 nullptr,
1406 nullptr,
1407 false);
1408 utils::SpatialTransform X_to_COM(RigidBodyDynamics::Math::Xtrans(com));
1409
1410 std::vector<utils::Vector3d> h_segment;
1411 for (size_t i = 1; i < updatedModel.mBodies.size(); i++) {
1412 updatedModel.Ic[i] = updatedModel.I[i];
1413 updatedModel.hc[i] = updatedModel.Ic[i].toMatrix() * updatedModel.v[i];
1414
1416 updatedModel.X_lambda[i].applyTranspose(updatedModel.hc[i]);
1417 if (updatedModel.lambda[i] != 0) {
1418 size_t j(i);
1419 do {
1420 j = updatedModel.lambda[j];
1421 h = updatedModel.X_lambda[j].applyTranspose(h);
1422 } while (updatedModel.lambda[j] != 0);
1423 }
1424 h = X_to_COM.applyAdjoint(h);
1425 h_segment.push_back(utils::Vector3d(h[0], h[1], h[2]));
1426 }
1427
1428 return h_segment;
1429}
1430
1431size_t rigidbody::Joints::nbQuat() const { return *m_nRotAQuat; }
1432
1436 const utils::Scalar &k_stab) {
1437 rigidbody::GeneralizedVelocity QdotOut(static_cast<int>(Q.size()));
1438 // Verify if there are quaternions, if not the derivate is directly Qdot
1439 if (!m_nRotAQuat) {
1440 QdotOut = Qdot;
1441 return QdotOut;
1442 }
1443 unsigned int cmpQuat(0);
1444 unsigned int cmpDof(0);
1445 for (size_t i = 0; i < nbSegment(); ++i) {
1446 const rigidbody::Segment &segment_i = segment(i);
1447 if (segment_i.isRotationAQuaternion()) {
1448 // Extraire le quaternion
1449 utils::Quaternion quat_tp(
1450 Q(Q.size() - static_cast<unsigned int>(*m_nRotAQuat + cmpQuat)),
1451 Q.block(
1452 cmpDof + static_cast<unsigned int>(segment_i.nbDofTrans()),
1453 0,
1454 3,
1455 1),
1456 k_stab);
1457
1458 // Qdot for translation is actual Qdot
1459 QdotOut.block(
1460 cmpDof, 0, static_cast<unsigned int>(segment_i.nbDofTrans()), 1) =
1461 Qdot.block(
1462 cmpDof, 0, static_cast<unsigned int>(segment_i.nbDofTrans()), 1);
1463
1464 // Get the 4d derivative for the quaternion part
1465 quat_tp.derivate(Qdot.block(
1466 cmpDof + static_cast<unsigned int>(segment_i.nbDofTrans()), 0, 3, 1));
1467 QdotOut.block(
1468 cmpDof + static_cast<unsigned int>(segment_i.nbDofTrans()), 0, 3, 1) =
1469 quat_tp.block(1, 0, 3, 1);
1470 QdotOut(Q.size() - static_cast<unsigned int>(*m_nRotAQuat + cmpQuat)) =
1471 quat_tp(0); // Placer dans le vecteur de sortie
1472
1473 // Increment the number of done quaternions
1474 ++cmpQuat;
1475 } else {
1476 // If it's a normal, do what it usually does
1477 QdotOut.block(
1478 cmpDof, 0, static_cast<unsigned int>(segment_i.nbDof()), 1) =
1479 Qdot.block(
1480 cmpDof, 0, static_cast<unsigned int>(segment_i.nbDof()), 1);
1481 }
1482 cmpDof += static_cast<unsigned int>(segment_i.nbDof());
1483 }
1484 return QdotOut;
1485}
1486
1490 bool updateKin) {
1491#ifdef BIORBD_USE_CASADI_MATH
1493#else
1495#endif
1496 updatedModel = this->UpdateKinematicsCustom(
1497 updateKin ? &Q : nullptr, updateKin ? &Qdot : nullptr);
1498
1499 return RigidBodyDynamics::Utils::CalcKineticEnergy(
1500 updatedModel, Q, Qdot, false);
1501}
1502
1505 bool updateKin) {
1506#ifdef BIORBD_USE_CASADI_MATH
1508#else
1510#endif
1511 updatedModel = this->UpdateKinematicsCustom(updateKin ? &Q : nullptr);
1512
1513 return RigidBodyDynamics::Utils::CalcPotentialEnergy(updatedModel, Q, false);
1514}
1515
1519 bool updateKin) {
1520#ifdef BIORBD_USE_CASADI_MATH
1522#else
1524#endif
1525 updatedModel = this->UpdateKinematicsCustom(
1526 updateKin ? &Q : nullptr, updateKin ? &Qdot : nullptr);
1527
1528 utils::Scalar kinetic(
1529 RigidBodyDynamics::Utils::CalcKineticEnergy(
1530 updatedModel, Q, Qdot, false));
1531 utils::Scalar potential(
1532 RigidBodyDynamics::Utils::CalcPotentialEnergy(updatedModel, Q, false));
1533 return kinetic - potential;
1534}
1535
1539 bool updateKin) {
1540#ifdef BIORBD_USE_CASADI_MATH
1542#else
1544#endif
1545 updatedModel = this->UpdateKinematicsCustom(
1546 updateKin ? &Q : nullptr, updateKin ? &Qdot : nullptr);
1547
1548 utils::Scalar kinetic(
1549 RigidBodyDynamics::Utils::CalcKineticEnergy(
1550 updatedModel, Q, Qdot, false));
1551 utils::Scalar potential(
1552 RigidBodyDynamics::Utils::CalcPotentialEnergy(updatedModel, Q, false));
1553 return kinetic + potential;
1554}
1555
1568 rigidbody::ExternalForceSet &externalForces) {
1569#ifdef BIORBD_USE_CASADI_MATH
1571#else
1573#endif
1574 updatedModel = this->UpdateKinematicsCustom(&Q, &Qdot);
1575
1577 auto fExt = externalForces.computeRbdlSpatialVectors(updatedModel, Q, Qdot);
1578
1579 RigidBodyDynamics::InverseDynamics(updatedModel, Q, Qdot, Qddot, Tau, &fExt);
1580 return Tau - computeDampedTau(Qdot);
1581}
1582
1593 rigidbody::ExternalForceSet &externalForces) {
1594#ifdef BIORBD_USE_CASADI_MATH
1596#else
1598#endif
1599 updatedModel = this->UpdateKinematicsCustom(&Q, &Qdot);
1600
1601 rigidbody::GeneralizedTorque Tau(updatedModel);
1602 auto fExt = externalForces.computeRbdlSpatialVectors(updatedModel, Q, Qdot);
1603 RigidBodyDynamics::NonlinearEffects(updatedModel, Q, Qdot, Tau, &fExt);
1604 return Tau;
1605}
1606
1619 rigidbody::ExternalForceSet &externalForces) {
1620#ifdef BIORBD_USE_CASADI_MATH
1622#else
1624#endif
1625 updatedModel = this->UpdateKinematicsCustom(&Q, &Qdot);
1626
1627 rigidbody::GeneralizedAcceleration Qddot(updatedModel);
1628 auto fExt = externalForces.computeRbdlSpatialVectors(updatedModel, Q, Qdot);
1629 rigidbody::GeneralizedTorque dampedTau = Tau - computeDampedTau(Qdot);
1630
1631 RigidBodyDynamics::ForwardDynamics(
1632 updatedModel, Q, Qdot, dampedTau, Qddot, &fExt);
1633 return Qddot;
1634}
1635
1640 const rigidbody::GeneralizedAcceleration &QddotJoints) {
1642 QddotJoints.size() == this->nbQddot() - this->nbRoot(),
1643 "Size of QddotJ must be equal to number of Qddot - "
1644 "number of root coordinates.");
1645
1647 this->nbRoot() > 0, "Must have a least one degree of freedom on root.");
1648
1651 rigidbody::GeneralizedTorque MassMatrixNlEffects;
1652
1653 utils::Matrix massMatrixRoot = this->massMatrix(Q).block(
1654 0,
1655 0,
1656 static_cast<unsigned int>(this->nbRoot()),
1657 static_cast<unsigned int>(this->nbRoot()));
1658
1659 Qddot.block(0, 0, static_cast<unsigned int>(this->nbRoot()), 1) =
1660 utils::Vector(this->nbRoot()).setZero();
1661 Qddot.block(
1662 static_cast<unsigned int>(this->nbRoot()),
1663 0,
1664 static_cast<unsigned int>(this->nbQddot() - this->nbRoot()),
1665 1) = QddotJoints;
1666
1667 MassMatrixNlEffects = InverseDynamics(Q, Qdot, Qddot);
1668
1669#ifdef BIORBD_USE_CASADI_MATH
1670 auto linsol =
1671 casadi::Linsol("linsol", "symbolicqr", massMatrixRoot.sparsity());
1672 QRootDDot = linsol.solve(
1673 massMatrixRoot,
1674 -MassMatrixNlEffects.block(
1675 0, 0, static_cast<unsigned int>(this->nbRoot()), 1));
1676#else
1677 QRootDDot = massMatrixRoot.llt().solve(-MassMatrixNlEffects.block(
1678 0, 0, static_cast<unsigned int>(this->nbRoot()), 1));
1679#endif
1680
1681 return QRootDDot;
1682}
1683
1689 bool updateKin) {
1691 dynamic_cast<rigidbody::Contacts *>(this)->getConstraints();
1692 return ForwardDynamicsConstraintsDirect(Q, Qdot, Tau, CS, updateKin);
1693}
1694
1700 rigidbody::ExternalForceSet &externalForces,
1701 bool updateKin) {
1703 dynamic_cast<rigidbody::Contacts *>(this)->getConstraints();
1705 Q, Qdot, Tau, CS, externalForces, updateKin);
1706}
1707
1714 bool updateKin) {
1716 static_cast<BIORBD_NAMESPACE::Model &>(*this));
1718 Q, Qdot, Tau, CS, forceSet, updateKin);
1719}
1720
1727 rigidbody::ExternalForceSet &externalForces,
1728 bool updateKin) {
1729#ifdef BIORBD_USE_CASADI_MATH
1731#else
1733#endif
1734 updatedModel = this->UpdateKinematicsCustom(
1735 updateKin ? &Q : nullptr, updateKin ? &Qdot : nullptr);
1736
1737 auto fExt = externalForces.computeRbdlSpatialVectors(updatedModel, Q, Qdot);
1738 rigidbody::GeneralizedTorque dampedTau = Tau - computeDampedTau(Qdot);
1739
1741 RigidBodyDynamics::ForwardDynamicsConstraintsDirect(
1742 updatedModel, Q, Qdot, dampedTau, CS, Qddot, updateKin, &fExt);
1743 return Qddot;
1744}
1745
1761 rigidbody::ExternalForceSet &externalForces) {
1763 dynamic_cast<rigidbody::Contacts *>(this)->getConstraints();
1764 this->ForwardDynamicsConstraintsDirect(Q, Qdot, Tau, CS, externalForces);
1765 return CS.getForce();
1766}
1767
1771 const rigidbody::GeneralizedVelocity &QdotPre) {
1773 dynamic_cast<rigidbody::Contacts *>(this)->getConstraints();
1774 if (CS.nbContacts() == 0) {
1775 return QdotPre;
1776 } else {
1777#ifdef BIORBD_USE_CASADI_MATH
1778 rigidbody::Joints model = this->DeepCopy();
1779#else
1780 rigidbody::Joints &model = *this;
1781#endif
1782
1783 CS = dynamic_cast<rigidbody::Contacts *>(this)->getConstraints();
1784
1785 rigidbody::GeneralizedVelocity QdotPost(model);
1786 RigidBodyDynamics::ComputeConstraintImpulsesDirect(
1787 model, Q, QdotPre, CS, QdotPost);
1788 return QdotPost;
1789 }
1790}
1791
1794 bool updateKin) {
1795#ifdef BIORBD_USE_CASADI_MATH
1797#else
1799#endif
1800 model = this->UpdateKinematicsCustom(updateKin ? &q : nullptr);
1801
1802 for (size_t i = 1; i < model.mBodies.size(); i++) {
1803 model.Ic[i] = model.I[i];
1804 }
1805
1806 RigidBodyDynamics::Math::SpatialRigidBodyInertia Itot;
1807
1808 for (size_t i = model.mBodies.size() - 1; i > 0; i--) {
1809 size_t lambda = static_cast<size_t>(model.lambda[i]);
1810
1811 if (lambda != 0) {
1812 model.Ic[lambda] =
1813 model.Ic[lambda] + model.X_lambda[i].applyTranspose(model.Ic[i]);
1814 } else {
1815 Itot = Itot + model.X_lambda[i].applyTranspose(model.Ic[i]);
1816 }
1817 }
1818
1819 utils::Vector3d com = Itot.h / Itot.m;
1820 return RigidBodyDynamics::Math::Xtrans(-com)
1821 .applyTranspose(Itot)
1822 .toMatrix()
1823 .block(0, 0, 3, 3);
1824}
1825
1829 bool updateKin) {
1831 utils::Scalar mass;
1832 this->CalcCenterOfMass(
1833 Q,
1834 Qdot,
1835 nullptr,
1836 mass,
1837 com,
1838 nullptr,
1839 nullptr,
1841 nullptr,
1842 updateKin);
1843 utils::Matrix3d body_inertia = this->bodyInertia(Q, updateKin);
1844
1845#ifdef BIORBD_USE_CASADI_MATH
1846 auto linsol =
1847 casadi::Linsol("linear_solver", "symbolicqr", body_inertia.sparsity());
1848 RigidBodyDynamics::Math::Vector3d out =
1849 linsol.solve(body_inertia, angularMomentum);
1850#else
1851 RigidBodyDynamics::Math::Vector3d out =
1852 body_inertia.colPivHouseholderQr().solve(angularMomentum);
1853#endif
1854
1855 return out;
1856}
1857
1859 const utils::String &segmentName,
1860 const utils::String &dofName) {
1861 size_t idx = 0;
1862
1863 size_t iB = 0;
1864 bool found = false;
1865 while (1) {
1866 utils::Error::check(iB != m_segments->size(), "Segment not found");
1867
1868 if (segmentName.compare((*m_segments)[iB].name())) {
1869 idx += (*m_segments)[iB].nbDof();
1870 } else {
1871 idx += (*m_segments)[iB].getDofIdx(dofName);
1872 found = true;
1873 break;
1874 }
1875
1876 ++iB;
1877 }
1878
1879 utils::Error::check(found, "Dof not found");
1880 return idx;
1881}
1882
1883#ifdef BIORBD_USE_CASADI_MATH
1885#else
1887#endif
1892 checkGeneralizedDimensions(Q, Qdot, Qddot);
1893 if (Q == nullptr && Qdot == nullptr && Qddot == nullptr) return *this;
1894
1895#ifdef BIORBD_USE_CASADI_MATH
1896 rigidbody::Joints model = this->DeepCopy();
1897#else
1898 rigidbody::Joints &model = *this;
1899#endif
1900 RigidBodyDynamics::UpdateKinematicsCustom(model, Q, Qdot, Qddot);
1901
1902 return model;
1903}
1904
1907 size_t segmentIdx,
1908 const utils::Matrix3d &rotation,
1909 utils::Matrix &G,
1910 bool updateKin) {
1911#ifdef RBDL_ENABLE_LOGGING
1912 LOG << "-------- " << __func__ << " --------" << std::endl;
1913#endif
1914
1915#ifdef BIORBD_USE_CASADI_MATH
1917#else
1919#endif
1920 model = this->UpdateKinematicsCustom(updateKin ? &Q : nullptr);
1921 updateKin = false;
1922
1923 assert(G.rows() == 9 && G.cols() == model.qdot_size);
1924
1925 std::vector<utils::Vector3d> axes;
1926 axes.push_back(utils::Vector3d(1, 0, 0));
1927 axes.push_back(utils::Vector3d(0, 1, 0));
1928 axes.push_back(utils::Vector3d(0, 0, 1));
1929 for (unsigned int iAxes = 0; iAxes < 3; ++iAxes) {
1930 utils::Matrix3d bodyMatRot(
1931 RigidBodyDynamics::CalcBodyWorldOrientation(
1932 model, Q, static_cast<unsigned int>(segmentIdx), updateKin)
1933 .transpose());
1934 utils::SpatialTransform point_trans(
1936 utils::Matrix3d::Identity(),
1937 bodyMatRot * rotation * *(axes.begin() + iAxes)));
1938
1939 size_t reference_body_id = segmentIdx;
1940 if (model.IsFixedBodyId(static_cast<unsigned int>(segmentIdx))) {
1941 size_t fbody_id = segmentIdx - model.fixed_body_discriminator;
1942 reference_body_id = model.mFixedBodies[fbody_id].mMovableParent;
1943 }
1944 size_t j = reference_body_id;
1945
1946 // e[j] is set to 1 if joint j contributes to the jacobian that we are
1947 // computing. For all other joints the column will be zero.
1948 while (j != 0) {
1949 unsigned int q_index = model.mJoints[j].q_index;
1950 // If it's not a DoF in translation (3 4 5 in model.S)
1951#ifdef BIORBD_USE_CASADI_MATH
1952 if (model.S[j].is_zero() && model.S[j](4).is_zero() &&
1953 model.S[j](5).is_zero())
1954#else
1955 if (model.S[j](3) != 1.0 && model.S[j](4) != 1.0 && model.S[j](5) != 1.0)
1956#endif
1957 {
1958 utils::SpatialTransform X_base = model.X_base[j];
1959 X_base.r =
1960 utils::Vector3d(0, 0, 0); // Remove all concept of translation
1961 // (only keep the rotation matrix)
1962
1963 if (model.mJoints[j].mDoFCount == 3) {
1964 G.block(iAxes * 3, q_index, 3, 3) =
1965 ((point_trans * X_base.inverse()).toMatrix() *
1966 model.multdof3_S[j])
1967 .block(3, 0, 3, 3);
1968 } else {
1969 G.block(iAxes * 3, q_index, 3, 1) =
1970 point_trans.apply(X_base.inverse().apply(model.S[j]))
1971 .block(3, 0, 3, 1);
1972 }
1973 }
1974 j = model.lambda[j]; // Pass to parent segment
1975 }
1976 }
1977}
1978
1981 size_t biorbdSegmentIdx,
1982 bool updateKin) {
1983 size_t segmentIdx =
1984 getBodyBiorbdIdToRbdlId(static_cast<int>(biorbdSegmentIdx));
1985
1986 utils::Matrix jacobianMat(
1987 utils::Matrix::Zero(9, static_cast<unsigned int>(nbQ())));
1988 this->CalcMatRotJacobian(
1989 Q, segmentIdx, utils::Matrix3d::Identity(), jacobianMat, updateKin);
1990 return jacobianMat;
1991}
1992
1997 const rigidbody::GeneralizedTorque *torque) {
1998#ifndef SKIP_ASSERT
1999 if (Q) {
2001 Q->size() == nbQ(),
2002 "Wrong size for the Generalized Coordiates, " +
2003 utils::String("expected ") + std::to_string(nbQ()) + " got " +
2004 std::to_string(Q->size()));
2005 }
2006 if (Qdot) {
2008 Qdot->size() == nbQdot(),
2009 "Wrong size for the Generalized Velocities, " +
2010 utils::String("expected ") + std::to_string(nbQdot()) + " got " +
2011 std::to_string(Qdot->size()));
2012 }
2013 if (Qddot) {
2015 Qddot->size() == nbQddot(),
2016 "Wrong size for the Generalized Accelerations, " +
2017 utils::String("expected ") + std::to_string(nbQddot()) + " got " +
2018 std::to_string(Qddot->size()));
2019 }
2020
2021 if (torque) {
2023 torque->size() == nbGeneralizedTorque(),
2024 "Wrong size for the Generalized Torques, " +
2025 utils::String("expected ") + std::to_string(nbGeneralizedTorque()) +
2026 " got " + std::to_string(torque->size()));
2027 }
2028#endif
2029}
The actual musculoskeletal model that holds everything in biorbd.
utils::Vector getForce() const
Return the force acting on the constraint.
Definition Contacts.cpp:478
size_t nbContacts() const
Return the number of contacts.
Definition Contacts.cpp:253
An External force set that can apply forces to the model while computing the dynamics.
std::vector< RigidBodyDynamics::Math::SpatialVector > computeRbdlSpatialVectors(rigidbody::Joints &updatedModel)
The forces in a rbdl compatible format. This won't work if useTranslationalForces or useSoftContacts ...
This is the core of the musculoskeletal model in biorbd.
Definition Joints.h:48
void setGravity(const utils::Vector3d &newGravity)
Set the gravity.
Definition Joints.cpp:193
size_t nbQdot() const
Return the number of generalized velocities (Qdot).
Definition Joints.cpp:106
std::shared_ptr< std::vector< Segment > > m_segments
All the articulations.
Definition Joints.h:1365
void overrideMeshFolders(const utils::String &folder)
Set the folder for all the meshes.
Definition Joints.cpp:1285
rigidbody::GeneralizedAcceleration ForwardDynamicsConstraintsDirect(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, const GeneralizedTorque &Tau, bool updateKin=true)
Interface for the forward dynamics with contact of RBDL.
Definition Joints.cpp:1685
std::vector< utils::Vector3d > CoMddotBySegment(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, const rigidbody::GeneralizedAcceleration &Qddot, bool updateKin=true)
Return the acceleration of the center of mass of each segment.
Definition Joints.cpp:1157
utils::Scalar PotentialEnergy(const rigidbody::GeneralizedCoordinates &Q, bool updateKin=true)
Interface for the Potential Energy of RBDL.
Definition Joints.cpp:1503
size_t nbQuat() const
Return the number of segments that are described using quaternions.
Definition Joints.cpp:1431
int getBodyRbdlIdToBiorbdId(const int idx) const
Return the Biorbd body identification from rbdl.
Definition Joints.cpp:258
utils::Vector3d CalcAngularMomentum(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, bool updateKin)
Calculate the angular momentum of the center of mass.
Definition Joints.cpp:1291
utils::Matrix CalcPointJacobian(const rigidbody::GeneralizedCoordinates &Q, utils::String segmentName, const utils::Vector3d &pointInLocal, bool updateKin=true)
Interface to call CalcPointJacobian. Instead of sending the jacobian as an parameter,...
Definition Joints.cpp:508
std::shared_ptr< size_t > m_nbRoot
The number of DoF on the root segment.
Definition Joints.h:1367
utils::Matrix massMatrix(const GeneralizedCoordinates &Q, bool updateKin=true)
Get the mass matrix at a given position Q.
Definition Joints.cpp:782
std::vector< NodeSegment > CoMbySegment(const GeneralizedCoordinates &Q, bool updateKin=true)
Return the position of the center of mass of each segment.
Definition Joints.cpp:1078
std::vector< utils::Vector3d > CoMdotBySegment(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, bool updateKin=true)
Return the velocity of the center of mass of each segment.
Definition Joints.cpp:1121
rigidbody::GeneralizedAcceleration ForwardDynamicsFreeFloatingBase(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, const GeneralizedAcceleration &QddotJoints)
Biorbd's implementation of forward dynamics with a free floating base.
Definition Joints.cpp:1637
std::vector< std::vector< size_t > > getDofSubTrees()
Return the rbdl idx of subtrees of each segments.
Definition Joints.cpp:270
utils::SpatialVector CalcPointVelocity6D(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, utils::String segmentName, const utils::Vector3d &pointInLocal, bool updateKin=true)
Interface to call CalcPointVelocity6D. It is pretty much useless in Eigen, but fixes an issue in Casa...
Definition Joints.cpp:443
std::vector< Mesh > mesh() const
Return the segment mesh.
Definition Joints.cpp:1273
utils::Vector3d CoMddot(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, const rigidbody::GeneralizedAcceleration &Qddot, bool updateKin=true)
Return the acceleration of the center of mass.
Definition Joints.cpp:953
size_t nbSegment() const
Return the actual number of segment.
Definition Joints.cpp:244
size_t AddSegment(const utils::String &segmentName, const utils::String &parentName, const utils::String &translationSequence, const utils::String &rotationSequence, const std::vector< utils::Range > &QRanges, const std::vector< utils::Range > &QdotRanges, const std::vector< utils::Range > &QddotRanges, const std::vector< utils::Scalar > &jointDampings, const SegmentCharacteristics &characteristics, const utils::RotoTrans &referenceFrame)
Add a segment to the model.
Definition Joints.cpp:112
void checkGeneralizedDimensions(const GeneralizedCoordinates *Q=nullptr, const GeneralizedVelocity *Qdot=nullptr, const rigidbody::GeneralizedAcceleration *Qddot=nullptr, const GeneralizedTorque *torque=nullptr)
Check for the Generalized coordinates, velocities, acceleration and torque dimensions.
Definition Joints.cpp:1993
int getBodyRbdlId(const utils::String &segmentName) const
Return the rbdl body identification.
Definition Joints.cpp:254
GeneralizedTorque InverseDynamics(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, const rigidbody::GeneralizedAcceleration &Qddot)
Interface for the inverse dynamics of RBDL.
Definition Joints.cpp:1556
std::shared_ptr< size_t > m_nbQdot
The total number of Qdot.
Definition Joints.h:1370
utils::Matrix projectPointJacobian(const GeneralizedCoordinates &Q, NodeSegment node, bool updateKin)
Return the jacobian matrix of the projected markers for a marker from the model.
Definition Joints.cpp:614
utils::Vector3d CalcPointAcceleration(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, const rigidbody::GeneralizedAcceleration &Qddot, utils::String segmentName, const utils::Vector3d &pointInLocal, bool updateKin=true)
Interface to call CalcPointAcceleration. It is pretty much useless in Eigen, but fixes an issue in Ca...
Definition Joints.cpp:471
void CalcMatRotJacobian(const GeneralizedCoordinates &Q, size_t segmentIdx, const utils::Matrix3d &rotation, utils::Matrix &G, bool updateKin)
Calculate the jacobian matrix of a rotation matrix.
Definition Joints.cpp:1905
std::vector< Segment > & segments() const
Get all the segments.
Definition Joints.cpp:226
void CalcCenterOfMass(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, const rigidbody::GeneralizedAcceleration *Qddot, utils::Scalar &mass, utils::Vector3d &com, utils::Vector3d *comVelocity=NULL, utils::Vector3d *comAcceleration=NULL, utils::Vector3d *angularMomentum=NULL, utils::Vector3d *changeOfAngularMomentum=NULL, bool updateKin=true)
Interface to CalcCenterOfMass. It is pretty much useless in Eigen, but fixes an issue in Casadi where...
Definition Joints.cpp:1003
utils::Vector3d CoMdot(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, bool updateKin=true)
Return the velocity of the center of mass.
Definition Joints.cpp:925
std::shared_ptr< size_t > m_nbDof
The total number of degrees of freedom.
Definition Joints.h:1368
std::shared_ptr< utils::Scalar > m_totalMass
Mass of all the bodies combined.
Definition Joints.h:1377
size_t nbDof() const
Return the number of degrees of freedom (DoF).
Definition Joints.cpp:87
size_t getBodyBiorbdIdToRbdlId(const int idx) const
Return the Rbdl body identification from Biorbd.
Definition Joints.cpp:266
std::vector< utils::RotoTrans > localJCS() const
Return all the joint coordinate system (JCS) in its parent reference frame.
Definition Joints.cpp:375
std::vector< utils::Vector3d > CalcSegmentsAngularMomentum(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, bool updateKin)
Calculate the segment's contribution to the whole body angular momentum with respect to the global ce...
Definition Joints.cpp:1332
std::vector< utils::RotoTrans > allGlobalJCS(const GeneralizedCoordinates &Q, bool updateKin=true)
Return the joint coordinate system (JCS) in global reference frame at a given Q.
Definition Joints.cpp:343
GeneralizedVelocity computeQdot(const GeneralizedCoordinates &Q, const GeneralizedCoordinates &Qdot, const utils::Scalar &k_stab=1)
Return the derivate of Q in function of Qdot (if not Quaternion, Qdot is directly returned).
Definition Joints.cpp:1433
utils::Scalar Lagrangian(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, bool updateKin=true)
Compute the Lagrangian of the system.
Definition Joints.cpp:1516
utils::Scalar mass() const
Return the total mass of the model.
Definition Joints.cpp:110
Joints DeepCopy() const
Deep copy of the joints.
Definition Joints.cpp:64
utils::Scalar KineticEnergy(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, bool updateKin=true)
Interface for the Kinetic Energy of RBDL.
Definition Joints.cpp:1487
std::shared_ptr< size_t > m_nbQ
The total number of Q.
Definition Joints.h:1369
size_t nbGeneralizedTorque() const
Return the number of generalized torque.
Definition Joints.cpp:86
Segment & segment(size_t idx)
Get a segment of index idx.
Definition Joints.cpp:205
utils::Matrix massMatrixInverse(const rigidbody::GeneralizedCoordinates &Q, bool updateKin=true)
Get the inverse mass matrix at a given position Q.
Definition Joints.cpp:800
utils::Vector3d CoM(const GeneralizedCoordinates &Q, bool updateKin=true)
Return the position of the center of mass.
Definition Joints.cpp:756
utils::Vector3d angularMomentum(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, bool updateKin=true)
Calculate the angular momentum of the center of mass. This method is an interface to CalcAngularMomem...
Definition Joints.cpp:775
utils::Vector3d bodyAngularVelocity(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, bool updateKin=true)
Calculate the angular velocity of the model around its center of mass.
Definition Joints.cpp:1826
NodeSegment projectPoint(const GeneralizedCoordinates &Q, const utils::Vector3d &v, int segmentIdx, const utils::String &axesToRemove, bool updateKin=true)
Project a point on specific axis of a segment.
Definition Joints.cpp:574
std::shared_ptr< bool > m_isKinematicsComputed
If the kinematics are computed.
Definition Joints.h:1375
int getBodyBiorbdId(const utils::String &segmentName) const
Return the biorbd body identification.
Definition Joints.cpp:246
utils::Matrix JacobianSegmentRotMat(const rigidbody::GeneralizedCoordinates &Q, size_t segmentIdx, bool updateKin)
Calculate the jacobian matrix of a rotation matrix for a given segment idx.
Definition Joints.cpp:1979
utils::SpatialTransform CalcBodyWorldTransformation(const GeneralizedCoordinates &Q, const size_t segmentIdx, bool updateKin=true)
Calculate the joint coordinate system (JCS) in global reference frame of a specified segment.
Definition Joints.cpp:709
utils::Vector3d getGravity() const
Get the current gravity.
Definition Joints.cpp:191
utils::Vector ContactForcesFromForwardDynamicsConstraintsDirect(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, const GeneralizedTorque &Tau)
Interface for contacts of the forward dynamics with contact of RBDL.
Definition Joints.cpp:1747
utils::Matrix CoMbySegmentInMatrix(const GeneralizedCoordinates &Q, bool updateKin=true)
Return the position of the center of mass of each segment in a matrix.
Definition Joints.cpp:1095
void updateSegmentCharacteristics(size_t idx, const SegmentCharacteristics &characteristics)
updateSegmentCharacteristics Change the inertia characteristics of the segment idx
Definition Joints.cpp:197
GeneralizedVelocity ComputeConstraintImpulsesDirect(const GeneralizedCoordinates &Q, const GeneralizedVelocity &QdotPre)
Compute the Qdot post from an impact.
Definition Joints.cpp:1769
utils::Matrix3d bodyInertia(const rigidbody::GeneralizedCoordinates &Q, bool updateKin=true)
bodyInertia Return the matrix of inertia of the body expressed in the global reference frame computed...
Definition Joints.cpp:1792
utils::Vector3d CalcPointVelocity(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, utils::String segmentName, const utils::Vector3d &pointInLocal, bool updateKin=true)
Interface to call CalcPointVelocity. It is pretty much useless in Eigen, but fixes an issue in Casadi...
Definition Joints.cpp:415
Joints & UpdateKinematicsCustom(const GeneralizedCoordinates *Q=nullptr, const GeneralizedVelocity *Qdot=nullptr, const rigidbody::GeneralizedAcceleration *Qddot=nullptr)
Update the kinematic variables such as body velocities and accelerations in the model to reflect the ...
Definition Joints.cpp:1888
GeneralizedTorque NonLinearEffect(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot)
Interface to NonLinearEffect.
Definition Joints.cpp:1583
size_t nbQ() const
Return the number of generalized coordinates (Q).
Definition Joints.cpp:105
size_t getDofIndex(const utils::String &SegmentName, const utils::String &dofName)
Return the index of a DoF in a segment.
Definition Joints.cpp:1858
size_t nbQddot() const
Return the number of generalized acceleration (Qddot).
Definition Joints.cpp:107
std::shared_ptr< size_t > m_nbQddot
The total number of Qddot.
Definition Joints.h:1371
rigidbody::GeneralizedAcceleration ForwardDynamics(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, const GeneralizedTorque &Tau)
Interface for the forward dynamics of RBDL.
Definition Joints.cpp:1607
utils::RotoTrans globalJCS(const GeneralizedCoordinates &Q, const utils::String &name, bool updateKin=true)
Return the joint coordinate system (JCS) for the segment in global reference frame at a given Q.
Definition Joints.cpp:360
utils::Vector3d CalcBodyToBaseCoordinates(const rigidbody::GeneralizedCoordinates &Q, utils::String segmentName, const utils::Vector3d &pointInLocal, bool updateKin=true)
Interface to call CalcBodyToBaseCoordinates. It is pretty much useless in Eigen, but fixes an issue i...
Definition Joints.cpp:390
std::vector< std::vector< size_t > > recursiveDofSubTrees(std::vector< std::vector< size_t > > subTrees, size_t idx)
Return the rbdl idx of subtrees of each segments.
Definition Joints.cpp:316
std::shared_ptr< size_t > m_nRotAQuat
The number of segments per quaternion.
Definition Joints.h:1373
size_t nbRoot() const
Return the dof on the root.
Definition Joints.cpp:108
Joints()
Construct a joint model.
Definition Joints.cpp:35
virtual ~Joints()
Properly destroy class.
Definition Joints.cpp:62
utils::Scalar TotalEnergy(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, bool updateKin=true)
Compute the Total Energy of System.
Definition Joints.cpp:1536
utils::Vector3d segmentAngularVelocity(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, size_t idx, bool updateKin=true)
Return the angular velocity of the segment.
Definition Joints.cpp:744
GeneralizedTorque computeDampedTau(const GeneralizedVelocity &Qdot) const
Get the joint dampings to apply to the dynamics.
Definition Joints.cpp:230
std::vector< utils::String > nameDof() const
Return the names of the degree of freedom (DoF).
Definition Joints.cpp:89
std::vector< utils::Matrix > meshPointsInMatrix(const GeneralizedCoordinates &Q, bool updateKin=true)
Return all the vertices of the mesh points in a matrix.
Definition Joints.cpp:1227
std::vector< std::vector< utils::Vector3d > > meshPoints(const GeneralizedCoordinates &Q, bool updateKin=true)
Return the vertices of the mesh for all segments in global reference frame.
Definition Joints.cpp:1197
utils::Vector3d CalcZeroMomentPoint(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, const rigidbody::GeneralizedAcceleration &Qddot, bool updateKin=true)
Compute the zero moment point on a contact surface.
Definition Joints.cpp:1038
utils::Matrix CoMJacobian(const GeneralizedCoordinates &Q, bool updateKin=true)
Return the jacobian matrix of the center of mass.
Definition Joints.cpp:976
std::vector< std::vector< MeshFace > > meshFaces() const
Return the mesh faces for all the segments.
Definition Joints.cpp:1258
Holder for the marker set.
Definition Markers.h:26
const NodeSegment & marker(size_t idx) const
Return a marker of index idx in the marker set.
Definition Markers.cpp:72
std::vector< utils::Matrix > markersJacobian(rigidbody::Joints &updatedModel, const GeneralizedCoordinates &Q, bool removeAxis=true)
Return the jacobian of the markers.
Definition Markers.cpp:538
size_t nbMarkers() const
Return the number of markers.
Definition Markers.cpp:518
A class that holds the geometry of a segment.
Definition Mesh.h:22
A point attached to a segment, generally speaking a skin marker.
Definition NodeSegment.h:20
bool isAxisKept(size_t i) const
Check if axis is kept.
int nbAxesToRemove() const
Return the number of axes to remove.
Characteristics of a segment, namely the mass, the center of mass, the inertia, the length and its me...
Description of a segment.
Definition Segment.h:29
bool isRotationAQuaternion() const
Return if the rotation DoF of this segment is a quaternion.
Definition Segment.cpp:188
size_t nbDofTrans() const
Return the number of translation DoF of the segment.
Definition Segment.cpp:206
size_t nbQddot() const
Return the number of generalized accelerations.
Definition Segment.cpp:210
size_t nbQ() const
Return the number of generalized coordinates.
Definition Segment.cpp:208
size_t nbQdot() const
Return the number of generalized velocities.
Definition Segment.cpp:209
size_t nbDof() const
Return the number of DoF of the segment.
Definition Segment.cpp:205
static void check(bool cond, const String &message)
Assert that raises the error message if false.
Definition Error.cpp:10
A wrapper for the Eigen::MatrixXd.
Definition Matrix3d.h:21
A wrapper for the Eigen::MatrixXd.
Definition Matrix.h:18
const String & parent() const
Return the parent name of the node.
Definition Node.cpp:45
void derivate(const Vector &w)
Return the time derivative of the quaterion.
A RotoTrans which is attached to a segment.
Homogenous matrix to describe translations and rotations simultaneously.
Definition RotoTrans.h:33
Rotation rot() const
Return the rotation matrix.
Vector3d trans() const
Return the translation vector.
Wrapper of the SpatialTransform (rotation and translation).
Wrapper of the Eigen::Matrix<double, 6, 1> or Casadi::MX(6, 1).
Wrapper around the std::string class with augmented functionality.
Definition String.h:18
Wrapper around Eigen Vector3d and attach it to a parent.
Definition Vector3d.h:27
Vector3d applyRT(const RotoTrans &rt) const
Apply a RotoTrans to the 3D vector.
Definition Vector3d.cpp:79
Wrapper of the Eigen VectorXd.
Definition Vector.h:21