Biorbd Release_1.13.0
Loading...
Searching...
No Matches
Joints.h
1#ifndef BIORBD_RIGIDBODY_JOINTS_H
2#define BIORBD_RIGIDBODY_JOINTS_H
3
4#include "biorbdConfig.h"
5
6#include <memory>
7
8#include <rbdl/Model.h>
9
10#include <rbdl/Constraints.h>
11
12#include "Utils/Scalar.h"
13
14namespace BIORBD_NAMESPACE {
15namespace utils {
16class String;
17class RotoTrans;
18class Matrix;
19class Matrix3d;
20class Vector;
21class Vector3d;
22class Range;
23class SpatialVector;
25} // namespace utils
26
27namespace rigidbody {
33class NodeSegment;
34class MeshFace;
35class Segment;
37class Mesh;
38class Contacts;
39
43#ifdef SWIG
44class BIORBD_API Joints
45#else
46class BIORBD_API Joints : public RigidBodyDynamics::Model
47#endif
48{
49 public:
53 Joints();
54
59 Joints(const Joints& other);
60
64 virtual ~Joints();
65
70 Joints DeepCopy() const;
71
76 void DeepCopy(const Joints& other);
77
98 size_t AddSegment(
99 const utils::String& segmentName,
100 const utils::String& parentName,
101 const utils::String& translationSequence,
102 const utils::String& rotationSequence,
103 const std::vector<utils::Range>& QRanges,
104 const std::vector<utils::Range>& QdotRanges,
105 const std::vector<utils::Range>& QddotRanges,
106 const std::vector<utils::Scalar>& jointDampings,
107 const SegmentCharacteristics& characteristics,
108 const utils::RotoTrans& referenceFrame);
109
129 size_t AddSegment(
130 const utils::String& segmentName,
131 const utils::String& parentName,
132 const utils::String& translationSequence,
133 const std::vector<utils::Range>& QRanges,
134 const std::vector<utils::Range>& QdotRanges,
135 const std::vector<utils::Range>& QddotRanges,
136 const std::vector<utils::Scalar>& jointDampings,
137 const SegmentCharacteristics& characteristics,
138 const utils::RotoTrans& referenceFrame);
139
140 // -- GENERAL MODELLING -- //
146
151 void setGravity(const utils::Vector3d& newGravity);
152
153 // -- INFORMATION ON THE MODEL -- //
159 int getBodyBiorbdId(const utils::String& segmentName) const;
160
166 int getBodyRbdlId(const utils::String& segmentName) const;
167
173 int getBodyRbdlIdToBiorbdId(const int idx) const;
174
180 size_t getBodyBiorbdIdToRbdlId(const int idx) const;
181
186 std::vector<std::vector<size_t>> getDofSubTrees();
187
188 protected:
196 std::vector<std::vector<size_t>> recursiveDofSubTrees(
197 std::vector<std::vector<size_t>> subTrees,
198 size_t idx);
199
200 public:
205 size_t nbGeneralizedTorque() const;
206
211 size_t nbSegment() const;
212
217 size_t nbDof() const;
218
225 size_t getDofIndex(
226 const utils::String& SegmentName,
227 const utils::String& dofName);
228
233 std::vector<utils::String> nameDof() const;
234
239 size_t nbQ() const;
240
245 size_t nbQdot() const;
246
251 size_t nbQddot() const;
252
257 size_t nbRoot() const;
258
263 size_t nbQuat() const;
264
281 size_t idx,
282 const SegmentCharacteristics& characteristics);
283
289 Segment& segment(size_t idx);
290
296 Segment& segment(const utils::String& name);
297
303 const Segment& segment(size_t idx) const;
304
310 const Segment& segment(const utils::String& name) const;
311
316 std::vector<Segment>& segments() const;
317
324
325 public:
335#ifdef BIORBD_USE_CASADI_MATH
336 Joints
337#else
338 Joints&
339#endif
341 const GeneralizedCoordinates* Q = nullptr,
342 const GeneralizedVelocity* Qdot = nullptr,
343 const rigidbody::GeneralizedAcceleration* Qddot = nullptr);
344
345 // -- POSITION INTERFACE OF THE MODEL -- //
346
354 std::vector<utils::RotoTrans> allGlobalJCS(
355 const GeneralizedCoordinates& Q,
356 bool updateKin = true);
357
367 const GeneralizedCoordinates& Q,
368 const utils::String& name,
369 bool updateKin = true);
370
380 globalJCS(const GeneralizedCoordinates& Q, size_t idx, bool updateKin = true);
381
387 std::vector<utils::RotoTrans> localJCS() const;
388
395 utils::RotoTrans localJCS(const utils::String& name) const;
396
403 utils::RotoTrans localJCS(const size_t idx) const;
404
417 utils::String segmentName,
418 const utils::Vector3d& pointInLocal,
419 bool updateKin = true);
420
433 unsigned int bodyId,
434 const utils::Vector3d& pointInLocal,
435 bool updateKin = true);
436
451 utils::String segmentName,
452 const utils::Vector3d& pointInLocal,
453 bool updateKin = true);
454
469 unsigned int bodyId,
470 const utils::Vector3d& pointInLocal,
471 bool updateKin = true);
472
487 utils::String segmentName,
488 const utils::Vector3d& pointInLocal,
489 bool updateKin = true);
490
505 unsigned int bodyId,
506 const utils::Vector3d& pointInLocal,
507 bool updateKin = true);
508
525 utils::String segmentName,
526 const utils::Vector3d& pointInLocal,
527 bool updateKin = true);
528
545 unsigned int bodyId,
546 const utils::Vector3d& pointInLocal,
547 bool updateKin = true);
548
562 utils::String segmentName,
563 const utils::Vector3d& pointInLocal,
564 bool updateKin = true);
565
579 unsigned int bodyId,
580 const utils::Vector3d& pointInLocal,
581 bool updateKin = true);
582
593 const GeneralizedCoordinates& Q,
594 const utils::Vector3d& v,
595 int segmentIdx,
596 const utils::String& axesToRemove,
597 bool updateKin = true);
598
612 std::vector<NodeSegment> projectPoint(
613 const GeneralizedCoordinates& Q,
614 const std::vector<NodeSegment>& v,
615 bool updateKin = true);
616
627 const GeneralizedCoordinates& Q,
628 const NodeSegment& n,
629 bool updateKin);
630
640 const GeneralizedCoordinates& Q,
641 NodeSegment node,
642 bool updateKin);
643
656 const GeneralizedCoordinates& Q,
657 const utils::Vector3d& v,
658 int segmentIdx,
659 const utils::String& axesToRemove,
660 bool updateKin);
661
674 std::vector<utils::Matrix> projectPointJacobian(
675 const GeneralizedCoordinates& Q,
676 const std::vector<NodeSegment>& v,
677 bool updateKin);
678 // ------------------------------------- //
679
680 // -- MASS RELATED STUFF -- //
685 utils::Scalar mass() const;
686
693 utils::Vector3d CoM(const GeneralizedCoordinates& Q, bool updateKin = true);
694
720 void CalcCenterOfMass(
724 utils::Scalar& mass,
725 utils::Vector3d& com,
726 utils::Vector3d* comVelocity = NULL,
727 utils::Vector3d* comAcceleration = NULL,
729 utils::Vector3d* changeOfAngularMomentum = NULL,
730 bool updateKin = true);
731
744 bool updateKin = true);
745
760 const utils::Vector3d& normal,
761 const utils::Vector3d& point,
762 bool updateKin = true);
763
770 std::vector<NodeSegment> CoMbySegment(
771 const GeneralizedCoordinates& Q,
772 bool updateKin = true);
773
782 const GeneralizedCoordinates& Q,
783 bool updateKin = true);
784
793 const GeneralizedCoordinates& Q,
794 const size_t idx,
795 bool updateKin = true);
796
805 const GeneralizedCoordinates& Q,
806 const GeneralizedVelocity& Qdot,
807 bool updateKin = true);
808
818 const GeneralizedCoordinates& Q,
819 const GeneralizedVelocity& Qdot,
821 bool updateKin = true);
822
830 std::vector<utils::Vector3d> CoMdotBySegment(
831 const GeneralizedCoordinates& Q,
832 const GeneralizedVelocity& Qdot,
833 bool updateKin = true);
834
844 const GeneralizedCoordinates& Q,
845 const GeneralizedVelocity& Qdot,
846 const size_t idx,
847 bool updateKin = true);
848
857 std::vector<utils::Vector3d> CoMddotBySegment(
858 const GeneralizedCoordinates& Q,
859 const GeneralizedVelocity& Qdot,
861 bool updateKin = true);
862
873 const GeneralizedCoordinates& Q,
874 const GeneralizedVelocity& Qdot,
876 const size_t idx,
877 bool updateKin = true);
878
886 const GeneralizedCoordinates& Q,
887 bool updateKin = true);
888 // ------------------------ //
889
890 // -- MESH OF THE MODEL -- //
898 std::vector<std::vector<utils::Vector3d>> meshPoints(
899 const GeneralizedCoordinates& Q,
900 bool updateKin = true);
901
909 std::vector<utils::Vector3d> meshPoints(
910 const GeneralizedCoordinates& Q,
911 size_t idx,
912 bool updateKin = true);
913
920 std::vector<utils::Matrix> meshPointsInMatrix(
921 const GeneralizedCoordinates& Q,
922 bool updateKin = true);
923
928 std::vector<std::vector<MeshFace>> meshFaces() const;
929
935 const std::vector<MeshFace>& meshFaces(size_t idx) const;
936
941 std::vector<Mesh> mesh() const;
942
948 const Mesh& mesh(size_t idx) const;
949
954 void overrideMeshFolders(const utils::String& folder);
955 // ----------------------- //
956
957 // -- ANGULAR MOMENTUM FUNCTIONS -- //
967 const GeneralizedCoordinates& Q,
968 const GeneralizedVelocity& Qdot,
969 bool updateKin = true); // Wrapper pour le moment angulaire
970
978 const GeneralizedCoordinates& Q,
979 bool updateKin = true);
980
989 bool updateKin = true);
990
999 const GeneralizedCoordinates& Q,
1000 const GeneralizedVelocity& Qdot,
1001 bool updateKin);
1002
1012 const GeneralizedCoordinates& Q,
1013 const GeneralizedVelocity& Qdot,
1015 bool updateKin);
1016
1028 std::vector<utils::Vector3d> CalcSegmentsAngularMomentum(
1029 const GeneralizedCoordinates& Q,
1030 const GeneralizedVelocity& Qdot,
1031 bool updateKin);
1032
1045 std::vector<utils::Vector3d> CalcSegmentsAngularMomentum(
1046 const GeneralizedCoordinates& Q,
1047 const GeneralizedVelocity& Qdot,
1049 bool updateKin);
1050
1061 const GeneralizedCoordinates& Q,
1062 const GeneralizedVelocity& Qdot,
1063 bool updateKin = true);
1064 // -------------------------------- //
1065
1074 void CalcMatRotJacobian(
1075 const GeneralizedCoordinates& Q,
1076 size_t segmentIdx,
1077 const utils::Matrix3d& rotation,
1078 utils::Matrix& G,
1079 bool updateKin);
1080
1091 size_t segmentIdx,
1092 bool updateKin);
1093
1103 const GeneralizedCoordinates& Q,
1104 const GeneralizedCoordinates& Qdot,
1105 const utils::Scalar& k_stab = 1);
1106
1116 const GeneralizedCoordinates& Q,
1117 const GeneralizedVelocity& Qdot,
1118 size_t idx,
1119 bool updateKin = true);
1120
1129 utils::Scalar KineticEnergy(
1132 bool updateKin = true);
1133
1141 utils::Scalar PotentialEnergy(
1143 bool updateKin = true);
1144
1153 utils::Scalar Lagrangian(
1156 bool updateKin = true);
1157
1166 utils::Scalar TotalEnergy(
1169 bool updateKin = true);
1170
1171 // ---- DYNAMIC INTERFACE ---- //
1172
1181 const GeneralizedCoordinates& Q,
1182 const GeneralizedVelocity& Qdot,
1193 const GeneralizedCoordinates& Q,
1194 const GeneralizedVelocity& Qdot,
1196 rigidbody::ExternalForceSet& externalForces);
1197
1205 const GeneralizedCoordinates& Q,
1206 const GeneralizedVelocity& Qdot);
1215 const GeneralizedCoordinates& Q,
1216 const GeneralizedVelocity& Qdot,
1217 rigidbody::ExternalForceSet& externalForces);
1218
1227 const GeneralizedCoordinates& Q,
1228 const GeneralizedVelocity& Qdot,
1229 const GeneralizedTorque& Tau);
1239 const GeneralizedCoordinates& Q,
1240 const GeneralizedVelocity& Qdot,
1241 const GeneralizedTorque& Tau,
1242 rigidbody::ExternalForceSet& externalForces);
1243
1253 const GeneralizedCoordinates& Q,
1254 const GeneralizedVelocity& Qdot,
1255 const GeneralizedAcceleration& QddotJoints);
1256
1266 const GeneralizedCoordinates& Q,
1267 const GeneralizedVelocity& Qdot,
1268 const GeneralizedTorque& Tau,
1269 bool updateKin = true);
1280 const GeneralizedCoordinates& Q,
1281 const GeneralizedVelocity& Qdot,
1282 const GeneralizedTorque& Tau,
1283 rigidbody::ExternalForceSet& externalForces,
1284 bool updateKin = true);
1295 const GeneralizedCoordinates& Q,
1296 const GeneralizedVelocity& Qdot,
1297 const GeneralizedTorque& Tau,
1298 Contacts& CS,
1299 bool updateKin = true);
1311 const GeneralizedCoordinates& Q,
1312 const GeneralizedVelocity& Qdot,
1313 const GeneralizedTorque& Tau,
1314 Contacts& CS,
1315 rigidbody::ExternalForceSet& externalForces,
1316 bool updateKin = true);
1317
1326 const GeneralizedCoordinates& Q,
1327 const GeneralizedVelocity& Qdot,
1328 const GeneralizedTorque& Tau);
1338 const GeneralizedCoordinates& Q,
1339 const GeneralizedVelocity& Qdot,
1340 const GeneralizedTorque& Tau,
1341 rigidbody::ExternalForceSet& externalForces);
1342
1352 bool updateKin = true);
1353
1361 const GeneralizedCoordinates& Q,
1362 const GeneralizedVelocity& QdotPre);
1363
1364 protected:
1365 std::shared_ptr<std::vector<Segment>> m_segments;
1366
1367 std::shared_ptr<size_t> m_nbRoot;
1368 std::shared_ptr<size_t> m_nbDof;
1369 std::shared_ptr<size_t> m_nbQ;
1370 std::shared_ptr<size_t> m_nbQdot;
1371 std::shared_ptr<size_t> m_nbQddot;
1372 std::shared_ptr<size_t>
1374 std::shared_ptr<bool>
1376 std::shared_ptr<utils::Scalar>
1378
1388 const GeneralizedCoordinates& Q,
1389 const size_t segmentIdx,
1390 bool updateKin = true);
1391
1398 std::vector<utils::Vector3d> meshPoints(
1399 const std::vector<utils::RotoTrans>& RT,
1400 size_t idx) const;
1401
1402 public:
1412 const GeneralizedCoordinates* Q = nullptr,
1413 const GeneralizedVelocity* Qdot = nullptr,
1414 const rigidbody::GeneralizedAcceleration* Qddot = nullptr,
1415 const GeneralizedTorque* torque = nullptr);
1416};
1417
1418} // namespace rigidbody
1419} // namespace BIORBD_NAMESPACE
1420
1421#endif // BIORBD_RIGIDBODY_JOINTS_H
An External force set that can apply forces to the model while computing the dynamics.
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
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
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
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
A wrapper for the Eigen::MatrixXd.
Definition Matrix3d.h:21
A wrapper for the Eigen::MatrixXd.
Definition Matrix.h:18
Homogenous matrix to describe translations and rotations simultaneously.
Definition RotoTrans.h:33
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
Wrapper of the Eigen VectorXd.
Definition Vector.h:21