ArmModel.h

Go to the documentation of this file.

 
#ifndef __ARMMODEL_H__
#define __ARMMODEL_H__
 
#include "RMLExceptions.h"
#include "RobotLink.h"
#include "Types.h"
#include <algorithm>
#include <eigen3/Eigen/Dense>
#include <unordered_map>
#include <vector>
 
namespace rml {
 
//typedef std::pair<int, Eigen::TransformationMatrix> IndexedTMat;
typedef std::pair<std::string, Eigen::TransformationMatrix> IndexedTMat;
 
class ArmModel {
public:
    ArmModel(const std::string id) noexcept(false);
    virtual ~ArmModel();
    void AddJointLink(JointType type, const Eigen::Vector3d& axis, const Eigen::TransformationMatrix& baseTransf, double jointLimMin, double jointLimMax);
    void AddFixedLink(const Eigen::TransformationMatrix& baseTransf);
    virtual void JointsPosition(const Eigen::VectorXd q) noexcept(false);
    virtual auto JointsPosition() const -> const Eigen::VectorXd& { return q_moving_; }
    virtual void JointsVelocity(const Eigen::VectorXd qdot) noexcept(false);
    virtual auto JointsVelocity() const -> const Eigen::VectorXd& { return q_dot_moving_; }
    void JointsAcceleration(const Eigen::VectorXd qddot) noexcept(false);
    auto JointsAcceleration() const -> const Eigen::VectorXd& { return q_ddot_moving_; }
    void AttachRigidBodyFrame(std::string frameID, std::string attachedFrameID, Eigen::TransformationMatrix attachedFrameID_T_frameID) noexcept(false);
    /*
    * For CartesianJacobian, TransformationMatrix, Jacobian and ManipulabilityJacobian the id must be provieded according the following logic:
    *
    * * Joint Frame: armID + FrameID::Joint + joint°;
    *
    * * Rigid Body: armID + "_" + frameID
    */
    Eigen::TransformationMatrix TransformationMatrix(const std::string& frameID) noexcept(false);
    Eigen::TransformationMatrix TransformationMatrix(const std::string& frameID_j, const std::string& frameID_k);
    virtual Eigen::MatrixXd Jacobian(const std::string& frameID) noexcept(false);
    Eigen::MatrixXd ManipulabilityJacobian(const std::string& frameID);
    virtual auto NumJoints() const -> unsigned int { return movingNumJoints_; }
    auto dJdq() const -> const std::vector<Eigen::MatrixXd>& { return dJdq_; }
    RobotLink& Link(int jointIndex) noexcept(false);
    auto IsModelInitialized() const -> bool { return modelInitialized_; }
    auto ControlVector() const -> const Eigen::VectorXd& { return controlRef_; }
    void ControlVector(const Eigen::VectorXd controlRef) noexcept(false);
    double Manipulability(const std::string& frameID);
    Eigen::MatrixXd DexterityJacobian(const std::string& frameID);
    double Dexterity(const std::string& frameID);
    auto ID() const -> const std::string& { return id_; }
    auto ID() -> std::string& { return id_; }
 
    std::vector<std::string> GetRigidBodyFrameIDs() const noexcept;
 
    std::vector<std::string> GetJointFrameIDs() const noexcept;
 
    std::vector<std::string> GetJacobianFrameIDs() const noexcept;
 
    std::string GetEndEffectorFrameID() const noexcept;
    
protected:
    void EvaluateTotalForwardGeometry();
    virtual void EvaluatedJdqNumeric();
    virtual Eigen::MatrixXd EvaluateManipulability(const std::string& frameID);
    virtual Eigen::MatrixXd EvaluateDexterity(const std::string& frameID);
    Eigen::TransformationMatrix EvaluateRigidBodyTransf(const std::string& frameID);
    Eigen::MatrixXd EvaluateRigidBodyJacobian(const std::string& frameID);
    Eigen::MatrixXd EvaluateBase2JointJacobian(unsigned int jointIndex);
    void ForwardDirectGeometry(unsigned int jointNumber);
    void BackwardDirectGeometry(unsigned int jointNumber, unsigned int endEffectorIndex); 
 
    bool modelInitialized_; 
    bool isMapInitialized_; 
    unsigned int totalNumJoints_; //<! links number (fixedLink+movingJoints)
    unsigned int movingNumJoints_; 
    std::vector<unsigned int> movingJoints_; 
    std::vector<RobotLink> links_; 
    std::unordered_map<std::string, IndexedTMat> rigidBodyFrames_; //<! map of the attached body frames
    std::unordered_map<std::string, Eigen::MatrixXd> jacobians_; //<! map of the jacobians
    std::unordered_map<std::string, Eigen::TransformationMatrix> transformation_; //<! map of the transformations
    std::unordered_map<std::string, Eigen::MatrixXd> manipulabilityJacobians_; 
    std::unordered_map<std::string, double> manipulability_; 
    Eigen::VectorXd q_total_; //<! vector of links position
    Eigen::VectorXd q_moving_; //<! vector of moving joints position
    Eigen::VectorXd q_dot_moving_; //<! vector of moving joints velocity
    Eigen::VectorXd q_ddot_moving_; //<! vector of moving joints acceleration
    Eigen::VectorXd controlRef_; //<! control vector
    std::vector<Eigen::TransformationMatrix> baseTei_; //<! vector of transformation matrix from base to joint   ??
    std::vector<Eigen::TransformationMatrix> biTei_; //<! vector of transformation matrix from joint i-1 to joint i ??
    Eigen::TransformationMatrix baseTbi_; //<! transformation matrix from base to joint i ??
    Eigen::TransformationMatrix Tz_; 
    Eigen::Vector3d base_ki_; 
    std::vector<Eigen::Vector6d> h_; 
    std::vector<Eigen::MatrixXd> dJdq_; 
    Eigen::MatrixXd djdqJpinv_; 
    Eigen::RotationMatrix I3_; 
    bool modelReadFromFile_; 
    std::string id_; 
    std::unordered_map<std::string, Eigen::MatrixXd> dexterityJacobians_; 
    std::unordered_map<std::string, double> dexterity_; 
    Eigen::MatrixXd Jdjdq_; 
    Eigen::MatrixXd Jpinvdjpinvdq_; 
};
}
 
#endif /* __ARMMODEL_H__ */