robot-utils.hh

Go to the documentation of this file.

/*
 * Copyright 2017, 2019
 * LAAS-CNRS
 * A. Del Prete, T. Flayols, O. Stasse, F. Bailly
 *
 */
 
#ifndef __sot_core_robot_utils_H__
#define __sot_core_robot_utils_H__
 
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
#include <map>
#include <pinocchio/fwd.hpp>
// keep pinocchio before boost
 
#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/logger.h>
#include <dynamic-graph/signal-helper.h>
 
#include <boost/assign.hpp>
#include <boost/property_tree/ptree.hpp>
#include <sot/core/matrix-geometry.hh>
 
namespace dynamicgraph {
namespace sot {
 
struct SOT_CORE_EXPORT JointLimits {
  double upper;
  double lower;
 
  JointLimits() : upper(0.0), lower(0.0) {}
 
  JointLimits(double l, double u) : upper(u), lower(l) {}
};
 
typedef Eigen::VectorXd::Index Index;
 
class SOT_CORE_EXPORT ExtractJointMimics {
 public:
  ExtractJointMimics(std::string &robot_model);
 
  const std::vector<std::string> &get_mimic_joints();
 
 private:
  void go_through(boost::property_tree::ptree &pt, int level, int stage);
 
  // Create empty property tree object
  boost::property_tree::ptree tree_;
  std::vector<std::string> mimic_joints_;
  std::string current_joint_name_;
  void go_through_full();
};
 
struct SOT_CORE_EXPORT ForceLimits {
  Eigen::VectorXd upper;
  Eigen::VectorXd lower;
 
  ForceLimits() : upper(Vector6d::Zero()), lower(Vector6d::Zero()) {}
 
  ForceLimits(const Eigen::VectorXd &l, const Eigen::VectorXd &u)
      : upper(u), lower(l) {}
 
  void display(std::ostream &os) const;
};
 
struct SOT_CORE_EXPORT ForceUtil {
  std::map<Index, ForceLimits> m_force_id_to_limits;
  std::map<std::string, Index> m_name_to_force_id;
  std::map<Index, std::string> m_force_id_to_name;
 
  Index m_Force_Id_Left_Hand, m_Force_Id_Right_Hand, m_Force_Id_Left_Foot,
      m_Force_Id_Right_Foot;
 
  void set_name_to_force_id(const std::string &name, const Index &force_id);
 
  void set_force_id_to_limits(const Index &force_id,
                              const dynamicgraph::Vector &lf,
                              const dynamicgraph::Vector &uf);
 
  void create_force_id_to_name_map();
 
  Index get_id_from_name(const std::string &name);
 
  const std::string &get_name_from_id(Index idx);
  std::string cp_get_name_from_id(Index idx);
 
  const ForceLimits &get_limits_from_id(Index force_id);
  ForceLimits cp_get_limits_from_id(Index force_id);
 
  Index get_force_id_left_hand() { return m_Force_Id_Left_Hand; }
 
  void set_force_id_left_hand(Index anId) { m_Force_Id_Left_Hand = anId; }
 
  Index get_force_id_right_hand() { return m_Force_Id_Right_Hand; }
 
  void set_force_id_right_hand(Index anId) { m_Force_Id_Right_Hand = anId; }
 
  Index get_force_id_left_foot() { return m_Force_Id_Left_Foot; }
 
  void set_force_id_left_foot(Index anId) { m_Force_Id_Left_Foot = anId; }
 
  Index get_force_id_right_foot() { return m_Force_Id_Right_Foot; }
 
  void set_force_id_right_foot(Index anId) { m_Force_Id_Right_Foot = anId; }
 
  void display(std::ostream &out) const;
 
};  // struct ForceUtil
 
struct SOT_CORE_EXPORT FootUtil {
  dynamicgraph::Vector m_Right_Foot_Sole_XYZ;
 
  dynamicgraph::Vector m_Right_Foot_Force_Sensor_XYZ;
 
  std::string m_Left_Foot_Frame_Name;
  std::string m_Right_Foot_Frame_Name;
  void display(std::ostream &os) const;
};
 
struct SOT_CORE_EXPORT HandUtil {
  std::string m_Left_Hand_Frame_Name;
  std::string m_Right_Hand_Frame_Name;
  void display(std::ostream &os) const;
};
 
struct SOT_CORE_EXPORT RobotUtil {
 public:
  RobotUtil();
 
