talos-common.hh

Go to the documentation of this file.

/*
 * Copyright 2014, Oscar E. Ramos Ponce, LAAS-CNRS
 *
 */

#ifndef __sot_torque_control_talos_common_H__
#define __sot_torque_control_talos_common_H__

/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

#if defined (WIN32)
#  if defined (talos_common_EXPORTS)
#    define TALOSCOMMON_EXPORT __declspec(dllexport)
#  else
#    define TALOSCOMMON_EXPORT __declspec(dllimport)
#  endif
#else
#  define TALOSCOMMON_EXPORT
#endif


/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

#include <map>
#include <initializer_list>
#include "boost/assign.hpp"
/* HELPER */
#include <dynamic-graph/signal-helper.h>
#include <sot/core/matrix-geometry.hh>

namespace dynamicgraph {
namespace sot {
namespace torque_control {

#define N_JOINTS 32

#define RIGHT_FOOT_FRAME_NAME "RLEG_JOINT5"
#define LEFT_FOOT_FRAME_NAME  "LLEG_JOINT5"

const double DEFAULT_MAX_DELTA_Q = 0.1; 

const double DEFAULT_MAX_CURRENT = 5;     

// Information on the location of the IMU and F/T sensors of HRP-2
// copied from the urdf file (in stacks/hrp2/hrp2_14_description/urdf):
//      <joint name="AccelerometerJoint" type="fixed">
//        <origin xyz="-0.13 0.0 0.118" rpy="0.0 0.0 0.0"/>
//        <joint name="RightFootForceSensorJoint" type="fixed">
//          <origin xyz="0.0 0.0 -0.105" rpy="0.0 0.0 0.0"/>
//        <joint name="LeftFootForceSensorJoint" type="fixed">
//          <origin xyz="0.0 0.0 -0.105" rpy="0.0 0.0 0.0"/>
//        <joint name="RightHandForceSensorJoint" type="fixed">
//          <origin xyz="0.005 0.0 -0.05925" rpy="0.0 0.0 0.0"/>
//        <joint name="LeftHandForceSensorJoint" type="fixed">
//          <origin xyz="0.005 0.0 -0.05925" rpy="0.0 0.0 0.0"/>

const double IMU_XYZ[3]                     = { -0.13, 0.0,  0.118}; // not updated for talos/talos

const double RIGHT_FOOT_FORCE_SENSOR_XYZ[3] = {0.0,   0.0, -0.0}; // not updated for talos/talos
const double LEFT_FOOT_FORCE_SENSOR_XYZ[3]  = {0.0,   0.0, -0.0}; // not updated for talos/talos
const double RIGHT_HAND_FORCE_SENSOR_XYZ[3] = {0.0, 0.0, -0.051};
const double LEFT_HAND_FORCE_SENSOR_XYZ[3]  = {0.005, 0.0, -0.051};

const double RIGHT_HAND_FORCE_SENSOR_Z_ROTATION = -0.5 * M_PI; // not updated for talos/talos
const double LEFT_HAND_FORCE_SENSOR_Z_ROTATION  = -0.5 * M_PI; // not updated for talos/talos

const double RIGHT_FOOT_SOLE_XYZ[3]         = {0.0,   0.0, -0.107};
const double LEFT_FOOT_SOLE_XYZ[3]          = {0.0,   0.0, -0.107};

const double RIGHT_HAND_GRIPPER_XYZ[3]         = {0.0,   0.0, -0.02875};
const double LEFT_HAND_GRIPPER_XYZ[3]          = {0.0,   0.0, 0.02025};

const double RIGHT_FOOT_FORCE_SENSOR_MASS_PERCENTAGE = 0.65; // not updated for talos/talos
const double LEFT_FOOT_FORCE_SENSOR_MASS_PERCENTAGE  = 0.65; // not updated for talos/talos
const double RIGHT_HAND_FORCE_SENSOR_MASS_PERCENTAGE = 0.75; // not updated for talos/talos
const double LEFT_HAND_FORCE_SENSOR_MASS_PERCENTAGE  = 0.75; // not updated for talos/talos

struct JointLimits {
  double upper;
  double lower;

