Joint.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 __JOINT_H__
#define __JOINT_H__
 
#include <string>
#include <memory>
#include <iostream>
#include <srdfdom_advr/model.h>
#include <XBotInterface/SyncFlags.h>
#include <bprinter/table_printer.h>
 
 
#include <XBotInterface/ControlMode.h>
 
 
 
namespace XBot
{
 
    class KinematicChain;
    class RobotChain;
    class RobotInterface;
    class XBotInterface;
 
class Joint
{
 
 
public:
 
    typedef std::shared_ptr<Joint> Ptr;
    typedef std::shared_ptr<const Joint> ConstPtr;
 
    friend XBot::KinematicChain;
    friend XBot::RobotChain;
    friend XBot::XBotInterface;
    friend XBot::RobotInterface;
 
    Joint();
 
    Joint(const std::string &joint_name,
          int joint_id,
          urdf::JointConstSharedPtr urdf_joint,
          const std::string &chain_name);
 
    const std::string &getJointName() const;
 
    int getJointId() const;
 
    const std::string &getChainName() const;
 
    void getJointLimits(double& qmin, double& qmax) const;
 
    void getVelocityLimit(double& qdot_max) const;
 
    void getEffortLimit(double& tau_max) const;
 
    bool checkJointLimits(double q) const;
 
    bool checkVelocityLimit(double qdot) const;
 
    bool checkEffortLimit(double tau) const;
 
 
    void enforceJointLimits(double& q) const;
 
    void enforceVelocityLimit(double& qdot) const;
 
    void enforceEffortLimit(double& tau) const;
 
 
 
 
    urdf::JointConstSharedPtr getUrdfJoint() const;
 
    friend std::ostream& operator<< ( std::ostream& os, const XBot::Joint& j );
 
    void print() const;
 
    void printTracking() const;
 
    bool isFixedControlledJoint() const;
 
 
    // RX VALUES //
 
 
    void setJointPosition(double link_pos);
 
    void setMotorPosition(double motor_pos);
 
    void setJointVelocity(double link_vel);
 
    void setMotorVelocity(double motor_vel);
 
    void setJointAcceleration(double link_acc);
 
    void setJointEffort(double effort);
 
    void setTemperature(double temperature);
 
 
    double getJointPosition() const;
 
    double getMotorPosition() const;
 
    double getJointVelocity() const;
 
    double getMotorVelocity() const;
 
    double getJointAcceleration() const;
 
    double getJointEffort() const;
 
    double getTemperature() const;
 
 
    // TX VALUES //
 
 
    void setPositionReference(double pos_ref);
 
    void setVelocityReference(double vel_ref);
 
    void setEffortReference(double effort_ref);
 
    void setStiffness(double stiffness);
 
    void setDamping(double damping);
 
 
    double getPositionReference() const;
 
    double getVelocityReference() const;
 
    double getEffortReference() const;
 
    double getStiffness() const;
 
    double getDamping() const;
 
    // CONTROL MODE //
 
    void setControlMode(const ControlMode& control_mode);
 
    void getControlMode(ControlMode& control_mode) const;
 
 
protected:
 
    void setJointName(const std::string &joint_name);
 
    void setJointId(int joint_id);
 
    template <typename... SyncFlags>
    bool syncFrom(const Joint &other, SyncFlags... flags);
 
    template <typename... SyncFlags>
    bool setReferenceFrom(const Joint& other, SyncFlags... flags);
 
    const XBot::Joint& operator<< ( const XBot::Joint& from);
 
    bool isVirtualJoint() const;
 
 
 
 
private:
    // Joint info //
 
    std::string _joint_name;
 
    int _joint_id;
 
    std::string _chain_name;
 
    // TBD implement getter and setter
    std::string _joint_control_type;
 
 
    urdf::JointConstSharedPtr _urdf_joint;
 
 
    // RX from board //
 
    double _link_pos;
 
    double _motor_pos;
 
    double _link_vel;
 
    double _motor_vel;
 
    double _link_acc;
 
    double _effort;
 
    double _temperature;
 
    // TX to board //
 
    double _pos_ref;
 
    double _vel_ref;
 
    double _effort_ref;
 
    double _stiffness;
 
    double _damping;
 
    ControlMode _control_mode;
 
    void init();
 
 
 
};
 
std::ostream& operator<< ( std::ostream& os, const XBot::Joint& j );
 
 
 
 
template <typename... SyncFlags>
bool XBot::Joint::syncFrom(const XBot::Joint &other, SyncFlags... flags)
{
 
 
    if(_joint_name != other._joint_name){
        std::cerr << "ERROR in " << __func__ << "! Attempt to synchronize joints with different names!" << std::endl;
        return false;
    }
 
    bool sync_position = false,
         sync_velocity = false,
         sync_acceleration = false,
         sync_effort = false,
         sync_stiffness = false,
         sync_damping = false,
         sync_ft = false,
         sync_imu = false,
         motor_side = false;
 
    parseFlags(sync_position,
               sync_velocity,
               sync_acceleration,
               sync_effort,
               sync_stiffness,
               sync_damping,
               sync_ft,
               sync_imu,
               motor_side,
               flags...);
 
 
 
 
    if(sync_position){
 
        // RX
        if(motor_side){
            _link_pos = other._motor_pos;
        }
        else{
            _link_pos = other._link_pos;
        }
        
        _motor_pos = other._motor_pos;
 
        // TX
        _pos_ref = other._pos_ref;
    }
 
    if(sync_velocity){
 
        // RX
        if(motor_side){
            _link_vel = other._motor_vel;
        }
        else{
            _link_vel = other._link_vel;
        }
        
        _motor_vel = other._motor_vel;
 
        // TX
        _vel_ref = other._vel_ref;
    }
 
    if(sync_acceleration){
 
        _link_acc = other._link_acc;
    }
 
    if(sync_effort){
 
        // RX
        _effort = other._effort;
 
        // TX
        _effort_ref = other._effort_ref;
    }
 
    if(sync_stiffness){
        _stiffness = other._stiffness;
    }
 
    if(sync_damping){
        _damping = other._damping;
    }
 
 
    // RX from board //
 
    _temperature = other._temperature;
 
    return true;
 
}
 
template <typename... SyncFlags>
bool XBot::Joint::setReferenceFrom(const XBot::Joint& other, SyncFlags... flags)
{
    if(_joint_name != other._joint_name){
        std::cerr << "ERROR in " << __func__ << "! Attempt to set reference from joint with different name! " << std::endl;
        return false;
    }
 
    bool sync_position = false,
         sync_velocity = false,
         sync_acceleration = false,
         sync_effort = false,
         sync_stiffness = false,
         sync_damping = false,
         sync_ft = false,
         sync_imu = false,
         sync_motor_side = false;
 
    parseFlags(sync_position,
               sync_velocity,
               sync_acceleration,
               sync_effort,
               sync_stiffness,
               sync_damping,
               sync_ft,
               sync_imu,
               sync_motor_side,
               flags...);
 
    // TX to board //
 
    if(sync_position){
        _pos_ref = other._link_pos;
    }
    if(sync_velocity){
        _vel_ref = other._link_vel;
    }
    if(sync_effort){
        _effort_ref = other._effort;
    }
    if(sync_stiffness){
        _stiffness = other._stiffness;
    }
    if(sync_damping){
        _damping = other._damping;
    }
 
    return true;
}
 
 
 
}
 
#endif // __JOINT_H__