KinematicChain.h

Go to the documentation of this file.

/*
 * Copyright (C) 2016 IIT-ADVR
 * Author: Arturo Laurenzi, Luca Muratore
 * email:  arturo.laurenzi@iit.it, luca.muratore@iit.it
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>
*/
 
#ifndef __KINEMATIC_CHAIN_H__
#define __KINEMATIC_CHAIN_H__
 
#include <string>
#include <vector>
#include <map>
#include <memory>
#include <iostream>
 
#include <eigen3/Eigen/Dense>
 
#include <XBotInterface/Joint.h>
#include <XBotCoreModel/XBotCoreModel.h>
#include <XBotInterface/ForceTorqueSensor.h>
#include <XBotInterface/ImuSensor.h>
// #include <XBotInterface/Hand.h>
 
 
namespace XBot
{
    
// NOTE forward declaration because of friendship
class XBotInterface;
class RobotInterface;
 
class KinematicChain
{
 
public:
 
    typedef std::shared_ptr<KinematicChain> Ptr;
    typedef std::shared_ptr<const KinematicChain> ConstPtr;
    
    friend XBot::XBotInterface;
    friend XBot::RobotInterface;
 
    KinematicChain();
 
    KinematicChain(const std::string &chain_name,
                   const XBot::XBotCoreModel &XBotModel);
    
    explicit KinematicChain(const std::string& chain_name);
 
    KinematicChain(const KinematicChain &other);
 
    KinematicChain &operator= (const KinematicChain &rhs);
    
    bool getChainState(const std::string& state_name, JointIdMap& q) const;
    
    bool getChainState(const std::string& state_name, JointNameMap& q) const;
    
    
    bool getChainState(const std::string& state_name, Eigen::VectorXd& q) const;
    
    std::map<std::string, ForceTorqueSensor::ConstPtr> getForceTorque() const;
    
    std::map<std::string, ImuSensor::ConstPtr> getImu() const;
    
    ForceTorqueSensor::ConstPtr getForceTorque(const std::string& parent_link_name) const;
    
    ImuSensor::ConstPtr getImu(const std::string& parent_link_name) const;
 
    
    void getJointLimits(Eigen::VectorXd& q_min, Eigen::VectorXd& q_max) const;
    
    void getVelocityLimits(Eigen::VectorXd& qdot_max) const;
    void getEffortLimits(Eigen::VectorXd& tau_max) const;
    
    void getJointLimits(int i, double& q_min, double& q_max) const;
    
    void getVelocityLimits(int i, double& qdot_max) const;
    
    void getEffortLimits(int i, double& tau_max) const;
    
    bool checkJointLimits(const Eigen::VectorXd& q, 
                          std::vector<std::string>& violating_joints) const;
 
    bool checkVelocityLimits(const Eigen::VectorXd& qdot, 
                          std::vector<std::string>& violating_joints) const;
 
    bool checkEffortLimits(const Eigen::VectorXd& tau, 
                          std::vector<std::string>& violating_joints) const;
                          
    bool checkJointLimits(const Eigen::VectorXd& q) const;
    
    bool checkVelocityLimits(const Eigen::VectorXd& qdot) const;
    
    bool checkEffortLimits(const Eigen::VectorXd& tau) const;
    
    bool enforceJointLimits(Eigen::VectorXd& q) const;
    
    bool enforceVelocityLimit(Eigen::VectorXd& qdot) const;
    
    bool enforceEffortLimit(Eigen::VectorXd& tau) const;
    
    bool mapToEigen(const JointIdMap& id_map, Eigen::VectorXd& eigen_vector) const;
    
    bool mapToEigen(const JointNameMap& name_map, Eigen::VectorXd& eigen_vector) const;
    
    bool eigenToMap(const Eigen::VectorXd& eigen_vector, JointIdMap& id_map) const;
    
    bool eigenToMap(const Eigen::VectorXd& eigen_vector, JointNameMap& name_map) const;
    
    void pushBackJoint(Joint::Ptr joint);
    
    void removeJoint(int i);
    
    bool isVirtual() const;
 
    const std::string &getChainName() const;
 
    const std::string &getBaseLinkName() const;
 
    const std::string &getTipLinkName() const;
 
    const std::string &getChildLinkName(int i) const;
 
    const std::string &getParentLinkName(int i) const;
 
