Joints.h

#ifndef BIORBD_RIGIDBODY_JOINTS_H
#define BIORBD_RIGIDBODY_JOINTS_H
 
#include <memory>
#include <rbdl/Model.h>
#include <rbdl/Constraints.h>
#include "biorbdConfig.h"
 
namespace biorbd {
namespace utils {
class String;
class RotoTrans;
class Matrix;
class Vector;
class Vector3d;
class Range;
class SpatialVector;
}
 
namespace rigidbody {
class GeneralizedCoordinates;
class GeneralizedVelocity;
class GeneralizedAcceleration;
class GeneralizedTorque;
class NodeSegment;
class MeshFace;
class Segment;
class SegmentCharacteristics;
class Mesh;
class Contacts;
 
#ifdef SWIG
class BIORBD_API Joints
#else
class BIORBD_API Joints : public RigidBodyDynamics::Model
#endif
{
public:
 
    Joints();
 
    Joints(
            const biorbd::rigidbody::Joints& other);
 
    virtual ~Joints();
 
    biorbd::rigidbody::Joints DeepCopy() const;
 
    void DeepCopy(
            const biorbd::rigidbody::Joints& other);
 
    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& centreOfRotation,
            int forcePlates=-1);
 
    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& centreOfRotation, 
            int forcePlates=-1); 
 
 
    // -- GENERAL MODELLING -- //
    biorbd::utils::Vector3d getGravity() const;
 
    void setGravity(
            const biorbd::utils::Vector3d& newGravity);
 
    // -- INFORMATION ON THE MODEL -- //
    int GetBodyBiorbdId(
            const biorbd::utils::String &segmentName) const;
 
    unsigned int nbGeneralizedTorque() const;
 
    unsigned int nbSegment() const;
 
    unsigned int nbDof() const;
 
    unsigned int getDofIndex(
            const biorbd::utils::String& SegmentName,
            const biorbd::utils::String& dofName);
 
    std::vector<biorbd::utils::String> nameDof() const;
 
    unsigned int nbQ() const;
 
    unsigned int nbQdot() const;
 
    unsigned int nbQddot() const;
 
    unsigned int nbRoot() const;
 
    unsigned int nbQuat() const;
 
    const biorbd::rigidbody::Segment& segment(
            unsigned int idx) const;
 
    const biorbd::rigidbody::Segment& segment(
            const biorbd::utils::String& name) const;
    // ------------------------------ //
 
 
    // -- FORCE PLATE DISPATCHER -- //
    std::vector<RigidBodyDynamics::Math::SpatialVector> dispatchedForce(
            std::vector<std::vector<biorbd::utils::SpatialVector>> &spatialVector,
            unsigned int frame) const;
 
    std::vector<RigidBodyDynamics::Math::SpatialVector> dispatchedForce(
            std::vector<biorbd::utils::SpatialVector> &sv) const;
    // ---------------------------- //
 
 
    void UpdateKinematicsCustom(
            const biorbd::rigidbody::GeneralizedCoordinates *Q = nullptr,
            const biorbd::rigidbody::GeneralizedVelocity *Qdot = nullptr,
            const biorbd::rigidbody::GeneralizedAcceleration *Qddot = nullptr);
 
 
    // -- POSITION INTERFACE OF THE MODEL -- //
 
    std::vector<biorbd::utils::RotoTrans> allGlobalJCS(
            const biorbd::rigidbody::GeneralizedCoordinates &Q); 
 
    std::vector<biorbd::utils::RotoTrans> allGlobalJCS() const;
 
    biorbd::utils::RotoTrans globalJCS(
            const biorbd::rigidbody::GeneralizedCoordinates &Q,
            const biorbd::utils::String &name); 
 
    biorbd::utils::RotoTrans globalJCS(
            const biorbd::rigidbody::GeneralizedCoordinates &Q,
            unsigned int idx); 
 
    biorbd::utils::RotoTrans globalJCS(
            const biorbd::utils::String &name) const;
 
    biorbd::utils::RotoTrans globalJCS(
            unsigned int idx) const;
 
    std::vector<biorbd::utils::RotoTrans> localJCS() const;
 
    biorbd::utils::RotoTrans localJCS(
            const biorbd::utils::String &name) const;
 
    biorbd::utils::RotoTrans localJCS(
            const unsigned int idx) const;
 