  JointLimits():
    upper(0.0),
    lower(0.0)
  {}

  JointLimits(double l, double u):
    upper(u),
    lower(l)
  {}
};

struct JointUtil {
  static std::map<unsigned int, JointLimits> create_id_2_limits_map() {
    std::map<unsigned int, JointLimits> m;
    m[0] = JointLimits(-0.349065850399, 1.57079632679);   // left leg 1
    m[1] = JointLimits(-0.5236, 0.5236);                  // left leg 2
    m[2] = JointLimits(-2.095, 0.7);                      // left leg 3
    m[3]  = JointLimits(0.0, 2.618);                      // left leg 4
    m[4] = JointLimits(-1.309, 0.768);                    // left leg 5
    m[5] = JointLimits(-0.5236, 0.5236);                  // left leg 6
    m[6] = JointLimits(-1.57079632679, 0.349065850399);       // right leg 1
    m[7] = JointLimits(-0.5236, 0.5236);                      // right leg 2
    m[8] = JointLimits(-2.095, 0.7);                          // right leg 3
    m[9]  = JointLimits(0.0, 2.618);                          // right leg 4
    m[10] = JointLimits(-1.309, 0.768);                       // right leg 5
    m[11] = JointLimits(-0.5236, 0.5236);                     // right leg 6
    m[12]  = JointLimits(-1.308996939, 1.308996939);              // torso 1
    m[13]  = JointLimits(-0.261799387799, 0.785398163397);        // torso 2
    m[14]  = JointLimits(-1.57079632679, 0.523598775598); // left arm 1 shoulder
    m[15]  = JointLimits(0.0, 2.87979326579);             // left arm 2 shoulder
    m[16] = JointLimits(-2.44346095279, 2.44346095279);   // left arm 3 shoulder
    m[17] = JointLimits(-2.35619449019, 0.0);             // left arm 4 elbow
    m[18] = JointLimits(-2.53072741539, 2.53072741539);   // left arm 5 wrist
    m[19] = JointLimits(-1.3962634016, 1.3962634016);     // left arm 6 wrist
    m[20] = JointLimits(-0.698131700798, 0.698131700798); // left arm 7 wrist
    m[21]  = JointLimits(-1.0471975512, 0.0);                     // left gripper
    m[22]  = JointLimits(-0.523598775598, 1.57079632679);     // right arm 1 shoulder
    m[23] = JointLimits(-2.87979326579, 0.0);                 // right arm 2 shoulder
    m[24] = JointLimits(-2.44346095279, 2.44346095279);       // right arm 3 shoulder
    m[25] = JointLimits(-2.35619449019, 0.0);                 // right arm 4 elbow
    m[26] = JointLimits(-2.53072741539, 2.53072741539);       // right arm 5 wrist
    m[27] = JointLimits(-1.3962634016, 1.3962634016);         // right arm 6 wrist
    m[28]  = JointLimits(-0.698131700798, 0.698131700798);    // right arm 7 wrist
    m[29]  = JointLimits(-1.0471975512, 0.0);          //         right gripper
    m[30]  = JointLimits(-0.261799387799, 0.785398163397);    // head 1
    m[31]  = JointLimits(-1.308996939, 1.308996939);          // head 2
    return m;
  }

