6 #ifndef __sot_torque_control_talos_common_H__
7 #define __sot_torque_control_talos_common_H__
14 # if defined (talos_common_EXPORTS)
15 # define TALOSCOMMON_EXPORT __declspec(dllexport)
17 # define TALOSCOMMON_EXPORT __declspec(dllimport)
20 # define TALOSCOMMON_EXPORT
29 #include <initializer_list>
30 #include "boost/assign.hpp"
32 #include <dynamic-graph/signal-helper.h>
33 #include <sot/core/matrix-geometry.hh>
37 namespace torque_control {
41 #define RIGHT_FOOT_FRAME_NAME "RLEG_JOINT5"
42 #define LEFT_FOOT_FRAME_NAME "LLEG_JOINT5"
62 const double IMU_XYZ[3] = { -0.13, 0.0, 0.118};
106 std::map<unsigned int, JointLimits> m;
107 m[0] =
JointLimits(-0.349065850399, 1.57079632679);
113 m[6] =
JointLimits(-1.57079632679, 0.349065850399);
120 m[13] =
JointLimits(-0.261799387799, 0.785398163397);
121 m[14] =
JointLimits(-1.57079632679, 0.523598775598);
123 m[16] =
JointLimits(-2.44346095279, 2.44346095279);
125 m[18] =
JointLimits(-2.53072741539, 2.53072741539);
127 m[20] =
JointLimits(-0.698131700798, 0.698131700798);
129 m[22] =
JointLimits(-0.523598775598, 1.57079632679);
131 m[24] =
JointLimits(-2.44346095279, 2.44346095279);
133 m[26] =
JointLimits(-2.53072741539, 2.53072741539);
135 m[28] =
JointLimits(-0.698131700798, 0.698131700798);
137 m[30] =
JointLimits(-0.261799387799, 0.785398163397);
143 std::map<std::string, unsigned int> m;
180 const std::map<std::string, unsigned int>& name_2_id_map) {
181 std::map<unsigned int, std::string> m;
182 std::map<std::string, unsigned int>::const_iterator it;
183 for (it = name_2_id_map.begin(); it != name_2_id_map.end(); it++)
184 m[it->second] = it->first;
193 std::map<std::string, unsigned int>::const_iterator iter =
name_2_id.find(name);
204 std::map<unsigned int, std::string>::const_iterator iter =
id_2_name.find(
id);
206 return "Joint name not found";
215 std::map<unsigned int, JointLimits>::const_iterator iter =
id_2_limits.find(
id);
221 static const std::map<std::string, unsigned int>
name_2_id;
222 static const std::map<unsigned int, std::string>
id_2_name;
256 dynamicgraph::sot::Vector6d fMax, fMin;
257 fMax << 100.0, 100.0, 300.0, 80.0, 80.0, 30.0;
259 std::map<unsigned int, ForceLimits> m;
268 std::map<std::string, unsigned int> m;
277 const std::map<std::string, unsigned int>& name_2_id_map) {
278 std::map<unsigned int, std::string> m;
279 std::map<std::string, unsigned int>::const_iterator it;
280 for (it = name_2_id_map.begin(); it != name_2_id_map.end(); it++)
281 m[it->second] = it->first;
290 std::map<std::string, unsigned int>::const_iterator iter =
name_2_id.find(name);
301 std::map<unsigned int, std::string>::const_iterator iter =
id_2_name.find(
id);
303 return "Force name not found";
312 std::map<unsigned int, ForceLimits>::const_iterator iter =
id_2_limits.find(
id);
318 static const std::map<std::string, unsigned int>
name_2_id;
319 static const std::map<unsigned int, std::string>
id_2_name;
327 dynamicgraph::sot::ConstRefMatrix R,
328 dynamicgraph::sot::RefVector q_sot);
329 bool base_urdf_to_sot(dynamicgraph::sot::ConstRefVector q_urdf, dynamicgraph::sot::RefVector q_sot);
330 bool base_sot_to_urdf(dynamicgraph::sot::ConstRefVector q_sot, dynamicgraph::sot::RefVector q_urdf);
331 bool config_urdf_to_sot(dynamicgraph::sot::ConstRefVector q_urdf, dynamicgraph::sot::RefVector q_sot);
332 bool config_sot_to_urdf(dynamicgraph::sot::ConstRefVector q_sot, dynamicgraph::sot::RefVector q_urdf);
335 bool joints_urdf_to_sot(dynamicgraph::sot::ConstRefVector q_urdf, dynamicgraph::sot::RefVector q_sot);
336 bool joints_sot_to_urdf(dynamicgraph::sot::ConstRefVector q_sot, dynamicgraph::sot::RefVector q_urdf);
const double RIGHT_FOOT_FORCE_SENSOR_MASS_PERCENTAGE
Percentage of mass of the link that is measured by the F/T sensors.
const double RIGHT_FOOT_SOLE_XYZ[3]
Position of the foot soles w.r.t. the frame of the foot.
const double RIGHT_HAND_FORCE_SENSOR_XYZ[3]
const double DEFAULT_MAX_CURRENT
max joint position tracking error [rad]
const double DEFAULT_MAX_DELTA_Q
const double LEFT_HAND_FORCE_SENSOR_Z_ROTATION
bool base_sot_to_urdf(dynamicgraph::sot::ConstRefVector q_sot, dynamicgraph::sot::RefVector q_urdf)
const double LEFT_HAND_FORCE_SENSOR_XYZ[3]
const double LEFT_HAND_FORCE_SENSOR_MASS_PERCENTAGE
bool base_urdf_to_sot(dynamicgraph::sot::ConstRefVector q_urdf, dynamicgraph::sot::RefVector q_sot)
const double RIGHT_FOOT_FORCE_SENSOR_XYZ[3]
Position of the force/torque sensors w.r.t. the frame of the hosting link.
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 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 base_se3_to_sot(dynamicgraph::sot::ConstRefVector pos, dynamicgraph::sot::ConstRefMatrix R, dynamicgraph::sot::RefVector q_sot)
bool joints_sot_to_urdf(dynamicgraph::sot::ConstRefVector q_sot, dynamicgraph::sot::RefVector q_urdf)
const double LEFT_FOOT_FORCE_SENSOR_XYZ[3]
const double LEFT_HAND_GRIPPER_XYZ[3]
const double RIGHT_HAND_FORCE_SENSOR_Z_ROTATION
Rotation angle around Z axis of the force/torque sensors w.r.t. the frame of the hosting link.
const double RIGHT_HAND_FORCE_SENSOR_MASS_PERCENTAGE
const double LEFT_FOOT_FORCE_SENSOR_MASS_PERCENTAGE
bool config_urdf_to_sot(dynamicgraph::sot::ConstRefVector q_urdf, dynamicgraph::sot::RefVector q_sot)
const double RIGHT_HAND_GRIPPER_XYZ[3]
Position of the hand grippers w.r.t. the frame of the hand.
const double LEFT_FOOT_SOLE_XYZ[3]
const double IMU_XYZ[3]
max CURRENT (double in [0 Amp, 20 Amp])
ForceLimits(const Eigen::VectorXd &l, const Eigen::VectorXd &u)
Map from force names to force ids.
static ForceLimits get_limits_from_id(unsigned int id)
static std::string get_name_from_id(unsigned int id)
static std::map< unsigned int, ForceLimits > create_id_2_limits_map()
static const std::map< unsigned int, std::string > id_2_name
static const std::map< unsigned int, ForceLimits > id_2_limits
static std::map< std::string, unsigned int > create_name_2_id_map()
static int get_id_from_name(std::string name)
static const std::map< std::string, unsigned int > name_2_id
static std::map< unsigned int, std::string > create_id_2_name_map(const std::map< std::string, unsigned int > &name_2_id_map)
JointLimits(double l, double u)
Map from joint names to joint ids.
static std::string get_name_from_id(unsigned int id)
static const std::map< unsigned int, std::string > id_2_name
static JointLimits get_limits_from_id(unsigned int id)
static std::map< unsigned int, JointLimits > create_id_2_limits_map()
static std::map< std::string, unsigned int > create_name_2_id_map()
static int get_id_from_name(std::string name)
static const std::map< unsigned int, JointLimits > id_2_limits
static const std::map< std::string, unsigned int > name_2_id
static std::map< unsigned int, std::string > create_id_2_name_map(const std::map< std::string, unsigned int > &name_2_id_map)