iCub-main
Loading...
Searching...
No Matches
eomcUtils.h
Go to the documentation of this file.
1// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2
3
4/* Copyright (C) 2012 iCub Facility, Istituto Italiano di Tecnologia
5 * Author: Valentina Gaggero
6 * email: valentina.gaggero@iit.it
7 * Permission is granted to copy, distribute, and/or modify this program
8 * under the terms of the GNU General Public License, version 2 or any
9 * later version published by the Free Software Foundation.
10 *
11 * A copy of the license can be found at
12 * http://www.robotcub.org/icub/license/gpl.txt
13 *
14 * This program is distributed in the hope that it will be useful, but
15 * WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
17 * Public License for more details
18 */
19
20//here there is only a name space that contains all utils funtion of embObjMotionControl
21
22namespace yarp {
23 namespace dev {
24 namespace eomc
25
26{
27 enum { MAX_SHORT = 32767, MIN_SHORT = -32768, MAX_INT = 0x7fffffff, MIN_INT = 0x80000000, MAX_U32 = 0xffffffff, MIN_U32 = 0x00, MAX_U16 = 0xffff, MIN_U16 = 0x0000};
28 enum { CAN_SKIP_ADDR = 0x80 };
29
30 // saturation check and rounding for 16 bit unsigned integer
31 int U_16(double x)
32 {
33 if (x <= double(MIN_U16) )
34 return MIN_U16;
35 else
36 if (x >= double(MAX_U16))
37 return MAX_U16;
38 else
39 return int(x + .5);
40 }
41
42 // saturation check and rounding for 16 bit signed integer
43 short S_16(double x)
44 {
45 if (x <= double(-(MAX_SHORT))-1)
46 return MIN_SHORT;
47 else
48 if (x >= double(MAX_SHORT))
49 return MAX_SHORT;
50 else
51 if (x>0)
52 return short(x + .5);
53 else
54 return short(x - .5);
55 }
56
57 // saturation check and rounding for 32 bit unsigned integer
58 int U_32(double x)
59 {
60 if (x <= double(MIN_U32) )
61 return MIN_U32;
62 else
63 if (x >= double(MAX_U32))
64 return MAX_U32;
65 else
66 return int(x + .5);
67 }
68
69 // saturation check and rounding for 32 bit signed integer
70 int S_32(double x)
71 {
72 if (x <= double(-(MAX_INT))-1.0)
73 return MIN_INT;
74 else
75 if (x >= double(MAX_INT))
76 return MAX_INT;
77 else
78 if (x>0)
79 return int(x + .5);
80 else
81 return int(x - .5);
82 }
83
84 bool EncoderType_iCub2eo(const string* in, uint8_t *out)
85 {
86 if (*in == "NONE")
87 {
88 *out = 0;
89 return true;
90 }
91 else if (*in == "AEA")
92 {
93 *out = 1;
94 return true;
95 }
96 else if (*in == "ROIE")
97 {
98 *out = 2;
99 return true;
100 }
101 else if (*in == "HALL_ADC")
102 {
103 *out = 3;
104 return true;
105 }
106 else if (*in == "MAIS")
107 {
108 *out = 4;
109 return true;
110 }
111 else if (*in == "OPTICAL_QUAD")
112 {
113 *out = 5;
114 return true;
115 }
116 else if (*in == "HALL_MOTOR_SENS")
117 {
118 *out = 6;
119 return true;
120 }
121 *out = 0;
122 return false;
123 }
124
125 bool EncoderType_eo2iCub(const uint8_t *in, string* out)
126 {
127 if (*in == 0)
128 {
129 *out = "NONE";
130 return true;
131 }
132 else if (*in == 1)
133 {
134 *out = "AEA";
135 return true;
136 }
137 else if (*in == 2)
138 {
139 *out = "ROIE";
140 return true;
141 }
142 else if (*in == 3)
143 {
144 *out = "HALL_ADC";
145 return true;
146 }
147 else if (*in == 4)
148 {
149 *out = "MAIS";
150 return true;
151 }
152 else if (*in == 5)
153 {
154 *out = "OPTICAL_QUAD";
155 return true;
156 }
157 else if (*in == 6)
158 {
159 *out = "HALL_MOTOR_SENS";
160 return true;
161 }
162 else if(*in == 9)
163 {
164 *out = "AMO";
165 return true;
166 }
167 *out = "ERROR";
168 return false;
169 }
170
171 void copyPid_iCub2eo(const Pid *in, eOmc_PID_t *out)
172 {
173 memset(out, 0, sizeof(eOmc_PID_t)); // marco.accame: it is good thing to clear the out struct before copying. this prevent future members of struct not yet managed to be dirty.
174 out->kp = (float) (in->kp);
175 out->ki = (float) (in->ki);
176 out->kd = (float) (in->kd);
177 out->limitonintegral = (float)(in->max_int);
178 out->limitonoutput = (float)(in->max_output);
179 out->offset = (float) (in->offset);
180 out->scale = (int8_t) (in->scale);
181 out->kff = (float) (in->kff);
182 out->stiction_down_val = (float)(in->stiction_down_val);
183 out->stiction_up_val = (float)(in->stiction_up_val);
184 }
185
186 void copyPid_eo2iCub(eOmc_PID_t *in, Pid *out)
187 {
188 // marco.accame: in here i dont clear the out class because there is not a clear() method
189 out->kp = (double) in->kp;
190 out->ki = (double) in->ki;
191 out->kd = (double) in->kd;
192 out->max_int = (double) in->limitonintegral;
193 out->max_output = (double) in->limitonoutput;
194 out->offset = (double) in->offset;
195 out->scale = (double) in->scale;
196 out->setStictionValues(in->stiction_up_val, in->stiction_down_val);
197 out->setKff(in->kff);
198 }
199
200
201 bool controlModeCommandConvert_yarp2embObj(int vocabMode, eOenum08_t &embOut)
202 {
203 bool ret = true;
204
205 switch(vocabMode)
206 {
207 case VOCAB_CM_IDLE:
208 embOut = eomc_controlmode_cmd_idle;
209 break;
210
211 case VOCAB_CM_POSITION:
212 embOut = eomc_controlmode_cmd_position;
213 break;
214
215 case VOCAB_CM_POSITION_DIRECT:
216 embOut = eomc_controlmode_cmd_direct;
217 break;
218
219 case VOCAB_CM_VELOCITY:
220 embOut = eomc_controlmode_cmd_velocity;
221 break;
222
223 case VOCAB_CM_MIXED:
224 embOut = eomc_controlmode_cmd_mixed;
225 break;
226
227 case VOCAB_CM_TORQUE:
228 embOut = eomc_controlmode_cmd_torque;
229 break;
230
231 case VOCAB_CM_IMPEDANCE_POS:
232 embOut = eomc_controlmode_cmd_impedance_pos;
233 break;
234
235 case VOCAB_CM_IMPEDANCE_VEL:
236 embOut = eomc_controlmode_cmd_impedance_vel;
237 break;
238
239 case VOCAB_CM_PWM:
240 embOut = eomc_controlmode_cmd_openloop;
241 break;
242
243 case VOCAB_CM_CURRENT:
244 embOut = eomc_controlmode_cmd_current;
245 break;
246
247 case VOCAB_CM_FORCE_IDLE:
248 embOut = eomc_controlmode_cmd_force_idle;
249 break;
250
251 default:
252 ret = false;
253 break;
254 }
255 return ret;
256 }
257
258 int controlModeStatusConvert_embObj2yarp(eOenum08_t embObjMode)
259 {
260 int vocabOut;
261 switch(embObjMode)
262 {
263 case eomc_controlmode_idle:
264 vocabOut = VOCAB_CM_IDLE;
265 break;
266
267 case eomc_controlmode_position:
268 vocabOut = VOCAB_CM_POSITION;
269 break;
270
271 case eomc_controlmode_velocity:
272 vocabOut = VOCAB_CM_VELOCITY;
273 break;
274
275 case eomc_controlmode_direct:
276 vocabOut = VOCAB_CM_POSITION_DIRECT;
277 break;
278
279 case eomc_controlmode_mixed:
280 case eomc_controlmode_velocity_pos: // they are the same, this will probably removed in the future
281 vocabOut = VOCAB_CM_MIXED;
282 break;
283
284 case eomc_controlmode_torque:
285 vocabOut = VOCAB_CM_TORQUE;
286 break;
287
288 case eomc_controlmode_calib:
289 vocabOut = VOCAB_CM_CALIBRATING;
290 break;
291
292 case eomc_controlmode_impedance_pos:
293 vocabOut = VOCAB_CM_IMPEDANCE_POS;
294 break;
295
296 case eomc_controlmode_impedance_vel:
297 vocabOut = VOCAB_CM_IMPEDANCE_VEL;
298 break;
299
300 case eomc_controlmode_openloop:
301 vocabOut = VOCAB_CM_PWM;
302 break;
303
304 case eomc_controlmode_current:
305 vocabOut = VOCAB_CM_CURRENT;
306 break;
307
308 case eomc_controlmode_hwFault:
309 vocabOut = VOCAB_CM_HW_FAULT;
310 break;
311
312 case eomc_controlmode_notConfigured:
313 vocabOut = VOCAB_CM_NOT_CONFIGURED;
314 break;
315
316 case eomc_controlmode_configured:
317 vocabOut = VOCAB_CM_CONFIGURED;
318 break;
319
320 default:
321 printf("embObj to yarp unknown controlmode %d\n", embObjMode);
322 vocabOut = VOCAB_CM_UNKNOWN;
323 break;
324 }
325 return vocabOut;
326 }
327
328 bool controlModeStatusConvert_yarp2embObj(int vocabMode, eOmc_controlmode_t &embOut)
329 {
330 yError() << "controlModeStatusConvert_yarp2embObj" << " is not yet implemented for embObjMotionControl";
331 return false;
332 }
333
334 int controlModeCommandConvert_embObj2yarp(eOmc_controlmode_command_t embObjMode)
335 {
336 yError() << "embObjMotionControl::controlModeCommandConvert_embObj2yarp" << " is not yet implemented for embObjMotionControl";
337 return 0;
338
339 }
340 /*
341 eOmc_pidoutputtype_t pidOutputTypeConver_eomc2fw(PidAlgorithmType_t controlLaw)
342 {
343 switch(controlLaw)
344 {
345 case PidAlgo_simple:
346 return(eomc_pidoutputtype_pwm);
347
348 case PIdAlgo_velocityInnerLoop:
349 return(eomc_pidoutputtype_vel);
350
351 case PidAlgo_currentInnerLoop:
352 return(eomc_pidoutputtype_iqq);
353 default:
354 {
355 yError() << "pidOutputTypeConver_eomc2fw: unknown pid output type" ;
356 return(eomc_pidoutputtype_unknown);
357 }
358 }
359 }
360 */
361
362 bool interactionModeCommandConvert_yarp2embObj(int vocabMode, eOenum08_t &embOut)
363 {
364 bool ret = true;
365
366 switch(vocabMode)
367 {
368 case VOCAB_IM_STIFF:
369 embOut = eOmc_interactionmode_stiff;
370 break;
371
372 case VOCAB_IM_COMPLIANT:
373 embOut = eOmc_interactionmode_compliant;
374 break;
375
376 default:
377 ret = false;
378 break;
379 }
380 return ret;
381 }
382
383
384 bool interactionModeStatusConvert_embObj2yarp(eOenum08_t embObjMode, int &vocabOut)
385 {
386 bool ret = true;
387 switch(embObjMode)
388 {
389 case eOmc_interactionmode_stiff:
390 vocabOut = VOCAB_IM_STIFF;
391 break;
392
393 case eOmc_interactionmode_compliant:
394 vocabOut = VOCAB_IM_COMPLIANT;
395 break;
396
397 default:
398 vocabOut = 666; //VOCAB_CM_UNKNOWN;
399 yError() << "Received an unknown interactionMode from the EMS boards with value " << embObjMode;
400 // ret = false;
401 break;
402 }
403 return ret;
404 }
405
406
407
408 }
409 }}
x
x
Definition compute_ekf_sym.m:21
yarp::dev::eomc::S_32
int S_32(double x)
Definition eomcUtils.h:70
yarp::dev::eomc::EncoderType_eo2iCub
bool EncoderType_eo2iCub(const uint8_t *in, string *out)
Definition eomcUtils.h:125
yarp::dev::eomc::controlModeStatusConvert_embObj2yarp
int controlModeStatusConvert_embObj2yarp(eOenum08_t embObjMode)
Definition eomcUtils.h:258
yarp::dev::eomc::controlModeCommandConvert_embObj2yarp
int controlModeCommandConvert_embObj2yarp(eOmc_controlmode_command_t embObjMode)
Definition eomcUtils.h:334
yarp::dev::eomc::interactionModeStatusConvert_embObj2yarp
bool interactionModeStatusConvert_embObj2yarp(eOenum08_t embObjMode, int &vocabOut)
Definition eomcUtils.h:384
yarp::dev::eomc::controlModeCommandConvert_yarp2embObj
bool controlModeCommandConvert_yarp2embObj(int vocabMode, eOenum08_t &embOut)
Definition eomcUtils.h:201
yarp::dev::eomc::copyPid_eo2iCub
void copyPid_eo2iCub(eOmc_PID_t *in, Pid *out)
Definition eomcUtils.h:186
yarp::dev::eomc::S_16
short S_16(double x)
Definition eomcUtils.h:43
yarp::dev::eomc::controlModeStatusConvert_yarp2embObj
bool controlModeStatusConvert_yarp2embObj(int vocabMode, eOmc_controlmode_t &embOut)
Definition eomcUtils.h:328
yarp::dev::eomc::copyPid_iCub2eo
void copyPid_iCub2eo(const Pid *in, eOmc_PID_t *out)
Definition eomcUtils.h:171
yarp::dev::eomc::interactionModeCommandConvert_yarp2embObj
bool interactionModeCommandConvert_yarp2embObj(int vocabMode, eOenum08_t &embOut)
Definition eomcUtils.h:362
yarp::dev::eomc::U_16
int U_16(double x)
Definition eomcUtils.h:31
yarp::dev::eomc::EncoderType_iCub2eo
bool EncoderType_iCub2eo(const string *in, uint8_t *out)
Definition eomcUtils.h:84
yarp::dev::eomc::U_32
int U_32(double x)
Definition eomcUtils.h:58
yarp
Copyright (C) 2008 RobotCub Consortium.
Definition DebugInterfaces.h:51
out
out
Definition sine.m:8
Generated on Wed Dec 18 2024 16:34:47 for iCub-main by doxygen 1.9.8