  static std::map<std::string, unsigned int> create_name_2_id_map() {
    std::map<std::string, unsigned int> m;
    m["lhy"] = 0;   // left hip yaw
    m["lhr"] = 1;   // left hip roll
    m["lhp"] = 2;   // left hip pitch
    m["lk"]  = 3;   // left knee
    m["lap"] = 4;   // left ankle pitch
    m["lar"] = 5;   // left ankle roll
    m["rhy"] = 6;   // right hip yaw
    m["rhr"] = 7;   // right hip roll
    m["rhp"] = 8;   // right hip pitch
    m["rk"]  = 9;   // right knee
    m["rap"] = 10;  // right ankle pitch
    m["rar"] = 11;  // right ankle roll
    m["ty"]  = 12;  // torso yaw
    m["tp"]  = 13;  // torso pitch
    m["lsy"]  = 14;  // left shoulder yaw
    m["lsr"]  = 15;  // left shoulder roll
    m["lay"]  = 16;  // left arm yaw
    m["le"]   = 17;  // left elbow
    m["lwy"]  = 18;  // left wrist yaw
    m["lwp"]  = 19;  // left wrist pitch
    m["lwr"]  = 20;  // left wrist roll
    m["lh"]   = 21;  // left hand
    m["rsy"]  = 22;  // right shoulder yaw
    m["rsr"]  = 23;  // right shoulder roll
    m["ray"]  = 24;  // right arm yaw
    m["re"]   = 25;  // right elbow
    m["rwy"]  = 26;  // right wrist yaw
    m["rwp"]  = 27;  // right wrist pitch
    m["rwr"]  = 28;  // right wrist roll
    m["rh"]   = 29;  // right hand
    m["hp"]   = 30;  // head pitch
    m["hy"]   = 31;  // head yaw
    return m;
  }

  static std::map<unsigned int, std::string> create_id_2_name_map(
    const std::map<std::string, unsigned int>& name_2_id_map) {
    std::map<unsigned int, std::string> m;
    std::map<std::string, unsigned int>::const_iterator it;
    for (it = name_2_id_map.begin(); it != name_2_id_map.end(); it++)
      m[it->second] = it->first;
    return m;
  }

  static int get_id_from_name(std::string name) {
    std::map<std::string, unsigned int>::const_iterator iter = name_2_id.find(name);
    if (iter == name_2_id.end())
      return -1;
    return iter->second;
  }

  static std::string get_name_from_id(unsigned int id) {
    std::map<unsigned int, std::string>::const_iterator iter = id_2_name.find(id);
    if (iter == id_2_name.end())
      return "Joint name not found";
    return iter->second;
  }

  static JointLimits get_limits_from_id(unsigned int id) {
    std::map<unsigned int, JointLimits>::const_iterator iter = id_2_limits.find(id);
    if (iter == id_2_limits.end())
      return JointLimits(0.0, 0.0);
    return iter->second;
  }

  static const std::map<std::string, unsigned int> name_2_id;
  static const std::map<unsigned int, std::string> id_2_name;
  static const std::map<unsigned int, JointLimits> id_2_limits;
};
const std::map<std::string, unsigned int> JointUtil::name_2_id = JointUtil::create_name_2_id_map();
const std::map<unsigned int, std::string> JointUtil::id_2_name = JointUtil::create_id_2_name_map(JointUtil::name_2_id);
const std::map<unsigned int, JointLimits> JointUtil::id_2_limits = JointUtil::create_id_2_limits_map();



struct ForceLimits {
  Eigen::VectorXd upper;
  Eigen::VectorXd lower;

  ForceLimits():
    upper(dynamicgraph::sot::Vector6d::Zero()),
    lower(dynamicgraph::sot::Vector6d::Zero())
  {}

  ForceLimits(const Eigen::VectorXd& l, const Eigen::VectorXd& u):
    upper(u),
    lower(l)
  {}
};

enum ForceID {
  FORCE_ID_RIGHT_FOOT = 0,
  FORCE_ID_LEFT_FOOT  = 1,
  FORCE_ID_RIGHT_HAND = 2,
  FORCE_ID_LEFT_HAND  = 3
};

struct ForceUtil {
  static std::map<unsigned int, ForceLimits> create_id_2_limits_map() {
    dynamicgraph::sot::Vector6d fMax, fMin;
    fMax << 100.0, 100.0, 300.0, 80.0, 80.0, 30.0;
    fMin = -fMax;
    std::map<unsigned int, ForceLimits> m;
    m[0] = ForceLimits(fMin, fMax);  // right foot
    m[1] = ForceLimits(fMin, fMax);  // left foot
    m[2] = ForceLimits(fMin, fMax);   // right hand
    m[3] = ForceLimits(fMin, fMax); // left hand
    return m;
  }

