XBotInterface.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 __I_XBOT_INTERFACE_H__
#define __I_XBOT_INTERFACE_H__
 
#include <vector>
#include <map>
#include <memory>
#include <algorithm>
 
#include <yaml-cpp/yaml.h>
 
#include <matlogger2/matlogger2.h>
 
#include <XBotCoreModel/XBotCoreModel.h>
#include <XBotInterface/KinematicChain.h>
#include <XBotInterface/Hand.h>
#include <XBotInterface/TypedefAndEnums.h>
#include <XBotInterface/Logger.hpp>
#include <XBotInterface/ConfigOptions.h>
 
namespace XBot {
 
class XBotInterface {
 
public:
 
 
     typedef std::shared_ptr<XBotInterface> Ptr;
     typedef std::shared_ptr<const XBotInterface> ConstPtr;
 
     // Constructor, copy constructor, copy assignment, virtual destructor, init function
 
     XBotInterface();
     XBotInterface ( const XBotInterface& other );
     XBotInterface& operator= ( const XBotInterface& rhs );
     virtual ~XBotInterface();
 
     bool init (const std::string& path_to_cfg);
     bool init (const ConfigOptions& options);
 
 
     // URDF, SRDF getters
 
     const ConfigOptions& getConfigOptions() const;
 
 
     const std::string& getPathToConfig() const;
 
     const urdf::ModelInterface& getUrdf() const;
 
     const std::string& getUrdfPath() const;
 
     const srdf_advr::Model& getSrdf() const;
 
     const std::string& getSrdfPath() const;
 
     const std::string& getUrdfString() const;
 
     const std::string& getSrdfString() const;
 
 
     // Group states
 
     bool getRobotState ( const std::string& state_name, Eigen::VectorXd& q ) const;
 
     bool getRobotState ( const std::string& state_name, JointIdMap& q ) const;
 
     bool getRobotState ( const std::string& state_name, JointNameMap& q ) const;
 
 
     // Kinematic chains
 
     std::vector<std::string> getChainNames() const;
 
     bool hasChain ( const std::string& chain_name ) const;
 
     int legs() const;
 
     int arms() const;
 
     // Joints
 
     const std::vector<std::string>& getEnabledJointNames() const;
 
     const std::vector<int>& getEnabledJointId() const;
 
     bool hasJoint ( const std::string& joint_name ) const;
 
     bool hasJoint ( int joint_id ) const;
 
     int getJointNum() const;
 
     XBot::Joint::ConstPtr getJointByName ( const std::string& joint_name ) const;
 
     XBot::Joint::ConstPtr getJointByID ( int joint_id ) const;
 
     XBot::Joint::ConstPtr getJointByDofIndex ( int idx ) const;
 
     int getDofIndex ( const std::string& joint_name ) const;
 
     int getDofIndex ( int joint_id ) const;
 
     bool getDofIndex ( const std::string& chain_name, std::vector<int>& ids ) const;
 
     // Logging utilities
 
     void initLog(MatLogger2::Ptr logger, int buffer_size = -1, int interleave = 1, std::string prefix = "");
 
     void log(MatLogger2::Ptr logger, double timestamp, const std::string& prefix = "") const;
 
 
 
     // Force-torque sensors
 
     std::map< std::string, ForceTorqueSensor::ConstPtr > getForceTorque() const;
 
     ForceTorqueSensor::ConstPtr getForceTorque ( const std::string& parent_link_name ) const;
 
     ForceTorqueSensor::ConstPtr getForceTorque ( int ft_id ) const;
 
     // IMU sensors
 
     std::map< std::string, ImuSensor::ConstPtr > getImu();
 
     ImuSensor::ConstPtr getImu ( const std::string& parent_link_name ) const;
 
     ImuSensor::ConstPtr getImu ( int imu_id ) const;
 
    // Hands
 
     std::map< std::string, Hand::Ptr > getHand();
 
     Hand::Ptr getHand ( int hand_id ) const;
 
 
    bool eigenToMap(const Eigen::VectorXd& vector, JointNameMap& name_map) const;
 
    bool eigenToMap(const Eigen::VectorXd& vector, JointIdMap& id_map) const;
 
    bool mapToEigen(const JointNameMap& map, Eigen::VectorXd& vector) const;
 
    bool mapToEigen(const JointIdMap& map, Eigen::VectorXd& vector) 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;
 
     // 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 );
 
     // 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;
 
     // 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 );
 
 
 
     // Joint limits
 
     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;
 
     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( JointIdMap& q ) const;
 
     bool enforceJointLimits( JointNameMap& q ) const;
 
