iCub-main
Loading...
Searching...
No Matches
eomcParser.cpp
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* Authors: Valentina Gaggero
6* CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
7*
8*/
9
10//#include <yarp/dev/CanBusInterface.h>
11#include <yarp/os/Bottle.h>
12#include <string.h>
13#include <iostream>
14
15
16
17#include <eomcParser.h>
18
19#include <yarp/os/LogStream.h>
20
21#include "EoCommon.h"
22#include "EOarray.h"
23#include "EoProtocol.h"
24#include "EoProtocolMN.h"
25#include "EoProtocolMC.h"
26#include "EoProtocolAS.h"
27#include <vector>
28
29
30using namespace std;
31using namespace yarp::dev;
32using namespace yarp::os;
33using namespace yarp::dev::eomc;
34
35
36
37
38yarp::dev::eomc::Parser::Parser(int numofjoints, string boardname)
39{
40 _njoints = numofjoints;
41 _boardname = boardname;
42 _verbosewhenok = 0;
43 _positionControlLaw.resize(0);
44 _velocityControlLaw.resize(0);
45 _mixedControlLaw.resize(0);
46 //_posDirectControlLaw.resize(0);
47 //_velDirectControlLaw.resize(0);
48 _torqueControlLaw.resize(0);
49 _currentControlLaw.resize(0);
50 _speedControlLaw.resize(0);
51
52 _kbemf=allocAndCheck<double>(_njoints);
53 _ktau=allocAndCheck<double>(_njoints);
54 _filterType=allocAndCheck<int>(_njoints);
55 _viscousPos=allocAndCheck<double>(_njoints);
56 _viscousNeg=allocAndCheck<double>(_njoints);
57 _coulombPos=allocAndCheck<double>(_njoints);
58 _coulombNeg=allocAndCheck<double>(_njoints);
59 _velocityThres=allocAndCheck<double>(_njoints);
60
61 minjerkAlgoMap.clear();
62 //directAlgoMap.clear();
63 torqueAlgoMap.clear();
64};
65
66Parser::~Parser()
67{
68 checkAndDestroy(_kbemf);
69 checkAndDestroy(_ktau);
70 checkAndDestroy(_filterType);
71 checkAndDestroy(_viscousPos);
72 checkAndDestroy(_viscousNeg);
73 checkAndDestroy(_coulombPos);
74 checkAndDestroy(_coulombNeg);
75 checkAndDestroy(_velocityThres);
76}
77
78
79bool Parser::parsePids(yarp::os::Searchable &config, PidInfo *ppids/*, PidInfo *vpids*/, TrqPidInfo *tpids, PidInfo *cpids, PidInfo *spids, bool lowLevPidisMandatory)
80{
81 // compila la lista con i tag dei pid per ciascun modo
82 // di controllo per ciascun giunto
83 //std::vector<std::string> _positionControlLaw;
84 //std::vector<std::string> _velocityControlLaw;
85 //std::vector<std::string> _mixedControlLaw;
86 //std::vector<std::string> _posDirectControlLaw;
87 //std::vector<std::string> _velDirectControlLaw;
88 //std::vector<std::string> _torqueControlLaw;
89 //std::vector<std::string> _currentControlLaw;
90 //std::vector<std::string> _speedControlLaw;
91 if(!parseControlsGroup(config)) // OK
92 return false;
93
94 // legge i pid di corrente per ciascun motore
95 // come specificato in _currentControlLaw
96 if(!parseSelectedCurrentPid(config, lowLevPidisMandatory, cpids)) // OK
97 return false;
98
99 // legge i pid di velocità per ciascun motore
100 // come specificato in _speedControlLaw
101 if(!parseSelectedSpeedPid(config, lowLevPidisMandatory, spids)) // OK
102 return false;
103
104 // usa _positionControlLaw per recuperare i PID
105 // del position control per ogni giunto
106 if(!parseSelectedPositionControl(config)) // OK
107 return false;
108
109 // usa _velocityControlLaw per recuperare i PID
110 // del velocity control per ogni giunto
111 if(!parseSelectedVelocityControl(config)) // OK
112 return false;
113
114 // usa _positionControlLaw per recuperare i PID
115 // del mixed control per ogni giunto
116 if(!parseSelectedMixedControl(config)) // OK
117 return false;
118
119 // usa _posDirectControlLaw per recuperare i PID
120 // del position direct control per ogni giunto
121 //if(!parseSelectedPosDirectControl(config)) // OK
122 // return false;
123
124 // usa _velDirectControlLaw per recuperare i PID
125 // del velocity direct control per ogni giunto
126 //if(!parseSelectedVelDirectControl(config)) // OK
127 // return false;
128
129 // usa _torqueControlLaw per recuperare i PID
130 // del torque control per ogni giunto
131 if(!parseSelectedTorqueControl(config)) // OK
132 return false;
133
134
135
136 if(!getCorrectPidForEachJoint(ppids/*, vpids*/, tpids))
137 return false;
138
139
140 return true;
141}
142
143#define LOAD_STRINGS(dest, source) for (unsigned int i=1; i<source.size(); ++i) dest.push_back(source.get(i).asString())
144
145bool Parser::parseControlsGroup(yarp::os::Searchable &config) // OK
146{
147 Bottle xtmp;
148
149 Bottle controlsGroup = config.findGroup("CONTROLS", "Configuration of used control laws ");
150 if(controlsGroup.isNull())
151 {
152 yError() << "embObjMC BOARD " << _boardname << " no CONTROLS group found in config file, returning";
153 return false;
154 }
155
156 if (!extractGroup(controlsGroup, xtmp, "positionControl", "Position Control ", _njoints))
157 return false;
158 LOAD_STRINGS(_positionControlLaw, xtmp);
159
160 if (!extractGroup(controlsGroup, xtmp, "velocityControl", "Velocity Control ", _njoints))
161 return false;
162 LOAD_STRINGS(_velocityControlLaw, xtmp);
163
164 if (!extractGroup(controlsGroup, xtmp, "mixedControl", "Mixed Control ", _njoints))
165 return false;
166 LOAD_STRINGS(_mixedControlLaw, xtmp);
167
168 //if (!extractGroup(controlsGroup, xtmp, "posDirectControl", "Position Direct Control ", _njoints))
169 // return false;
170 //LOAD_STRINGS(_posDirectControlLaw, xtmp);
171
172 //if (!extractGroup(controlsGroup, xtmp, "velDirectControl", "Velocity Direct Control ", _njoints))
173 // return false;
174 //LOAD_STRINGS(_velDirectControlLaw, xtmp);
175
176 if (!extractGroup(controlsGroup, xtmp, "torqueControl", "Torque Control ", _njoints))
177 return false;
178 LOAD_STRINGS(_torqueControlLaw, xtmp);
179
180 if (!extractGroup(controlsGroup, xtmp, "currentPid", "Current Pid ", _njoints))
181 return false;
182 LOAD_STRINGS(_currentControlLaw, xtmp);
183
184 if (!extractGroup(controlsGroup, xtmp, "speedPid", "Speed Pid ", _njoints))
185 return false;
186 LOAD_STRINGS(_speedControlLaw, xtmp);
187
188 return true;
189}
190
191
192
193bool Parser::parseSelectedCurrentPid(yarp::os::Searchable &config, bool pidisMandatory, PidInfo *pids) // OK
194{
195 //first of all verify current pid has been configured if it is mandatory
196 for (int i = 0; i<_njoints; i++)
197 {
198 if (_currentControlLaw[i] == "none")
199 {
200 if (pidisMandatory)
201 {
202 yError() << "embObjMC BOARD " << _boardname << "CurrentPid is mandatory. It should be different from none ";
203 return false;
204 }
205 }
206 else
207 {
208 // 1) verify that selected control law is defined in file
209 Bottle botControlLaw = config.findGroup(_currentControlLaw[i]);
210 if (botControlLaw.isNull())
211 {
212 yError() << "embObjMC BOARD " << _boardname << "Missing " << i << " current control law " << _currentControlLaw[i].c_str();
213 return false;
214 }
215
216 // 2) read control_law
217 Value &valControlLaw = botControlLaw.find("controlLaw");
218 if ((valControlLaw.isNull()) || (!valControlLaw.isString()))
219 {
220 yError() << "embObjMC BOARD " << _boardname << "Unable read " << i << " control law parameter for " << _currentControlLaw[i].c_str() << ". Quitting.";
221 return false;
222 }
223
224 string strControlLaw = valControlLaw.toString();
225 if (strControlLaw != string("low_lev_current"))
226 {
227 yError() << "embObjMC BOARD " << _boardname << "Unable to use " << i << " control law " << strControlLaw << " for current pid. Quitting.";
228 return false;
229 }
230
231 yarp::dev::PidFeedbackUnitsEnum fbk_unitstype;
232 yarp::dev::PidOutputUnitsEnum out_unitstype;
233 if (!parsePidUnitsType(botControlLaw, fbk_unitstype, out_unitstype))
234 return false;
235
236 yarp::dev::Pid *mycpids = new yarp::dev::Pid[_njoints];
237
238 if (!parsePidsGroup2FOC(botControlLaw, mycpids))
239 {
240 delete[] mycpids;
241
242 return false;
243 }
244
245 pids[i].enabled = true;
246 pids[i].out_type = eomc_ctrl_out_type_cur;
247 pids[i].fbk_PidUnits = fbk_unitstype;
248 pids[i].out_PidUnits = out_unitstype;
249 //pids[i].controlLaw = PidAlgo_simple;
250 pids[i].pid = mycpids[i];
251
252 delete[] mycpids;
253 }
254 }
255
256 return checkJointTypes(pids, "CURRENT");
257}
258
259bool Parser::parseSelectedSpeedPid(yarp::os::Searchable &config, bool pidisMandatory, PidInfo *pids) // OK
260{
261 //first of all verify current pid has been configured if it is mandatory
262 for (int i = 0; i<_njoints; i++)
263 {
264 if (_speedControlLaw[i] == "none")
265 {
266 if (pidisMandatory)
267 {
268 yError() << "embObjMC BOARD " << _boardname << "SpeedPid is mandatory. It should be different from none ";
269 return false;
270 }
271 }
272 else
273 {
274 // 1) verify that selected control law is defined in file
275 Bottle botControlLaw = config.findGroup(_speedControlLaw[i]);
276 if (botControlLaw.isNull())
277 {
278 yError() << "embObjMC BOARD " << _boardname << "Missing " << i << " control law " << _speedControlLaw[i].c_str();
279 return false;
280 }
281
282 // 2) read control_law
283 Value &valControlLaw = botControlLaw.find("controlLaw");
284 if ((valControlLaw.isNull()) || (!valControlLaw.isString()))
285 {
286 yError() << "embObjMC BOARD " << _boardname << "Unable read " << i << " control law parameter for " << _speedControlLaw[i].c_str() << ". Quitting.";
287 return false;
288 }
289
290 string strControlLaw= valControlLaw.toString();
291 if (strControlLaw != string("low_lev_speed"))
292 {
293 yError() << "embObjMC BOARD " << _boardname << "Unable to use " << i << " control law " << strControlLaw << " for speed pid. Quitting.";
294 return false;
295 }
296
297 yarp::dev::PidFeedbackUnitsEnum fbk_unitstype;
298 yarp::dev::PidOutputUnitsEnum out_unitstype;
299 if (!parsePidUnitsType(botControlLaw, fbk_unitstype, out_unitstype))
300 return false;
301
302 yarp::dev::Pid *mycpids = new yarp::dev::Pid[_njoints];
303
304 if (!parsePidsGroup2FOC(botControlLaw, mycpids))
305 {
306 delete[] mycpids;
307
308 return false;
309 }
310
311 pids[i].enabled = true;
312 pids[i].fbk_PidUnits = fbk_unitstype;
313 pids[i].out_PidUnits = out_unitstype;
314 //pids[i].controlLaw = PidAlgo_simple;
315 pids[i].pid = mycpids[i];
316
317 delete[] mycpids;
318 }
319 }
320
321 return checkJointTypes(pids, "SPEED");
322}
323
324bool Parser::parseSelectedPositionControl(yarp::os::Searchable &config) // OK
325{
326 for(int i=0; i<_njoints; i++)
327 {
328 // 1) verify that selected control law is defined in file
329 Bottle botControlLaw = config.findGroup(_positionControlLaw[i]);
330 if (botControlLaw.isNull())
331 {
332 yError() << "embObjMC BOARD " << _boardname << "Missing " << _positionControlLaw[i].c_str();
333 return false;
334 }
335
336 // 2) read control_law
337 Value &valControlLaw= botControlLaw.find("controlLaw");
338 {
339 if ((valControlLaw.isNull()) || (!valControlLaw.isString()))
340 {
341 yError() << "embObjMC BOARD " << _boardname << "Unable read controlLaw parameter for " << _positionControlLaw[i].c_str() << ". Quitting.";
342 return false;
343 }
344 }
345
346 string strControlLaw = valControlLaw.toString();
347 if (strControlLaw != "minjerk")
348 {
349 yError() << "embObjMC BOARD " << _boardname << "Unknown control law for " << _positionControlLaw[i].c_str() << ". Quitting.";
350 return false;
351 }
352
353 Value &valOutputType= botControlLaw.find("outputType");
354 if( (valOutputType.isNull()) || (!valOutputType.isString()) )
355 {
356 yError() << "embObjMC BOARD " << _boardname << "Unable read outputType parameter for " << _positionControlLaw[i].c_str() <<". Quitting.";
357 return false;
358 }
359
360 string strOutputType= valOutputType.toString();
361 if (strOutputType == string("pwm"))
362 {
363 if (!parsePid_minJerk_outPwm(botControlLaw, _positionControlLaw[i]))
364 {
365 yError() << "embObjMC BOARD " << _boardname << "format error in " << _positionControlLaw[i];
366 return false;
367 }
368 }
369 else if (strOutputType == string("velocity"))
370 {
371 if (!parsePid_minJerk_outVel(botControlLaw, _positionControlLaw[i]))
372 {
373 yError() << "embObjMC BOARD " << _boardname << "format error in " << _positionControlLaw[i];
374 return false;
375 }
376 }
377 else if (strOutputType == string("current"))
378 {
379 if (!parsePid_minJerk_outCur(botControlLaw, _positionControlLaw[i]))
380 {
381 yError() << "embObjMC BOARD " << _boardname << "format error in " << _positionControlLaw[i];
382 return false;
383 }
384 }
385 else
386 {
387 yError() << "embObjMC BOARD " << _boardname << "Unable to use output type " << strOutputType << " for position control. Quitting.";
388 return false;
389 }
390
391 }
392 return true;
393
394}
395
396bool Parser::parseSelectedVelocityControl(yarp::os::Searchable &config) // OK
397{
398 for(int i=0; i<_njoints; i++)
399 {
400 if(_velocityControlLaw[i] == "none")
401 {
402 continue;
403 }
404
405 // 1) verify that selected control law is defined in file
406 Bottle botControlLaw = config.findGroup(_velocityControlLaw[i]);
407 if (botControlLaw.isNull())
408 {
409 yError() << "embObjMC BOARD " << _boardname << "Missing " << _velocityControlLaw[i].c_str();
410 return false;
411 }
412
413 // 2) read control_law
414 Value &valControlLaw=botControlLaw.find("controlLaw");
415 {
416 if ((valControlLaw.isNull()) || (!valControlLaw.isString()))
417 {
418 yError() << "embObjMC BOARD " << _boardname << "Unable read controlLaw parameter for " << _velocityControlLaw[i].c_str() << ". Quitting.";
419 return false;
420 }
421 }
422
423 string strControlLaw = valControlLaw.toString();
424 if (strControlLaw != "minjerk")
425 {
426 yError() << "embObjMC BOARD " << _boardname << "Unknown control law for " << _velocityControlLaw[i].c_str() << ". Quitting.";
427 return false;
428 }
429
430 Value &valOutputType = botControlLaw.find("outputType");
431 if ((valOutputType.isNull()) || (!valOutputType.isString()))
432 {
433 yError() << "embObjMC BOARD " << _boardname << "Unable read control law parameter for " << _velocityControlLaw[i].c_str() <<". Quitting.";
434 return false;
435 }
436
437 string strOutputType = valOutputType.toString();
438 if (strOutputType == string("pwm"))
439 {
440 if (!parsePid_minJerk_outPwm(botControlLaw, _velocityControlLaw[i]))
441 {
442 yError() << "embObjMC BOARD " << _boardname << "format error in "<< _velocityControlLaw[i];
443 return false;
444 }
445 }
446 else if (strOutputType == string("velocity"))
447 {
448 if (!parsePid_minJerk_outVel(botControlLaw, _velocityControlLaw[i]))
449 {
450 yError() << "embObjMC BOARD " << _boardname << "format error in " << _velocityControlLaw[i];
451 return false;
452 }
453 }
454 else if (strOutputType == string("current"))
455 {
456 if (!parsePid_minJerk_outCur(botControlLaw, _velocityControlLaw[i]))
457 {
458 yError() << "embObjMC BOARD " << _boardname << "format error in " << _velocityControlLaw[i];
459 return false;
460 }
461 }
462 else
463 {
464 yError() << "embObjMC BOARD " << _boardname << "Unable to use output type " << strOutputType << " for velocity control. Quitting.";
465 return false;
466 }
467
468 }
469 return true;
470
471}
472
473bool Parser::parseSelectedMixedControl(yarp::os::Searchable &config) // OK
474{
475 for (int i = 0; i<_njoints; i++)
476 {
477 if(_mixedControlLaw[i] == "none")
478 {
479 continue;
480 }
481
482 // 1) verify that selected control law is defined in file
483 Bottle botControlLaw = config.findGroup(_mixedControlLaw[i]);
484 if (botControlLaw.isNull())
485 {
486 yError() << "embObjMC BOARD " << _boardname << "Missing " << _mixedControlLaw[i].c_str();
487 return false;
488 }
489
490 // 2) read control_law
491 Value &valControlLaw = botControlLaw.find("controlLaw");
492 {
493 if ((valControlLaw.isNull()) || (!valControlLaw.isString()))
494 {
495 yError() << "embObjMC BOARD " << _boardname << "Unable read controlLaw parameter for " << _mixedControlLaw[i].c_str() << ". Quitting.";
496 return false;
497 }
498 }
499
500 string strControlLaw = valControlLaw.toString();
501 if (strControlLaw != "minjerk")
502 {
503 yError() << "embObjMC BOARD " << _boardname << "Unknown control law for " << _mixedControlLaw[i].c_str() << ". Quitting.";
504 return false;
505 }
506
507
508 Value &valOutputType = botControlLaw.find("outputType");
509 if ((valOutputType.isNull()) || (!valOutputType.isString()))
510 {
511 yError() << "embObjMC BOARD " << _boardname << "Unable read control law parameter for " << _mixedControlLaw[i].c_str() << ". Quitting.";
512 return false;
513 }
514
515 string strOutputType = valOutputType.toString();
516 if (strOutputType == string("pwm"))
517 {
518 if (!parsePid_minJerk_outPwm(botControlLaw, _mixedControlLaw[i]))
519 {
520 yError() << "embObjMC BOARD " << _boardname << "format error in " << _velocityControlLaw[i];
521 return false;
522 }
523 }
524 else if (strOutputType == string("velocity"))
525 {
526 if (!parsePid_minJerk_outVel(botControlLaw, _mixedControlLaw[i]))
527 {
528 yError() << "embObjMC BOARD " << _boardname << "format error in " << _velocityControlLaw[i];
529 return false;
530 }
531 }
532 else if (strOutputType == string("current"))
533 {
534 if (!parsePid_minJerk_outCur(botControlLaw, _mixedControlLaw[i]))
535 {
536 yError() << "embObjMC BOARD " << _boardname << "format error in " << _velocityControlLaw[i];
537 return false;
538 }
539 }
540 else
541 {
542 yError() << "embObjMC BOARD " << _boardname << "Unable to use output type " << strOutputType << " for mixed control. Quitting.";
543 return false;
544 }
545
546 }
547 return true;
548
549}
550#if 0
551bool Parser::parseSelectedPosDirectControl(yarp::os::Searchable &config) // OK
552{
553 for (int i = 0; i<_njoints; i++)
554 {
555 // 1) verify that selected control law is defined in file
556 Bottle botControlLaw = config.findGroup(_posDirectControlLaw[i]);
557 if (botControlLaw.isNull())
558 {
559 yError() << "embObjMC BOARD " << _boardname << "Missing " << _posDirectControlLaw[i].c_str();
560 return false;
561 }
562
563 // 2) read control_law
564 Value &valControlLaw= botControlLaw.find("controlLaw");
565 {
566 if ((valControlLaw.isNull()) || (!valControlLaw.isString()))
567 {
568 yError() << "embObjMC BOARD " << _boardname << "Unable read controlLaw parameter for " << _posDirectControlLaw[i].c_str() << ". Quitting.";
569 return false;
570 }
571 }
572
573 string strControlLaw = valControlLaw.toString();
574 if (strControlLaw != "direct")
575 {
576 yError() << "embObjMC BOARD " << _boardname << "Unknown control law for " << _posDirectControlLaw[i].c_str() << ". Quitting.";
577 return false;
578 }
579
580 Value &valOutputType = botControlLaw.find("outputType");
581 if ((valOutputType.isNull()) || (!valOutputType.isString()))
582 {
583 yError() << "embObjMC BOARD " << _boardname << "Unable read outputType parameter for " << _posDirectControlLaw[i].c_str() << ". Quitting.";
584 return false;
585 }
586
587 string strOutputType = valOutputType.toString();
588 if (strOutputType == string("pwm"))
589 {
590 if (!parsePid_direct_outPwm(botControlLaw, _posDirectControlLaw[i]))
591 {
592 yError() << "embObjMC BOARD " << _boardname << "format error in " << _posDirectControlLaw[i];
593 return false;
594 }
595 }
596 else if (strOutputType == string("velocity"))
597 {
598 if (!parsePid_direct_outVel(botControlLaw, _posDirectControlLaw[i]))
599 {
600 yError() << "embObjMC BOARD " << _boardname << "format error in " << _posDirectControlLaw[i];
601 return false;
602 }
603 }
604 else if (strOutputType == string("current"))
605 {
606 if (!parsePid_direct_outCur(botControlLaw, _posDirectControlLaw[i]))
607 {
608 yError() << "embObjMC BOARD " << _boardname << "format error in " << _posDirectControlLaw[i];
609 return false;
610 }
611 }
612 else
613 {
614 yError() << "embObjMC BOARD " << _boardname << "Unable to use output type " << strOutputType << " for direct position control. Quitting.";
615 return false;
616 }
617
618 }
619 return true;
620
621}
622
623bool Parser::parseSelectedVelDirectControl(yarp::os::Searchable &config) // OK
624{
625 for (int i = 0; i<_njoints; i++)
626 {
627 // 1) verify that selected control law is defined in file
628 Bottle botControlLaw = config.findGroup(_velDirectControlLaw[i]);
629 if (botControlLaw.isNull())
630 {
631 yError() << "embObjMC BOARD " << _boardname << "Missing " << _velDirectControlLaw[i].c_str();
632 return false;
633 }
634
635 // 2) read control_law
636 Value &valControlLaw = botControlLaw.find("controlLaw");
637 {
638 if ((valControlLaw.isNull()) || (!valControlLaw.isString()))
639 {
640 yError() << "embObjMC BOARD " << _boardname << "Unable read controlLaw parameter for " << _velDirectControlLaw[i].c_str() << ". Quitting.";
641 return false;
642 }
643 }
644
645 string strControlLaw = valControlLaw.toString();
646 if (strControlLaw != "direct")
647 {
648 yError() << "embObjMC BOARD " << _boardname << "Unknown control law for " << _velDirectControlLaw[i].c_str() << ". Quitting.";
649 return false;
650 }
651
652 Value &valOutputType = botControlLaw.find("outputType");
653 if ((valOutputType.isNull()) || (!valOutputType.isString()))
654 {
655 yError() << "embObjMC BOARD " << _boardname << "Unable read outputType parameter for " << _velDirectControlLaw[i].c_str() << ". Quitting.";
656 return false;
657 }
658
659 string strOutputType = valOutputType.toString();
660 if (strOutputType == string("pwm"))
661 {
662 if (!parsePid_direct_outPwm(botControlLaw, _velDirectControlLaw[i]))
663 {
664 yError() << "embObjMC BOARD " << _boardname << "format error in " << _velDirectControlLaw[i];
665 return false;
666 }
667 }
668 else if (strOutputType == string("velocity"))
669 {
670 if (!parsePid_direct_outVel(botControlLaw, _velDirectControlLaw[i]))
671 {
672 yError() << "embObjMC BOARD " << _boardname << "format error in " << _velDirectControlLaw[i];
673 return false;
674 }
675 }
676 else if (strOutputType == string("current"))
677 {
678 if (!parsePid_direct_outCur(botControlLaw, _velDirectControlLaw[i]))
679 {
680 yError() << "embObjMC BOARD " << _boardname << "format error in " << _velDirectControlLaw[i];
681 return false;
682 }
683 }
684 else
685 {
686 yError() << "embObjMC BOARD " << _boardname << "Unable to use output type " << strOutputType << " for direct velocity control. Quitting.";
687 return false;
688 }
689
690 }
691 return true;
692
693}
694#endif
695
696bool Parser::parseSelectedTorqueControl(yarp::os::Searchable &config) // OK
697{
698 for(int i=0; i<_njoints; i++)
699 {
700 if(_torqueControlLaw[i] == "none")
701 {
702 continue;
703 }
704 // 1) verify that selected control law is defined in file
705 Bottle botControlLaw = config.findGroup(_torqueControlLaw[i]);
706 if (botControlLaw.isNull())
707 {
708 yError() << "embObjMC BOARD " << _boardname << "Missing " << _torqueControlLaw[i].c_str();
709 return false;
710 }
711
712 // 2) read control_law
713 Value &valControlLaw= botControlLaw.find("controlLaw");
714 if( (valControlLaw.isNull()) || (!valControlLaw.isString()) )
715 {
716 yError() << "embObjMC BOARD " << _boardname << "Unable read control law parameter for " << _torqueControlLaw[i].c_str() <<". Quitting.";
717 return false;
718 }
719
720 string strControlLaw = valControlLaw.toString();
721 if (strControlLaw != "torque")
722 {
723 yError() << "embObjMC BOARD " << _boardname << "Unknown control law for " << _torqueControlLaw[i].c_str() << ". Quitting.";
724 return false;
725 }
726
727 Value &valOutputType = botControlLaw.find("outputType");
728 if ((valOutputType.isNull()) || (!valOutputType.isString()))
729 {
730 yError() << "embObjMC BOARD " << _boardname << "Unable read outputType parameter for " << _torqueControlLaw[i].c_str() << ". Quitting.";
731 return false;
732 }
733
734 string strOutputType = valOutputType.toString();
735 if (strOutputType == string("pwm"))
736 {
737 if (!parsePid_torque_outPwm(botControlLaw, _torqueControlLaw[i]))
738 {
739 yError() << "embObjMC BOARD " << _boardname << "format error in " << _torqueControlLaw[i];
740 return false;
741 }
742 }
743 else if (strOutputType == string("velocity"))
744 {
745 if (!parsePid_torque_outVel(botControlLaw, _torqueControlLaw[i]))
746 {
747 yError() << "embObjMC BOARD " << _boardname << "format error in " << _torqueControlLaw[i];
748 return false;
749 }
750 }
751 else if (strOutputType == string("current"))
752 {
753 if (!parsePid_torque_outCur(botControlLaw, _torqueControlLaw[i]))
754 {
755 yError() << "embObjMC BOARD " << _boardname << "format error in " << _torqueControlLaw[i];
756 return false;
757 }
758 }
759 else
760 {
761 yError() << "embObjMC BOARD " << _boardname << "Unable to use output type " << strOutputType << " for torque control. Quitting.";
762 return false;
763 }
764 }
765 return true;
766
767}
768
769
770/*
771 <group name="2FOC_CUR_CONTROL">
772 <param name="controlLaw"> low_lev_current </param>
773 <param name="fbkControlUnits"> machine_units </param>
774 <param name="outputControlUnits"> machine_units </param>
775 <param name="cur_kff"> 0 0 </param>
776 <param name="cur_kp"> 8 8 </param>
777 <param name="cur_kd"> 0 0 </param>
778 <param name="cur_ki"> 2 2 </param>
779 <param name="cur_shift"> 10 10 </param>
780 <param name="cur_maxOutput"> 32000 32000 </param>
781 <param name="cur_maxInt"> 32000 32000 </param>
782 </group>
783
784 <group name="2FOC_VEL_CONTROL">
785 <param name="controlLaw"> low_lev_velocity </param>
786 <param name="fbkControlUnits"> machine_units </param>
787 <param name="outputControlUnits"> machine_units </param>
788 <param name="spd_kff"> 0 0 </param>
789 <param name="spd_kp"> 12 12 </param>
790 <param name="spd_kd"> 0 0 </param>
791 <param name="spd_ki"> 16 16 </param>
792 <param name="spd_shift"> 10 10 </param>
793 <param name="spd_maxOutput"> 32000 32000 </param>
794 <param name="spd_maxInt"> 32000 32000 </param>
795 </group>
796*/
797
798bool Parser::parsePidsGroup2FOC(Bottle& pidsGroup, Pid myPid[])
799{
800
801 Bottle xtmp;
802
803 if (!extractGroup(pidsGroup, xtmp, "kff", "kff parameter", _njoints)) return false;
804 for (int j = 0; j<_njoints; j++) myPid[j].kff = xtmp.get(j + 1).asFloat64();
805
806 if (!extractGroup(pidsGroup, xtmp, "kp", "kp parameter", _njoints)) return false;
807 for (int j = 0; j<_njoints; j++) myPid[j].kp = xtmp.get(j + 1).asFloat64();
808
809 if (!extractGroup(pidsGroup, xtmp, "kd", "kd parameter", _njoints)) return false;
810 for (int j = 0; j<_njoints; j++) myPid[j].kd = xtmp.get(j + 1).asFloat64();
811
812 if (!extractGroup(pidsGroup, xtmp, "maxOutput", "maxOutput parameter", _njoints)) return false;
813 for (int j = 0; j<_njoints; j++) myPid[j].max_output = xtmp.get(j + 1).asFloat64();
814
815 if (!extractGroup(pidsGroup, xtmp, "ki", "ki parameter", _njoints)) return false;
816 for (int j = 0; j<_njoints; j++) myPid[j].ki = xtmp.get(j + 1).asFloat64();
817
818 if (!extractGroup(pidsGroup, xtmp, "maxInt", "maxInt parameter", _njoints)) return false;
819 for (int j = 0; j<_njoints; j++) myPid[j].max_int = xtmp.get(j + 1).asFloat64();
820
821 if (!extractGroup(pidsGroup, xtmp, "shift", "shift parameter", _njoints)) return false;
822 for (int j = 0; j<_njoints; j++) myPid[j].scale = xtmp.get(j + 1).asFloat64();
823
824 return true;
825}
826
827
828bool Parser::parsePidsGroupSimple(Bottle& pidsGroup, Pid myPid[])
829{
830 /*
831 <param name = "kff"> 1 1 < / param>
832 <param name = "kp"> 5 5 < / param>
833 <param name = "kd"> 0 0 < / param>
834 <param name = "maxOutput"> 32000 32000 < / param>
835 */
836
837 Bottle xtmp;
838
839 if (!extractGroup(pidsGroup, xtmp, "kff", "kff parameter", _njoints)) return false;
840 for (int j = 0; j<_njoints; j++) myPid[j].kff = xtmp.get(j + 1).asFloat64();
841
842 if (!extractGroup(pidsGroup, xtmp, "kp", "kp parameter", _njoints)) return false;
843 for (int j = 0; j<_njoints; j++) myPid[j].kp = xtmp.get(j + 1).asFloat64();
844
845 if (!extractGroup(pidsGroup, xtmp, "kd", "kd parameter", _njoints)) return false;
846 for (int j = 0; j<_njoints; j++) myPid[j].kd = xtmp.get(j + 1).asFloat64();
847
848 if (!extractGroup(pidsGroup, xtmp, "maxOutput", "maxOutput parameter", _njoints)) return false;
849 for (int j = 0; j<_njoints; j++) myPid[j].max_output = xtmp.get(j + 1).asFloat64();
850
851 return true;
852}
853
854bool Parser::parsePidsGroupExtended(Bottle& pidsGroup, Pid myPid[])
855{
856 /*
857 <param name = "kff"> 1 1 < / param>
858 <param name = "kp"> 5 5 < / param>
859 <param name = "kd"> 0 0 < / param>
860 <param name = "maxOutput"> 32000 32000 < / param>
861 */
862
863 if (!parsePidsGroupSimple(pidsGroup, myPid)) return false;
864
865 /*
866 <param name = "ki"> 7111.0 1066.0 < / param>
867 <param name = "maxInt"> 750 1000 < / param>
868 <param name = "stictionUp"> 0 0 < / param>
869 <param name = "stictionDwn"> 0 0 < / param>
870 */
871
872 Bottle xtmp;
873
874 if (!extractGroup(pidsGroup, xtmp, "ki", "ki parameter", _njoints)) return false;
875 for (int j = 0; j<_njoints; j++) myPid[j].ki = xtmp.get(j + 1).asFloat64();
876
877 if (!extractGroup(pidsGroup, xtmp, "maxInt", "maxInt parameter", _njoints)) return false;
878 for (int j = 0; j<_njoints; j++) myPid[j].max_int = xtmp.get(j + 1).asFloat64();
879
880 if (!extractGroup(pidsGroup, xtmp, "stictionUp", "stictionUp parameter", _njoints)) return false;
881 for (int j = 0; j<_njoints; j++) myPid[j].stiction_up_val = xtmp.get(j + 1).asFloat64();
882
883 if (!extractGroup(pidsGroup, xtmp, "stictionDown", "stictionDown parameter", _njoints)) return false;
884 for (int j = 0; j<_njoints; j++) myPid[j].stiction_down_val = xtmp.get(j + 1).asFloat64();
885
886 return true;
887}
888
889bool Parser::parsePidsGroupDeluxe(Bottle& pidsGroup, Pid myPid[])
890{
891 /*
892 <param name = "kff"> 1 1 < / param>
893 <param name = "kp"> 5 5 < / param>
894 <param name = "kd"> 0 0 < / param>
895 <param name = "maxOutput"> 32000 32000 < / param>
896 <param name = "ki"> 7111.0 1066.0 < / param>
897 <param name = "maxInt"> 750 1000 < / param>
898 <param name = "stictionUp"> 0 0 < / param>
899 <param name = "stictionDwn"> 0 0 < / param>
900 */
901
902 if (!parsePidsGroupExtended(pidsGroup, myPid)) return false;
903
904 Bottle xtmp;
905
906 if (!extractGroup(pidsGroup, xtmp, "kbemf", "kbemf parameter", _njoints, false))
907 {
908 yWarning() << "kbemf not found. Using fallback where kbemf is set to 0.";
909 for (int j = 0; j<_njoints; j++) _kbemf[j] = 0;
910 }
911 else
912 {
913 for (int j = 0; j<_njoints; j++) _kbemf[j] = xtmp.get(j + 1).asFloat64();
914 }
915
916 if (!extractGroup(pidsGroup, xtmp, "ktau", "ktau parameter", _njoints)) return false;
917 for (int j = 0; j<_njoints; j++) _ktau[j] = xtmp.get(j + 1).asFloat64();
918
919 if (!extractGroup(pidsGroup, xtmp, "filterType", "filterType param", _njoints)) return false;
920 for (int j = 0; j<_njoints; j++) _filterType[j] = xtmp.get(j + 1).asInt32();
921
922 // Friction parameters
923 if (!extractGroup(pidsGroup, xtmp, "viscousPos", "viscousPos parameter", _njoints, false))
924 {
925 for (int j = 0; j<_njoints; j++) _viscousPos[j] = 0;
926 }
927 else
928 {
929 for (int j = 0; j<_njoints; j++) _viscousPos[j] = xtmp.get(j + 1).asFloat64();
930 }
931
932 if (!extractGroup(pidsGroup, xtmp, "viscousNeg", "viscousNeg parameter", _njoints, false))
933 {
934 for (int j = 0; j<_njoints; j++) _viscousNeg[j] = 0;
935 }
936 else
937 {
938 for (int j = 0; j<_njoints; j++) _viscousNeg[j] = xtmp.get(j + 1).asFloat64();
939 }
940
941 if (!extractGroup(pidsGroup, xtmp, "coulombPos", "coulombPos parameter", _njoints, false))
942 {
943 for (int j = 0; j<_njoints; j++) _coulombPos[j] = 0;
944 }
945 else
946 {
947 for (int j = 0; j<_njoints; j++) _coulombPos[j] = xtmp.get(j + 1).asFloat64();
948 }
949
950 if (!extractGroup(pidsGroup, xtmp, "coulombNeg", "coulombNeg parameter", _njoints, false))
951 {
952 for (int j = 0; j<_njoints; j++) _coulombNeg[j] = 0;
953 }
954 else
955 {
956 for (int j = 0; j<_njoints; j++) _coulombNeg[j] = xtmp.get(j + 1).asFloat64();
957 }
958
959 if (!extractGroup(pidsGroup, xtmp, "velocityThres", "velocity threshold parameter for torque control", _njoints, false))
960 {
961 for (int j = 0; j<_njoints; j++) _velocityThres[j] = 0;
962 }
963 else
964 {
965 for (int j = 0; j<_njoints; j++) _velocityThres[j] = xtmp.get(j + 1).asFloat64();
966 }
967
968
969 return true;
970}
971
972/*
973bool Parser::parsePidsGroup(Bottle& pidsGroup, Pid myPid[], string prefix)
974{
975 int j=0;
976 Bottle xtmp;
977
978 if (!extractGroup(pidsGroup, xtmp, prefix + string("kp"), "Pid kp parameter", _njoints)) return false; for (j=0; j<_njoints; j++) myPid[j].kp = xtmp.get(j+1).asFloat64();
979 if (!extractGroup(pidsGroup, xtmp, prefix + string("kd"), "Pid kd parameter", _njoints)) return false; for (j=0; j<_njoints; j++) myPid[j].kd = xtmp.get(j+1).asFloat64();
980 if (!extractGroup(pidsGroup, xtmp, prefix + string("ki"), "Pid ki parameter", _njoints)) return false; for (j=0; j<_njoints; j++) myPid[j].ki = xtmp.get(j+1).asFloat64();
981 if (!extractGroup(pidsGroup, xtmp, prefix + string("maxInt"), "Pid maxInt parameter", _njoints)) return false; for (j=0; j<_njoints; j++) myPid[j].max_int = xtmp.get(j+1).asFloat64();
982 if (!extractGroup(pidsGroup, xtmp, prefix + string("maxOutput"), "Pid maxOutput parameter", _njoints)) return false; for (j=0; j<_njoints; j++) myPid[j].max_output = xtmp.get(j+1).asFloat64();
983
984 if (!extractGroup(pidsGroup, xtmp, prefix + string("shift"), "Pid shift parameter", _njoints))
985 for (j = 0; j<_njoints; j++) myPid[j].scale = 0.0;
986 else
987 for (j=0; j<_njoints; j++) myPid[j].scale = xtmp.get(j+1).asFloat64();
988
989 if (!extractGroup(pidsGroup, xtmp, prefix + string("ko"), "Pid ko parameter", _njoints))
990 for (j = 0; j<_njoints; j++) myPid[j].offset = 0.0;
991 else
992 for (j=0; j<_njoints; j++) myPid[j].offset = xtmp.get(j+1).asFloat64();
993
994 if (!extractGroup(pidsGroup, xtmp, prefix + string("stictionUp"), "Pid stictionUp", _njoints))
995 for (j = 0; j<_njoints; j++) myPid[j].stiction_up_val = 0.0;
996 else
997 for (j=0; j<_njoints; j++) myPid[j].stiction_up_val = xtmp.get(j+1).asFloat64();
998
999 if (!extractGroup(pidsGroup, xtmp, prefix + string("stictionDwn"), "Pid stictionDwn", _njoints))
1000 for (j=0; j<_njoints; j++) myPid[j].stiction_down_val = 0.0;
1001 else
1002 for (j = 0; j<_njoints; j++) myPid[j].stiction_down_val = xtmp.get(j + 1).asFloat64();
1003
1004 if (!extractGroup(pidsGroup, xtmp, prefix + string("kff"), "Pid kff parameter", _njoints))
1005 for (j=0; j<_njoints; j++) myPid[j].kff = 0.0;
1006 else
1007 for (j = 0; j<_njoints; j++) myPid[j].kff = xtmp.get(j + 1).asFloat64();
1008
1009 return true;
1010}
1011*/
1012
1013bool Parser::extractGroup(Bottle &input, Bottle &out, const std::string &key1, const std::string &txt, int size, bool mandatory)
1014{
1015 size++;
1016 Bottle &tmp=input.findGroup(key1.c_str(), txt.c_str());
1017 if (tmp.isNull())
1018 {
1019 std::string message = key1 + " parameter not found for board " + _boardname + " in bottle " + input.toString();
1020 if(mandatory)
1021 yError () << message.c_str();
1022 else
1023 yWarning() << message.c_str();
1024 return false;
1025 }
1026
1027 if(tmp.size()!=size)
1028 {
1029 yError () << key1.c_str() << " incorrect number of entries in BOARD " << _boardname;
1030 return false;
1031 }
1032
1033 out=tmp;
1034 return true;
1035}
1036
1037bool Parser::parsePid_minJerk_outPwm(Bottle &b_pid, string controlLaw)
1038{
1039 if (minjerkAlgoMap.find(controlLaw) != minjerkAlgoMap.end()) return true;
1040
1041 Pid_Algorithm_simple *pidAlgo_ptr = new Pid_Algorithm_simple(_njoints, eomc_ctrl_out_type_pwm);
1042
1043 yarp::dev::PidFeedbackUnitsEnum fbk_PidUnits;
1044 yarp::dev::PidOutputUnitsEnum out_PidUnits;
1045 if (!parsePidUnitsType(b_pid, fbk_PidUnits, out_PidUnits)) return false;
1046 pidAlgo_ptr->setUnits(fbk_PidUnits, out_PidUnits);
1047
1048 parsePidsGroupExtended(b_pid, pidAlgo_ptr->pid);
1049
1050 minjerkAlgoMap.insert(std::pair<std::string, Pid_Algorithm*>(controlLaw, pidAlgo_ptr));
1051
1052 return true;
1053}
1054
1055bool Parser::parsePid_minJerk_outCur(Bottle &b_pid, string controlLaw)
1056{
1057 if (minjerkAlgoMap.find(controlLaw) != minjerkAlgoMap.end()) return true;
1058
1059 Pid_Algorithm_simple *pidAlgo_ptr = new Pid_Algorithm_simple(_njoints, eomc_ctrl_out_type_cur);
1060
1061 yarp::dev::PidFeedbackUnitsEnum fbk_PidUnits;
1062 yarp::dev::PidOutputUnitsEnum out_PidUnits;
1063 if (!parsePidUnitsType(b_pid, fbk_PidUnits, out_PidUnits)) return false;
1064 pidAlgo_ptr->setUnits(fbk_PidUnits, out_PidUnits);
1065
1066 parsePidsGroupExtended(b_pid, pidAlgo_ptr->pid);
1067
1068 minjerkAlgoMap.insert(std::pair<std::string, Pid_Algorithm*>(controlLaw, pidAlgo_ptr));
1069
1070 return true;
1071}
1072
1073bool Parser::parsePid_minJerk_outVel(Bottle &b_pid, string controlLaw)
1074{
1075 if (minjerkAlgoMap.find(controlLaw) != minjerkAlgoMap.end()) return true;
1076
1077 Pid_Algorithm_simple *pidAlgo_ptr = new Pid_Algorithm_simple(_njoints, eomc_ctrl_out_type_vel);
1078
1079 yarp::dev::PidFeedbackUnitsEnum fbk_PidUnits;
1080 yarp::dev::PidOutputUnitsEnum out_PidUnits;
1081 if (!parsePidUnitsType(b_pid, fbk_PidUnits, out_PidUnits)) return false;
1082 pidAlgo_ptr->setUnits(fbk_PidUnits, out_PidUnits);
1083
1084 parsePidsGroupSimple(b_pid, pidAlgo_ptr->pid);
1085
1086 minjerkAlgoMap.insert(std::pair<std::string, Pid_Algorithm*>(controlLaw, pidAlgo_ptr));
1087
1088 return true;
1089}
1090
1091/*
1092bool Parser::parsePid_direct_outPwm(Bottle &b_pid, string controlLaw)
1093{
1094 if (directAlgoMap.find(controlLaw) != directAlgoMap.end()) return true;
1095
1096 Pid_Algorithm_simple *pidAlgo_ptr = new Pid_Algorithm_simple(_njoints, eomc_ctrl_out_type_pwm);
1097
1098 yarp::dev::PidFeedbackUnitsEnum fbk_PidUnits;
1099 yarp::dev::PidOutputUnitsEnum out_PidUnits;
1100 if (!parsePidUnitsType(b_pid, fbk_PidUnits, out_PidUnits)) return false;
1101 pidAlgo_ptr->setUnits(fbk_PidUnits, out_PidUnits);
1102
1103 parsePidsGroupExtended(b_pid, pidAlgo_ptr->pid);
1104
1105 directAlgoMap.insert(std::pair<std::string, Pid_Algorithm*>(controlLaw, pidAlgo_ptr));
1106
1107 return true;
1108}
1109*/
1110/*
1111bool Parser::parsePid_direct_outCur(Bottle &b_pid, string controlLaw)
1112{
1113 if (directAlgoMap.find(controlLaw) != directAlgoMap.end()) return true;
1114
1115 Pid_Algorithm_simple *pidAlgo_ptr = new Pid_Algorithm_simple(_njoints, eomc_ctrl_out_type_cur);
1116
1117 yarp::dev::PidFeedbackUnitsEnum fbk_PidUnits;
1118 yarp::dev::PidOutputUnitsEnum out_PidUnits;
1119 if (!parsePidUnitsType(b_pid, fbk_PidUnits, out_PidUnits)) return false;
1120 pidAlgo_ptr->setUnits(fbk_PidUnits, out_PidUnits);
1121
1122 parsePidsGroupExtended(b_pid, pidAlgo_ptr->pid);
1123
1124 directAlgoMap.insert(std::pair<std::string, Pid_Algorithm*>(controlLaw, pidAlgo_ptr));
1125
1126 return true;
1127}
1128
1129bool Parser::parsePid_direct_outVel(Bottle &b_pid, string controlLaw)
1130{
1131 if (directAlgoMap.find(controlLaw) != directAlgoMap.end()) return true;
1132
1133 Pid_Algorithm_simple *pidAlgo_ptr = new Pid_Algorithm_simple(_njoints, eomc_ctrl_out_type_vel);
1134
1135 yarp::dev::PidFeedbackUnitsEnum fbk_PidUnits;
1136 yarp::dev::PidOutputUnitsEnum out_PidUnits;
1137 if (!parsePidUnitsType(b_pid, fbk_PidUnits, out_PidUnits)) return false;
1138 pidAlgo_ptr->setUnits(fbk_PidUnits, out_PidUnits);
1139
1140 parsePidsGroupSimple(b_pid, pidAlgo_ptr->pid);
1141
1142 directAlgoMap.insert(std::pair<std::string, Pid_Algorithm*>(controlLaw, pidAlgo_ptr));
1143
1144 return true;
1145}
1146*/
1147bool Parser::parsePid_torque_outPwm(Bottle &b_pid, string controlLaw)
1148{
1149 if (torqueAlgoMap.find(controlLaw) != torqueAlgoMap.end()) return true;
1150
1151 Pid_Algorithm_simple *pidAlgo_ptr = new Pid_Algorithm_simple(_njoints, eomc_ctrl_out_type_pwm);
1152
1153 yarp::dev::PidFeedbackUnitsEnum fbk_PidUnits;
1154 yarp::dev::PidOutputUnitsEnum out_PidUnits;
1155 if(!parsePidUnitsType(b_pid, fbk_PidUnits, out_PidUnits)) return false;
1156 pidAlgo_ptr->setUnits(fbk_PidUnits, out_PidUnits);
1157
1158 parsePidsGroupDeluxe(b_pid, pidAlgo_ptr->pid);
1159
1160 torqueAlgoMap.insert( std::pair<std::string, Pid_Algorithm*>(controlLaw, pidAlgo_ptr));
1161
1162 return true;
1163}
1164
1165bool Parser::parsePid_torque_outCur(Bottle &b_pid, string controlLaw)
1166{
1167 if (torqueAlgoMap.find(controlLaw) != torqueAlgoMap.end()) return true;
1168
1169 Pid_Algorithm_simple *pidAlgo_ptr = new Pid_Algorithm_simple(_njoints, eomc_ctrl_out_type_cur);
1170
1171 yarp::dev::PidFeedbackUnitsEnum fbk_PidUnits;
1172 yarp::dev::PidOutputUnitsEnum out_PidUnits;
1173 if (!parsePidUnitsType(b_pid, fbk_PidUnits, out_PidUnits)) return false;
1174 pidAlgo_ptr->setUnits(fbk_PidUnits, out_PidUnits);
1175
1176 parsePidsGroupDeluxe(b_pid, pidAlgo_ptr->pid);
1177
1178 torqueAlgoMap.insert(std::pair<std::string, Pid_Algorithm*>(controlLaw, pidAlgo_ptr));
1179
1180 return true;
1181}
1182
1183bool Parser::parsePid_torque_outVel(Bottle &b_pid, string controlLaw)
1184{
1185 if (torqueAlgoMap.find(controlLaw) != torqueAlgoMap.end()) return true;
1186
1187 Pid_Algorithm_simple *pidAlgo_ptr = new Pid_Algorithm_simple(_njoints, eomc_ctrl_out_type_vel);
1188
1189 yarp::dev::PidFeedbackUnitsEnum fbk_PidUnits;
1190 yarp::dev::PidOutputUnitsEnum out_PidUnits;
1191 if(!parsePidUnitsType(b_pid, fbk_PidUnits, out_PidUnits)) return false;
1192 pidAlgo_ptr->setUnits(fbk_PidUnits, out_PidUnits);
1193
1194 parsePidsGroupExtended(b_pid, pidAlgo_ptr->pid);
1195
1196 torqueAlgoMap.insert ( std::pair<std::string, Pid_Algorithm*>(controlLaw, pidAlgo_ptr) );
1197
1198 return true;
1199}
1200
1201bool Parser::getCorrectPidForEachJoint(PidInfo *ppids/*, PidInfo *vpids*/, TrqPidInfo *tpids)
1202{
1203 Pid_Algorithm *minjerkAlgo_ptr = NULL;
1204 //Pid_Algorithm *directAlgo_ptr = NULL;
1205 Pid_Algorithm *torqueAlgo_ptr = NULL;
1206
1207 //since some joints could not have all pid configured, reset pid values to 0.
1208 memset(ppids, 0, sizeof(PidInfo)*_njoints);
1209 //memset(vpids, 0, sizeof(PidInfo)*_njoints);
1210 memset(tpids, 0, sizeof(TrqPidInfo)*_njoints);
1211
1212 map<string, Pid_Algorithm*>::iterator it;
1213
1214 for (int i = 0; i < _njoints; i++)
1215 {
1216 //get position pid
1217 it = minjerkAlgoMap.find(_positionControlLaw[i]);
1218 if (it == minjerkAlgoMap.end())
1219 {
1220 yError() << "embObjMC BOARD " << _boardname << "Cannot find " << _positionControlLaw[i].c_str() << "in parsed pos pid";
1221 return false;
1222 }
1223
1224 minjerkAlgo_ptr = minjerkAlgoMap[_positionControlLaw[i]];
1225
1226 ppids[i].pid = minjerkAlgo_ptr->getPID(i);
1227 ppids[i].fbk_PidUnits = minjerkAlgo_ptr->fbk_PidUnits;
1228 ppids[i].out_PidUnits = minjerkAlgo_ptr->out_PidUnits;
1229 //ppids[i].controlLaw = minjerkAlgo_ptr->type;
1230 ppids[i].out_type = minjerkAlgo_ptr->out_type;
1231 ppids[i].usernamePidSelected = _positionControlLaw[i];
1232 ppids[i].enabled = true;
1233
1234 /*
1235 //get velocity pid
1236 if (_posDirectControlLaw[i] == "none")
1237 {
1238 directAlgo_ptr = NULL;
1239 }
1240 else
1241 {
1242 it = directAlgoMap.find(_posDirectControlLaw[i]);
1243 if (it == directAlgoMap.end())
1244 {
1245 yError() << "embObjMC BOARD " << _boardname << "Cannot find " << _posDirectControlLaw[i].c_str() << "in parsed vel pid";
1246 return false;
1247 }
1248
1249 directAlgo_ptr = directAlgoMap[_posDirectControlLaw[i]];
1250 }
1251
1252 if (directAlgo_ptr)
1253 {
1254 vpids[i].pid = directAlgo_ptr->getPID(i);
1255 vpids[i].fbk_PidUnits = directAlgo_ptr->fbk_PidUnits;
1256 vpids[i].out_PidUnits = directAlgo_ptr->out_PidUnits;
1257 //vpids[i].controlLaw = directAlgo_ptr->type;
1258 vpids[i].out_type = directAlgo_ptr->out_type;
1259 vpids[i].usernamePidSelected = _posDirectControlLaw[i];
1260 vpids[i].enabled = true;
1261 }
1262 else
1263 {
1264 vpids[i].enabled = false;
1265 vpids[i].usernamePidSelected = "none";
1266 }
1267 */
1268
1269 //get torque pid
1270 if (_torqueControlLaw[i] == "none")
1271 {
1272 torqueAlgo_ptr = NULL;
1273 }
1274 else
1275 {
1276 it = torqueAlgoMap.find(_torqueControlLaw[i]);
1277 if (it == torqueAlgoMap.end())
1278 {
1279 yError() << "embObjMC BOARD " << _boardname << "Cannot find " << _torqueControlLaw[i].c_str() << "in parsed trq pid";
1280 return false;
1281 }
1282
1283 torqueAlgo_ptr = torqueAlgoMap[_torqueControlLaw[i]];
1284 }
1285
1286 if (torqueAlgo_ptr)
1287 {
1288 tpids[i].pid = torqueAlgo_ptr->getPID(i);
1289 tpids[i].fbk_PidUnits = torqueAlgo_ptr->fbk_PidUnits;
1290 tpids[i].out_PidUnits = torqueAlgo_ptr->out_PidUnits;
1291 //tpids[i].controlLaw = torqueAlgo_ptr->type;
1292 tpids[i].out_type = torqueAlgo_ptr->out_type;
1293 tpids[i].usernamePidSelected = _torqueControlLaw[i];
1294 tpids[i].enabled = true;
1295 tpids[i].kbemf = _kbemf[i];
1296 tpids[i].ktau = _ktau[i];
1297 tpids[i].viscousPos = _viscousPos[i];
1298 tpids[i].viscousNeg = _viscousNeg[i];
1299 tpids[i].coulombPos = _coulombPos[i];
1300 tpids[i].coulombNeg = _coulombNeg[i];
1301 tpids[i].velocityThres = _velocityThres[i];
1302 tpids[i].filterType = _filterType[i];
1303 }
1304 else
1305 {
1306 tpids[i].enabled = false;
1307 tpids[i].usernamePidSelected = "none";
1308 }
1309 }
1310
1311 //eomc_ctrl_out_type_n_a = 0,
1312 //eomc_ctrl_out_type_pwm = 1,
1313 //eomc_ctrl_out_type_vel = 2,
1314 //eomc_ctrl_out_type_cur = 3
1315
1316 return checkJointTypes(tpids, "TORQUE") && checkJointTypes(ppids, "POSITION");
1317}
1318
1319bool Parser::parsePidUnitsType(Bottle &bPid, yarp::dev::PidFeedbackUnitsEnum &fbk_pidunits, yarp::dev::PidOutputUnitsEnum& out_pidunits)
1320{
1321
1322 Value &fbkControlUnits=bPid.find("fbkControlUnits");
1323 Value &outControlUnits = bPid.find("outputControlUnits");
1324 if(fbkControlUnits.isNull())
1325 {
1326 yError() << "embObjMC BOARD " << _boardname << " missing fbkControlUnits parameter";
1327 return false;
1328 }
1329 if(!fbkControlUnits.isString())
1330 {
1331 yError() << "embObjMC BOARD " << _boardname << " fbkControlUnits parameter is not a string";
1332 return false;
1333 }
1334 if (outControlUnits.isNull())
1335 {
1336 yError() << "embObjMC BOARD " << _boardname << " missing outputControlUnits parameter";
1337 return false;
1338 }
1339 if (!outControlUnits.isString())
1340 {
1341 yError() << "embObjMC BOARD " << _boardname << " outputControlUnits parameter is not a string";
1342 return false;
1343 }
1344
1345 if(fbkControlUnits.toString()==string("metric_units"))
1346 {
1347 fbk_pidunits = yarp::dev::PidFeedbackUnitsEnum::METRIC;
1348 }
1349 else if(fbkControlUnits.toString()==string("machine_units"))
1350 {
1351 fbk_pidunits = yarp::dev::PidFeedbackUnitsEnum::RAW_MACHINE_UNITS;
1352 }
1353 else
1354 {
1355 yError() << "embObjMC BOARD " << _boardname << "invalid fbkControlUnits value: " << fbkControlUnits.toString().c_str();
1356 return false;
1357 }
1358
1359 if (outControlUnits.toString() == string("dutycycle_percent"))
1360 {
1361 out_pidunits = yarp::dev::PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT;
1362 }
1363 else if (outControlUnits.toString() == string("machine_units"))
1364 {
1365 out_pidunits = yarp::dev::PidOutputUnitsEnum::RAW_MACHINE_UNITS;
1366 }
1367 else
1368 {
1369 yError() << "embObjMC BOARD " << _boardname << "invalid outputControlUnits value: " << outControlUnits.toString().c_str();
1370 return false;
1371 }
1372 return true;
1373}
1374
1375bool Parser::parseFocGroup(yarp::os::Searchable &config, eomc::focBasedSpecificInfo_t *foc_based_info, std::string groupName, std::vector<std::unique_ptr<eomc::ITemperatureSensor>>& temperatureSensorsVector)
1376{
1377 Bottle &focGroup=config.findGroup(groupName);
1378 if (focGroup.isNull() )
1379 {
1380 yError() << "embObjMC BOARD " << _boardname << " detected that Group " << groupName << " is not found in configuration file";
1381 return false;
1382 }
1383
1384 Bottle xtmp;
1385 unsigned int i;
1386
1387 if (!extractGroup(focGroup, xtmp, "HasHallSensor", "HasHallSensor 0/1 ", _njoints))
1388 {
1389 return false;
1390 }
1391 else
1392 {
1393 for (i = 1; i < xtmp.size(); i++)
1394 foc_based_info[i - 1].hasHallSensor = xtmp.get(i).asInt32() != 0;
1395 }
1396
1397 if (!extractGroup(focGroup, xtmp, "TemperatureSensorType", "TemperatureSensorType PT100/PT1000/NONE ", _njoints, false))
1398 {
1399 // 1. check if I have old config
1400 if (extractGroup(focGroup, xtmp, "HasTempSensor", "HasTempSensor 0/1 ", _njoints, false))
1401 {
1402 for (i = 1; i < xtmp.size(); i++)
1403 {
1404 if (xtmp.get(i).asInt32() != 0)
1405 {
1406 yError() << "In " << _boardname << "entry" << i << ": inconsistent configuration. HasTempSensor cannot be used alone. Will be soon deprecated. Use TemperatureSensorType in 2FOC group and set Temperature limits in LIMITS group." ;
1407 return false;
1408 }
1409 }
1410 }
1411
1412 // if I'm here all joints have HasTempSensor =0
1413 for (i = 0; i < _njoints; i++)
1414 {
1415 foc_based_info[i].hasTempSensor = 0;
1416 temperatureSensorsVector.at(i) = std::make_unique<eomc::TemperatureSensorNONE>();
1417 yWarning()<< _boardname << "ATTENTION HasTempSensor will be soon DEPRECATED in favour of TemperatureSensorType (PT100, PT1000, NONE(=default)). Currently kept for backward compatibility but update your configuration files if using a Temperature Sensor";
1418 }
1419
1420 }
1421 else
1422 {
1423 for (i = 1; i < xtmp.size(); i++)
1424 {
1425 std::string s = xtmp.get(i).asString();
1426 if(s == "PT100")
1427 {
1428 foc_based_info[i - 1].hasTempSensor = 1;
1429 temperatureSensorsVector.at(i-1) = std::make_unique<eomc::TemperatureSensorPT100>();
1430
1431 }
1432 else if (s == "PT1000")
1433 {
1434
1435 foc_based_info[i - 1].hasTempSensor = 1;
1436 temperatureSensorsVector.at(i-1) = std::make_unique<eomc::TemperatureSensorPT1000>();
1437 }
1438 else
1439 {
1440 if(s != "NONE")//if sis == NONE the warning is not correct
1441 yWarning("Not available or Not supported TemperatureSensorType: %s. Setting NONE as default", s.c_str());
1442 foc_based_info[i - 1].hasTempSensor = 0;
1443 temperatureSensorsVector.at(i-1) = std::make_unique<eomc::TemperatureSensorNONE>();
1444 }
1445 }
1446 }
1447 if (!extractGroup(focGroup, xtmp, "HasRotorEncoder", "HasRotorEncoder 0/1 ", _njoints))
1448 {
1449 return false;
1450 }
1451 else
1452 {
1453
1454 for (i = 1; i < xtmp.size(); i++)
1455 foc_based_info[i - 1].hasRotorEncoder = xtmp.get(i).asInt32() != 0;
1456 }
1457 if (!extractGroup(focGroup, xtmp, "HasRotorEncoderIndex", "HasRotorEncoderIndex 0/1 ", _njoints))
1458 {
1459 return false;
1460 }
1461 else
1462 {
1463 for (i = 1; i < xtmp.size(); i++)
1464 foc_based_info[i - 1].hasRotorEncoderIndex = xtmp.get(i).asInt32() != 0;
1465 }
1466
1467 if (!extractGroup(focGroup, xtmp, "Verbose", "Verbose 0/1 ", _njoints, false))
1468 {
1469 //return false;
1470 yWarning() << "In " << _boardname << " there isn't " << groupName << ". Verbose failed. By default it is disabled" ;
1471 for (i = 0; i < (unsigned)_njoints; i++)
1472 foc_based_info[i].verbose = 0;
1473 }
1474 else
1475 {
1476 for (i = 1; i < xtmp.size(); i++)
1477 foc_based_info[i - 1].verbose = xtmp.get(i).asInt32() != 0;
1478 }
1479
1480 std::vector<int> AutoCalibration (_njoints);
1481 if (!extractGroup(focGroup, xtmp, "AutoCalibration", "AutoCalibration 0/1 ", _njoints, false))
1482 {
1483 //return false;
1484 yWarning() << "In " << _boardname << " there isn't " << groupName << ". AutoCalibration failed. By default it is disabled" ;
1485 for (i = 0; i < (unsigned)_njoints; i++)
1486 AutoCalibration[i] = 0;
1487 }
1488 else
1489 {
1490 for (i = 1; i < xtmp.size(); i++)
1491 AutoCalibration[i - 1] = xtmp.get(i).asInt32();
1492 }
1493
1494 std::vector<float> Kbemf (_njoints);
1495 if (!extractGroup(focGroup, xtmp, "Kbemf", "Kbemf", _njoints, false))
1496 {
1497 //return false;
1498 yWarning() << "In " << _boardname << " there isn't " << groupName << ". Kbemf failed. By default Kbemf is set to 0" ;
1499 for (i = 0; i < (unsigned)_njoints; i++)
1500 foc_based_info[i].kbemf = 0.0f;
1501 }
1502 else
1503 {
1504 for (i = 1; i < xtmp.size(); i++)
1505 foc_based_info[i-1].kbemf = xtmp.get(i).asFloat32();
1506 }
1507
1508 if (!extractGroup(focGroup, xtmp, "RotorIndexOffset", "RotorIndexOffset", _njoints))
1509 {
1510 return false;
1511 }
1512 else
1513 {
1514 for (i = 1; i < xtmp.size(); i++)
1515 {
1516 if(AutoCalibration[i-1] == 0)
1517 {
1518 foc_based_info[i - 1].rotorIndexOffset = xtmp.get(i).asInt32();
1519 if (foc_based_info[i - 1].rotorIndexOffset <0 || foc_based_info[i - 1].rotorIndexOffset >359)
1520 {
1521 yError() << "In " << _boardname << "joint " << i-1 << ": rotorIndexOffset should be in [0,359] range." ;
1522 return false;
1523 }
1524 }
1525 else
1526 {
1527 yWarning() << "In " << _boardname << "joint " << i-1 << ": motor autocalibration is enabled!!! ATTENTION!!!" ;
1528 foc_based_info[i - 1].rotorIndexOffset = -1;
1529 }
1530 }
1531 }
1532
1533
1534 //Now I verify if rotor encoder hasn't index, then rotor offset must be zero.
1535 for (i = 0; i < (unsigned)_njoints; i++)
1536 {
1537 if((0 == foc_based_info[i].hasRotorEncoderIndex) && (0 != foc_based_info[i].rotorIndexOffset))
1538 {
1539 yError() << "In " << _boardname << "joint " << i << ": inconsistent configuration: if rotor encoder hasn't index then its offset should be 0." ;
1540 return false;
1541 }
1542 }
1543
1544 // Number of motor poles
1545 if (!extractGroup(focGroup, xtmp, "MotorPoles", "MotorPoles", _njoints))
1546 {
1547 return false;
1548 }
1549 else
1550 {
1551 for (i = 1; i < xtmp.size(); i++)
1552 foc_based_info[i - 1].motorPoles = xtmp.get(i).asInt32();
1553 }
1554
1555 if (!extractGroup(focGroup, xtmp, "HasSpeedEncoder", "HasSpeedEncoder 0/1 ", _njoints))
1556 {
1557 yWarning () << "missing param HasSpeedEncoder";
1558 // optional by now
1559 //return false;
1560 }
1561 else
1562 {
1563 for (i = 1; i < xtmp.size(); i++)
1564 foc_based_info[i - 1].hasSpeedEncoder = xtmp.get(i).asInt32() != 0;
1565 }
1566
1567 return true;
1568
1569}
1570
1571bool Parser::parseJointsetCfgGroup(yarp::os::Searchable &config, std::vector<JointsSet> &jsets, std::vector<int> &joint2set)
1572{
1573 Bottle jointsetcfg = config.findGroup("JOINTSET_CFG");
1574 if (jointsetcfg.isNull())
1575 {
1576 yError() << "embObjMC BOARD " << _boardname << "Missing JOINTSET_CFG group";
1577 return false;
1578 }
1579
1580
1581 Bottle xtmp;
1582 int numofsets = 0;
1583
1584 if(!extractGroup(jointsetcfg, xtmp, "numberofsets", "number of sets ", 1))
1585 {
1586 return false;
1587 }
1588
1589 numofsets = xtmp.get(1).asInt32();
1590
1591 if((0 == numofsets) || (numofsets > _njoints))
1592 {
1593 yError() << "embObjMC BOARD " << _boardname << "Number of jointsets is not correct. it should belong to (1, " << _njoints << ")";
1594 return false;
1595 }
1596
1597
1598
1599 if(!checkAndSetVectorSize(jsets, numofsets, "parseJointsetCfgGroup"))
1600 return false;
1601
1602 if(!checkAndSetVectorSize(joint2set, _njoints, "parseJointsetCfgGroup"))
1603 return false;
1604
1605 for(unsigned int s=0;s<(unsigned)numofsets;s++)
1606 {
1607 char jointset_string[80];
1608 sprintf(jointset_string, "JOINTSET_%d", s);
1609 bool formaterror = false;
1610
1611
1612 Bottle &js_cfg = jointsetcfg.findGroup(jointset_string);
1613 if(js_cfg.isNull())
1614 {
1615 yError() << "embObjMC BOARD " << _boardname << "cannot find " << jointset_string;
1616 return false;
1617 }
1618
1619 //1) id of set
1620 jsets.at(s).id=s;
1621
1622
1623 //2) list of joints
1624 Bottle &b_listofjoints=js_cfg.findGroup("listofjoints", "list of joints");
1625 if (b_listofjoints.isNull())
1626 {
1627 yError() << "embObjMC BOARD " << _boardname << "listofjoints parameter not found";
1628 return false;
1629 }
1630
1631 int numOfJointsInSet = b_listofjoints.size()-1;
1632 if((numOfJointsInSet < 1) || (numOfJointsInSet>_njoints))
1633 {
1634 yError() << "embObjMC BOARD " << _boardname << "numof joints of set " << s << " is not correct";
1635 return false;
1636 }
1637
1638
1639 for (int j = 0; j <numOfJointsInSet; j++)
1640 {
1641 int jointofthisset = b_listofjoints.get(j+1).asInt32();
1642
1643 if((jointofthisset< 0) || (jointofthisset>_njoints))
1644 {
1645 yError() << "embObjMC BOARD " << _boardname << "invalid joint number for set " << s;
1646 return false;
1647 }
1648
1649 jsets.at(s).joints.push_back(jointofthisset);
1650
1651 //2.1) fill map joint to set
1652 joint2set.at(jointofthisset) = s;
1653 }
1654
1655 // 3) constraints
1656 if(!extractGroup(js_cfg, xtmp, "constraint", "type of jointset constraint ", 1))
1657 {
1658 return false;
1659 }
1660
1661 eOmc_jsetconstraint_t constraint;
1662 if(!convert(xtmp.get(1).asString(), constraint, formaterror))
1663 {
1664 return false;
1665 }
1666 jsets.at(s).cfg.constraints.type = constraint;
1667
1668 //param1
1669 if(!extractGroup(js_cfg, xtmp, "param1", "param1 of jointset constraint ", 1))
1670 {
1671 return false;
1672 }
1673 jsets.at(s).cfg.constraints.param1 = (float)xtmp.get(1).asFloat64();
1674
1675 //param2
1676 if(!extractGroup(js_cfg, xtmp, "param2", "param2 of jointset constraint ", 1))
1677 {
1678 return false;
1679 }
1680 jsets.at(s).cfg.constraints.param2 = (float)xtmp.get(1).asFloat64();
1681
1682
1683 }
1684 return true;
1685}
1686
1687bool Parser::parseTimeoutsGroup(yarp::os::Searchable &config, std::vector<timeouts_t> &timeouts, int defaultTimeout)
1688{
1689 if(!checkAndSetVectorSize(timeouts, _njoints, "parseTimeoutsGroup"))
1690 return false;
1691
1692 unsigned int i;
1693
1694 Bottle timeoutsGroup =config.findGroup("TIMEOUTS");
1695 if(timeoutsGroup.isNull())
1696 {
1697 yError() << "embObjMC BOARD " << _boardname << " no TIMEOUTS group found in config file.";
1698 return false;
1699 }
1700
1701 Bottle xtmp;
1702 xtmp.clear();
1703 if (!extractGroup(timeoutsGroup, xtmp, "velocity", "a list of timeout to be used in the vmo control", _njoints, false))
1704 {
1705 yWarning() << "embObjMC BOARD " << _boardname << " no velocity parameter found in TIMEOUTS group in motion control config file, using default = " << defaultTimeout << " ms.";
1706 for(i=0; i<_njoints; i++)
1707 timeouts[i].velocity_ref = defaultTimeout;
1708 }
1709 else
1710 {
1711 for(i=1; i<xtmp.size(); i++)
1712 timeouts[i-1].velocity_ref = xtmp.get(i).asInt32();
1713 }
1714
1715 xtmp.clear();
1716 if (!extractGroup(timeoutsGroup, xtmp, "current", "a list of timeout to be used in the vmo control", _njoints, false))
1717 {
1718 yWarning() << "embObjMC BOARD " << _boardname << " no current parameter found in TIMEOUTS group in motion control config file, using default = " << defaultTimeout << " ms.";
1719 for(i=0; i<_njoints; i++)
1720 timeouts[i].current_ref = defaultTimeout;
1721 }
1722 else
1723 {
1724 for(i=1; i<xtmp.size(); i++)
1725 timeouts[i-1].current_ref = xtmp.get(i).asInt32();
1726 }
1727
1728 xtmp.clear();
1729 if (!extractGroup(timeoutsGroup, xtmp, "pwm", "a list of timeout to be used in the vmo control", _njoints, false))
1730 {
1731 yWarning() << "embObjMC BOARD " << _boardname << " no pwm parameter found in TIMEOUTS group in motion control config file, using default = " << defaultTimeout << " ms.";
1732 for(i=0; i<_njoints; i++)
1733 timeouts[i].pwm_ref = defaultTimeout;
1734 }
1735 else
1736 {
1737 for(i=1; i<xtmp.size(); i++)
1738 timeouts[i-1].pwm_ref = xtmp.get(i).asInt32();
1739 }
1740
1741 xtmp.clear();
1742 if (!extractGroup(timeoutsGroup, xtmp, "torque", "a list of timeout to be used in the vmo control", _njoints, false))
1743 {
1744 yWarning() << "embObjMC BOARD " << _boardname << " no torque parameter found in TIMEOUTS group in motion control config file, using default = " << defaultTimeout << " ms.";
1745 for(i=0; i<_njoints; i++)
1746 timeouts[i].torque_ref = defaultTimeout;
1747 }
1748 else
1749 {
1750 for(i=1; i<xtmp.size(); i++)
1751 timeouts[i-1].torque_ref = xtmp.get(i).asInt32();
1752 }
1753
1754 xtmp.clear();
1755 if (!extractGroup(timeoutsGroup, xtmp, "torque_measure", "a list of timeout to be used in the vmo control", _njoints, false))
1756 {
1757 yWarning() << "embObjMC BOARD " << _boardname << " no torque_measure parameter found in TIMEOUTS group in motion control config file, using default = " << defaultTimeout << " ms.";
1758 for(i=0; i<_njoints; i++)
1759 timeouts[i].torque_fbk = defaultTimeout;
1760 }
1761 else
1762 {
1763 for(i=1; i<xtmp.size(); i++)
1764 timeouts[i-1].torque_fbk = xtmp.get(i).asInt32();
1765 }
1766
1767 return true;
1768
1769}
1770
1771bool Parser::parseCurrentLimits(yarp::os::Searchable &config, std::vector<motorCurrentLimits_t> &currLimits)
1772{
1773 Bottle &limits=config.findGroup("LIMITS");
1774 if (limits.isNull())
1775 {
1776 yError() << "embObjMC BOARD " << _boardname << " detected that Group LIMITS is not found in configuration file";
1777 return false;
1778 }
1779
1780 currLimits.resize(_njoints);
1781 unsigned int i;
1782 Bottle xtmp;
1783
1784 // current limit
1785 if (!extractGroup(limits, xtmp, "motorOverloadCurrents","a list of current limits", _njoints))
1786 return false;
1787 else
1788 for(i=1; i<xtmp.size(); i++) currLimits[i-1].overloadCurrent=xtmp.get(i).asFloat64();
1789
1790 // nominal current
1791 if (!extractGroup(limits, xtmp, "motorNominalCurrents","a list of nominal current limits", _njoints))
1792 return false;
1793 else
1794 for(i=1; i<xtmp.size(); i++) currLimits[i-1].nominalCurrent =xtmp.get(i).asFloat64();
1795
1796 // peak current
1797 if (!extractGroup(limits, xtmp, "motorPeakCurrents","a list of peak current limits", _njoints))
1798 return false;
1799 else
1800 for(i=1; i<xtmp.size(); i++) currLimits[i-1].peakCurrent=xtmp.get(i).asFloat64();
1801
1802 return true;
1803
1804}
1805
1806bool Parser::parseTemperatureLimits(yarp::os::Searchable &config, std::vector<temperatureLimits_t> &temperatureLimits)
1807{
1808 Bottle &limits = config.findGroup("LIMITS");
1809 if (limits.isNull())
1810 {
1811 yError() << "embObjMC BOARD " << _boardname << " detected that Group LIMITS is not found in configuration file";
1812 return false;
1813 }
1814
1815 temperatureLimits.resize(_njoints);
1816 unsigned int i;
1817 Bottle xtmp;
1818
1819 // hardware limit
1820 if(!extractGroup(limits, xtmp, "hardwareTemperatureLimits", "a list of temperature limits", _njoints, false))
1821 {
1822 yWarning("hardwareTemperatureLimits param not found in config file for board %s. Please update robot configuration files or contact https://github.com/robotology/icub-support if needed. Using default values.", _boardname.c_str());
1823 for (i = 0; i < (unsigned)_njoints; i++)
1824 {
1825 temperatureLimits[i].hardwareTemperatureLimit = 0;
1826 temperatureLimits[i].warningTemperatureLimit = 0;
1827 }
1828 }
1829 else
1830 {
1831 for (i = 1; i < xtmp.size(); i++) temperatureLimits[i - 1].hardwareTemperatureLimit = xtmp.get(i).asFloat64();
1832 if (!extractGroup(limits, xtmp, "warningTemperatureLimits", "a list of warning temperature limits", _njoints, false))
1833 {
1834 // warning limit - parsing it only if hardwareTemperatureLimit available
1835 yWarning("warningTemperatureLimits param not found in config file for board %s. Please update robot configuration files or contact https://github.com/robotology/icub-support if needed. Using default values.", _boardname.c_str());
1836
1837 for (i = 0; i < (unsigned)_njoints; i++) temperatureLimits[i].warningTemperatureLimit = 0;
1838 }
1839 else
1840 {
1841 for (i = 1; i < xtmp.size(); i++) temperatureLimits[i - 1].warningTemperatureLimit = xtmp.get(i).asFloat64();
1842 }
1843 }
1844
1845 //Now I verify that warning temperature limits is < 85% of hardwareTemperatureLimit.
1846 for (i = 0; i < (unsigned)_njoints; i++)
1847 {
1848 if(temperatureLimits[i].warningTemperatureLimit > (0.85 * temperatureLimits[i].hardwareTemperatureLimit))
1849 {
1850 yError() << "In " << _boardname << "joint " << i << ": inconsistent temperature limits. warningTemperatureLimit must be smaller than 85% of hardwareTemperatureLimit" ;
1851 return false;
1852 }
1853 }
1854 return true;
1855}
1856
1857bool Parser::parseJointsLimits(yarp::os::Searchable &config, std::vector<jointLimits_t> &jointsLimits)
1858{
1859 Bottle &limits=config.findGroup("LIMITS");
1860 if (limits.isNull())
1861 {
1862 yError() << "embObjMC BOARD " << _boardname << " detected that Group LIMITS is not found in configuration file";
1863 return false;
1864 }
1865
1866 jointsLimits.resize(_njoints);
1867 unsigned int i;
1868 Bottle xtmp;
1869
1870 // max limit
1871 if (!extractGroup(limits, xtmp, "jntPosMax","a list of user maximum angles (in degrees)", _njoints))
1872 return false;
1873 else
1874 for(i=1; i<xtmp.size(); i++) jointsLimits[i-1].posMax = xtmp.get(i).asFloat64();
1875
1876 // min limit
1877 if (!extractGroup(limits, xtmp, "jntPosMin","a list of user minimum angles (in degrees)", _njoints))
1878 return false;
1879 else
1880 for(i=1; i<xtmp.size(); i++) jointsLimits[i-1].posMin = xtmp.get(i).asFloat64();
1881
1882 // max hardware limit
1883 if (!extractGroup(limits, xtmp, "hardwareJntPosMax","a list of hardware maximum angles (in degrees)", _njoints))
1884 return false;
1885 else
1886 {
1887 for(i=1; i<xtmp.size(); i++) jointsLimits[i-1].posHwMax = xtmp.get(i).asFloat64();
1888
1889 //check hardware limits are bigger then user limits
1890 for(i=0; i<(unsigned)_njoints; i++)
1891 {
1892 if(jointsLimits[i].posMax > jointsLimits[i].posHwMax)
1893 {
1894 yError() << "embObjMotionControl: user has set a limit bigger then hardware limit!. Please check jntPosMax.";
1895 return false;
1896 }
1897 }
1898 }
1899
1900 // min hardware limit
1901 if (!extractGroup(limits, xtmp, "hardwareJntPosMin","a list of hardware minimum angles (in degrees)", _njoints))
1902 {
1903 return false;
1904 }
1905 else
1906 {
1907 for(i=1; i<xtmp.size(); i++) jointsLimits[i-1].posHwMin = xtmp.get(i).asFloat64();
1908
1909 //check hardware limits are bigger then user limits
1910 for(i=0; i<(unsigned)_njoints; i++)
1911 {
1912 if(jointsLimits[i].posMin < jointsLimits[i].posHwMin)
1913 {
1914 yError() << "embObjMotionControl: user has set a limit bigger then hardware limit!. Please check jntPosMin.";
1915 return false;
1916 }
1917 }
1918
1919 }
1920
1921 // joint Velocity command max limit
1922 if (!extractGroup(limits, xtmp, "jntVelMax", "a list of maximum velocities for the joints (in degrees/s)", _njoints))
1923 return false;
1924 else
1925 for (i = 1; i<xtmp.size(); i++) jointsLimits[i - 1].velMax = xtmp.get(i).asFloat64();
1926
1927 return true;
1928}
1929
1930bool Parser::parseRotorsLimits(yarp::os::Searchable &config, std::vector<rotorLimits_t> &rotorsLimits)
1931{
1932 Bottle &limits=config.findGroup("LIMITS");
1933 if (limits.isNull())
1934 {
1935 yError() << "embObjMC BOARD " << _boardname << " detected that Group LIMITS is not found in configuration file";
1936 return false;
1937 }
1938
1939 if(!checkAndSetVectorSize(rotorsLimits, _njoints, "parseRotorsLimits"))
1940 return false;
1941
1942 Bottle xtmp;
1943 unsigned int i;
1944
1945 // Rotor max limit
1946 if (!extractGroup(limits, xtmp, "rotorPosMax","a list of maximum rotor angles (in degrees)", _njoints))
1947 return false;
1948 else
1949 for(i=1; i<xtmp.size(); i++) rotorsLimits[i-1].posMax = xtmp.get(i).asFloat64();
1950
1951
1952
1953 // Rotor min limit
1954 if (!extractGroup(limits, xtmp, "rotorPosMin","a list of minimum roto angles (in degrees)", _njoints))
1955 return false;
1956 else
1957 for(i=1; i<xtmp.size(); i++) rotorsLimits[i-1].posMin = xtmp.get(i).asFloat64();
1958
1959 // Motor pwm limit
1960 if (!extractGroup(limits, xtmp, "motorPwmLimit","a list of motor PWM limits", _njoints))
1961 return false;
1962 else
1963 for(i=1; i<xtmp.size(); i++)
1964 {
1965 rotorsLimits[i-1].pwmMax = xtmp.get(i).asFloat64();
1966 if(rotorsLimits[i-1].pwmMax<0)
1967 {
1968 yError() << "motorPwmLimit should be a positive value";
1969 return false;
1970 }
1971 }
1972
1973 return true;
1974
1975}
1976
1977bool Parser::parseCouplingInfo(yarp::os::Searchable &config, couplingInfo_t &couplingInfo)
1978{
1979 Bottle coupling_bottle = config.findGroup("COUPLINGS");
1980 if (coupling_bottle.isNull())
1981 {
1982 yError() << "embObjMC BOARD " << _boardname << "Missing Coupling group";
1983 return false;
1984 }
1985 Bottle xtmp;
1986 int fixedMatrix4X4Size = 16;
1987 int fixedMatrix4X6Size = 24;
1988 bool formaterror =false;
1989
1990 // matrix J2M
1991 if (!extractGroup(coupling_bottle, xtmp, "matrixJ2M", "matrixJ2M ", fixedMatrix4X4Size))
1992 {
1993 return false;
1994 }
1995
1996 if(false == convert(xtmp, couplingInfo.matrixJ2M, formaterror, fixedMatrix4X4Size))
1997 {
1998 yError() << "embObjMC BOARD " << _boardname << " has detected an illegal format for some of the values of CONTROLLER.matrixJ2M";
1999 return false;
2000 }
2001
2002
2003 // matrix E2J
2004 if (!extractGroup(coupling_bottle, xtmp, "matrixE2J", "matrixE2J ", fixedMatrix4X6Size))
2005 {
2006 return false;
2007 }
2008
2009 formaterror = false;
2010 if(false == convert(xtmp, couplingInfo.matrixE2J, formaterror, fixedMatrix4X6Size))
2011 {
2012 yError() << "embObjMC BOARD " << _boardname << " has detected an illegal format for some of the values of CONTROLLER.matrixE2J";
2013 return false;
2014 }
2015
2016
2017 // matrix M2J
2018 if (!extractGroup(coupling_bottle, xtmp, "matrixM2J", "matrixM2J ", fixedMatrix4X4Size))
2019 {
2020 return false;
2021 }
2022
2023 formaterror = false;
2024 if( false == convert(xtmp, couplingInfo.matrixM2J, formaterror, fixedMatrix4X4Size))
2025 {
2026 yError() << "embObjMC BOARD " << _boardname << " has detected an illegal format for some of the values of CONTROLLER.matrixM2J";
2027 return false;
2028 }
2029
2030 return true;
2031}
2032
2033bool Parser::parseMotioncontrolVersion(yarp::os::Searchable &config, int &version)
2034{
2035 if (!config.findGroup("GENERAL").find("MotioncontrolVersion").isInt32())
2036 {
2037 yError() << "Missing MotioncontrolVersion parameter. RobotInterface cannot start. Please contact icub-support@iit.it";
2038 return false;
2039 }
2040
2041 version = config.findGroup("GENERAL").find("MotioncontrolVersion").asInt32();
2042 return true;
2043
2044}
2045
2046bool Parser::isVerboseEnabled(yarp::os::Searchable &config)
2047{
2048 bool ret = false;
2049 if(!config.findGroup("GENERAL").find("verbose").isBool())
2050 {
2051 yError() << "embObjMotionControl::open() detects that general->verbose bool param is different from accepted values (true / false). Assuming false";
2052 ret = false;
2053 }
2054 else
2055 {
2056 ret = config.findGroup("GENERAL").find("verbose").asBool();
2057 }
2058 _verbosewhenok = ret;
2059 return ret;
2060}
2061
2062bool Parser::parseBehaviourFalgs(yarp::os::Searchable &config, bool &useRawEncoderData, bool &pwmIsLimited)
2063{
2064
2065 // Check useRawEncoderData = do not use calibration data!
2066 Value use_raw = config.findGroup("GENERAL").find("useRawEncoderData");
2067
2068 if(use_raw.isNull())
2069 {
2070 useRawEncoderData = false;
2071 }
2072 else
2073 {
2074 if(!use_raw.isBool())
2075 {
2076 yWarning() << "embObjMotionControl::open() detected that useRawEncoderData bool param is different from accepted values (true / false). Assuming false";
2077 useRawEncoderData = false;
2078 }
2079 else
2080 {
2081 useRawEncoderData = use_raw.asBool();
2082 if(useRawEncoderData)
2083 {
2084 yWarning() << "embObjMotionControl::open() detected that it is using raw data from encoders! Be careful See 'useRawEncoderData' param in config file";
2085 yWarning() << "DO NOT USE OR CALIBRATE THE ROBOT IN THIS CONFIGURATION!";
2086 yWarning() << "CHECK IF THE FAULT BUTTON IS PRESSED and press ENTER to continue";
2087 getchar();
2088 }
2089 }
2090 }
2091
2092 // Check useRawEncoderData = do not use calibration data!
2093 Value use_limitedPWM = config.findGroup("GENERAL").find("useLimitedPWM");
2094 if(use_limitedPWM.isNull())
2095 {
2096 pwmIsLimited = false;
2097 }
2098 else
2099 {
2100 if(!use_limitedPWM.isBool())
2101 {
2102 pwmIsLimited = false;
2103 }
2104 else
2105 {
2106 pwmIsLimited = use_limitedPWM.asBool();
2107 }
2108 }
2109
2110 return true;
2111}
2112
2113bool Parser::parseAxisInfo(yarp::os::Searchable &config, int axisMap[], std::vector<axisInfo_t> &axisInfo)
2114{
2115
2116 Bottle xtmp;
2117 unsigned int i;
2118 axisInfo.resize(_njoints);
2119
2120 Bottle general = config.findGroup("GENERAL");
2121 if (general.isNull())
2122 {
2123 yError() << "embObjMC BOARD " << _boardname << "Missing General group" ;
2124 return false;
2125 }
2126
2127
2128 if (!extractGroup(general, xtmp, "AxisMap", "a list of reordered indices for the axes", _njoints))
2129 return false;
2130
2131 for (i = 1; i < xtmp.size(); i++)
2132 {
2133 int user_joint = xtmp.get(i).asInt32();
2134 if(user_joint>= _njoints)
2135 {
2136 yError() << "embObjMC BOARD " << _boardname << "In AxisMap param: joint " << i-1 << "has been mapped to not-existing joint ("<< user_joint <<"). Here there are only "<< _njoints <<"joints";
2137 return false;
2138 }
2139 axisMap[i-1] = user_joint;
2140 }
2141
2142
2143 if (!extractGroup(general, xtmp, "AxisName", "a list of strings representing the axes names", _njoints))
2144 return false;
2145
2146 //beware: axis name has to be remapped here because they are not set using the toHw() helper function
2147 for (i = 1; i < xtmp.size(); i++)
2148 {
2149 int mappedto = axisInfo[i-1].mappedto;
2150 axisInfo[axisMap[i - 1]].name = xtmp.get(i).asString();
2151 }
2152
2153 if (!extractGroup(general, xtmp, "AxisType", "a list of strings representing the axes type (revolute/prismatic)", _njoints))
2154 return false;
2155
2156 //beware: axis type has to be remapped here because they are not set using the toHw() helper function
2157 for (i = 1; i < xtmp.size(); i++)
2158 {
2159 string s = xtmp.get(i).asString();
2160 int mappedto = axisInfo[i-1].mappedto;
2161 if (s == "revolute") axisInfo[axisMap[i - 1]].type = VOCAB_JOINTTYPE_REVOLUTE;
2162 else if (s == "prismatic") axisInfo[axisMap[i - 1]].type = VOCAB_JOINTTYPE_PRISMATIC;
2163 else
2164 {
2165 yError("Unknown AxisType value %s!", s.c_str());
2166 axisInfo[axisMap[i - 1]].type = VOCAB_JOINTTYPE_UNKNOWN;
2167 return false;
2168 }
2169 }
2170
2171 return true;
2172}
2173
2174bool Parser::parseEncoderFactor(yarp::os::Searchable &config, double encoderFactor[])
2175{
2176 Bottle general = config.findGroup("GENERAL");
2177 if (general.isNull())
2178 {
2179 yError() << "embObjMC BOARD " << _boardname << "Missing General group" ;
2180 return false;
2181 }
2182 Bottle xtmp;
2183 unsigned int i;
2184 double tmp_A2E;
2185
2186 // Encoder scales
2187 if (!extractGroup(general, xtmp, "Encoder", "a list of scales for the encoders", _njoints))
2188 {
2189 return false;
2190 }
2191
2192 for (i = 1; i < xtmp.size(); i++)
2193 {
2194 tmp_A2E = xtmp.get(i).asFloat64();
2195 if (tmp_A2E<0)
2196 {
2197 yWarning() << "embObjMC BOARD " << _boardname << "Encoder parameter should be positive!";
2198 }
2199 encoderFactor[i - 1] = tmp_A2E;
2200 }
2201
2202 return true;
2203}
2204
2205bool Parser::parsefullscalePWM(yarp::os::Searchable &config, double dutycycleToPWM[])
2206{
2207 Bottle general = config.findGroup("GENERAL");
2208 if (general.isNull())
2209 {
2210 yError() << "embObjMC BOARD " << _boardname << "Missing General group";
2211 return false;
2212 }
2213 Bottle xtmp;
2214 unsigned int i;
2215 double tmpval;
2216
2217 // fullscalePWM
2218 if (!extractGroup(general, xtmp, "fullscalePWM", "a list of scales for the fullscalePWM conversion factor", _njoints))
2219 {
2220 yError("fullscalePWM param not found in config file. Please update robot configuration files or contact https://github.com/robotology/icub-support");
2221 return false;
2222 }
2223
2224 for (i = 1; i < xtmp.size(); i++)
2225 {
2226 tmpval = xtmp.get(i).asFloat64();
2227 if (tmpval<0)
2228 {
2229 yError() << "embObjMC BOARD " << _boardname << "fullscalePWM parameter should be positive!";
2230 return false;
2231 }
2232 dutycycleToPWM[i - 1] = tmpval / 100.0;
2233 }
2234
2235 return true;
2236}
2237
2238bool Parser::parseAmpsToSensor(yarp::os::Searchable &config, double ampsToSensor[])
2239{
2240 Bottle general = config.findGroup("GENERAL");
2241 if (general.isNull())
2242 {
2243 yError() << "embObjMC BOARD " << _boardname << "Missing General group";
2244 return false;
2245 }
2246 Bottle xtmp;
2247 unsigned int i;
2248 double tmpval;
2249
2250 // ampsToSensor
2251 if (!extractGroup(general, xtmp, "ampsToSensor", "a list of scales for the ampsToSensor conversion factor", _njoints))
2252 {
2253 yError("ampsToSensor param not found in config file. Please update robot configuration files or contact https://github.com/robotology/icub-support");
2254 return false;
2255 }
2256
2257 for (i = 1; i < xtmp.size(); i++)
2258 {
2259 tmpval = xtmp.get(i).asFloat64();
2260 if (tmpval<0)
2261 {
2262 yError() << "embObjMC BOARD " << _boardname << "ampsToSensor parameter should be positive!";
2263 return false;
2264 }
2265 ampsToSensor[i - 1] = tmpval;
2266 }
2267
2268 return true;
2269}
2270
2271bool Parser::parseGearboxValues(yarp::os::Searchable &config, double gearbox_M2J[], double gearbox_E2J[])
2272{
2273 Bottle general = config.findGroup("GENERAL");
2274 if (general.isNull())
2275 {
2276 yError() << "embObjMC BOARD " << _boardname << "Missing General group" ;
2277 return false;
2278 }
2279
2280 Bottle xtmp;
2281 unsigned int i;
2282
2283 // Gearbox_M2J
2284 if (!extractGroup(general, xtmp, "Gearbox_M2J", "The gearbox reduction ratio", _njoints))
2285 {
2286 return false;
2287 }
2288
2289 for (i = 1; i < xtmp.size(); i++)
2290 {
2291 gearbox_M2J[i-1] = xtmp.get(i).asFloat64();
2292 if (gearbox_M2J[i-1]==0)
2293 {
2294 yError() << "embObjMC BOARD " << _boardname << "Using a gearbox value = 0 may cause problems! Check your configuration files";
2295 return false;
2296 }
2297 }
2298
2299
2300 //Gearbox_E2J
2301 if (!extractGroup(general, xtmp, "Gearbox_E2J", "The gearbox reduction ratio between encoder and joint", _njoints))
2302 {
2303 return false;
2304 }
2305
2306 int test = xtmp.size();
2307 for (i = 1; i < xtmp.size(); i++)
2308 {
2309 gearbox_E2J[i-1] = xtmp.get(i).asFloat64();
2310 if (gearbox_E2J[i-1]==0)
2311 {
2312 yError() << "embObjMC BOARD " << _boardname << "Using a gearbox value = 0 may cause problems! Check your configuration files";
2313 return false;
2314 }
2315 }
2316
2317
2318 return true;
2319}
2320
2321bool Parser::parseDeadzoneValue(yarp::os::Searchable &config, double deadzone[], bool *found)
2322{
2323// Bottle general = config.findGroup("GENERAL");
2324// if (general.isNull())
2325// {
2326// yError() << "embObjMC BOARD " << _boardname << "Missing General group" ;
2327// return false;
2328// }
2329
2330 Bottle general = config.findGroup("OTHER_CONTROL_PARAMETERS");
2331 if (general.isNull())
2332 {
2333 yWarning() << "embObjMC BOARD " << _boardname << "Missing OTHER_CONTROL_PARAMETERS.DeadZone parameter. I'll use default value. (see documentation for more datails)";
2334 *found = false;
2335 return true;
2336 }
2337 Bottle xtmp;
2338 unsigned int i;
2339
2340 // DeadZone
2341 if (!extractGroup(general, xtmp, "deadZone", "The deadzone of joint", _njoints, false))
2342 {
2343 yWarning() << "embObjMC BOARD " << _boardname << "Missing OTHER_CONTROL_PARAMETERS group.DeadZone parameter. I'll use default value. (see documentation for more datails)";
2344 *found = false;
2345 return true;
2346 }
2347
2348 *found = true;
2349 for (i = 1; i < xtmp.size(); i++)
2350 {
2351 deadzone[i-1] = xtmp.get(i).asFloat64();
2352 }
2353
2354 return true;
2355}
2356
2357bool Parser::parseKalmanFilterParams(yarp::os::Searchable &config, std::vector<kalmanFilterParams_t> &kalmanFilterParams)
2358{
2359 Bottle general = config.findGroup("KALMAN_FILTER");
2360 if (general.isNull())
2361 {
2362 yWarning() << "embObjMC BOARD " << _boardname << "Missing KALMAN_FILTER group. Kalman Filter will be disabled by default.";
2363
2364 // if you don't specify the Kalman Filter group disable the kalmam filter for all the joints.
2365 for(int j=0; j<_njoints; j++)
2366 {
2367 kalmanFilterParams[j].enabled = false;
2368 kalmanFilterParams[j].x0.fill(0.0);
2369 kalmanFilterParams[j].Q.fill(0.0);
2370 kalmanFilterParams[j].R = 0.0;
2371 kalmanFilterParams[j].P0 = 0.0;
2372 }
2373 return true;
2374 }
2375 else
2376 {
2377 Bottle xtmp;
2378
2379 // kalmanFilterEnabled
2380 if (!extractGroup(general, xtmp, "kalmanFilterEnabled", "kalman filter of joint: ", _njoints, true))
2381 {
2382 yWarning() << "embObjMC BOARD " << _boardname << "Missing kalmanFilterEnabled parameter. It will be disabled by default.";
2383 return false;
2384 }
2385 for (int j = 0; j<_njoints; j++) kalmanFilterParams[j].enabled = xtmp.get(j + 1).asBool();
2386
2387 // x0_0
2388 if (!extractGroup(general, xtmp, "x0", "Initial state x0 of kalman filter of joint: ", _njoints, true))
2389 {
2390 yWarning() << "embObjMC BOARD " << _boardname << "Missing initial state x0 parameter. Kalman Filter will be disabled by default.";
2391 return false;
2392 }
2393 for (int j = 0; j<_njoints; j++) kalmanFilterParams[j].x0.at(0) = xtmp.get(j + 1).asFloat32();
2394
2395 // x0_1
2396 if (!extractGroup(general, xtmp, "x1", "Initial state x1 of kalman filter of joint: ", _njoints, true))
2397 {
2398 yWarning() << "embObjMC BOARD " << _boardname << "Missing initial state x1 parameter. Kalman Filter will be disabled by default.";
2399 return false;
2400 }
2401 for (int j = 0; j<_njoints; j++) kalmanFilterParams[j].x0.at(1) = xtmp.get(j + 1).asFloat32();
2402
2403 // x0_2
2404 if (!extractGroup(general, xtmp, "x2", "Initial state x2 of kalman filter of joint: ", _njoints, true))
2405 {
2406 yWarning() << "embObjMC BOARD " << _boardname << "Missing initial state x2 parameter. Kalman Filter will be disabled by default.";
2407 return false;
2408 }
2409 for (int j = 0; j<_njoints; j++) kalmanFilterParams[j].x0.at(2) = xtmp.get(j + 1).asFloat32();
2410
2411 // Q0
2412 if (!extractGroup(general, xtmp, "Q0", "Process Q0 noise covariance matrix of kalman filter of joint: ", _njoints, true))
2413 {
2414 yWarning() << "embObjMC BOARD " << _boardname << "Missing Q0 parameter. Kalman Filter will be disabled by default.";
2415 return false;
2416 }
2417 for (int j = 0; j<_njoints; j++) kalmanFilterParams[j].Q.at(0) = xtmp.get(j + 1).asFloat32();
2418
2419 // Q1
2420 if (!extractGroup(general, xtmp, "Q1", "Process Q1 noise covariance matrix of kalman filter of joint: ", _njoints, true))
2421 {
2422 yWarning() << "embObjMC BOARD " << _boardname << "Missing Q1 parameter. Kalman Filter will be disabled by default.";
2423 return false;
2424 }
2425 for (int j = 0; j<_njoints; j++) kalmanFilterParams[j].Q.at(1) = xtmp.get(j + 1).asFloat32();
2426
2427 // Q2
2428 if (!extractGroup(general, xtmp, "Q2", "Process Q2 noise covariance matrix of kalman filter of joint: ", _njoints, true))
2429 {
2430 yWarning() << "embObjMC BOARD " << _boardname << "Missing Q2 parameter. Kalman Filter will be disabled by default.";
2431 return false;
2432 }
2433 for (int j = 0; j<_njoints; j++) kalmanFilterParams[j].Q.at(2) = xtmp.get(j + 1).asFloat32();
2434
2435 // R
2436 if (!extractGroup(general, xtmp, "R", "Measurement noise covariance matrix of kalman filter for joint: ", _njoints, true))
2437 {
2438 yWarning() << "embObjMC BOARD " << _boardname << "Missing R scalar parameter. Kalman Filter will be disabled by default.";
2439 return false;
2440 }
2441 for (int j = 0; j<_njoints; j++) kalmanFilterParams[j].R = xtmp.get(j + 1).asFloat32();
2442
2443 // P0
2444 if (!extractGroup(general, xtmp, "P0", "Initial state estimation error covariance matrix of kalman filter of joint: ", _njoints, true))
2445 {
2446 yWarning() << "embObjMC BOARD " << _boardname << "Missing P0 scalar parameter. Kalman Filter will be disabled by default.";
2447 return false;
2448 }
2449 for (int j = 0; j<_njoints; j++) kalmanFilterParams[j].P0 = xtmp.get(j + 1).asFloat32();
2450 }
2451
2452 return true;
2453}
2454
2455
2456bool Parser::parseMechanicalsFlags(yarp::os::Searchable &config, int useMotorSpeedFbk[])
2457{
2458 Bottle general = config.findGroup("GENERAL");
2459 if (general.isNull())
2460 {
2461 yError() << "embObjMC BOARD " << _boardname << "Missing General group" ;
2462 return false;
2463 }
2464 Bottle xtmp;
2465 unsigned int i;
2466
2467 if(!extractGroup(general, xtmp, "useMotorSpeedFbk", "Use motor speed feedback", _njoints))
2468 {
2469 return false;
2470 }
2471
2472 for (i = 1; i < xtmp.size(); i++)
2473 {
2474 useMotorSpeedFbk[i-1] = xtmp.get(i).asInt32();
2475 }
2476
2477 return true;
2478
2479}
2480
2481bool Parser::parseImpedanceGroup(yarp::os::Searchable &config,std::vector<impedanceParameters_t> &impedance)
2482{
2483 Bottle impedanceGroup;
2484 impedanceGroup=config.findGroup("IMPEDANCE","IMPEDANCE parameters");
2485
2486 if(impedanceGroup.isNull())
2487 {
2488 yError() <<"embObjMC BOARD " << _boardname << "fromConfig(): Error: no IMPEDANCE group found in config file, returning";
2489 return false;
2490 }
2491
2492
2493
2494 if(_verbosewhenok)
2495 {
2496 yDebug() << "embObjMC BOARD " << _boardname << ":fromConfig() detected that IMPEDANCE parameters section is found";
2497 }
2498
2499 if(!checkAndSetVectorSize(impedance, _njoints, "parseImpedanceGroup"))
2500 return false;
2501
2502
2503 int j=0;
2504 Bottle xtmp;
2505 if (!extractGroup(impedanceGroup, xtmp, "stiffness", "stiffness parameter", _njoints))
2506 return false;
2507
2508 for (j=0; j<_njoints; j++)
2509 impedance[j].stiffness = xtmp.get(j+1).asFloat64();
2510
2511 if (!extractGroup(impedanceGroup, xtmp, "damping", "damping parameter", _njoints))
2512 return false;
2513
2514 for (j=0; j<_njoints; j++)
2515 impedance[j].damping = xtmp.get(j+1).asFloat64();
2516
2517 if(_verbosewhenok)
2518 {
2519 yInfo() << "embObjMC BOARD " << _boardname << "IMPEDANCE section: parameters successfully loaded";
2520 }
2521 return true;
2522
2523}
2524
2525/*
2526typedef struct
2527{
2528 double Km;
2529 double Kw;
2530 double S0;
2531 double S1;
2532 double Vth;
2533 double Fc_pos;
2534 double Fc_neg;
2535 double Fs_pos;
2536 double Fs_neg;
2537 double tbd0;
2538 double tbd1;
2539 double tbd2;
2540} lugreParameters_t;
2541*/
2542
2543bool Parser::parseLugreGroup(yarp::os::Searchable &config,std::vector<lugreParameters_t> &lugre)
2544{
2545 Bottle lugreGroup;
2546 lugreGroup=config.findGroup("LUGRE","LUGRE parameters");
2547
2548 if(lugreGroup.isNull())
2549 {
2550 for (int j = 0; j<_njoints; ++j)
2551 {
2552 lugre[j].Km = -1.0;
2553 }
2554
2555 yWarning() <<"embObjMC BOARD " << _boardname << "fromConfig(): Warning: no LUGRE group found in config file, returning";
2556 return true;
2557 }
2558
2559
2560
2561 if(_verbosewhenok)
2562 {
2563 yDebug() << "embObjMC BOARD " << _boardname << ":fromConfig() detected that LUGRE parameters section is found";
2564 }
2565
2566 if(!checkAndSetVectorSize(lugre, _njoints, "parseLugreGroup"))
2567 return false;
2568
2569
2570 int j=0;
2571 Bottle xtmp;
2572 if (!extractGroup(lugreGroup, xtmp, "Km", "torque constant parameter", _njoints))
2573 return false;
2574
2575 for (j=0; j<_njoints; j++)
2576 lugre[j].Km = xtmp.get(j+1).asFloat64();
2577
2578 if (!extractGroup(lugreGroup, xtmp, "Kw", "viscous friction parameter", _njoints))
2579 return false;
2580
2581 for (j=0; j<_njoints; j++)
2582 lugre[j].Kw = xtmp.get(j+1).asFloat64();
2583
2584 if (!extractGroup(lugreGroup, xtmp, "S0", "hysteresis position parameter", _njoints))
2585 return false;
2586
2587 for (j=0; j<_njoints; j++)
2588 lugre[j].S0 = xtmp.get(j+1).asFloat64();
2589
2590 if (!extractGroup(lugreGroup, xtmp, "S1", "hysteresis velocity parameter", _njoints))
2591 return false;
2592
2593 for (j=0; j<_njoints; j++)
2594 lugre[j].S1 = xtmp.get(j+1).asFloat64();
2595
2596 if (!extractGroup(lugreGroup, xtmp, "Vth", "velocity threshold parameter", _njoints))
2597 return false;
2598
2599 for (j=0; j<_njoints; j++)
2600 lugre[j].Vth = xtmp.get(j+1).asFloat64();
2601
2602 if (!extractGroup(lugreGroup, xtmp, "Fc_pos", "Coulomb force parameter (forward)", _njoints))
2603 return false;
2604
2605 for (j=0; j<_njoints; j++)
2606 lugre[j].Fc_pos = xtmp.get(j+1).asFloat64();
2607
2608 if (!extractGroup(lugreGroup, xtmp, "Fc_neg", "Coulomb force parameter (backward)", _njoints))
2609 return false;
2610
2611 for (j=0; j<_njoints; j++)
2612 lugre[j].Fc_neg = xtmp.get(j+1).asFloat64();
2613
2614 if (!extractGroup(lugreGroup, xtmp, "Fs_pos", "Stribeck force parameter (forward)", _njoints))
2615 return false;
2616
2617 for (j=0; j<_njoints; j++)
2618 lugre[j].Fs_pos = xtmp.get(j+1).asFloat64();
2619
2620 if (!extractGroup(lugreGroup, xtmp, "Fs_neg", "Stribeck force parameter (backward)", _njoints))
2621 return false;
2622
2623 for (j=0; j<_njoints; j++)
2624 lugre[j].Fs_neg = xtmp.get(j+1).asFloat64();
2625
2626 if(_verbosewhenok)
2627 {
2628 yInfo() << "embObjMC BOARD " << _boardname << "LUGRE section: parameters successfully loaded";
2629 }
2630 return true;
2631
2632}
2633
2634
2635bool Parser::parseMaintenanceModeGroup(yarp::os::Searchable &config, bool &skipRecalibrationEnabled)
2636{
2637 // Extract group MaintenanceModeGroup
2638 Bottle &maintenanceGroup=config.findGroup("MAINTENANCE");
2639 if (maintenanceGroup.isNull())
2640 {
2641 skipRecalibrationEnabled = false;
2642 return true;
2643 }
2644
2645 // Check skipRecalibrationEnabled = do not recalibrate joint at yarprobointerface restart! Used by low level motion controller
2646 Value skip_recalibration = maintenanceGroup.find("skipRecalibration");
2647 if (skip_recalibration.isNull())
2648 {
2649 skipRecalibrationEnabled = false;
2650 }
2651 else
2652 {
2653 if (!skip_recalibration.isBool())
2654 {
2655 yError() << "embObjMotionControl::open() detected that skipRecalibration bool param is different from accepted values (true / false). Assuming false";
2656 skipRecalibrationEnabled = false;
2657 }
2658 else
2659 {
2660 skipRecalibrationEnabled = skip_recalibration.asBool();
2661 if(skipRecalibrationEnabled)
2662 {
2663 yWarning() << "embObjMotionControl::open() detected that skipRecalibration is requested! Be careful See 'skipRecalibration' param in config file";
2664 yWarning() << "THE ROBOT WILL SKIP THE RECALIBRATION IN THIS CONFIGURATION!";
2665 }
2666 }
2667 }
2668 return true;
2669}
2670
2671bool Parser::convert(std::string const &fromstring, eOmc_jsetconstraint_t &jsetconstraint, bool& formaterror)
2672{
2673 const char *t = fromstring.c_str();
2674
2675 eObool_t usecompactstring = eobool_false;
2676 jsetconstraint = eomc_string2jsetconstraint(t, usecompactstring);
2677
2678 if(eomc_jsetconstraint_unknown == jsetconstraint)
2679 {
2680 usecompactstring = eobool_true;
2681 jsetconstraint = eomc_string2jsetconstraint(t, usecompactstring);
2682 }
2683
2684 if(eomc_jsetconstraint_unknown == jsetconstraint)
2685 {
2686 yError() << "embObjMC BOARD " << _boardname << "String" << t << "cannot be converted into a proper eOmc_jsetconstraint_t";
2687 formaterror = true;
2688 return false;
2689 }
2690
2691 return true;
2692}
2693
2694bool Parser::convert(Bottle &bottle, vector<double> &matrix, bool &formaterror, int targetsize)
2695{
2696 matrix.resize(0);
2697
2698 int tmp = bottle.size();
2699 int sizeofmatrix = tmp - 1; // first position of bottle contains the tag "matrix"
2700
2701 // check if there are really the target number of elements in matrix.
2702 if(targetsize != sizeofmatrix)
2703 {
2704 yError() << "embObjMC BOARD " << _boardname << " in converting string do matrix.In the matrix there are not" << targetsize << "elements";
2705 return false;
2706 }
2707
2708 formaterror = false;
2709 for(int i=0; i<sizeofmatrix; i++)
2710 {
2711 double item = 0;
2712
2713 // ok, i use the standard converter ... but what if it is not a double format? so far we dont check.
2714 item = bottle.get(i+1).asFloat64();
2715 matrix.push_back(item);
2716 }
2717
2718 // in here we could decide to return false if any previous conversion function has returned error
2719
2720 return true;
2721}
2722
2726void Parser::debugUtil_printControlLaws(void)
2727{
2729 yError() << "position control law: ";
2730 for(int x=0; x<_njoints; x++)
2731 {
2732 yError() << " - j " << x << _positionControlLaw[x].c_str();
2733 }
2734
2735 yError() << "velocity control law: ";
2736 for(int x=0; x<_njoints; x++)
2737 {
2738 yError() << "- j " << x << _velocityControlLaw[x].c_str();
2739 }
2740
2741
2742 yError() << "torque control law: ";
2743 for(int x=0; x<_njoints; x++)
2744 {
2745 yError() << " - j " << x << _torqueControlLaw[x].c_str();
2746 }
2748
2749}
2750
2751/*
2752void PidInfo::dumpdata(void)
2753{
2754
2755 cout << "Is enabled " << enabled;
2756 cout << ". Username pid selected is " << usernamePidSelected;
2757 switch(controlLaw)
2758 {
2759 case PidAlgo_simple:
2760 cout << ". Control law is " << "PidAlgo_simple";
2761 break;
2762
2763 case PIdAlgo_velocityInnerLoop:
2764 cout << ". Control law is " << "PIdAlgo_velocityInnerLoop";
2765 break;
2766
2767 case PidAlgo_currentInnerLoop:
2768 cout << ". Control law is " << "PidAlgo_currentInnerLoop";
2769 break;
2770 default :
2771 cout << ". Control law is " << "unknown";
2772 }
2773
2774 cout << ". PID fbk Unit type is " << (int)fbk_PidUnits;
2775 cout << ". PID out Unit type is " << (int)out_PidUnits;
2776
2777 cout << " kp is " << pid.kp;
2778 cout << endl;
2779
2780}
2781*/
2782void JointsSet::dumpdata(void)
2783{
2784 switch(cfg.constraints.type)
2785 {
2786 case eomc_jsetconstraint_none:
2787 cout << "constraint is " << "eomc_jsetconstraint_none";
2788 break;
2789 case eomc_jsetconstraint_cerhand:
2790 cout << "constraint is " << "eomc_jsetconstraint_cerhand";
2791 break;
2792 case eomc_jsetconstraint_trifid:
2793 cout << "constraint is " << "eomc_jsetconstraint_trifid";
2794 break;
2795 default :
2796 cout << ". constraint is " << "unknown";
2797 }
2798
2799 cout << " param1="<< cfg.constraints.param1 << " param2=" << cfg.constraints.param2 << endl;
2800
2801}
2802
2803bool Parser::checkJointTypes(PidInfo *pids, const std::string &pid_type)
2804{
2805 //Here we check that all joints have same type units in order to create pid_type helper with correct factor.
2806
2807 int firstjoint = -1;
2808
2809 // verify if pid type is torque or some other
2810 if(pid_type == "TORQUE")
2811 {
2812 // since we are working with a torque pid, we first cast it as such
2813 // this allows the loop to correctly point to the corresponding memory
2814 TrqPidInfo* trq_pids = (TrqPidInfo*) pids;
2815
2816 for(int i=0; i<_njoints; i++)
2817 {
2818 // if we already had an enabled PID, compare with current one
2819 if(firstjoint != -1 && !checkSinglePid(trq_pids[firstjoint], trq_pids[i], firstjoint, i, pid_type))
2820 {
2821 return false;
2822 }
2823 // if we haven't found an enabled PID yet, and this one is enabled, save it
2824 if(firstjoint == -1 && trq_pids[i].enabled)
2825 {
2826 firstjoint = i;
2827 }
2828 }
2829 }
2830 else
2831 {
2832 for(int i=0; i<_njoints; i++)
2833 {
2834 // if we already had an enabled PID, compare with current one
2835 if(firstjoint != -1 && !checkSinglePid(pids[firstjoint], pids[i], firstjoint, i, pid_type))
2836 {
2837 return false;
2838 }
2839 // if we haven't found an enabled PID yet, and this one is enabled, save it
2840 if(firstjoint == -1 && pids[i].enabled)
2841 {
2842 firstjoint = i;
2843 }
2844 }
2845 }
2846
2847 return true;
2848}
2849
2850bool Parser::checkSinglePid(PidInfo &firstPid, PidInfo &currentPid, const int &firstjoint, const int &currentjoint, const std::string &pid_type)
2851{
2852 // check if the PID we are checking is enabled
2853 if(currentPid.enabled)
2854 {
2855 // if it has different unit types from the previous enabled PIDs
2856 if(firstPid.fbk_PidUnits != currentPid.fbk_PidUnits ||
2857 firstPid.out_PidUnits != currentPid.out_PidUnits)
2858 {
2859 yError() << "embObjMC BOARD " << _boardname << "all joints with " << pid_type << " enabled should have same controlunits type. Joint " << firstjoint << " differs from joint " << currentjoint;
2860 return false;
2861 }
2862 }
2863 return true;
2864}
2865
2866
2867
2868
2869
yarp::dev::eomc::JointsSet::cfg
eOmc_jointset_configuration_t cfg
Definition eomcParser.h:428
yarp::dev::eomc::Parser::parseJointsetCfgGroup
bool parseJointsetCfgGroup(yarp::os::Searchable &config, std::vector< JointsSet > &jsets, std::vector< int > &jointtoset)
Definition eomcParser.cpp:1571
yarp::dev::eomc::Parser::parseJointsLimits
bool parseJointsLimits(yarp::os::Searchable &config, std::vector< jointLimits_t > &jointsLimits)
Definition eomcParser.cpp:1857
yarp::dev::eomc::Parser::parseTimeoutsGroup
bool parseTimeoutsGroup(yarp::os::Searchable &config, std::vector< timeouts_t > &timeouts, int defaultTimeout)
Definition eomcParser.cpp:1687
yarp::dev::eomc::Parser::parseMaintenanceModeGroup
bool parseMaintenanceModeGroup(yarp::os::Searchable &config, bool &skipRecalibrationEnabled)
Definition eomcParser.cpp:2635
yarp::dev::eomc::Parser::parseAmpsToSensor
bool parseAmpsToSensor(yarp::os::Searchable &config, double ampsToSensor[])
Definition eomcParser.cpp:2238
yarp::dev::eomc::Parser::Parser
Parser(int numofjoints, std::string boardname)
Definition eomcParser.cpp:38
yarp::dev::eomc::Parser::parseMechanicalsFlags
bool parseMechanicalsFlags(yarp::os::Searchable &config, int useMotorSpeedFbk[])
Definition eomcParser.cpp:2456
yarp::dev::eomc::Parser::parseMotioncontrolVersion
bool parseMotioncontrolVersion(yarp::os::Searchable &config, int &version)
Definition eomcParser.cpp:2033
yarp::dev::eomc::Parser::parseLugreGroup
bool parseLugreGroup(yarp::os::Searchable &config, std::vector< lugreParameters_t > &lugre)
Definition eomcParser.cpp:2543
yarp::dev::eomc::Parser::parseAxisInfo
bool parseAxisInfo(yarp::os::Searchable &config, int axisMap[], std::vector< axisInfo_t > &axisInfo)
Definition eomcParser.cpp:2113
yarp::dev::eomc::Parser::parseFocGroup
bool parseFocGroup(yarp::os::Searchable &config, focBasedSpecificInfo_t *foc_based_info, std::string groupName, std::vector< std::unique_ptr< eomc::ITemperatureSensor > > &temperatureSensorsVector)
Definition eomcParser.cpp:1375
yarp::dev::eomc::Parser::parseCouplingInfo
bool parseCouplingInfo(yarp::os::Searchable &config, couplingInfo_t &couplingInfo)
Definition eomcParser.cpp:1977
yarp::dev::eomc::Parser::parsePids
bool parsePids(yarp::os::Searchable &config, PidInfo *ppids, TrqPidInfo *tpids, PidInfo *cpids, PidInfo *spids, bool lowLevPidisMandatory)
Definition eomcParser.cpp:79
yarp::dev::eomc::Parser::parseTemperatureLimits
bool parseTemperatureLimits(yarp::os::Searchable &config, std::vector< temperatureLimits_t > &temperatureLimits)
Definition eomcParser.cpp:1806
yarp::dev::eomc::Parser::parseDeadzoneValue
bool parseDeadzoneValue(yarp::os::Searchable &config, double deadzone[], bool *found)
Definition eomcParser.cpp:2321
yarp::dev::eomc::Parser::isVerboseEnabled
bool isVerboseEnabled(yarp::os::Searchable &config)
Definition eomcParser.cpp:2046
yarp::dev::eomc::Parser::parseCurrentLimits
bool parseCurrentLimits(yarp::os::Searchable &config, std::vector< motorCurrentLimits_t > &currLimits)
Definition eomcParser.cpp:1771
yarp::dev::eomc::Parser::parseEncoderFactor
bool parseEncoderFactor(yarp::os::Searchable &config, double encoderFactor[])
Definition eomcParser.cpp:2174
yarp::dev::eomc::Parser::parsefullscalePWM
bool parsefullscalePWM(yarp::os::Searchable &config, double dutycycleToPWM[])
Definition eomcParser.cpp:2205
yarp::dev::eomc::Parser::parseRotorsLimits
bool parseRotorsLimits(yarp::os::Searchable &config, std::vector< rotorLimits_t > &rotorsLimits)
Definition eomcParser.cpp:1930
yarp::dev::eomc::Parser::parseImpedanceGroup
bool parseImpedanceGroup(yarp::os::Searchable &config, std::vector< impedanceParameters_t > &impedance)
Definition eomcParser.cpp:2481
yarp::dev::eomc::Parser::parseKalmanFilterParams
bool parseKalmanFilterParams(yarp::os::Searchable &config, std::vector< kalmanFilterParams_t > &kalmanFilterParams)
Definition eomcParser.cpp:2357
yarp::dev::eomc::Parser::parseGearboxValues
bool parseGearboxValues(yarp::os::Searchable &config, double gearbox_M2J[], double gearbox_E2J[])
Definition eomcParser.cpp:2271
yarp::dev::eomc::Parser::parseBehaviourFalgs
bool parseBehaviourFalgs(yarp::os::Searchable &config, bool &useRawEncoderData, bool &pwmIsLimited)
Definition eomcParser.cpp:2062
yarp::dev::eomc::PidInfo::usernamePidSelected
std::string usernamePidSelected
Definition eomcParser.h:374
yarp::dev::eomc::PidInfo::fbk_PidUnits
yarp::dev::PidFeedbackUnitsEnum fbk_PidUnits
Definition eomcParser.h:368
yarp::dev::eomc::PidInfo::out_type
eOmc_ctrl_out_type_t out_type
Definition eomcParser.h:370
yarp::dev::eomc::PidInfo::out_PidUnits
yarp::dev::PidOutputUnitsEnum out_PidUnits
Definition eomcParser.h:369
yarp::dev::eomc::Pid_Algorithm_simple
Definition eomcParser.h:288
yarp::dev::eomc::Pid_Algorithm_simple::pid
yarp::dev::Pid * pid
Definition eomcParser.h:290
yarp::dev::eomc::Pid_Algorithm
Definition eomcParser.h:265
yarp::dev::eomc::Pid_Algorithm::fbk_PidUnits
yarp::dev::PidFeedbackUnitsEnum fbk_PidUnits
Definition eomcParser.h:269
yarp::dev::eomc::Pid_Algorithm::setUnits
void setUnits(yarp::dev::PidFeedbackUnitsEnum fu, yarp::dev::PidOutputUnitsEnum ou)
Definition eomcParser.h:277
yarp::dev::eomc::Pid_Algorithm::out_type
eOmc_ctrl_out_type_t out_type
Definition eomcParser.h:271
yarp::dev::eomc::Pid_Algorithm::out_PidUnits
yarp::dev::PidOutputUnitsEnum out_PidUnits
Definition eomcParser.h:270
yarp::dev::eomc::Pid_Algorithm::getPID
virtual yarp::dev::Pid getPID(int joint)=0
yarp::dev::eomc::TrqPidInfo::viscousPos
double viscousPos
motor torque constant
Definition eomcParser.h:400
yarp::dev::eomc::TrqPidInfo::ktau
double ktau
back-emf compensation parameter
Definition eomcParser.h:399
x
x
Definition compute_ekf_sym.m:21
LOAD_STRINGS
#define LOAD_STRINGS(dest, source)
Definition eomcParser.cpp:143
out
out
Definition sine.m:8
yarp::dev::eomc::couplingInfo_t
Definition eomcParser.h:486
yarp::dev::eomc::couplingInfo_t::matrixM2J
std::vector< double > matrixM2J
Definition eomcParser.h:488
yarp::dev::eomc::couplingInfo_t::matrixE2J
std::vector< double > matrixE2J
Definition eomcParser.h:489
yarp::dev::eomc::couplingInfo_t::matrixJ2M
std::vector< double > matrixJ2M
Definition eomcParser.h:487
yarp::dev::eomc::focBasedSpecificInfo_t
Definition eomcParser.h:409
yarp::dev::eomc::focBasedSpecificInfo_t::rotorIndexOffset
int rotorIndexOffset
Definition eomcParser.h:415
yarp::dev::eomc::focBasedSpecificInfo_t::hasTempSensor
bool hasTempSensor
Definition eomcParser.h:412
Generated on Fri Sep 26 2025 14:18:32 for iCub-main by doxygen 1.9.8