  ForceUtil m_force_util;
 
  FootUtil m_foot_util;
 
  HandUtil m_hand_util;
 
  std::vector<Index> m_urdf_to_sot;
 
  std::size_t m_nbJoints;
 
  std::map<std::string, Index> m_name_to_id;
 
  std::map<Index, std::string> m_id_to_name;
 
  std::map<Index, JointLimits> m_limits_map;
 
  std::string m_imu_joint_name;
 
  void create_id_to_name_map();
 
  std::string m_urdf_filename;
 
  dynamicgraph::Vector m_dgv_urdf_to_sot;
 
  const Index &get_id_from_name(const std::string &name);
 
  const std::string &get_name_from_id(Index id);
 
  void set_name_to_id(const std::string &jointName, const Index &jointId);
 
  void set_urdf_to_sot(const std::vector<Index> &urdf_to_sot);
  void set_urdf_to_sot(const dynamicgraph::Vector &urdf_to_sot);
 
  void set_joint_limits_for_id(const Index &idx, const double &lq,
                               const double &uq);
 
  bool joints_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
 
  bool joints_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
 
  bool velocity_urdf_to_sot(ConstRefVector q_urdf, ConstRefVector v_urdf,
                            RefVector v_sot);
 
  bool velocity_sot_to_urdf(ConstRefVector q_urdf, ConstRefVector v_sot,
                            RefVector v_urdf);
 
  bool config_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
  bool config_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
 
  bool base_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
  bool base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
 
  const JointLimits &get_joint_limits_from_id(Index id);
  JointLimits cp_get_joint_limits_from_id(Index id);
 
  void sendMsg(const std::string &msg, MsgType t = MSG_TYPE_INFO,
               const std::string &lineId = "");
 
  void setLoggerVerbosityLevel(LoggerVerbosity lv) { logger_.setVerbosity(lv); }
 
  LoggerVerbosity getLoggerVerbosityLevel() { return logger_.getVerbosity(); };
 
  void display(std::ostream &os) const;
 
  template <typename Type>
  void set_parameter(const std::string &parameter_name,
                     const Type &parameter_value) {
    try {
      typedef boost::property_tree::ptree::path_type path;
      path apath(parameter_name, '/');
      property_tree_.put<Type>(apath, parameter_value);
    } catch (const boost::property_tree::ptree_error &e) {
      std::ostringstream oss;
      oss << "Robot utils: parameter path is invalid " << '\n'
          << " for set_parameter(" << parameter_name << ")\n"
          << e.what() << std::endl;
      sendMsg(oss.str(), MSG_TYPE_ERROR);
      return;
    }
  }
 
  template <typename Type>
  Type get_parameter(const std::string &parameter_name) {
    try {
      boost::property_tree::ptree::path_type apath(parameter_name, '/');
      const Type &res = property_tree_.get<Type>(apath);
 
      return res;
    } catch (const boost::property_tree::ptree_error &e) {
      std::ostringstream oss;
      oss << "Robot utils: parameter path is invalid " << '\n'
          << " for get_parameter(" << parameter_name << ")\n"
          << e.what() << std::endl;
      sendMsg(oss.str(), MSG_TYPE_ERROR);
      throw;
    }
  }
  boost::property_tree::ptree &get_property_tree();
 
 protected:
  Logger logger_;
 
  std::map<std::string, std::string> parameters_strings_;
 
  boost::property_tree::ptree property_tree_;
};  // struct RobotUtil
 
typedef std::shared_ptr<RobotUtil> RobotUtilShrPtr;
 
RobotUtilShrPtr RefVoidRobotUtil();
RobotUtilShrPtr getRobotUtil(std::string &robotName);
bool isNameInRobotUtil(std::string &robotName);
RobotUtilShrPtr createRobotUtil(std::string &robotName);
std::shared_ptr<std::vector<std::string> > getListOfRobots();
 
bool base_se3_to_sot(ConstRefVector pos, ConstRefMatrix R, RefVector q_sot);
 
}  // namespace sot
}  // namespace dynamicgraph
 
#endif  // sot_torque_control_common_h_