    biorbd::rigidbody::NodeSegment projectPoint(
            const biorbd::rigidbody::GeneralizedCoordinates &Q,
            const biorbd::utils::Vector3d &v,
            int segmentIdx,
            const biorbd::utils::String& axesToRemove,
            bool updateKin=true);
 
    std::vector<biorbd::rigidbody::NodeSegment>  projectPoint(
            const biorbd::rigidbody::GeneralizedCoordinates &Q,
            const std::vector<biorbd::rigidbody::NodeSegment> &v,
            bool updateKin=true); 
 
    biorbd::rigidbody::NodeSegment projectPoint(
        const biorbd::rigidbody::GeneralizedCoordinates& Q,
        const biorbd::rigidbody::NodeSegment& n,
        bool updateKin);
 
    biorbd::utils::Matrix projectPointJacobian(
            const biorbd::rigidbody::GeneralizedCoordinates &Q,
            biorbd::rigidbody::NodeSegment p,
            bool updateKin);
 
    biorbd::utils::Matrix projectPointJacobian(
            const biorbd::rigidbody::GeneralizedCoordinates &Q,
            const biorbd::utils::Vector3d &v,
            int segmentIdx,
            const biorbd::utils::String& axesToRemove,
            bool updateKin);
 
    std::vector<biorbd::utils::Matrix> projectPointJacobian(
            const biorbd::rigidbody::GeneralizedCoordinates &Q,
            const std::vector<biorbd::rigidbody::NodeSegment> &v,
            bool updateKin); 
    // ------------------------------------- //
 
 
    // -- MASS RELATED STUFF -- //
    double mass() const; 
 
    biorbd::utils::Vector3d CoM(
            const biorbd::rigidbody::GeneralizedCoordinates &Q,
            bool updateKin=true); 
 
    std::vector<biorbd::rigidbody::NodeSegment> CoMbySegment(
            const biorbd::rigidbody::GeneralizedCoordinates &Q,
            bool updateKin=true);
 
    biorbd::utils::Matrix CoMbySegmentInMatrix(
            const biorbd::rigidbody::GeneralizedCoordinates &Q,
            bool updateKin=true);
 
    biorbd::utils::Vector3d CoMbySegment(
            const biorbd::rigidbody::GeneralizedCoordinates &Q,
            const unsigned int idx,
            bool updateKin=true);
 
    biorbd::utils::Vector3d CoMdot(
            const biorbd::rigidbody::GeneralizedCoordinates &Q,
            const biorbd::rigidbody::GeneralizedVelocity &Qdot);
 
    biorbd::utils::Vector3d CoMddot(
            const biorbd::rigidbody::GeneralizedCoordinates &Q,
            const biorbd::rigidbody::GeneralizedVelocity &Qdot,
            const biorbd::rigidbody::GeneralizedAcceleration &Qddot);
 
    std::vector<biorbd::utils::Vector3d> CoMdotBySegment(
            const biorbd::rigidbody::GeneralizedCoordinates &Q,
            const biorbd::rigidbody::GeneralizedVelocity &Qdot,
            bool updateKin=true); 
 
    biorbd::utils::Vector3d CoMdotBySegment(
            const biorbd::rigidbody::GeneralizedCoordinates &Q,
            const biorbd::rigidbody::GeneralizedVelocity &Qdot,
            const unsigned int idx,
            bool updateKin=true);
 