     bool enforceJointLimits( Eigen::VectorXd& q ) const;
 
     bool enforceEffortLimit( Eigen::VectorXd& tau ) const;
 
     bool enforceVelocityLimit( Eigen::VectorXd& qdot ) const;
 
     template <typename... SyncFlags>
     bool sync ( const XBotInterface& other, SyncFlags... flags );
 
     const std::map<std::string, XBot::KinematicChain::Ptr>&  getChainMap() const;
 
     friend std::ostream& operator<< ( std::ostream& os, const XBot::XBotInterface& robot );
    void print() const;
 
    void printTracking() const;
 
protected:
 
    // Joint getters for developers
 
 
    Joint::Ptr getJointByNameInternal(const std::string& joint_name) const;
    Joint::Ptr getJointByIdInternal(int joint_id) const;
    Joint::Ptr getJointByDofIndexInternal(int dof_index) const;
 
 
 
     // Chain getter for developers
 
     const std::map<std::string, ForceTorqueSensor::Ptr>& getForceTorqueInternal() const; // TBD change the Internal with something more meaningful
     const std::map<std::string, ImuSensor::Ptr>& getImuInternal() const; // TBD change the Internal with something more meaningful
 
 
 
     virtual bool init_internal (const ConfigOptions& config) {
          return true;
     }
 
     virtual bool post_init() {
         return true;
     }
 
     const std::vector<std::string>& getModelOrderedChainName() const;
 
 
     // internal XBotCoreModel object: it does the trick using URDF, SRDF and joint map configuration
     XBotCoreModel _XBotModel;
 
     std::map<std::string, XBot::KinematicChain::Ptr> _chain_map;
     std::vector<Joint::Ptr> _ordered_joint_vector;
     std::vector<Joint::Ptr> _joint_vector;
     std::map<std::string, ForceTorqueSensor::Ptr> _ft_map;
     std::map<int, ForceTorqueSensor::Ptr> _ft_id_map;
     std::map<std::string, ImuSensor::Ptr> _imu_map;
     std::map<int, ImuSensor::Ptr> _imu_id_map;
     std::map<std::string, Hand::Ptr> _hand_map;
     std::map<int, Hand::Ptr> _hand_id_map;
     std::vector<std::string> _ordered_chain_names;
 
     std::map<int, int> _joint_id_to_eigen_id;
     std::map<std::string, int> _joint_name_to_eigen_id;
 
private:
 
     int _joint_num;
 
     mutable Eigen::VectorXd _qlink, _qmot, _qdotlink, _qdotmot, _tau, _stiffness, _damping, _temp;
 
     std::string _urdf_string, _srdf_string, _joint_map_string; // TBD add to copy constr/assign
     std::string _urdf_path;
     std::string _srdf_path;
     std::string _joint_map_config;
 
     ConfigOptions _cfg;
 
     std::vector<std::string> _ordered_joint_name;
     std::vector<int> _ordered_joint_id;
 
     std::string _path_to_cfg;
 
     MatLogger2::Ptr _matlogger;
 
};
 
std::ostream& operator<< ( std::ostream& os, const XBot::XBotInterface& robot );
 
template <typename... SyncFlags>
bool XBot::XBotInterface::sync ( const XBot::XBotInterface &other, SyncFlags... flags )
{
     bool success = true;
 
     /* Sync chains */
     for ( const auto & c : other._chain_map ) {
          const std::string &chain_name = c.first;
          const KinematicChain &chain = *c.second;
          if ( _chain_map.count ( chain_name ) ) {
               _chain_map.at ( chain_name )->syncFrom ( chain, flags... );
 
          }
     }
 
     XBot::FlagsParser parser;
     parser.load_flags(flags...);
 
     /* Sync FT */
     if(parser.ft_flag()){
        for(const auto& pair : other._ft_map){
            auto it = _ft_map.find(pair.first);
            if(it != _ft_map.end()){
                *(it->second) = *(pair.second);
            }
            else{
                std::cerr << "ERROR in " << __func__ << "! Unable to find FT \"" << pair.first << " inside this interface!" << std::endl;
                success = false;
            }
        }
     }
 
     /* Sync IMU */
     if(parser.imu_flag()){
         for(const auto& pair : other._imu_map){
            auto it = _imu_map.find(pair.first);
            if(it != _imu_map.end()){
                *(it->second) = *(pair.second);
            }
            else{
                std::cerr << "ERROR in " << __func__ << "! Unable to find IMU \"" << pair.first << " inside this interface!" << std::endl;
                success = false;
            }
        }
 
     }
 
 
     return success;
}
 
}
 
#endif // __I_XBOT_INTERFACE_H__