  static std::map<std::string, unsigned int> create_name_2_id_map() {
    std::map<std::string, unsigned int> m;
    m["rf"] = 0;   // right foot
    m["lf"] = 1;   // left foot
    m["rh"] = 2;   // right hand
    m["lh"] = 3;   // left hand
    return m;
  }

  static std::map<unsigned int, std::string> create_id_2_name_map(
    const std::map<std::string, unsigned int>& name_2_id_map) {
    std::map<unsigned int, std::string> m;
    std::map<std::string, unsigned int>::const_iterator it;
    for (it = name_2_id_map.begin(); it != name_2_id_map.end(); it++)
      m[it->second] = it->first;
    return m;
  }

  static int get_id_from_name(std::string name) {
    std::map<std::string, unsigned int>::const_iterator iter = name_2_id.find(name);
    if (iter == name_2_id.end())
      return -1;
    return iter->second;
  }

  static std::string get_name_from_id(unsigned int id) {
    std::map<unsigned int, std::string>::const_iterator iter = id_2_name.find(id);
    if (iter == id_2_name.end())
      return "Force name not found";
    return iter->second;
  }

  static ForceLimits get_limits_from_id(unsigned int id) {
    std::map<unsigned int, ForceLimits>::const_iterator iter = id_2_limits.find(id);
    if (iter == id_2_limits.end())
      return ForceLimits();
    return iter->second;
  }

  static const std::map<std::string, unsigned int> name_2_id;
  static const std::map<unsigned int, std::string> id_2_name;
  static const std::map<unsigned int, ForceLimits> id_2_limits;
};
const std::map<std::string, unsigned int> ForceUtil::name_2_id = ForceUtil::create_name_2_id_map();
const std::map<unsigned int, std::string> ForceUtil::id_2_name = ForceUtil::create_id_2_name_map(ForceUtil::name_2_id);
const std::map<unsigned int, ForceLimits> ForceUtil::id_2_limits = ForceUtil::create_id_2_limits_map();

bool base_se3_to_sot(dynamicgraph::sot::ConstRefVector pos,
                     dynamicgraph::sot::ConstRefMatrix R,
                     dynamicgraph::sot::RefVector q_sot);
bool base_urdf_to_sot(dynamicgraph::sot::ConstRefVector q_urdf, dynamicgraph::sot::RefVector q_sot);
bool base_sot_to_urdf(dynamicgraph::sot::ConstRefVector q_sot, dynamicgraph::sot::RefVector q_urdf);
bool config_urdf_to_sot(dynamicgraph::sot::ConstRefVector q_urdf, dynamicgraph::sot::RefVector q_sot);
bool config_sot_to_urdf(dynamicgraph::sot::ConstRefVector q_sot, dynamicgraph::sot::RefVector q_urdf);
bool velocity_urdf_to_sot(dynamicgraph::sot::ConstRefVector v_urdf, dynamicgraph::sot::RefVector v_sot);
bool velocity_sot_to_urdf(dynamicgraph::sot::ConstRefVector v_sot, dynamicgraph::sot::RefVector v_urdf);
bool joints_urdf_to_sot(dynamicgraph::sot::ConstRefVector q_urdf, dynamicgraph::sot::RefVector q_sot);
bool joints_sot_to_urdf(dynamicgraph::sot::ConstRefVector q_sot, dynamicgraph::sot::RefVector q_urdf);

}    // namespace torque_control
}      // namespace sot
}        // namespace dynamicgraph



#endif // #ifndef __sot_torque_control_talos_common_H__