mia_hand_ros_control  rel 1.0.0
mia_index_transmission.cpp
Go to the documentation of this file.
1 /*********************************************************************************************************
2  Author: Prensilia srl
3  Desc: Transmission of the MIA index finger
4 
5  version 1.0
6 
7 **********************************************************************************************************/
8 
10 #include <math.h>
11 
12 using transmission_interface::ActuatorData;
13 using transmission_interface::JointData;
14 
15 namespace transmission_interface
16 {
17 
19  // Initialize broken lines fucntons imlementing mia index transmisiion
22  : Transmission()
23  {
24  // linear trasmission section
25  linear_reduction_ = 64.0 * 20.0; // TR of the linear segment of the trasmission
26 
27  // non linear trasmission section
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}; // N = 12
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}; //N = 15
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}; // N = 18
31 
32  // f functions
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};
35 
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};
38 
39  // f inverse functions
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};
42 
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};
45 
46  // df function
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};
50 
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};
54 
55  }
56 
57 
59  // Simple functions needed to evaluate trasmission ratio
61 
62  // mu = h_i(pos)
63  double MiaIndexTransmission::h_i(const double pos)
64  {
65  double Scale = -8.673;
66  double Offset = 580.8284;
67  double mu = Scale * pos + Offset;
68  return mu;
69  }
70 
71  // pos = h_i_inv(mu)
72  double MiaIndexTransmission::h_i_inv(const double mu)
73  {
74  double Scale = -0.1153;
75  double Offset = 66.9697;
76  double pos = (double) round( Scale * mu + Offset); // cmd
77  return pos;
78  }
79 
80  // omega_m = dh_i(spe)
81  double MiaIndexTransmission::dh_i(const double spe)
82  {
83  double Scale = 65.4167;
84  double Offset = 0;
85  double omega_m = Scale * spe + Offset;
86  return omega_m;
87  }
88 
89  // spe = dh_i_inv(omega_m)
90  double MiaIndexTransmission::dh_i_inv(const double omega_m)
91  {
92  double Scale = 0.015287;
93  double Offset = 0;
94  double spe = (double) round( Scale * omega_m + Offset); // cmd
95  return spe;
96  }
97 
98 
99  // delta = f(alpha)
100  double MiaIndexTransmission::f(const double alpha )
101  {
102 
103  double delta;
104 
105  // input larger than the last x interval limit
106  if (alpha > x_intervals_f[x_intervals_f.size() - 1 ])
107  {
108  delta = f_scale[x_intervals_f.size() - 1] * alpha + f_offset[x_intervals_f.size() - 1];
109  }
110  else
111  {
112 
113  for(unsigned int j=0; j < x_intervals_f.size() ; j++)
114  {
115 
116  // First interval
117  if (j == 0)
118  {
119  if (alpha <= x_intervals_f[j])
120  {
121  delta = f_scale[j] * alpha + f_offset[j];
122  break;
123  }
124  }
125  else // Other intervals
126  {
127  if (alpha > x_intervals_f[j-1] && alpha <= x_intervals_f[j])
128  {
129  delta = f_scale[j] * alpha + f_offset[j];
130  break;
131  }
132  }
133  }
134 
135  }
136  return delta;
137 
138 
139  }
140 
141 
142  // alpha = f_inv(delta)
143  double MiaIndexTransmission::f_inv( const double delta)
144  {
145 
146  double alpha;
147 
148  // input larger than the last x interval limit
149  if (delta > x_intervals_f_inv[x_intervals_f_inv.size() - 1 ])
150  {
151  alpha = f_inv_scale[x_intervals_f_inv.size() - 1] * delta + f_inv_offset[x_intervals_f_inv.size() - 1];
152  }
153  else
154  {
155 
156  for(unsigned int j=0; j < x_intervals_f_inv.size() ; j++)
157  {
158 
159  // First interval
160  if (j == 0)
161  {
162  if (delta <= x_intervals_f_inv[j])
163  {
164  alpha = f_inv_scale[j] * delta + f_inv_offset[j];
165  break;
166  }
167  }
168  else // Other intervals
169  {
170  if (delta > x_intervals_f_inv[j-1] && delta <= x_intervals_f_inv[j])
171  {
172  alpha = f_inv_scale[j] * delta + f_inv_offset[j];
173  break;
174  }
175  }
176  }
177 
178  }
179  return alpha;
180 
181  }
182 
183 
184  // Non linear TR-1
185  double MiaIndexTransmission::df( const double alpha )
186  {
187 
188  double f_alpha;
189 
190  // input larger than the last x interval limit
191  if (alpha > x_intervals_df[x_intervals_df.size() - 1 ])
192  {
193  f_alpha = df_scale[x_intervals_df.size() - 1] * alpha + df_offset[x_intervals_df.size() - 1];
194  }
195  else
196  {
197 
198  for(unsigned int j=0; j < x_intervals_df.size() ; j++)
199  {
200 
201  // First interval
202  if (j == 0)
203  {
204  if (alpha <= x_intervals_df[j])
205  {
206  f_alpha = df_scale[j] * alpha + df_offset[j];
207  break;
208  }
209  }
210  else // Other intervals
211  {
212  if (alpha > x_intervals_df[j-1] && alpha <= x_intervals_df[j])
213  {
214  f_alpha = df_scale[j] * alpha + df_offset[j];
215  break;
216  }
217  }
218  }
219 
220  }
221 
222  if (0.0 == f_alpha)
223  {
224  f_alpha = 0.0051;
225  }
226 
227  return abs(f_alpha);
228 
229 
230  }
231 
232 
233 
235  // Macro functions of for the ros trasmission
237 
238 
239  // Mapping from data pos returned by MIA to joint angle.
241  const ActuatorData &ind_act_state,
242  JointData &ind_jnt_data)
243  {
244 
245  double pos = *ind_act_state.position[0]; // -255 ; 255 (discreto)
246  double mu = h_i(pos);
247  double alpha = mu / linear_reduction_;
248  double delta = f(alpha);
249 
250  *ind_jnt_data.position[0] = delta;
251 
252  return;
253  }
254 
255  // Mapping from data spe returned by MIA to joint velocity.
257  const ActuatorData &ind_act_state,
258  JointData &ind_jnt_data)
259  {
260 
261  double spe = *ind_act_state.velocity[0]; // discreto (-90 ; +90)
262  double omega_m = dh_i(spe);
263  double omega_a = omega_m / linear_reduction_;
264 
265  double pos = *ind_act_state.position[0]; // -255 ; 255 (discreto)
266  double mu = h_i(pos);
267  double alpha = mu / linear_reduction_;
268 
269  double df_alpha = df(alpha); // non linear reduction ratio inv
270  double omega_d = omega_a * df_alpha;
271 
272  // ROS_INFO_STREAM("alpha: "<< alpha<< " | df_alpha " << df_alpha << " | omega_d " << omega_d);
273 
274  *ind_jnt_data.velocity[0] = omega_d;
275 
276  return;
277  }
278 
279  // Mapping from joint position to Mia pos command.
281  const JointData &ind_jnt_data,
282  ActuatorData &ind_act_cmd)
283  {
284 
285  double delta = *ind_jnt_data.position[0];
286  double alpha = f_inv(delta);
287  double mu = alpha * linear_reduction_;
288  double pos = h_i_inv(mu); // discreto (-255 ; 255)
289 
290 
291  *ind_act_cmd.position[0] = pos;
292 
293  return;
294  }
295 
296  // Mapping from joint velocity to Mia spe command.
298  const JointData &ind_jnt_data,
299  const ActuatorData &ind_act_state,
300  ActuatorData &ind_act_cmd)
301  {
302  double omega_d = *ind_jnt_data.velocity[0] ;
303 
304  double pos = *ind_act_state.position[0]; // -255 ; 255 (discreto)
305  double mu = h_i(pos);
306  double alpha = mu / linear_reduction_;
307 
308  double df_alpha = df(alpha); // non linear reduction ratio inv
309  double omega_a = omega_d/df_alpha;
310  double omega_m = omega_a * linear_reduction_;
311  double spe = dh_i_inv(omega_m);
312 
313  // ROS_INFO_STREAM("alpha: "<< alpha<< " | df_alpha " << df_alpha << " | omega_d " << omega_d << " | omega_m " << omega_m);
314 
315  *ind_act_cmd.velocity[0] = spe;
316 
317  return;
318  }
319 
320  // NOT USED
322  const transmission_interface::JointData& ind_jnt_data,
323  transmission_interface::ActuatorData& act_data)
324  {
325  *act_data.velocity[0] = *ind_jnt_data.velocity[0] * 1;
326  }
327 
328 
329 
331  // NOT IMPLEMENTED FUNCTIONS :since these cannot be used with MIA
333 
334  void MiaIndexTransmission::actuatorToJointEffort(const ActuatorData &ind_act_state,
335  JointData &ind_jnt_data)
336  {
337  /* TODO: replace this calculation with the mapping from motor to joint
338  * effort.
339  */
340  *ind_jnt_data.effort[0] = *ind_act_state.effort[0] * 1;
341 
342  return;
343  }
344 
345  // NOT IMPLEMENTED:
347  const JointData &ind_jnt_data,
348  ActuatorData &ind_act_cmd)
349  {
350  /* TODO: replace this calculation with the mapping from joint to motor
351  * effort.
352  */
353  *ind_act_cmd.effort[0] = *ind_jnt_data.effort[0] / 1;
354 
355  return;
356  }
357 
358 /*-------------------------------------------------------------------------------------*/
359 
360 } // namespace
transmission_interface::MiaIndexTransmission::df_offset
std::vector< double > df_offset
Offset factors of the df funtion.
Definition: mia_index_transmission.h:216
transmission_interface::MiaIndexTransmission::f
double f(const double alpha)
Index transmission second step function for position: delta = f(alpha)
Definition: mia_index_transmission.cpp:100
transmission_interface
Definition: mia_index_transmission.h:23
transmission_interface::MiaIndexTransmission::jointToActuatorEffort
void jointToActuatorEffort(const transmission_interface::JointData &ind_jnt_data, transmission_interface::ActuatorData &ind_act_cmd) override
Do not use this methos since Mia hand has not effort control.
Definition: mia_index_transmission.cpp:346
transmission_interface::MiaIndexTransmission::f_inv_offset
std::vector< double > f_inv_offset
Offset factors of the f_inv funtion.
Definition: mia_index_transmission.h:204
transmission_interface::MiaIndexTransmission::df
double df(const double omega_a)
Index transmission second step function for velocity: alpha = f_inv(delta)
Definition: mia_index_transmission.cpp:185
transmission_interface::MiaIndexTransmission::x_intervals_f_inv
std::vector< double > x_intervals_f_inv
Upper limiits of the inputs intervals of the f_inv funtion.
Definition: mia_index_transmission.h:174
transmission_interface::MiaIndexTransmission::f_offset
std::vector< double > f_offset
Offset factors of the f funtion.
Definition: mia_index_transmission.h:192
transmission_interface::MiaIndexTransmission::actuatorToJointVelocity
void actuatorToJointVelocity(const transmission_interface::ActuatorData &ind_act_state, transmission_interface::JointData &ind_jnt_data) override
Transform velocity variables from actuator to joint space.
Definition: mia_index_transmission.cpp:256
transmission_interface::MiaIndexTransmission::df_scale
std::vector< double > df_scale
Scale factors of the df funtion.
Definition: mia_index_transmission.h:210
mia_index_transmission.h
transmission_interface::MiaIndexTransmission::linear_reduction_
double linear_reduction_
Definition: mia_index_transmission.h:162
transmission_interface::MiaIndexTransmission::jointToActuatorVelocity
void jointToActuatorVelocity(const transmission_interface::JointData &ind_jnt_data, transmission_interface::ActuatorData &act_data) override
unused
Definition: mia_index_transmission.cpp:321
transmission_interface::MiaIndexTransmission::actuatorToJointEffort
void actuatorToJointEffort(const transmission_interface::ActuatorData &ind_act_state, transmission_interface::JointData &ind_jnt_data) override
Do not use this methos since Mia hand has not effort control.
Definition: mia_index_transmission.cpp:334
transmission_interface::MiaIndexTransmission::f_scale
std::vector< double > f_scale
Scales factors of the f funtion.
Definition: mia_index_transmission.h:186
transmission_interface::MiaIndexTransmission::IndexjointToActuatorVelocity
void IndexjointToActuatorVelocity(const transmission_interface::JointData &ind_jnt_data, const transmission_interface::ActuatorData &ind_act_state, transmission_interface::ActuatorData &ind_act_cmd)
Transform velocity variables from joint to actuator space.
Definition: mia_index_transmission.cpp:297
transmission_interface::MiaIndexTransmission::dh_i_inv
double dh_i_inv(const double omega_m)
Index transmission first step inverse function for velocity: spe = dh_i_inv(omega_m)
Definition: mia_index_transmission.cpp:90
transmission_interface::MiaIndexTransmission::actuatorToJointPosition
void actuatorToJointPosition(const transmission_interface::ActuatorData &ind_act_state, transmission_interface::JointData &ind_jnt_data) override
Transform position variables from actuator to joint space.
Definition: mia_index_transmission.cpp:240
transmission_interface::MiaIndexTransmission::x_intervals_df
std::vector< double > x_intervals_df
Upper limiits of the inputs intervals of the df funtion.
Definition: mia_index_transmission.h:180
transmission_interface::MiaIndexTransmission::dh_i
double dh_i(const double spe)
Index transmission first step function for velocity: omega_m = dh_i(spe).
Definition: mia_index_transmission.cpp:81
transmission_interface::MiaIndexTransmission::f_inv
double f_inv(const double delta)
Index transmission second step inverse function for position: alpha = f_inv(delta)
Definition: mia_index_transmission.cpp:143
transmission_interface::MiaIndexTransmission::h_i
double h_i(const double pos)
Index transmission first step function for pose: mu = h_i(pos).
Definition: mia_index_transmission.cpp:63
transmission_interface::MiaIndexTransmission::x_intervals_f
std::vector< double > x_intervals_f
Upper limiits of the inputs intervals of the f funtion.
Definition: mia_index_transmission.h:168
transmission_interface::MiaIndexTransmission::f_inv_scale
std::vector< double > f_inv_scale
Scale factors of the f_inv funtion.
Definition: mia_index_transmission.h:198
transmission_interface::MiaIndexTransmission::h_i_inv
double h_i_inv(const double mu)
Index transmission first step inverse function for pose: pos = h_i_inv(mu)
Definition: mia_index_transmission.cpp:72
transmission_interface::MiaIndexTransmission::jointToActuatorPosition
void jointToActuatorPosition(const transmission_interface::JointData &ind_jnt_data, transmission_interface::ActuatorData &ind_act_cmd) override
Transform position variables from joint to actuator space.
Definition: mia_index_transmission.cpp:280
transmission_interface::MiaIndexTransmission::MiaIndexTransmission
MiaIndexTransmission()
Class constructor.
Definition: mia_index_transmission.cpp:21