XBotCoreModel.h

Go to the documentation of this file.

/*
   Copyright (C) 2016 Italian Institute of Technology
 
   Developer:
       Luca Muratore (2016-, luca.muratore@iit.it)
   
*/
 
#ifndef __X_BOT_CORE_MODEL_HPP__
#define __X_BOT_CORE_MODEL_HPP__
 
#include <XBotCoreModel/IXBotModel.h>
 
#include <srdfdom_advr/model.h>
#include <urdf_parser/urdf_parser.h>
#include <kdl_parser/kdl_parser.hpp>
 
#include <fstream>
#include <sstream>
#include <stdexcept>
#include <algorithm>
 
#include <yaml-cpp/yaml.h>
 
#define CHAIN_PER_GROUP 1
 
namespace XBot 
{
    class XBotCoreModel;
}
 
typedef std::map<int, std::string>  Rid2JointMap;
typedef std::map<std::string, int>  Joint2RidMap;
 
class XBot::XBotCoreModel : public srdf_advr::Model,
                            public XBot::IXBotModel
{
private:
    
    std::string urdf_string, srdf_string, joint_map_string;
    std::string srdf_path;
    urdf::ModelInterfaceSharedPtr urdf_model;
    KDL::Tree robot_tree;
    
        
    std::map<std::string, std::vector<int>> robot;
    
    std::map<std::string, std::vector<std::string>> robot_string;
    
    std::map<std::string, int> ft_sensors;
    
    std::map<std::string, int> imu_sensors;
    
    std::map<std::string, int> hands;
    
    Rid2JointMap rid2joint;
    
    Joint2RidMap joint2rid;
    
    // enabled joint number of the robot
    int joint_num = 0;
    
    // vector for the chain names
    std::vector<std::string> chain_names;
    
    // vector for the chain names
    std::vector<std::string> ordered_chain_names;
    
    // vector for the chain names of the legs ORDERED as the SRDF
    std::vector<std::string> legs_names;
    
    // vector for the chain names of the arms ORDERED as the SRDF
    std::vector<std::string> arms_names;
    
    // vector for the disabled joints
    std::vector<std::string> disabled_joint_names;
    
    // map for the enabled joints in chains
    std::map<std::string, std::vector<std::string>> enabled_joints_in_chains;
    
    // map for the disabled joints in chains
    std::map<std::string, std::vector<std::string>> disabled_joints_in_chains;
 
    
    
    urdf::ModelInterfaceSharedPtr loadURDF(const std::string& filename);
    
    
    bool parseSRDF();
    
    bool get_joints_in_chain( std::string base_link, 
                              std::string tip_link,
                              std::vector<std::string>& enabled_joints_in_chain,
                              std::vector<std::string>& disabled_joints_in_chain);
 
    
    
    void parseJointMap(const YAML::Node& joint_map);
 
public:
    
    XBotCoreModel(void){}
    
    urdf::ModelInterfaceSharedPtr get_urdf_model(void) const
    {
        return urdf_model; 
    }
    
    KDL::Tree get_robot_tree(void)
    { 
        return robot_tree;
    }
    
    const std::vector<std::string>& get_chain_names(void) const 
    {
        return chain_names;
    }
    
    const std::vector<std::string>& get_ordered_chain_names(void) const 
    {
        return ordered_chain_names;
    }
    
    bool init(const std::string& urdf_filename, const std::string& srdf_filename,
              const std::string& joint_map_config);
    
    
    bool init_from_string(const std::string& urdf, const std::string& srdf,
              const std::string& joint_map_config);
 
    bool init(const std::string& urdf_filename, const std::string& srdf_filename);
    
    void generate_robot(void);
    
    virtual std::map<std::string, std::vector<int>> get_robot(void) final
    {
        return robot;
    }
    
    virtual std::map<std::string,int> get_ft_sensors(void) final
    {
        return ft_sensors;
    }
    
    virtual std::map<std::string, int> get_imu_sensors() final
    {
        return imu_sensors;
    }
    
    virtual std::map<std::string, int> get_hands() final
    {
        return hands;
    }
          
    virtual std::string rid2Joint(int rId) final
    {
        return rid2joint.find(rId) != rid2joint.end() ? rid2joint[rId] : ""; 
    }
    
    virtual int joint2Rid(std::string joint_name) final
    {
        return joint2rid.find(joint_name) != joint2rid.end() ? joint2rid[joint_name] : 0;
    }
    
 
    bool get_enabled_joints_in_chain( std::string chain_name, std::vector<std::string>& enabled_joints) const;
 
 
    bool get_disabled_joints_in_chain( std::string chain_name, std::vector<std::string>& disabled_joints) const;
    
    
    void get_enabled_joint_names(std::vector<std::string>& joint_names) const;
    
    void get_enabled_joint_ids(std::vector<int>& joint_ids) const;
    
    bool get_enabled_joint_ids_in_chain(std::string chain_name, std::vector<int>& joint_ids) const;
    
    int get_joint_num() const;
    
    int get_joint_num(std::string chain_name) const;
    
    const std::string& get_urdf_string() const;
    
    const std::string& get_srdf_string() const;
    
    const std::vector<std::string>& get_legs_chain() const;
    
    const std::vector<std::string>& get_arms_chain() const;
    
    bool check_joint_limits() const;
    
    ~XBotCoreModel() 
    {
    }
    
};
 
#endif //__X_BOT_CORE_MODEL_HPP__