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);
333 bool velocity_urdf_to_sot(dynamicgraph::sot::ConstRefVector v_urdf, dynamicgraph::sot::RefVector v_sot);
334 bool velocity_sot_to_urdf(dynamicgraph::sot::ConstRefVector v_sot, dynamicgraph::sot::RefVector v_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);
344 #endif // #ifndef __sot_torque_control_talos_common_H__