1 |
|
|
/* |
2 |
|
|
* Copyright 2014, Oscar E. Ramos Ponce, LAAS-CNRS |
3 |
|
|
* |
4 |
|
|
*/ |
5 |
|
|
|
6 |
|
|
#ifndef __sot_torque_control_talos_common_H__ |
7 |
|
|
#define __sot_torque_control_talos_common_H__ |
8 |
|
|
|
9 |
|
|
/* --------------------------------------------------------------------- */ |
10 |
|
|
/* --- API ------------------------------------------------------------- */ |
11 |
|
|
/* --------------------------------------------------------------------- */ |
12 |
|
|
|
13 |
|
|
#if defined(WIN32) |
14 |
|
|
#if defined(talos_common_EXPORTS) |
15 |
|
|
#define TALOSCOMMON_EXPORT __declspec(dllexport) |
16 |
|
|
#else |
17 |
|
|
#define TALOSCOMMON_EXPORT __declspec(dllimport) |
18 |
|
|
#endif |
19 |
|
|
#else |
20 |
|
|
#define TALOSCOMMON_EXPORT |
21 |
|
|
#endif |
22 |
|
|
|
23 |
|
|
/* --------------------------------------------------------------------- */ |
24 |
|
|
/* --- INCLUDE --------------------------------------------------------- */ |
25 |
|
|
/* --------------------------------------------------------------------- */ |
26 |
|
|
|
27 |
|
|
#include <initializer_list> |
28 |
|
|
#include <map> |
29 |
|
|
|
30 |
|
|
#include "boost/assign.hpp" |
31 |
|
|
/* HELPER */ |
32 |
|
|
#include <dynamic-graph/signal-helper.h> |
33 |
|
|
|
34 |
|
|
#include <sot/core/matrix-geometry.hh> |
35 |
|
|
|
36 |
|
|
namespace dynamicgraph { |
37 |
|
|
namespace sot { |
38 |
|
|
namespace torque_control { |
39 |
|
|
|
40 |
|
|
#define N_JOINTS 32 |
41 |
|
|
|
42 |
|
|
#define RIGHT_FOOT_FRAME_NAME "RLEG_JOINT5" |
43 |
|
|
#define LEFT_FOOT_FRAME_NAME "LLEG_JOINT5" |
44 |
|
|
|
45 |
|
|
const double DEFAULT_MAX_DELTA_Q = |
46 |
|
|
0.1; /// max joint position tracking error [rad] |
47 |
|
|
|
48 |
|
|
const double DEFAULT_MAX_CURRENT = |
49 |
|
|
5; /// max CURRENT (double in [0 Amp, 20 Amp]) |
50 |
|
|
|
51 |
|
|
// Information on the location of the IMU and F/T sensors of HRP-2 |
52 |
|
|
// copied from the urdf file (in stacks/hrp2/hrp2_14_description/urdf): |
53 |
|
|
// <joint name="AccelerometerJoint" type="fixed"> |
54 |
|
|
// <origin xyz="-0.13 0.0 0.118" rpy="0.0 0.0 0.0"/> |
55 |
|
|
// <joint name="RightFootForceSensorJoint" type="fixed"> |
56 |
|
|
// <origin xyz="0.0 0.0 -0.105" rpy="0.0 0.0 0.0"/> |
57 |
|
|
// <joint name="LeftFootForceSensorJoint" type="fixed"> |
58 |
|
|
// <origin xyz="0.0 0.0 -0.105" rpy="0.0 0.0 0.0"/> |
59 |
|
|
// <joint name="RightHandForceSensorJoint" type="fixed"> |
60 |
|
|
// <origin xyz="0.005 0.0 -0.05925" rpy="0.0 0.0 0.0"/> |
61 |
|
|
// <joint name="LeftHandForceSensorJoint" type="fixed"> |
62 |
|
|
// <origin xyz="0.005 0.0 -0.05925" rpy="0.0 0.0 0.0"/> |
63 |
|
|
|
64 |
|
|
/// Position of the IMU w.r.t. the frame of the hosting link (torso) |
65 |
|
|
const double IMU_XYZ[3] = {-0.13, 0.0, 0.118}; // not updated for talos/talos |
66 |
|
|
|
67 |
|
|
/// Position of the force/torque sensors w.r.t. the frame of the hosting link |
68 |
|
|
const double RIGHT_FOOT_FORCE_SENSOR_XYZ[3] = { |
69 |
|
|
0.0, 0.0, -0.0}; // not updated for talos/talos |
70 |
|
|
const double LEFT_FOOT_FORCE_SENSOR_XYZ[3] = { |
71 |
|
|
0.0, 0.0, -0.0}; // not updated for talos/talos |
72 |
|
|
const double RIGHT_HAND_FORCE_SENSOR_XYZ[3] = {0.0, 0.0, -0.051}; |
73 |
|
|
const double LEFT_HAND_FORCE_SENSOR_XYZ[3] = {0.005, 0.0, -0.051}; |
74 |
|
|
|
75 |
|
|
/// Rotation angle around Z axis of the force/torque sensors w.r.t. the frame of |
76 |
|
|
/// the hosting link |
77 |
|
|
const double RIGHT_HAND_FORCE_SENSOR_Z_ROTATION = |
78 |
|
|
-0.5 * M_PI; // not updated for talos/talos |
79 |
|
|
const double LEFT_HAND_FORCE_SENSOR_Z_ROTATION = |
80 |
|
|
-0.5 * M_PI; // not updated for talos/talos |
81 |
|
|
|
82 |
|
|
/// Position of the foot soles w.r.t. the frame of the foot |
83 |
|
|
const double RIGHT_FOOT_SOLE_XYZ[3] = {0.0, 0.0, -0.107}; |
84 |
|
|
const double LEFT_FOOT_SOLE_XYZ[3] = {0.0, 0.0, -0.107}; |
85 |
|
|
|
86 |
|
|
/// Position of the hand grippers w.r.t. the frame of the hand |
87 |
|
|
const double RIGHT_HAND_GRIPPER_XYZ[3] = {0.0, 0.0, -0.02875}; |
88 |
|
|
const double LEFT_HAND_GRIPPER_XYZ[3] = {0.0, 0.0, 0.02025}; |
89 |
|
|
|
90 |
|
|
/// Percentage of mass of the link that is measured by the F/T sensors |
91 |
|
|
const double RIGHT_FOOT_FORCE_SENSOR_MASS_PERCENTAGE = |
92 |
|
|
0.65; // not updated for talos/talos |
93 |
|
|
const double LEFT_FOOT_FORCE_SENSOR_MASS_PERCENTAGE = |
94 |
|
|
0.65; // not updated for talos/talos |
95 |
|
|
const double RIGHT_HAND_FORCE_SENSOR_MASS_PERCENTAGE = |
96 |
|
|
0.75; // not updated for talos/talos |
97 |
|
|
const double LEFT_HAND_FORCE_SENSOR_MASS_PERCENTAGE = |
98 |
|
|
0.75; // not updated for talos/talos |
99 |
|
|
|
100 |
|
|
struct JointLimits { |
101 |
|
|
double upper; |
102 |
|
|
double lower; |
103 |
|
|
|
104 |
|
|
JointLimits() : upper(0.0), lower(0.0) {} |
105 |
|
|
|
106 |
|
|
JointLimits(double l, double u) : upper(u), lower(l) {} |
107 |
|
|
}; |
108 |
|
|
|
109 |
|
|
/// Map from joint names to joint ids |
110 |
|
|
struct JointUtil { |
111 |
|
|
static std::map<unsigned int, JointLimits> create_id_2_limits_map() { |
112 |
|
|
std::map<unsigned int, JointLimits> m; |
113 |
|
|
m[0] = JointLimits(-0.349065850399, 1.57079632679); // left leg 1 |
114 |
|
|
m[1] = JointLimits(-0.5236, 0.5236); // left leg 2 |
115 |
|
|
m[2] = JointLimits(-2.095, 0.7); // left leg 3 |
116 |
|
|
m[3] = JointLimits(0.0, 2.618); // left leg 4 |
117 |
|
|
m[4] = JointLimits(-1.309, 0.768); // left leg 5 |
118 |
|
|
m[5] = JointLimits(-0.5236, 0.5236); // left leg 6 |
119 |
|
|
m[6] = JointLimits(-1.57079632679, 0.349065850399); // right leg 1 |
120 |
|
|
m[7] = JointLimits(-0.5236, 0.5236); // right leg 2 |
121 |
|
|
m[8] = JointLimits(-2.095, 0.7); // right leg 3 |
122 |
|
|
m[9] = JointLimits(0.0, 2.618); // right leg 4 |
123 |
|
|
m[10] = JointLimits(-1.309, 0.768); // right leg 5 |
124 |
|
|
m[11] = JointLimits(-0.5236, 0.5236); // right leg 6 |
125 |
|
|
m[12] = JointLimits(-1.308996939, 1.308996939); // torso 1 |
126 |
|
|
m[13] = JointLimits(-0.261799387799, 0.785398163397); // torso 2 |
127 |
|
|
m[14] = JointLimits(-1.57079632679, 0.523598775598); // left arm 1 shoulder |
128 |
|
|
m[15] = JointLimits(0.0, 2.87979326579); // left arm 2 shoulder |
129 |
|
|
m[16] = JointLimits(-2.44346095279, 2.44346095279); // left arm 3 shoulder |
130 |
|
|
m[17] = JointLimits(-2.35619449019, 0.0); // left arm 4 elbow |
131 |
|
|
m[18] = JointLimits(-2.53072741539, 2.53072741539); // left arm 5 wrist |
132 |
|
|
m[19] = JointLimits(-1.3962634016, 1.3962634016); // left arm 6 wrist |
133 |
|
|
m[20] = JointLimits(-0.698131700798, 0.698131700798); // left arm 7 wrist |
134 |
|
|
m[21] = JointLimits(-1.0471975512, 0.0); // left gripper |
135 |
|
|
m[22] = |
136 |
|
|
JointLimits(-0.523598775598, 1.57079632679); // right arm 1 shoulder |
137 |
|
|
m[23] = JointLimits(-2.87979326579, 0.0); // right arm 2 shoulder |
138 |
|
|
m[24] = JointLimits(-2.44346095279, 2.44346095279); // right arm 3 shoulder |
139 |
|
|
m[25] = JointLimits(-2.35619449019, 0.0); // right arm 4 elbow |
140 |
|
|
m[26] = JointLimits(-2.53072741539, 2.53072741539); // right arm 5 wrist |
141 |
|
|
m[27] = JointLimits(-1.3962634016, 1.3962634016); // right arm 6 wrist |
142 |
|
|
m[28] = JointLimits(-0.698131700798, 0.698131700798); // right arm 7 wrist |
143 |
|
|
m[29] = JointLimits(-1.0471975512, 0.0); // right gripper |
144 |
|
|
m[30] = JointLimits(-0.261799387799, 0.785398163397); // head 1 |
145 |
|
|
m[31] = JointLimits(-1.308996939, 1.308996939); // head 2 |
146 |
|
|
return m; |
147 |
|
|
} |
148 |
|
|
|
149 |
|
|
static std::map<std::string, unsigned int> create_name_2_id_map() { |
150 |
|
|
std::map<std::string, unsigned int> m; |
151 |
|
|
m["lhy"] = 0; // left hip yaw |
152 |
|
|
m["lhr"] = 1; // left hip roll |
153 |
|
|
m["lhp"] = 2; // left hip pitch |
154 |
|
|
m["lk"] = 3; // left knee |
155 |
|
|
m["lap"] = 4; // left ankle pitch |
156 |
|
|
m["lar"] = 5; // left ankle roll |
157 |
|
|
m["rhy"] = 6; // right hip yaw |
158 |
|
|
m["rhr"] = 7; // right hip roll |
159 |
|
|
m["rhp"] = 8; // right hip pitch |
160 |
|
|
m["rk"] = 9; // right knee |
161 |
|
|
m["rap"] = 10; // right ankle pitch |
162 |
|
|
m["rar"] = 11; // right ankle roll |
163 |
|
|
m["ty"] = 12; // torso yaw |
164 |
|
|
m["tp"] = 13; // torso pitch |
165 |
|
|
m["lsy"] = 14; // left shoulder yaw |
166 |
|
|
m["lsr"] = 15; // left shoulder roll |
167 |
|
|
m["lay"] = 16; // left arm yaw |
168 |
|
|
m["le"] = 17; // left elbow |
169 |
|
|
m["lwy"] = 18; // left wrist yaw |
170 |
|
|
m["lwp"] = 19; // left wrist pitch |
171 |
|
|
m["lwr"] = 20; // left wrist roll |
172 |
|
|
m["lh"] = 21; // left hand |
173 |
|
|
m["rsy"] = 22; // right shoulder yaw |
174 |
|
|
m["rsr"] = 23; // right shoulder roll |
175 |
|
|
m["ray"] = 24; // right arm yaw |
176 |
|
|
m["re"] = 25; // right elbow |
177 |
|
|
m["rwy"] = 26; // right wrist yaw |
178 |
|
|
m["rwp"] = 27; // right wrist pitch |
179 |
|
|
m["rwr"] = 28; // right wrist roll |
180 |
|
|
m["rh"] = 29; // right hand |
181 |
|
|
m["hp"] = 30; // head pitch |
182 |
|
|
m["hy"] = 31; // head yaw |
183 |
|
|
return m; |
184 |
|
|
} |
185 |
|
|
|
186 |
|
|
static std::map<unsigned int, std::string> create_id_2_name_map( |
187 |
|
|
const std::map<std::string, unsigned int>& name_2_id_map) { |
188 |
|
|
std::map<unsigned int, std::string> m; |
189 |
|
|
std::map<std::string, unsigned int>::const_iterator it; |
190 |
|
|
for (it = name_2_id_map.begin(); it != name_2_id_map.end(); it++) |
191 |
|
|
m[it->second] = it->first; |
192 |
|
|
return m; |
193 |
|
|
} |
194 |
|
|
|
195 |
|
|
/** Given a joint name it finds the associated joint id. |
196 |
|
|
* If the specified joint name is not found it returns -1; |
197 |
|
|
* @param name Name of the joint to find. |
198 |
|
|
* @return The id of the specified joint, -1 if not found. */ |
199 |
|
|
static int get_id_from_name(std::string name) { |
200 |
|
|
std::map<std::string, unsigned int>::const_iterator iter = |
201 |
|
|
name_2_id.find(name); |
202 |
|
|
if (iter == name_2_id.end()) return -1; |
203 |
|
|
return iter->second; |
204 |
|
|
} |
205 |
|
|
|
206 |
|
|
/** Given a joint id it finds the associated joint name. |
207 |
|
|
* If the specified joint is not found it returns "Joint name not found"; |
208 |
|
|
* @param id Id of the joint to find. |
209 |
|
|
* @return The name of the specified joint, "Joint name not found" if not |
210 |
|
|
* found. */ |
211 |
|
|
static std::string get_name_from_id(unsigned int id) { |
212 |
|
|
std::map<unsigned int, std::string>::const_iterator iter = |
213 |
|
|
id_2_name.find(id); |
214 |
|
|
if (iter == id_2_name.end()) return "Joint name not found"; |
215 |
|
|
return iter->second; |
216 |
|
|
} |
217 |
|
|
|
218 |
|
|
/** Given a joint id it finds the associated joint limits. |
219 |
|
|
* If the specified joint is not found it returns JointLimits(0,0). |
220 |
|
|
* @param id Id of the joint to find. |
221 |
|
|
* @return The limits of the specified joint, JointLimits(0,0) if not found. |
222 |
|
|
*/ |
223 |
|
|
static JointLimits get_limits_from_id(unsigned int id) { |
224 |
|
|
std::map<unsigned int, JointLimits>::const_iterator iter = |
225 |
|
|
id_2_limits.find(id); |
226 |
|
|
if (iter == id_2_limits.end()) return JointLimits(0.0, 0.0); |
227 |
|
|
return iter->second; |
228 |
|
|
} |
229 |
|
|
|
230 |
|
|
static const std::map<std::string, unsigned int> name_2_id; |
231 |
|
|
static const std::map<unsigned int, std::string> id_2_name; |
232 |
|
|
static const std::map<unsigned int, JointLimits> id_2_limits; |
233 |
|
|
}; |
234 |
|
|
const std::map<std::string, unsigned int> JointUtil::name_2_id = |
235 |
|
|
JointUtil::create_name_2_id_map(); |
236 |
|
|
const std::map<unsigned int, std::string> JointUtil::id_2_name = |
237 |
|
|
JointUtil::create_id_2_name_map(JointUtil::name_2_id); |
238 |
|
|
const std::map<unsigned int, JointLimits> JointUtil::id_2_limits = |
239 |
|
|
JointUtil::create_id_2_limits_map(); |
240 |
|
|
|
241 |
|
|
struct ForceLimits { |
242 |
|
|
Eigen::VectorXd upper; |
243 |
|
|
Eigen::VectorXd lower; |
244 |
|
|
|
245 |
|
|
ForceLimits() |
246 |
|
|
: upper(dynamicgraph::sot::Vector6d::Zero()), |
247 |
|
|
lower(dynamicgraph::sot::Vector6d::Zero()) {} |
248 |
|
|
|
249 |
|
|
ForceLimits(const Eigen::VectorXd& l, const Eigen::VectorXd& u) |
250 |
|
|
: upper(u), lower(l) {} |
251 |
|
|
}; |
252 |
|
|
|
253 |
|
|
enum ForceID { |
254 |
|
|
FORCE_ID_RIGHT_FOOT = 0, |
255 |
|
|
FORCE_ID_LEFT_FOOT = 1, |
256 |
|
|
FORCE_ID_RIGHT_HAND = 2, |
257 |
|
|
FORCE_ID_LEFT_HAND = 3 |
258 |
|
|
}; |
259 |
|
|
|
260 |
|
|
/// Map from force names to force ids |
261 |
|
|
struct ForceUtil { |
262 |
|
|
static std::map<unsigned int, ForceLimits> create_id_2_limits_map() { |
263 |
|
|
dynamicgraph::sot::Vector6d fMax, fMin; |
264 |
|
|
fMax << 100.0, 100.0, 300.0, 80.0, 80.0, 30.0; |
265 |
|
|
fMin = -fMax; |
266 |
|
|
std::map<unsigned int, ForceLimits> m; |
267 |
|
|
m[0] = ForceLimits(fMin, fMax); // right foot |
268 |
|
|
m[1] = ForceLimits(fMin, fMax); // left foot |
269 |
|
|
m[2] = ForceLimits(fMin, fMax); // right hand |
270 |
|
|
m[3] = ForceLimits(fMin, fMax); // left hand |
271 |
|
|
return m; |
272 |
|
|
} |
273 |
|
|
|
274 |
|
|
static std::map<std::string, unsigned int> create_name_2_id_map() { |
275 |
|
|
std::map<std::string, unsigned int> m; |
276 |
|
|
m["rf"] = 0; // right foot |
277 |
|
|
m["lf"] = 1; // left foot |
278 |
|
|
m["rh"] = 2; // right hand |
279 |
|
|
m["lh"] = 3; // left hand |
280 |
|
|
return m; |
281 |
|
|
} |
282 |
|
|
|
283 |
|
|
static std::map<unsigned int, std::string> create_id_2_name_map( |
284 |
|
|
const std::map<std::string, unsigned int>& name_2_id_map) { |
285 |
|
|
std::map<unsigned int, std::string> m; |
286 |
|
|
std::map<std::string, unsigned int>::const_iterator it; |
287 |
|
|
for (it = name_2_id_map.begin(); it != name_2_id_map.end(); it++) |
288 |
|
|
m[it->second] = it->first; |
289 |
|
|
return m; |
290 |
|
|
} |
291 |
|
|
|
292 |
|
|
/** Given a force name it finds the associated id. |
293 |
|
|
* If the specified force name is not found it returns -1; |
294 |
|
|
* @param name Name of the force to find. |
295 |
|
|
* @return The id of the specified force, -1 if not found. */ |
296 |
|
|
static int get_id_from_name(std::string name) { |
297 |
|
|
std::map<std::string, unsigned int>::const_iterator iter = |
298 |
|
|
name_2_id.find(name); |
299 |
|
|
if (iter == name_2_id.end()) return -1; |
300 |
|
|
return iter->second; |
301 |
|
|
} |
302 |
|
|
|
303 |
|
|
/** Given a force id it finds the associated name. |
304 |
|
|
* If the specified force is not found it returns "Force name not found"; |
305 |
|
|
* @param id Id of the force to find. |
306 |
|
|
* @return The name of the specified force, "Force name not found" if not |
307 |
|
|
* found. */ |
308 |
|
|
static std::string get_name_from_id(unsigned int id) { |
309 |
|
|
std::map<unsigned int, std::string>::const_iterator iter = |
310 |
|
|
id_2_name.find(id); |
311 |
|
|
if (iter == id_2_name.end()) return "Force name not found"; |
312 |
|
|
return iter->second; |
313 |
|
|
} |
314 |
|
|
|
315 |
|
|
/** Given a force id it finds the associated limits. |
316 |
|
|
* If the specified force is not found it returns ForceLimits(0,0). |
317 |
|
|
* @param id Id of the force to find. |
318 |
|
|
* @return The limits of the specified force, ForceLimits(0,0) if not found. |
319 |
|
|
*/ |
320 |
|
|
static ForceLimits get_limits_from_id(unsigned int id) { |
321 |
|
|
std::map<unsigned int, ForceLimits>::const_iterator iter = |
322 |
|
|
id_2_limits.find(id); |
323 |
|
|
if (iter == id_2_limits.end()) return ForceLimits(); |
324 |
|
|
return iter->second; |
325 |
|
|
} |
326 |
|
|
|
327 |
|
|
static const std::map<std::string, unsigned int> name_2_id; |
328 |
|
|
static const std::map<unsigned int, std::string> id_2_name; |
329 |
|
|
static const std::map<unsigned int, ForceLimits> id_2_limits; |
330 |
|
|
}; |
331 |
|
|
const std::map<std::string, unsigned int> ForceUtil::name_2_id = |
332 |
|
|
ForceUtil::create_name_2_id_map(); |
333 |
|
|
const std::map<unsigned int, std::string> ForceUtil::id_2_name = |
334 |
|
|
ForceUtil::create_id_2_name_map(ForceUtil::name_2_id); |
335 |
|
|
const std::map<unsigned int, ForceLimits> ForceUtil::id_2_limits = |
336 |
|
|
ForceUtil::create_id_2_limits_map(); |
337 |
|
|
|
338 |
|
|
bool base_se3_to_sot(dynamicgraph::sot::ConstRefVector pos, |
339 |
|
|
dynamicgraph::sot::ConstRefMatrix R, |
340 |
|
|
dynamicgraph::sot::RefVector q_sot); |
341 |
|
|
bool base_urdf_to_sot(dynamicgraph::sot::ConstRefVector q_urdf, |
342 |
|
|
dynamicgraph::sot::RefVector q_sot); |
343 |
|
|
bool base_sot_to_urdf(dynamicgraph::sot::ConstRefVector q_sot, |
344 |
|
|
dynamicgraph::sot::RefVector q_urdf); |
345 |
|
|
bool config_urdf_to_sot(dynamicgraph::sot::ConstRefVector q_urdf, |
346 |
|
|
dynamicgraph::sot::RefVector q_sot); |
347 |
|
|
bool config_sot_to_urdf(dynamicgraph::sot::ConstRefVector q_sot, |
348 |
|
|
dynamicgraph::sot::RefVector q_urdf); |
349 |
|
|
bool velocity_urdf_to_sot(dynamicgraph::sot::ConstRefVector v_urdf, |
350 |
|
|
dynamicgraph::sot::RefVector v_sot); |
351 |
|
|
bool velocity_sot_to_urdf(dynamicgraph::sot::ConstRefVector v_sot, |
352 |
|
|
dynamicgraph::sot::RefVector v_urdf); |
353 |
|
|
bool joints_urdf_to_sot(dynamicgraph::sot::ConstRefVector q_urdf, |
354 |
|
|
dynamicgraph::sot::RefVector q_sot); |
355 |
|
|
bool joints_sot_to_urdf(dynamicgraph::sot::ConstRefVector q_sot, |
356 |
|
|
dynamicgraph::sot::RefVector q_urdf); |
357 |
|
|
|
358 |
|
|
} // namespace torque_control |
359 |
|
|
} // namespace sot |
360 |
|
|
} // namespace dynamicgraph |
361 |
|
|
|
362 |
|
|
#endif // #ifndef __sot_torque_control_talos_common_H__ |