12 using transmission_interface::ActuatorData;
13 using transmission_interface::JointData;
28 x_intervals_f = {-0.85, -0.60, -0.28, -0.09, 0.04, 0.17, 0.58, 0.88, 1.16, 1.49, 1.88, 2.20};
29 x_intervals_f_inv = {-1.07, -0.70, -0.38, -0.17, -0.05, -0.01, 0.00, 0.01, 0.05, 0.15, 0.33, 0.80, 1.16, 1.33 ,1.5};
30 x_intervals_df = {-1.15, -0.99, -0.50, -0.38, -0.29, -0.19, -0.02, 0.09, 0.34, 0.50, 0.64, 0.82, 1.04, 1.30, 1.78 ,2.0 ,2.11, 2.20};
33 f_scale = {-0.782148766201303, -1.03058374167863 , -1.22368755900872 , -1.16847142154607 , -0.911231289024396, -0.601663029243979, -0.197880519756952,
34 -0.520051295462516, -0.719741468813803, -0.839379324439599, -0.942927436665641, -1.08442452979925};
36 f_offset = {0.439866538741061, 0.227645536860170, 0.111052936781279, 0.126181750141350, 0.150182742690696, 0.138771586278833, 0.0711398396794130,
37 0.256841159134046, 0.431658128795775, 0.569988622722396, 0.723948798521761, 0.989586451199345};
40 f_inv_scale = {-0.917582779538605, -1.05391335884287, -1.19433447974907, -1.431898346737150, -1.964168793593670, -3.38610487911141, -10.9773764088800,
41 -8.898456050626390, -2.86762990339221, -1.47787267188521, -0.974112549912150, -0.813805489118257, -0.978063597388615, -1.23941256214315, -1.42940996863406};
43 f_inv_offset = {0.9188279760227930, 0.773465321854399, 0.675795923603619, 0.586185379576233, 0.496912516702108, 0.424335359750378, 0.351597222222222, 0.342953500623609,
44 0.283965570504740, 0.217012833101473, 0.141950288885497, 0.0903279294135654, 0.222664061454050, 0.525807921540722, 0.778435404235634};
47 df_scale = {-0.502124686464031, -0.617569286637807, -0.786646228209404, -0.443887847250763, 0.0318882991362680, 0.642623008053360, 1.61741355519959,
48 2.37840936587392 , 2.42982807851509 , 1.82554337201208, 1.32452347512971 , 0.923449189994699 , 0.601344310816028, 0.389840076159333,
49 0.284011196830680, 0.385466139570889, 0.637843666986597, 1.01279322128042};
51 df_offset = {-1.29909527663917 , -1.432135538436430, -1.59994184747328 , -1.42755998530929, -1.24514798338551 ,-1.06634278699696, -0.879054630057223,
52 -0.859652872953407, -0.862880586642302, -0.659622532929257, -0.411137153002571,-0.155795702043499, 0.107118528620613, 0.326212634836997,
53 0.463277504816079, 0.282761648984580, -0.221130296730254, -1.01117012203451};
65 double Scale = -8.673;
66 double Offset = 580.8284;
67 double mu = Scale * pos + Offset;
74 double Scale = -0.1153;
75 double Offset = 66.9697;
76 double pos = (double) round( Scale * mu + Offset);
83 double Scale = 65.4167;
85 double omega_m = Scale * spe + Offset;
92 double Scale = 0.015287;
94 double spe = (double) round( Scale * omega_m + Offset);
241 const ActuatorData &ind_act_state,
242 JointData &ind_jnt_data)
245 double pos = *ind_act_state.position[0];
246 double mu =
h_i(pos);
248 double delta =
f(alpha);
250 *ind_jnt_data.position[0] = delta;
257 const ActuatorData &ind_act_state,
258 JointData &ind_jnt_data)
261 double spe = *ind_act_state.velocity[0];
262 double omega_m =
dh_i(spe);
265 double pos = *ind_act_state.position[0];
266 double mu =
h_i(pos);
269 double df_alpha =
df(alpha);
270 double omega_d = omega_a * df_alpha;
274 *ind_jnt_data.velocity[0] = omega_d;
281 const JointData &ind_jnt_data,
282 ActuatorData &ind_act_cmd)
285 double delta = *ind_jnt_data.position[0];
286 double alpha =
f_inv(delta);
291 *ind_act_cmd.position[0] = pos;
298 const JointData &ind_jnt_data,
299 const ActuatorData &ind_act_state,
300 ActuatorData &ind_act_cmd)
302 double omega_d = *ind_jnt_data.velocity[0] ;
304 double pos = *ind_act_state.position[0];
305 double mu =
h_i(pos);
308 double df_alpha =
df(alpha);
309 double omega_a = omega_d/df_alpha;
315 *ind_act_cmd.velocity[0] = spe;
322 const transmission_interface::JointData& ind_jnt_data,
323 transmission_interface::ActuatorData& act_data)
325 *act_data.velocity[0] = *ind_jnt_data.velocity[0] * 1;
335 JointData &ind_jnt_data)
340 *ind_jnt_data.effort[0] = *ind_act_state.effort[0] * 1;
347 const JointData &ind_jnt_data,
348 ActuatorData &ind_act_cmd)
353 *ind_act_cmd.effort[0] = *ind_jnt_data.effort[0] / 1;