    const std::string& getJointName(int i) const;
    
    
    const std::vector<std::string>& getJointNames() const;
 
    int getJointId(int i) const;
    
    int getChainDofIndex(int joint_id) const;
    
    int getChainDofIndex(const std::string& joint_name) const;
    
    bool hasJoint(int joint_id) const;
    
    bool hasJoint(const std::string& joint_name) const;
    
    const std::vector<int>& getJointIds() const;
 
    int getJointNum() const;
 
    const std::vector< urdf::JointConstSharedPtr > &getUrdfJoints() const;
 
    const std::vector< urdf::LinkConstSharedPtr > &getUrdfLinks() const;
    
    
    void shallowCopy(const KinematicChain& chain);
    
    Joint::ConstPtr getJoint(int i) const;
 
    // Getters for RX
    
    bool getJointPosition(Eigen::VectorXd &q) const;
    bool getMotorPosition(Eigen::VectorXd &q) const;
    bool getJointVelocity(Eigen::VectorXd &qdot) const;
    bool getMotorVelocity(Eigen::VectorXd &qdot) const;
    bool getJointAcceleration(Eigen::VectorXd &qddot) const;
    bool getJointEffort(Eigen::VectorXd &tau) const;
    bool getTemperature(Eigen::VectorXd &temp) const;
 
    bool getJointPosition(JointIdMap &q) const;
    bool getMotorPosition(JointIdMap &q) const;
    bool getJointVelocity(JointIdMap &qdot) const;
    bool getMotorVelocity(JointIdMap &qdot) const;
    bool getJointAcceleration(JointIdMap &qddot) const;
    bool getJointEffort(JointIdMap &tau) const;
    bool getTemperature(JointIdMap &temp) const;
 
    bool getJointPosition(JointNameMap &q) const;
    bool getMotorPosition(JointNameMap &q) const;
    bool getJointVelocity(JointNameMap &qdot) const;
    bool getMotorVelocity(JointNameMap &qdot) const;
    bool getJointAcceleration(JointNameMap &qddot) const;
    bool getJointEffort(JointNameMap &tau) const;
    bool getTemperature(JointNameMap &temp) const;
 
    double getJointPosition(int index) const;
    double getMotorPosition(int index) const;
    double getJointVelocity(int index) const;
    double getMotorVelocity(int index) const;
    double getJointAcceleration(int index) const;
    double getJointEffort(int index) const;
    double getTemperature(int index) const;
    
    // Setters for RX
    
    bool setJointPosition(const Eigen::VectorXd &q);
    bool setMotorPosition(const Eigen::VectorXd &q);
    bool setJointVelocity(const Eigen::VectorXd &qdot);
    bool setMotorVelocity(const Eigen::VectorXd &qdot);
    bool setJointAcceleration(const Eigen::VectorXd &qddot);
    bool setJointEffort(const Eigen::VectorXd &tau);
    bool setTemperature(const Eigen::VectorXd &temp);
 
    bool setJointPosition(const JointIdMap &q);
    bool setMotorPosition(const JointIdMap &q);
    bool setJointVelocity(const JointIdMap &qdot);
    bool setMotorVelocity(const JointIdMap &qdot);
    bool setJointAcceleration(const JointIdMap &qddot);
    bool setJointEffort(const JointIdMap &tau);
    bool setTemperature(const JointIdMap &temp);
 
    bool setJointPosition(const JointNameMap &q);
    bool setMotorPosition(const JointNameMap &q);
    bool setJointVelocity(const JointNameMap &qdot);
    bool setMotorVelocity(const JointNameMap &qdot);
    bool setJointAcceleration(const JointNameMap &qddot);
    bool setJointEffort(const JointNameMap &tau);
    bool setTemperature(const JointNameMap &temp);
 
    bool setJointPosition(int i, double q);
    bool setMotorPosition(int i, double q);
    bool setJointVelocity(int i, double qdot);
    bool setMotorVelocity(int i, double qdot);
    bool setJointAcceleration(int i, double qddot);
    bool setJointEffort(int i, double tau);
    bool setTemperature(int i, double temp);
 
    // Getters for TX
 
    bool getPositionReference(Eigen::VectorXd &q) const;
    bool getVelocityReference(Eigen::VectorXd &qdot) const;
    bool getEffortReference(Eigen::VectorXd &tau) const;
    bool getStiffness(Eigen::VectorXd &K) const;
    bool getDamping(Eigen::VectorXd &D) const;
 
