1#ifndef BIORBD_RIGIDBODY_JOINTS_H
2#define BIORBD_RIGIDBODY_JOINTS_H
4#include "biorbdConfig.h"
10#include <rbdl/Constraints.h>
12#include "Utils/Scalar.h"
14namespace BIORBD_NAMESPACE {
46class BIORBD_API
Joints :
public RigidBodyDynamics::Model
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,
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,
197 std::vector<std::vector<size_t>> subTrees,
217 size_t nbDof()
const;
233 std::vector<utils::String>
nameDof()
const;
316 std::vector<Segment>&
segments()
const;
335#ifdef BIORBD_USE_CASADI_MATH
356 bool updateKin =
true);
369 bool updateKin =
true);
387 std::vector<utils::RotoTrans>
localJCS()
const;
419 bool updateKin =
true);
435 bool updateKin =
true);
453 bool updateKin =
true);
471 bool updateKin =
true);
489 bool updateKin =
true);
507 bool updateKin =
true);
527 bool updateKin =
true);
547 bool updateKin =
true);
564 bool updateKin =
true);
581 bool updateKin =
true);
597 bool updateKin =
true);
614 const std::vector<NodeSegment>& v,
615 bool updateKin =
true);
676 const std::vector<NodeSegment>& v,
685 utils::Scalar
mass()
const;
730 bool updateKin =
true);
744 bool updateKin =
true);
762 bool updateKin =
true);
772 bool updateKin =
true);
783 bool updateKin =
true);
795 bool updateKin =
true);
807 bool updateKin =
true);
821 bool updateKin =
true);
833 bool updateKin =
true);
847 bool updateKin =
true);
861 bool updateKin =
true);
877 bool updateKin =
true);
887 bool updateKin =
true);
898 std::vector<std::vector<utils::Vector3d>>
meshPoints(
900 bool updateKin =
true);
912 bool updateKin =
true);
922 bool updateKin =
true);
928 std::vector<std::vector<MeshFace>>
meshFaces()
const;
935 const std::vector<MeshFace>&
meshFaces(
size_t idx)
const;
941 std::vector<Mesh>
mesh()
const;
969 bool updateKin =
true);
979 bool updateKin =
true);
989 bool updateKin =
true);
1063 bool updateKin =
true);
1105 const utils::Scalar& k_stab = 1);
1119 bool updateKin =
true);
1132 bool updateKin =
true);
1143 bool updateKin =
true);
1156 bool updateKin =
true);
1169 bool updateKin =
true);
1269 bool updateKin =
true);
1284 bool updateKin =
true);
1299 bool updateKin =
true);
1316 bool updateKin =
true);
1352 bool updateKin =
true);
1372 std::shared_ptr<size_t>
1374 std::shared_ptr<bool>
1376 std::shared_ptr<utils::Scalar>
1389 const size_t segmentIdx,
1390 bool updateKin =
true);
1399 const std::vector<utils::RotoTrans>& RT,
An External force set that can apply forces to the model while computing the dynamics.
Class GeneralizedAcceleration.
Class GeneralizedCoordinates.
Class GeneralizedVelocity.
This is the core of the musculoskeletal model in biorbd.
void setGravity(const utils::Vector3d &newGravity)
Set the gravity.
size_t nbQdot() const
Return the number of generalized velocities (Qdot).
std::shared_ptr< std::vector< Segment > > m_segments
All the articulations.
void overrideMeshFolders(const utils::String &folder)
Set the folder for all the meshes.
rigidbody::GeneralizedAcceleration ForwardDynamicsConstraintsDirect(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, const GeneralizedTorque &Tau, bool updateKin=true)
Interface for the forward dynamics with contact of RBDL.
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.
utils::Scalar PotentialEnergy(const rigidbody::GeneralizedCoordinates &Q, bool updateKin=true)
Interface for the Potential Energy of RBDL.
size_t nbQuat() const
Return the number of segments that are described using quaternions.
int getBodyRbdlIdToBiorbdId(const int idx) const
Return the Biorbd body identification from rbdl.
utils::Vector3d CalcAngularMomentum(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, bool updateKin)
Calculate the angular momentum of the center of mass.
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,...
std::shared_ptr< size_t > m_nbRoot
The number of DoF on the root segment.
utils::Matrix massMatrix(const GeneralizedCoordinates &Q, bool updateKin=true)
Get the mass matrix at a given position Q.
std::vector< NodeSegment > CoMbySegment(const GeneralizedCoordinates &Q, bool updateKin=true)
Return the position of the center of mass of each segment.
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.
rigidbody::GeneralizedAcceleration ForwardDynamicsFreeFloatingBase(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, const GeneralizedAcceleration &QddotJoints)
Biorbd's implementation of forward dynamics with a free floating base.
std::vector< std::vector< size_t > > getDofSubTrees()
Return the rbdl idx of subtrees of each segments.
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...
std::vector< Mesh > mesh() const
Return the segment mesh.
utils::Vector3d CoMddot(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, const rigidbody::GeneralizedAcceleration &Qddot, bool updateKin=true)
Return the acceleration of the center of mass.
size_t nbSegment() const
Return the actual number of segment.
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.
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.
int getBodyRbdlId(const utils::String &segmentName) const
Return the rbdl body identification.
GeneralizedTorque InverseDynamics(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, const rigidbody::GeneralizedAcceleration &Qddot)
Interface for the inverse dynamics of RBDL.
std::shared_ptr< size_t > m_nbQdot
The total number of Qdot.
utils::Matrix projectPointJacobian(const GeneralizedCoordinates &Q, NodeSegment node, bool updateKin)
Return the jacobian matrix of the projected markers for a marker from the model.
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...
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.
std::vector< Segment > & segments() const
Get all the segments.
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...
utils::Vector3d CoMdot(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, bool updateKin=true)
Return the velocity of the center of mass.
std::shared_ptr< size_t > m_nbDof
The total number of degrees of freedom.
std::shared_ptr< utils::Scalar > m_totalMass
Mass of all the bodies combined.
size_t nbDof() const
Return the number of degrees of freedom (DoF).
size_t getBodyBiorbdIdToRbdlId(const int idx) const
Return the Rbdl body identification from Biorbd.
std::vector< utils::RotoTrans > localJCS() const
Return all the joint coordinate system (JCS) in its parent reference frame.
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...
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.
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).
utils::Scalar Lagrangian(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, bool updateKin=true)
Compute the Lagrangian of the system.
utils::Scalar mass() const
Return the total mass of the model.
Joints DeepCopy() const
Deep copy of the joints.
utils::Scalar KineticEnergy(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, bool updateKin=true)
Interface for the Kinetic Energy of RBDL.
std::shared_ptr< size_t > m_nbQ
The total number of Q.
size_t nbGeneralizedTorque() const
Return the number of generalized torque.
Segment & segment(size_t idx)
Get a segment of index idx.
utils::Matrix massMatrixInverse(const rigidbody::GeneralizedCoordinates &Q, bool updateKin=true)
Get the inverse mass matrix at a given position Q.
utils::Vector3d CoM(const GeneralizedCoordinates &Q, bool updateKin=true)
Return the position of the center of mass.
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...
utils::Vector3d bodyAngularVelocity(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, bool updateKin=true)
Calculate the angular velocity of the model around its center of mass.
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.
std::shared_ptr< bool > m_isKinematicsComputed
If the kinematics are computed.
int getBodyBiorbdId(const utils::String &segmentName) const
Return the biorbd body identification.
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.
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.
utils::Vector3d getGravity() const
Get the current gravity.
utils::Vector ContactForcesFromForwardDynamicsConstraintsDirect(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, const GeneralizedTorque &Tau)
Interface for contacts of the forward dynamics with contact of RBDL.
utils::Matrix CoMbySegmentInMatrix(const GeneralizedCoordinates &Q, bool updateKin=true)
Return the position of the center of mass of each segment in a matrix.
void updateSegmentCharacteristics(size_t idx, const SegmentCharacteristics &characteristics)
updateSegmentCharacteristics Change the inertia characteristics of the segment idx
GeneralizedVelocity ComputeConstraintImpulsesDirect(const GeneralizedCoordinates &Q, const GeneralizedVelocity &QdotPre)
Compute the Qdot post from an impact.
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...
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...
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 ...
GeneralizedTorque NonLinearEffect(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot)
Interface to NonLinearEffect.
size_t nbQ() const
Return the number of generalized coordinates (Q).
size_t getDofIndex(const utils::String &SegmentName, const utils::String &dofName)
Return the index of a DoF in a segment.
size_t nbQddot() const
Return the number of generalized acceleration (Qddot).
std::shared_ptr< size_t > m_nbQddot
The total number of Qddot.
rigidbody::GeneralizedAcceleration ForwardDynamics(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, const GeneralizedTorque &Tau)
Interface for the forward dynamics of RBDL.
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.
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...
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.
std::shared_ptr< size_t > m_nRotAQuat
The number of segments per quaternion.
size_t nbRoot() const
Return the dof on the root.
Joints()
Construct a joint model.
utils::Scalar TotalEnergy(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, bool updateKin=true)
Compute the Total Energy of System.
utils::Vector3d segmentAngularVelocity(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, size_t idx, bool updateKin=true)
Return the angular velocity of the segment.
GeneralizedTorque computeDampedTau(const GeneralizedVelocity &Qdot) const
Get the joint dampings to apply to the dynamics.
std::vector< utils::String > nameDof() const
Return the names of the degree of freedom (DoF).
std::vector< utils::Matrix > meshPointsInMatrix(const GeneralizedCoordinates &Q, bool updateKin=true)
Return all the vertices of the mesh points in a matrix.
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.
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.
utils::Matrix CoMJacobian(const GeneralizedCoordinates &Q, bool updateKin=true)
Return the jacobian matrix of the center of mass.
std::vector< std::vector< MeshFace > > meshFaces() const
Return the mesh faces for all the segments.
A class that holds the geometry of a segment.
A point attached to a segment, generally speaking a skin marker.
Characteristics of a segment, namely the mass, the center of mass, the inertia, the length and its me...
Description of a segment.
A wrapper for the Eigen::MatrixXd.
A wrapper for the Eigen::MatrixXd.
Homogenous matrix to describe translations and rotations simultaneously.
Wrapper of the Eigen::Matrix<double, 6, 1> or Casadi::MX(6, 1).
Wrapper around the std::string class with augmented functionality.
Wrapper around Eigen Vector3d and attach it to a parent.
Wrapper of the Eigen VectorXd.