Biorbd
|
This is the core of the musculoskeletal model in biorbd. More...
#include <Joints.h>
Public Member Functions | |
Joints () | |
Construct a joint model. | |
Joints (const biorbd::rigidbody::Joints &other) | |
Construct a joint model from another model. More... | |
virtual | ~Joints () |
Properly destroy class. | |
biorbd::rigidbody::Joints | DeepCopy () const |
Deep copy of the joints. More... | |
void | DeepCopy (const biorbd::rigidbody::Joints &other) |
Deep copy of the joints. More... | |
unsigned int | AddSegment (const biorbd::utils::String &segmentName, const biorbd::utils::String &parentName, const biorbd::utils::String &translationSequence, const biorbd::utils::String &rotationSequence, const std::vector< biorbd::utils::Range > &QRanges, const std::vector< biorbd::utils::Range > &QDotRanges, const std::vector< biorbd::utils::Range > &QDDotRanges, const biorbd::rigidbody::SegmentCharacteristics &characteristics, const RigidBodyDynamics::Math::SpatialTransform ¢reOfRotation, int forcePlates=-1) |
Add a segment to the model. More... | |
unsigned int | AddSegment (const biorbd::utils::String &segmentName, const biorbd::utils::String &parentName, const biorbd::utils::String &translationSequence, const std::vector< biorbd::utils::Range > &QRanges, const std::vector< biorbd::utils::Range > &QDotRanges, const std::vector< biorbd::utils::Range > &QDDotRanges, const biorbd::rigidbody::SegmentCharacteristics &characteristics, const RigidBodyDynamics::Math::SpatialTransform ¢reOfRotation, int forcePlates=-1) |
Add a segment to the model. More... | |
biorbd::utils::Vector3d | getGravity () const |
Get the current gravity. More... | |
void | setGravity (const biorbd::utils::Vector3d &newGravity) |
Set the gravity. More... | |
int | GetBodyBiorbdId (const biorbd::utils::String &segmentName) const |
Return the biorbd body identification. More... | |
unsigned int | nbGeneralizedTorque () const |
Return the number of generalized torque. More... | |
unsigned int | nbSegment () const |
Return the actual number of segment. More... | |
unsigned int | nbDof () const |
Return the number of degrees of freedom (DoF) More... | |
unsigned int | getDofIndex (const biorbd::utils::String &SegmentName, const biorbd::utils::String &dofName) |
Return the index of a DoF in a segment. More... | |
std::vector< biorbd::utils::String > | nameDof () const |
Return the names of the degree of freedom (DoF) More... | |
unsigned int | nbQ () const |
Return the number of generalized coordinates (Q) More... | |
unsigned int | nbQdot () const |
Return the number of generalized velocities (Qdot) More... | |
unsigned int | nbQddot () const |
Return the number of generalized acceleration (Qddot) More... | |
unsigned int | nbRoot () const |
Return the dof on the root. More... | |
unsigned int | nbQuat () const |
Return the number of segments that are described using quaternions. More... | |
const biorbd::rigidbody::Segment & | segment (unsigned int idx) const |
Get a segment of index idx. More... | |
const biorbd::rigidbody::Segment & | segment (const biorbd::utils::String &name) const |
Get a segment of a specific name. More... | |
std::vector< RigidBodyDynamics::Math::SpatialVector > | dispatchedForce (std::vector< std::vector< biorbd::utils::SpatialVector >> &spatialVector, unsigned int frame) const |
Dispatch the forces from the force plate in a vector. More... | |
std::vector< RigidBodyDynamics::Math::SpatialVector > | dispatchedForce (std::vector< biorbd::utils::SpatialVector > &sv) const |
Dispatch the forces from the force plate in a spatial vector. More... | |
void | UpdateKinematicsCustom (const biorbd::rigidbody::GeneralizedCoordinates *Q=nullptr, const biorbd::rigidbody::GeneralizedVelocity *Qdot=nullptr, const biorbd::rigidbody::GeneralizedAcceleration *Qddot=nullptr) |
Update the kinematic variables such as body velocities and accelerations in the model to reflect the variables passed to this function. More... | |
std::vector< biorbd::utils::RotoTrans > | allGlobalJCS (const biorbd::rigidbody::GeneralizedCoordinates &Q) |
Return the joint coordinate system (JCS) in global reference frame at a given Q. More... | |
std::vector< biorbd::utils::RotoTrans > | allGlobalJCS () const |
Return the joint coordinate system (JCS) in global reference frame at a given Q. More... | |
biorbd::utils::RotoTrans | globalJCS (const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::utils::String &name) |
Return the joint coordinate system (JCS) for the segment in global reference frame at a given Q. More... | |
biorbd::utils::RotoTrans | globalJCS (const biorbd::rigidbody::GeneralizedCoordinates &Q, unsigned int idx) |
Return the joint coordinate system (JCS) for the segment idx in global reference frame at a given Q. More... | |
biorbd::utils::RotoTrans | globalJCS (const biorbd::utils::String &name) const |
Return the joint coordinate system (JCS) for the segment in global reference. More... | |
biorbd::utils::RotoTrans | globalJCS (unsigned int idx) const |
Return the joint coordinate system (JCS) for the segment idx in global reference. More... | |
std::vector< biorbd::utils::RotoTrans > | localJCS () const |
Return all the joint coordinate system (JCS) in its parent reference frame. More... | |
biorbd::utils::RotoTrans | localJCS (const biorbd::utils::String &name) const |
Return the joint coordinate system (JCS) of the segment its parent reference frame. More... | |
biorbd::utils::RotoTrans | localJCS (const unsigned int idx) const |
Return the joint coordinate system (JCS) of the segment idx its parent reference frame. More... | |
biorbd::rigidbody::NodeSegment | projectPoint (const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::utils::Vector3d &v, int segmentIdx, const biorbd::utils::String &axesToRemove, bool updateKin=true) |
Project a point on specific axis of a segment. More... | |
std::vector< biorbd::rigidbody::NodeSegment > | projectPoint (const biorbd::rigidbody::GeneralizedCoordinates &Q, const std::vector< biorbd::rigidbody::NodeSegment > &v, bool updateKin=true) |
Project multiples points on their respective segment. More... | |
biorbd::rigidbody::NodeSegment | projectPoint (const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::NodeSegment &n, bool updateKin) |
Return the projected markers from a point corresponding to a marker from the model. More... | |
biorbd::utils::Matrix | projectPointJacobian (const biorbd::rigidbody::GeneralizedCoordinates &Q, biorbd::rigidbody::NodeSegment p, bool updateKin) |
Return the jacobian matrix of the projected markers for a marker from the model. More... | |
biorbd::utils::Matrix | projectPointJacobian (const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::utils::Vector3d &v, int segmentIdx, const biorbd::utils::String &axesToRemove, bool updateKin) |
Return the Jacobian matrix of a projected marker on the segment segmentIdx. More... | |
std::vector< biorbd::utils::Matrix > | projectPointJacobian (const biorbd::rigidbody::GeneralizedCoordinates &Q, const std::vector< biorbd::rigidbody::NodeSegment > &v, bool updateKin) |
Return the jacobian matrix of the projected markers. More... | |
double | mass () const |
Return the total mass of the model. More... | |
biorbd::utils::Vector3d | CoM (const biorbd::rigidbody::GeneralizedCoordinates &Q, bool updateKin=true) |
Return the position of the center of mass. More... | |
std::vector< biorbd::rigidbody::NodeSegment > | CoMbySegment (const biorbd::rigidbody::GeneralizedCoordinates &Q, bool updateKin=true) |
Return the position of the center of mass of each segment. More... | |
biorbd::utils::Matrix | CoMbySegmentInMatrix (const biorbd::rigidbody::GeneralizedCoordinates &Q, bool updateKin=true) |
Return the position of the center of mass of each segment in a matrix. More... | |
biorbd::utils::Vector3d | CoMbySegment (const biorbd::rigidbody::GeneralizedCoordinates &Q, const unsigned int idx, bool updateKin=true) |
Return the position of the center of mass of segment idx. More... | |
biorbd::utils::Vector3d | CoMdot (const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot) |
Return the velocity of the center of mass. More... | |
biorbd::utils::Vector3d | CoMddot (const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, const biorbd::rigidbody::GeneralizedAcceleration &Qddot) |
Return the acceleration of the center of mass. More... | |
std::vector< biorbd::utils::Vector3d > | CoMdotBySegment (const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, bool updateKin=true) |
Return the velocity of the center of mass of each segment. More... | |
biorbd::utils::Vector3d | CoMdotBySegment (const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, const unsigned int idx, bool updateKin=true) |
Return the velocity of the center of mass of segment idx. More... | |
std::vector< biorbd::utils::Vector3d > | CoMddotBySegment (const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, const biorbd::rigidbody::GeneralizedAcceleration &Qddot, bool updateKin=true) |
Return the acceleration of the center of mass of each segment. More... | |
biorbd::utils::Vector3d | CoMddotBySegment (const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, const biorbd::rigidbody::GeneralizedAcceleration &Qddot, const unsigned int idx, bool updateKin=true) |
Return the acceleration of the center of mass of segment idx. More... | |
biorbd::utils::Matrix | CoMJacobian (const biorbd::rigidbody::GeneralizedCoordinates &Q) |
Return the jacobian matrix of the center of mass. More... | |
std::vector< std::vector< biorbd::utils::Vector3d > > | meshPoints (const biorbd::rigidbody::GeneralizedCoordinates &Q, bool updateKin=true) |
Return the vertices of the mesh for all segments in global reference frame. More... | |
std::vector< biorbd::utils::Vector3d > | meshPoints (const biorbd::rigidbody::GeneralizedCoordinates &Q, unsigned int idx, bool updateKin=true) |
Return the vertices of the mesh for the segment idx. More... | |
std::vector< biorbd::utils::Matrix > | meshPointsInMatrix (const biorbd::rigidbody::GeneralizedCoordinates &Q, bool updateKin=true) |
Return all the vertices of the mesh points in a matrix. More... | |
std::vector< std::vector< MeshFace > > | meshFaces () const |
Return the mesh faces for all the segments. More... | |
const std::vector< biorbd::rigidbody::MeshFace > & | meshFaces (unsigned int idx) const |
Return the mesh faces for segment idx. More... | |
std::vector< biorbd::rigidbody::Mesh > | mesh () const |
Return the segment mesh. More... | |
const biorbd::rigidbody::Mesh & | mesh (unsigned int idx) const |
Return the segment mesh for segment idx. More... | |
biorbd::utils::Vector3d | angularMomentum (const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, bool updateKin=true) |
Calculate the angular momentum of the center of mass. More... | |
biorbd::utils::Matrix | massMatrix (const biorbd::rigidbody::GeneralizedCoordinates &Q, bool updateKin=true) |
Get the mass matrix at a given position Q. More... | |
biorbd::utils::Vector3d | CalcAngularMomentum (const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, bool updateKin) |
Calculate the angular momentum of the center of mass. More... | |
biorbd::utils::Vector3d | CalcAngularMomentum (const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, const biorbd::rigidbody::GeneralizedAcceleration &Qddot, bool updateKin) |
Calculate the angular momentum of the center of mass. More... | |
std::vector< biorbd::utils::Vector3d > | CalcSegmentsAngularMomentum (const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, bool updateKin) |
Calculate the segment center of mass angular momentum. More... | |
std::vector< biorbd::utils::Vector3d > | CalcSegmentsAngularMomentum (const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, const biorbd::rigidbody::GeneralizedAcceleration &Qddot, bool updateKin) |
Calculate the segment center of mass angular momentum. More... | |
void | CalcMatRotJacobian (const biorbd::rigidbody::GeneralizedCoordinates &Q, unsigned int segmentIdx, const RigidBodyDynamics::Math::Matrix3d &rotation, RigidBodyDynamics::Math::MatrixNd &G, bool updateKin) |
Calculate the jacobian matrix of a rotation matrix. More... | |
biorbd::rigidbody::GeneralizedVelocity | computeQdot (const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedCoordinates &QDot, const double k_stab=1) |
Return the derivate of Q in function of Qdot (if not Quaternion, Qdot is directly returned) More... | |
biorbd::rigidbody::GeneralizedTorque | InverseDynamics (const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &QDot, const biorbd::rigidbody::GeneralizedAcceleration &QDDot, std::vector< biorbd::utils::SpatialVector > *f_ext=nullptr) |
Interface for the inverse dynamics of RBDL. More... | |
biorbd::rigidbody::GeneralizedAcceleration | ForwardDynamics (const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &QDot, const biorbd::rigidbody::GeneralizedTorque &Tau, std::vector< biorbd::utils::SpatialVector > *f_ext=nullptr) |
Interface for the forward dynamics of RBDL. More... | |
biorbd::rigidbody::GeneralizedAcceleration | ForwardDynamicsConstraintsDirect (const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &QDot, const biorbd::rigidbody::GeneralizedTorque &Tau, biorbd::rigidbody::Contacts &CS, std::vector< biorbd::utils::SpatialVector > *f_ext=nullptr) |
Interface for the forward dynamics with contact of RBDL. More... | |
biorbd::utils::Vector | ContactForcesFromForwardDynamicsConstraintsDirect (const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &QDot, const biorbd::rigidbody::GeneralizedTorque &Tau, std::vector< biorbd::utils::SpatialVector > *f_ext=nullptr) |
Interface for contacts of the forward dynamics with contact of RBDL. More... | |
biorbd::rigidbody::GeneralizedAcceleration | ForwardDynamicsConstraintsDirect (const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &QDot, const biorbd::rigidbody::GeneralizedTorque &Tau, std::vector< biorbd::utils::SpatialVector > *f_ext=nullptr) |
Interface for the forward dynamics with contact of RBDL. More... | |
biorbd::rigidbody::GeneralizedVelocity | ComputeConstraintImpulsesDirect (const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &QDotPre) |
Compute the QDot post from an impact. More... | |
void | checkGeneralizedDimensions (const biorbd::rigidbody::GeneralizedCoordinates *Q=nullptr, const biorbd::rigidbody::GeneralizedVelocity *Qdot=nullptr, const biorbd::rigidbody::GeneralizedAcceleration *Qddot=nullptr, const biorbd::rigidbody::GeneralizedTorque *torque=nullptr) |
Check for the Generalized coordinates, velocities, acceleration and torque dimensions. More... | |
Protected Member Functions | |
RigidBodyDynamics::Math::SpatialTransform | CalcBodyWorldTransformation (const biorbd::rigidbody::GeneralizedCoordinates &Q, const unsigned int segmentIdx, bool updateKin=true) |
Calculate the joint coordinate system (JCS) in global reference frame of a specified segment. More... | |
RigidBodyDynamics::Math::SpatialTransform | CalcBodyWorldTransformation (const unsigned int segmentIdx) const |
Calculate the joint coordinate system (JCS) in global of a specified segment. More... | |
std::vector< biorbd::utils::Vector3d > | meshPoints (const std::vector< biorbd::utils::RotoTrans > &RT, unsigned int idx) const |
Return the mesh vertices of segment idx. More... | |
Protected Attributes | |
std::shared_ptr< std::vector< biorbd::rigidbody::Segment > > | m_segments |
All the articulations. | |
std::shared_ptr< unsigned int > | m_nbRoot |
The number of DoF on the root segment. | |
std::shared_ptr< unsigned int > | m_nbDof |
The total number of degrees of freedom | |
std::shared_ptr< unsigned int > | m_nbQ |
The total number of Q. | |
std::shared_ptr< unsigned int > | m_nbQdot |
The total number of Qdot. | |
std::shared_ptr< unsigned int > | m_nbQddot |
The total number of Qddot. | |
std::shared_ptr< unsigned int > | m_nRotAQuat |
The number of segments per quaternion. | |
std::shared_ptr< bool > | m_isKinematicsComputed |
If the kinematics are computed. | |
std::shared_ptr< double > | m_totalMass |
Mass of all the bodies combined. | |
biorbd::rigidbody::Joints::Joints | ( | const biorbd::rigidbody::Joints & | other | ) |
Construct a joint model from another model.
other | The other joint model |
Definition at line 41 of file Joints.cpp.
unsigned int biorbd::rigidbody::Joints::AddSegment | ( | const biorbd::utils::String & | segmentName, |
const biorbd::utils::String & | parentName, | ||
const biorbd::utils::String & | translationSequence, | ||
const biorbd::utils::String & | rotationSequence, | ||
const std::vector< biorbd::utils::Range > & | QRanges, | ||
const std::vector< biorbd::utils::Range > & | QDotRanges, | ||
const std::vector< biorbd::utils::Range > & | QDDotRanges, | ||
const biorbd::rigidbody::SegmentCharacteristics & | characteristics, | ||
const RigidBodyDynamics::Math::SpatialTransform & | centreOfRotation, | ||
int | forcePlates = -1 |
||
) |
Add a segment to the model.
segmentName | Name of the segment |
parentName | Name of the segment parent |
translationSequence | The translation sequence |
rotationSequence | Euler sequence of rotations |
QRanges | Ranges of the translations and rotations dof. The length of QRanges must be equal to length of translations and rotations |
QDotRanges | Ranges of the translations and rotations dof velocity. The length of QDotRanges must be equal to length of translations and rotations |
QDDotRanges | Ranges of the translations and rotations dof acceleration. The length of QDDotRanges must be equal to length of translations and rotations |
characteristics | The characteristics of the semgent (mass, center of mass, inertia of the segment, etc) |
centreOfRotation | Transformation of the parent to child |
forcePlates | The number of the force platform attached to the Segment (if -1 no force platform is attached) |
Definition at line 125 of file Joints.cpp.
unsigned int biorbd::rigidbody::Joints::AddSegment | ( | const biorbd::utils::String & | segmentName, |
const biorbd::utils::String & | parentName, | ||
const biorbd::utils::String & | translationSequence, | ||
const std::vector< biorbd::utils::Range > & | QRanges, | ||
const std::vector< biorbd::utils::Range > & | QDotRanges, | ||
const std::vector< biorbd::utils::Range > & | QDDotRanges, | ||
const biorbd::rigidbody::SegmentCharacteristics & | characteristics, | ||
const RigidBodyDynamics::Math::SpatialTransform & | centreOfRotation, | ||
int | forcePlates = -1 |
||
) |
Add a segment to the model.
segmentName | Name of the segment |
parentName | Name of the segment parent |
translationSequence | The translation sequence |
QRanges | Ranges of the translations and rotations dof. The length of QRanges must be equal to length of translations and rotations |
QDotRanges | Ranges of the translations and rotations dof velocity. The length of QDotRanges must be equal to length of translations and rotations |
QDDotRanges | Ranges of the translations and rotations dof acceleration. The length of QDDotRanges must be equal to length of translations and rotations |
characteristics | The characteristics of the semgent (mass, center of mass, inertia of the segment, etc) |
centreOfRotation | Transformation of the parent to child |
forcePlates | The number of the force platform attached to the Segment (if -1 no force platform is attached) |
Definition at line 155 of file Joints.cpp.
std::vector< biorbd::utils::RotoTrans > biorbd::rigidbody::Joints::allGlobalJCS | ( | ) | const |
Return the joint coordinate system (JCS) in global reference frame at a given Q.
This function assumes kinematics has been already updated
Definition at line 259 of file Joints.cpp.
std::vector< biorbd::utils::RotoTrans > biorbd::rigidbody::Joints::allGlobalJCS | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q | ) |
Return the joint coordinate system (JCS) in global reference frame at a given Q.
Q | The generalized coordinates |
Definition at line 252 of file Joints.cpp.
biorbd::utils::Vector3d biorbd::rigidbody::Joints::angularMomentum | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
const biorbd::rigidbody::GeneralizedVelocity & | Qdot, | ||
bool | updateKin = true |
||
) |
Calculate the angular momentum of the center of mass.
Q | The generalized coordinates |
Qdot | The generalized velocities |
updateKin | If the kinematics of the model should be computed |
Definition at line 488 of file Joints.cpp.
biorbd::utils::Vector3d biorbd::rigidbody::Joints::CalcAngularMomentum | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
const biorbd::rigidbody::GeneralizedVelocity & | Qdot, | ||
bool | updateKin | ||
) |
Calculate the angular momentum of the center of mass.
Q | The generalized coordinates |
Qdot | The generalized velocities |
updateKin | If the kinematics of the model should be computed |
Definition at line 777 of file Joints.cpp.
biorbd::utils::Vector3d biorbd::rigidbody::Joints::CalcAngularMomentum | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
const biorbd::rigidbody::GeneralizedVelocity & | Qdot, | ||
const biorbd::rigidbody::GeneralizedAcceleration & | Qddot, | ||
bool | updateKin | ||
) |
Calculate the angular momentum of the center of mass.
Q | The generalized coordinates |
Qdot | The generalized velocities |
Qddot | The generalized accelerations |
updateKin | If the kinematics of the model should be computed |
Definition at line 795 of file Joints.cpp.
|
protected |
Calculate the joint coordinate system (JCS) in global reference frame of a specified segment.
Q | The generalized coordinates |
segmentIdx | The index of the segment |
updateKin | If the kinematics of the model should be computed |
Definition at line 433 of file Joints.cpp.
|
protected |
Calculate the joint coordinate system (JCS) in global of a specified segment.
segmentIdx | The index of the segment |
This function assumes that the kinematics was previously updated
Definition at line 445 of file Joints.cpp.
void biorbd::rigidbody::Joints::CalcMatRotJacobian | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
unsigned int | segmentIdx, | ||
const RigidBodyDynamics::Math::Matrix3d & | rotation, | ||
RigidBodyDynamics::Math::MatrixNd & | G, | ||
bool | updateKin | ||
) |
Calculate the jacobian matrix of a rotation matrix.
Q | The generalized coordinates |
segmentIdx | The index of the segment |
rotation | The rotation matrix |
G | The jacobian matrix (output) |
updateKin | If the kinematics of the model should be computed |
Definition at line 1072 of file Joints.cpp.
std::vector< biorbd::utils::Vector3d > biorbd::rigidbody::Joints::CalcSegmentsAngularMomentum | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
const biorbd::rigidbody::GeneralizedVelocity & | Qdot, | ||
bool | updateKin | ||
) |
Calculate the segment center of mass angular momentum.
Q | The generalized coordinates |
Qdot | The generalized velocities |
updateKin | If the kinematics of the model should be computed |
Definition at line 817 of file Joints.cpp.
std::vector< biorbd::utils::Vector3d > biorbd::rigidbody::Joints::CalcSegmentsAngularMomentum | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
const biorbd::rigidbody::GeneralizedVelocity & | Qdot, | ||
const biorbd::rigidbody::GeneralizedAcceleration & | Qddot, | ||
bool | updateKin | ||
) |
Calculate the segment center of mass angular momentum.
Q | The generalized coordinates |
Qdot | The generalized velocities |
Qddot | The generalized accelerations |
updateKin | If the kinematics of the model should be computed |
Definition at line 853 of file Joints.cpp.
void biorbd::rigidbody::Joints::checkGeneralizedDimensions | ( | const biorbd::rigidbody::GeneralizedCoordinates * | Q = nullptr , |
const biorbd::rigidbody::GeneralizedVelocity * | Qdot = nullptr , |
||
const biorbd::rigidbody::GeneralizedAcceleration * | Qddot = nullptr , |
||
const biorbd::rigidbody::GeneralizedTorque * | torque = nullptr |
||
) |
Check for the Generalized coordinates, velocities, acceleration and torque dimensions.
Q | The generalized coordinates |
Qdot | The generalized velocities |
Qddot | The generalized accelerations |
torque | The generalized torques |
Definition at line 1136 of file Joints.cpp.
biorbd::utils::Vector3d biorbd::rigidbody::Joints::CoM | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
bool | updateKin = true |
||
) |
Return the position of the center of mass.
Q | The generalized coordinates |
updateKin | If the kinematics of the model should be computed |
Definition at line 464 of file Joints.cpp.
std::vector< biorbd::rigidbody::NodeSegment > biorbd::rigidbody::Joints::CoMbySegment | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
bool | updateKin = true |
||
) |
Return the position of the center of mass of each segment.
Q | The generalized coordinates |
updateKin | If the kinematics of the model should be computed |
Definition at line 576 of file Joints.cpp.
biorbd::utils::Vector3d biorbd::rigidbody::Joints::CoMbySegment | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
const unsigned int | idx, | ||
bool | updateKin = true |
||
) |
Return the position of the center of mass of segment idx.
Q | The generalized coordinates |
idx | The index of the segment |
updateKin | If the kinematics of the model should be computed |
Definition at line 603 of file Joints.cpp.
biorbd::utils::Matrix biorbd::rigidbody::Joints::CoMbySegmentInMatrix | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
bool | updateKin = true |
||
) |
Return the position of the center of mass of each segment in a matrix.
Q | The generalized coordinates |
updateKin | If the kinematics of the model should be computed |
Definition at line 590 of file Joints.cpp.
biorbd::utils::Vector3d biorbd::rigidbody::Joints::CoMddot | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
const biorbd::rigidbody::GeneralizedVelocity & | Qdot, | ||
const biorbd::rigidbody::GeneralizedAcceleration & | Qddot | ||
) |
Return the acceleration of the center of mass.
Q | The generalized coordinates |
Qdot | The generalized velocities |
Qddot | The acceleration variables |
Definition at line 531 of file Joints.cpp.
std::vector< biorbd::utils::Vector3d > biorbd::rigidbody::Joints::CoMddotBySegment | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
const biorbd::rigidbody::GeneralizedVelocity & | Qdot, | ||
const biorbd::rigidbody::GeneralizedAcceleration & | Qddot, | ||
bool | updateKin = true |
||
) |
Return the acceleration of the center of mass of each segment.
Q | The generalized coordinates |
Qdot | The generalized velocities |
Qddot | The acceleration variables |
updateKin | If the kinematics of the model should be computed |
Definition at line 648 of file Joints.cpp.
biorbd::utils::Vector3d biorbd::rigidbody::Joints::CoMddotBySegment | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
const biorbd::rigidbody::GeneralizedVelocity & | Qdot, | ||
const biorbd::rigidbody::GeneralizedAcceleration & | Qddot, | ||
const unsigned int | idx, | ||
bool | updateKin = true |
||
) |
Return the acceleration of the center of mass of segment idx.
Q | The generalized coordinates |
Qdot | The generalized velocities |
Qddot | The acceleration variables |
idx | The segment identification |
updateKin | If the kinematics of the model should be computed |
Definition at line 666 of file Joints.cpp.
biorbd::utils::Vector3d biorbd::rigidbody::Joints::CoMdot | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
const biorbd::rigidbody::GeneralizedVelocity & | Qdot | ||
) |
Return the velocity of the center of mass.
Q | The generalized coordinates |
Qdot | The generalized velocities |
Definition at line 506 of file Joints.cpp.
std::vector< biorbd::utils::Vector3d > biorbd::rigidbody::Joints::CoMdotBySegment | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
const biorbd::rigidbody::GeneralizedVelocity & | Qdot, | ||
bool | updateKin = true |
||
) |
Return the velocity of the center of mass of each segment.
Q | The generalized coordinates |
Qdot | The generalized velocities |
updateKin | If the kinematics of the model should be computed |
Definition at line 618 of file Joints.cpp.
biorbd::utils::Vector3d biorbd::rigidbody::Joints::CoMdotBySegment | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
const biorbd::rigidbody::GeneralizedVelocity & | Qdot, | ||
const unsigned int | idx, | ||
bool | updateKin = true |
||
) |
Return the velocity of the center of mass of segment idx.
Q | The generalized coordinates |
Qdot | The generalized velocities |
idx | The index of the segment |
updateKin | If the kinematics of the model should be computed |
Definition at line 635 of file Joints.cpp.
biorbd::utils::Matrix biorbd::rigidbody::Joints::CoMJacobian | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q | ) |
Return the jacobian matrix of the center of mass.
Q | The generalized coordinates |
Definition at line 547 of file Joints.cpp.
biorbd::rigidbody::GeneralizedVelocity biorbd::rigidbody::Joints::ComputeConstraintImpulsesDirect | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
const biorbd::rigidbody::GeneralizedVelocity & | QDotPre | ||
) |
Compute the QDot post from an impact.
Q | The Generalized Coordinates |
QDotPre | The Generalized Velocities before impact |
Definition at line 1020 of file Joints.cpp.
biorbd::rigidbody::GeneralizedVelocity biorbd::rigidbody::Joints::computeQdot | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
const biorbd::rigidbody::GeneralizedCoordinates & | QDot, | ||
const double | k_stab = 1 |
||
) |
Return the derivate of Q in function of Qdot (if not Quaternion, Qdot is directly returned)
Q | The generalized coordinates |
QDot | The generalized velocities |
k_stab |
Definition at line 892 of file Joints.cpp.
biorbd::utils::Vector biorbd::rigidbody::Joints::ContactForcesFromForwardDynamicsConstraintsDirect | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
const biorbd::rigidbody::GeneralizedVelocity & | QDot, | ||
const biorbd::rigidbody::GeneralizedTorque & | Tau, | ||
std::vector< biorbd::utils::SpatialVector > * | f_ext = nullptr |
||
) |
Interface for contacts of the forward dynamics with contact of RBDL.
Q | The Generalized Coordinates |
QDot | The Generalized Velocities |
Tau | The Generalized Torques |
f_ext | External force acting on the system if there are any |
Definition at line 990 of file Joints.cpp.
biorbd::rigidbody::Joints biorbd::rigidbody::Joints::DeepCopy | ( | ) | const |
void biorbd::rigidbody::Joints::DeepCopy | ( | const biorbd::rigidbody::Joints & | other | ) |
Deep copy of the joints.
other | The joints to copy |
Definition at line 68 of file Joints.cpp.
std::vector< RigidBodyDynamics::Math::SpatialVector > biorbd::rigidbody::Joints::dispatchedForce | ( | std::vector< biorbd::utils::SpatialVector > & | sv | ) | const |
Dispatch the forces from the force plate in a spatial vector.
sv | One spatial vector per force platform |
Definition at line 217 of file Joints.cpp.
std::vector< RigidBodyDynamics::Math::SpatialVector > biorbd::rigidbody::Joints::dispatchedForce | ( | std::vector< std::vector< biorbd::utils::SpatialVector >> & | spatialVector, |
unsigned int | frame | ||
) | const |
Dispatch the forces from the force plate in a vector.
spatialVector | The values over time of one spatial vector per force platform |
frame | The frame to dispatch |
Definition at line 204 of file Joints.cpp.
biorbd::rigidbody::GeneralizedAcceleration biorbd::rigidbody::Joints::ForwardDynamics | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
const biorbd::rigidbody::GeneralizedVelocity & | QDot, | ||
const biorbd::rigidbody::GeneralizedTorque & | Tau, | ||
std::vector< biorbd::utils::SpatialVector > * | f_ext = nullptr |
||
) |
Interface for the forward dynamics of RBDL.
Q | The Generalized Coordinates |
QDot | The Generalized Velocities |
Tau | The Generalized Torques |
f_ext | External force acting on the system if there are any |
Definition at line 953 of file Joints.cpp.
biorbd::rigidbody::GeneralizedAcceleration biorbd::rigidbody::Joints::ForwardDynamicsConstraintsDirect | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
const biorbd::rigidbody::GeneralizedVelocity & | QDot, | ||
const biorbd::rigidbody::GeneralizedTorque & | Tau, | ||
biorbd::rigidbody::Contacts & | CS, | ||
std::vector< biorbd::utils::SpatialVector > * | f_ext = nullptr |
||
) |
Interface for the forward dynamics with contact of RBDL.
Q | The Generalized Coordinates |
QDot | The Generalized Velocities |
Tau | The Generalized Torques |
CS | The Constraint set that will be filled |
f_ext | External force acting on the system if there are any |
Definition at line 971 of file Joints.cpp.
biorbd::rigidbody::GeneralizedAcceleration biorbd::rigidbody::Joints::ForwardDynamicsConstraintsDirect | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
const biorbd::rigidbody::GeneralizedVelocity & | QDot, | ||
const biorbd::rigidbody::GeneralizedTorque & | Tau, | ||
std::vector< biorbd::utils::SpatialVector > * | f_ext = nullptr |
||
) |
Interface for the forward dynamics with contact of RBDL.
Q | The Generalized Coordinates |
QDot | The Generalized Velocities |
Tau | The Generalized Torques |
f_ext | External force acting on the system if there are any |
Definition at line 1001 of file Joints.cpp.
int biorbd::rigidbody::Joints::GetBodyBiorbdId | ( | const biorbd::utils::String & | segmentName | ) | const |
Return the biorbd body identification.
segmentName | The name of the segment |
Definition at line 245 of file Joints.cpp.
unsigned int biorbd::rigidbody::Joints::getDofIndex | ( | const biorbd::utils::String & | SegmentName, |
const biorbd::utils::String & | dofName | ||
) |
Return the index of a DoF in a segment.
SegmentName | The name of the Segment |
dofName | The name of the degree of freedom (DoF) |
Definition at line 1037 of file Joints.cpp.
biorbd::utils::Vector3d biorbd::rigidbody::Joints::getGravity | ( | ) | const |
biorbd::utils::RotoTrans biorbd::rigidbody::Joints::globalJCS | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
const biorbd::utils::String & | name | ||
) |
Return the joint coordinate system (JCS) for the segment in global reference frame at a given Q.
Q | The generalized coordinates |
name | The name of the segment |
Definition at line 267 of file Joints.cpp.
biorbd::utils::RotoTrans biorbd::rigidbody::Joints::globalJCS | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
unsigned int | idx | ||
) |
Return the joint coordinate system (JCS) for the segment idx in global reference frame at a given Q.
Q | The generalized coordinates |
idx | The index of the segment |
Definition at line 275 of file Joints.cpp.
biorbd::utils::RotoTrans biorbd::rigidbody::Joints::globalJCS | ( | const biorbd::utils::String & | name | ) | const |
Return the joint coordinate system (JCS) for the segment in global reference.
name | The name of the segment |
This function assumes kinematics has been already updated
Definition at line 284 of file Joints.cpp.
biorbd::utils::RotoTrans biorbd::rigidbody::Joints::globalJCS | ( | unsigned int | idx | ) | const |
Return the joint coordinate system (JCS) for the segment idx in global reference.
idx | The index of the segment |
This function assumes kinematics has been already updated
Definition at line 289 of file Joints.cpp.
biorbd::rigidbody::GeneralizedTorque biorbd::rigidbody::Joints::InverseDynamics | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
const biorbd::rigidbody::GeneralizedVelocity & | QDot, | ||
const biorbd::rigidbody::GeneralizedAcceleration & | QDDot, | ||
std::vector< biorbd::utils::SpatialVector > * | f_ext = nullptr |
||
) |
Interface for the inverse dynamics of RBDL.
Q | The Generalized Coordinates |
QDot | The Generalized Velocities |
QDDot | The Generalzed Acceleration |
f_ext | External force acting on the system if there are any |
Definition at line 935 of file Joints.cpp.
std::vector< biorbd::utils::RotoTrans > biorbd::rigidbody::Joints::localJCS | ( | ) | const |
Return all the joint coordinate system (JCS) in its parent reference frame.
Definition at line 295 of file Joints.cpp.
biorbd::utils::RotoTrans biorbd::rigidbody::Joints::localJCS | ( | const biorbd::utils::String & | name | ) | const |
Return the joint coordinate system (JCS) of the segment its parent reference frame.
name | The name of the segment |
Definition at line 303 of file Joints.cpp.
biorbd::utils::RotoTrans biorbd::rigidbody::Joints::localJCS | ( | const unsigned int | idx | ) | const |
Return the joint coordinate system (JCS) of the segment idx its parent reference frame.
idx | The index of the segment |
Definition at line 306 of file Joints.cpp.
double biorbd::rigidbody::Joints::mass | ( | ) | const |
Return the total mass of the model.
Definition at line 121 of file Joints.cpp.
biorbd::utils::Matrix biorbd::rigidbody::Joints::massMatrix | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
bool | updateKin = true |
||
) |
Get the mass matrix at a given position Q.
Q | The generalized coordinates |
updateKin | If the kinematics should be updated |
Definition at line 496 of file Joints.cpp.
std::vector< biorbd::rigidbody::Mesh > biorbd::rigidbody::Joints::mesh | ( | ) | const |
const biorbd::rigidbody::Mesh & biorbd::rigidbody::Joints::mesh | ( | unsigned int | idx | ) | const |
Return the segment mesh for segment idx.
idx | The index of the segment |
Definition at line 772 of file Joints.cpp.
std::vector< std::vector< biorbd::rigidbody::MeshFace > > biorbd::rigidbody::Joints::meshFaces | ( | ) | const |
Return the mesh faces for all the segments.
Definition at line 752 of file Joints.cpp.
const std::vector< biorbd::rigidbody::MeshFace > & biorbd::rigidbody::Joints::meshFaces | ( | unsigned int | idx | ) | const |
Return the mesh faces for segment idx.
idx | The index of the segment |
Definition at line 759 of file Joints.cpp.
std::vector< std::vector< biorbd::utils::Vector3d > > biorbd::rigidbody::Joints::meshPoints | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
bool | updateKin = true |
||
) |
Return the vertices of the mesh for all segments in global reference frame.
Q | The generalized coordinates |
updateKin | If the kinematics of the model should be computed |
Definition at line 682 of file Joints.cpp.
std::vector< biorbd::utils::Vector3d > biorbd::rigidbody::Joints::meshPoints | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
unsigned int | idx, | ||
bool | updateKin = true |
||
) |
Return the vertices of the mesh for the segment idx.
Q | The generalized coordinates |
idx | The index of the segment |
updateKin | If the kinematics of the model should be computed |
Definition at line 700 of file Joints.cpp.
|
protected |
Return the mesh vertices of segment idx.
RT | The RotoTrans of the segment |
idx | The index of the segment |
Definition at line 737 of file Joints.cpp.
std::vector< biorbd::utils::Matrix > biorbd::rigidbody::Joints::meshPointsInMatrix | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
bool | updateKin = true |
||
) |
Return all the vertices of the mesh points in a matrix.
Q | The generalized coordinates |
updateKin | If the kinematics of the model should be computed |
Definition at line 716 of file Joints.cpp.
std::vector< biorbd::utils::String > biorbd::rigidbody::Joints::nameDof | ( | ) | const |
Return the names of the degree of freedom (DoF)
Definition at line 91 of file Joints.cpp.
unsigned int biorbd::rigidbody::Joints::nbDof | ( | ) | const |
Return the number of degrees of freedom (DoF)
Definition at line 87 of file Joints.cpp.
unsigned int biorbd::rigidbody::Joints::nbGeneralizedTorque | ( | ) | const |
Return the number of generalized torque.
Definition at line 84 of file Joints.cpp.
unsigned int biorbd::rigidbody::Joints::nbQ | ( | ) | const |
Return the number of generalized coordinates (Q)
Definition at line 108 of file Joints.cpp.
unsigned int biorbd::rigidbody::Joints::nbQddot | ( | ) | const |
Return the number of generalized acceleration (Qddot)
Definition at line 114 of file Joints.cpp.
unsigned int biorbd::rigidbody::Joints::nbQdot | ( | ) | const |
Return the number of generalized velocities (Qdot)
Definition at line 111 of file Joints.cpp.
unsigned int biorbd::rigidbody::Joints::nbQuat | ( | ) | const |
Return the number of segments that are described using quaternions.
Definition at line 888 of file Joints.cpp.
unsigned int biorbd::rigidbody::Joints::nbRoot | ( | ) | const |
unsigned int biorbd::rigidbody::Joints::nbSegment | ( | ) | const |
Return the actual number of segment.
Definition at line 199 of file Joints.cpp.
biorbd::rigidbody::NodeSegment biorbd::rigidbody::Joints::projectPoint | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
const biorbd::rigidbody::NodeSegment & | n, | ||
bool | updateKin | ||
) |
Return the projected markers from a point corresponding to a marker from the model.
Q | The generalized coordinates |
n | A reference to a marker from the model |
updateKin | If the kinematics of the model should be computed |
Definition at line 361 of file Joints.cpp.
biorbd::rigidbody::NodeSegment biorbd::rigidbody::Joints::projectPoint | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
const biorbd::utils::Vector3d & | v, | ||
int | segmentIdx, | ||
const biorbd::utils::String & | axesToRemove, | ||
bool | updateKin = true |
||
) |
Project a point on specific axis of a segment.
Q | The generalized coordinates |
v | The point to project |
segmentIdx | The segment index to project the marker on |
axesToRemove | The axis to remove |
updateKin | If the kinematics of the model should be computed |
Definition at line 341 of file Joints.cpp.
std::vector< biorbd::rigidbody::NodeSegment > biorbd::rigidbody::Joints::projectPoint | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
const std::vector< biorbd::rigidbody::NodeSegment > & | v, | ||
bool | updateKin = true |
||
) |
Project multiples points on their respective segment.
Q | The generalized coordinates |
v | All the points to project. The number of points must match the number of marker in the joint model |
updateKin | If the kinematics of the model should be computed |
Return projected markers from points corresponding to markers from the model. The vector needs to be equal to the number of markers and in the order given by Markers and in global coordinates
Definition at line 311 of file Joints.cpp.
biorbd::utils::Matrix biorbd::rigidbody::Joints::projectPointJacobian | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
biorbd::rigidbody::NodeSegment | p, | ||
bool | updateKin | ||
) |
Return the jacobian matrix of the projected markers for a marker from the model.
Q | The generalized coordinates |
p | A reference to a marker from the model |
updateKin | If the kinematics of the model should be computed |
Definition at line 370 of file Joints.cpp.
biorbd::utils::Matrix biorbd::rigidbody::Joints::projectPointJacobian | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
const biorbd::utils::Vector3d & | v, | ||
int | segmentIdx, | ||
const biorbd::utils::String & | axesToRemove, | ||
bool | updateKin | ||
) |
Return the Jacobian matrix of a projected marker on the segment segmentIdx.
Q | The generalized coordinates |
v | The marker to project |
segmentIdx | The index of the segment to project the marker on |
axesToRemove | The axes to remove |
updateKin | If the kinematics of the model should be computed |
Definition at line 401 of file Joints.cpp.
std::vector< biorbd::utils::Matrix > biorbd::rigidbody::Joints::projectPointJacobian | ( | const biorbd::rigidbody::GeneralizedCoordinates & | Q, |
const std::vector< biorbd::rigidbody::NodeSegment > & | v, | ||
bool | updateKin | ||
) |
Return the jacobian matrix of the projected markers.
Q | The generalized coordinates |
v | All the markers. The size of which must match the one in the joint model |
updateKin | If the kinematics of the model should be computed |
Return the jacobian matrix of projected markers from points corresponding to markers from the model. The vector needs to be equal to the number of markers and in the order given by Markers and in global coordinates
Definition at line 415 of file Joints.cpp.
const biorbd::rigidbody::Segment & biorbd::rigidbody::Joints::segment | ( | const biorbd::utils::String & | name | ) | const |
Get a segment of a specific name.
name | The name of the segment to return |
Definition at line 194 of file Joints.cpp.
const biorbd::rigidbody::Segment & biorbd::rigidbody::Joints::segment | ( | unsigned int | idx | ) | const |
Get a segment of index idx.
idx | Index of the segment |
Definition at line 189 of file Joints.cpp.
void biorbd::rigidbody::Joints::setGravity | ( | const biorbd::utils::Vector3d & | newGravity | ) |
Set the gravity.
newGravity | The new gravity vector |
Definition at line 183 of file Joints.cpp.
void biorbd::rigidbody::Joints::UpdateKinematicsCustom | ( | const biorbd::rigidbody::GeneralizedCoordinates * | Q = nullptr , |
const biorbd::rigidbody::GeneralizedVelocity * | Qdot = nullptr , |
||
const biorbd::rigidbody::GeneralizedAcceleration * | Qddot = nullptr |
||
) |
Update the kinematic variables such as body velocities and accelerations in the model to reflect the variables passed to this function.
Q | The generalized coordinates |
Qdot | The generalized velocities |
Qddot | The generalized accelerations |
Definition at line 1063 of file Joints.cpp.