    bool getPositionReference(JointIdMap &q) const;
    bool getVelocityReference(JointIdMap &qdot) const;
    bool getEffortReference(JointIdMap &tau) const;
    bool getStiffness(JointIdMap &K) const;
    bool getDamping(JointIdMap &D) const;
 
    bool getPositionReference(JointNameMap &q) const;
    bool getVelocityReference(JointNameMap &qdot) const;
    bool getEffortReference(JointNameMap &tau) const;
    bool getStiffness(JointNameMap &K) const;
    bool getDamping(JointNameMap &D) const;
 
    double getPositionReference(int index) const;
    double getVelocityReference(int index) const;
    double getEffortReference(int index) const;
    double getStiffness(int index) const;
    double getDamping(int index) const;
 
    // Setters for TX
 
    bool setPositionReference(const Eigen::VectorXd &q);
    bool setVelocityReference(const Eigen::VectorXd &qdot);
    bool setEffortReference(const Eigen::VectorXd &tau);
    bool setStiffness(const Eigen::VectorXd &K);
    bool setDamping(const Eigen::VectorXd &D);
 
    bool setPositionReference(const JointIdMap &q);
    bool setVelocityReference(const JointIdMap &qdot);
    bool setEffortReference(const JointIdMap &tau);
    bool setStiffness(const JointIdMap &K);
    bool setDamping(const JointIdMap &D);
 
    bool setPositionReference(const JointNameMap &q);
    bool setVelocityReference(const JointNameMap &qdot);
    bool setEffortReference(const JointNameMap &tau);
    bool setStiffness(const JointNameMap &K);
    bool setDamping(const JointNameMap &D);
 
    bool setPositionReference(int i, double q);
    bool setVelocityReference(int i, double qdot);
    bool setEffortReference(int i, double tau);
    bool setStiffness(int i, double K);
    bool setDamping(int i, double D);
    
    template <typename... SyncFlags>
    bool syncFrom(const KinematicChain& other, SyncFlags... flags);
    
    void print() const;
    
    void printTracking() const;
 
    friend std::ostream& operator<<(std::ostream& os, const XBot::KinematicChain& c);
    
    
 
protected:
    
    std::map<std::string, XBot::Joint::Ptr> _joint_name_map;
    std::map<int, XBot::Joint::Ptr> _joint_id_map;
    std::vector<XBot::Joint::Ptr> _joint_vector;
    
    std::vector<ForceTorqueSensor::Ptr> _ft_vector;
    std::map<std::string, ForceTorqueSensor::Ptr> _ft_map;
    
    std::vector<ImuSensor::Ptr> _imu_vector;
    std::map<std::string, ImuSensor::Ptr> _imu_map;
//     std::map<std::string, Hand::Ptr> _hand_map;
    
    Joint::Ptr getJointInternal(int i) const;
    
    std::map< std::string, ForceTorqueSensor::Ptr > getForceTorqueInternal() const;
    std::map< std::string, ImuSensor::Ptr > getImuInternal() const;
    
    
private:
 
 
    std::vector<urdf::JointConstSharedPtr> _urdf_joints;
    std::vector<urdf::LinkConstSharedPtr> _urdf_links;
 
    std::vector<std::string> _ordered_joint_name;
    std::vector<int> _ordered_joint_id;
 
    XBot::XBotCoreModel _XBotModel;
 
    std::string _chain_name;
    int _joint_num;
    
    bool _is_virtual;
    
    XBot::Joint::Ptr getJointByName(const std::string& joint_name);
    XBot::Joint::ConstPtr getJointByName(const std::string& joint_name) const;
    XBot::Joint::ConstPtr getJointById(int joint_id) const;
 
 
};
 
std::ostream& operator<<(std::ostream& os, const XBot::KinematicChain& c);
 
template <typename... SyncFlags>
bool KinematicChain::syncFrom(const KinematicChain &other, SyncFlags... flags)
{
    
    for (const XBot::Joint::Ptr & j : other._joint_vector) {
        
        if(hasJoint(j->getJointName())){
                auto this_joint = getJointByName(j->getJointName());
                this_joint->syncFrom(*j, flags...);
        }
    }
 
    return true;
}
    
}
 
#endif // __KINEMATIC_CHAIN_H__