    std::vector<biorbd::utils::Vector3d> CoMddotBySegment(
            const biorbd::rigidbody::GeneralizedCoordinates &Q,
            const biorbd::rigidbody::GeneralizedVelocity &Qdot,
            const biorbd::rigidbody::GeneralizedAcceleration &Qddot,
            bool updateKin=true);
 
    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);
 
    biorbd::utils::Matrix CoMJacobian(
            const biorbd::rigidbody::GeneralizedCoordinates &Q); 
    // ------------------------ //
 
 
    // -- MESH OF THE MODEL -- //
    std::vector<std::vector<biorbd::utils::Vector3d>> meshPoints(
            const biorbd::rigidbody::GeneralizedCoordinates &Q,
            bool updateKin = true);
 
    std::vector<biorbd::utils::Vector3d> meshPoints(
            const biorbd::rigidbody::GeneralizedCoordinates &Q,
            unsigned int idx,
            bool updateKin = true);
 
    std::vector<biorbd::utils::Matrix> meshPointsInMatrix(
            const biorbd::rigidbody::GeneralizedCoordinates &Q,
            bool updateKin = true
            );
 
    std::vector<std::vector<MeshFace> > meshFaces() const;
 
    const std::vector<biorbd::rigidbody::MeshFace> &meshFaces(
            unsigned int idx) const;
 
    std::vector<biorbd::rigidbody::Mesh> mesh() const;
    
    const biorbd::rigidbody::Mesh& mesh(
            unsigned int  idx) const;
    // ----------------------- //
 
 
    // -- ANGULAR MOMENTUM FUNCTIONS -- //
    biorbd::utils::Vector3d angularMomentum(
            const biorbd::rigidbody::GeneralizedCoordinates &Q,
            const biorbd::rigidbody::GeneralizedVelocity &Qdot,
            bool updateKin = true); // Wrapper pour le moment angulaire
   
    biorbd::utils::Matrix massMatrix(
            const biorbd::rigidbody::GeneralizedCoordinates &Q,
            bool updateKin = true);
 
    biorbd::utils::Vector3d CalcAngularMomentum (
            const biorbd::rigidbody::GeneralizedCoordinates &Q,
            const biorbd::rigidbody::GeneralizedVelocity &Qdot,
            bool updateKin);
 
    biorbd::utils::Vector3d CalcAngularMomentum (
            const biorbd::rigidbody::GeneralizedCoordinates &Q,
            const biorbd::rigidbody::GeneralizedVelocity &Qdot,
            const biorbd::rigidbody::GeneralizedAcceleration &Qddot,
            bool updateKin);
 
    std::vector<biorbd::utils::Vector3d> CalcSegmentsAngularMomentum (
            const biorbd::rigidbody::GeneralizedCoordinates &Q,
            const biorbd::rigidbody::GeneralizedVelocity &Qdot,
            bool updateKin);
 
    std::vector<biorbd::utils::Vector3d> CalcSegmentsAngularMomentum (
            const biorbd::rigidbody::GeneralizedCoordinates &Q,
            const biorbd::rigidbody::GeneralizedVelocity &Qdot,
            const biorbd::rigidbody::GeneralizedAcceleration &Qddot,
            bool updateKin);
    // -------------------------------- //
 
    void CalcMatRotJacobian (
            const biorbd::rigidbody::GeneralizedCoordinates &Q,
            unsigned int segmentIdx,
            const RigidBodyDynamics::Math::Matrix3d &rotation,
            RigidBodyDynamics::Math::MatrixNd &G,
            bool updateKin);
 
    biorbd::rigidbody::GeneralizedVelocity computeQdot(
        const biorbd::rigidbody::GeneralizedCoordinates &Q,
        const biorbd::rigidbody::GeneralizedCoordinates &QDot,
        const double k_stab = 1);
 
    // ---- DYNAMIC INTERFACE ---- //
    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);
 
    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);
 
    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);
 
    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);
 
    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);
 
    biorbd::rigidbody::GeneralizedVelocity ComputeConstraintImpulsesDirect(
            const biorbd::rigidbody::GeneralizedCoordinates& Q,
            const biorbd::rigidbody::GeneralizedVelocity& QDotPre);
 
protected:
    std::shared_ptr<std::vector<biorbd::rigidbody::Segment>> m_segments; 
 
    std::shared_ptr<unsigned int> m_nbRoot; 
    std::shared_ptr<unsigned int> m_nbDof; 
    std::shared_ptr<unsigned int> m_nbQ; 
    std::shared_ptr<unsigned int> m_nbQdot; 
    std::shared_ptr<unsigned int> m_nbQddot; 
    std::shared_ptr<unsigned int> m_nRotAQuat; 
    std::shared_ptr<bool> m_isKinematicsComputed; 
    std::shared_ptr<double> m_totalMass; 
 
    RigidBodyDynamics::Math::SpatialTransform CalcBodyWorldTransformation(
            const biorbd::rigidbody::GeneralizedCoordinates &Q,
            const unsigned int segmentIdx,
            bool updateKin = true);
 
    RigidBodyDynamics::Math::SpatialTransform CalcBodyWorldTransformation(
            const unsigned int segmentIdx) const;
 
    std::vector<biorbd::utils::Vector3d> meshPoints(
            const std::vector<biorbd::utils::RotoTrans> &RT,
            unsigned int idx) const;
 
public:
    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);
};
 
}}
 
#endif // BIORBD_RIGIDBODY_JOINTS_H