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
1495 if (!extractGroup(focGroup, xtmp, "RotorIndexOffset", "RotorIndexOffset", _njoints))
1496 {
1497 return false;
1498 }
1499 else
1500 {
1501 for (i = 1; i < xtmp.size(); i++)
1502 {
1503 if(AutoCalibration[i-1] == 0)
1504 {
1505 foc_based_info[i - 1].rotorIndexOffset = xtmp.get(i).asInt32();
1506 if (foc_based_info[i - 1].rotorIndexOffset <0 || foc_based_info[i - 1].rotorIndexOffset >359)
1507 {
1508 yError() << "In " << _boardname << "joint " << i-1 << ": rotorIndexOffset should be in [0,359] range." ;
1509 return false;
1510 }
1511 }
1512 else
1513 {
1514 yWarning() << "In " << _boardname << "joint " << i-1 << ": motor autocalibration is enabled!!! ATTENTION!!!" ;
1515 foc_based_info[i - 1].rotorIndexOffset = -1;
1516 }
1517 }
1518 }
1519
1520
1521 //Now I verify if rotor encoder hasn't index, then rotor offset must be zero.
1522 for (i = 0; i < (unsigned)_njoints; i++)
1523 {
1524 if((0 == foc_based_info[i].hasRotorEncoderIndex) && (0 != foc_based_info[i].rotorIndexOffset))
1525 {
1526 yError() << "In " << _boardname << "joint " << i << ": inconsistent configuration: if rotor encoder hasn't index then its offset should be 0." ;
1527 return false;
1528 }
1529 }
1530
1531 // Number of motor poles
1532 if (!extractGroup(focGroup, xtmp, "MotorPoles", "MotorPoles", _njoints))
1533 {
1534 return false;
1535 }
1536 else
1537 {
1538 for (i = 1; i < xtmp.size(); i++)
1539 foc_based_info[i - 1].motorPoles = xtmp.get(i).asInt32();
1540 }
1541
1542 if (!extractGroup(focGroup, xtmp, "HasSpeedEncoder", "HasSpeedEncoder 0/1 ", _njoints))
1543 {
1544 yWarning () << "missing param HasSpeedEncoder";
1545 // optional by now
1546 //return false;
1547 }
1548 else
1549 {
1550 for (i = 1; i < xtmp.size(); i++)
1551 foc_based_info[i - 1].hasSpeedEncoder = xtmp.get(i).asInt32() != 0;
1552 }
1553
1554 return true;
1555
1556}
1557
1558bool Parser::parseJointsetCfgGroup(yarp::os::Searchable &config, std::vector<JointsSet> &jsets, std::vector<int> &joint2set)
1559{
1560 Bottle jointsetcfg = config.findGroup("JOINTSET_CFG");
1561 if (jointsetcfg.isNull())
1562 {
1563 yError() << "embObjMC BOARD " << _boardname << "Missing JOINTSET_CFG group";
1564 return false;
1565 }
1566
1567
1568 Bottle xtmp;
1569 int numofsets = 0;
1570
1571 if(!extractGroup(jointsetcfg, xtmp, "numberofsets", "number of sets ", 1))
1572 {
1573 return false;
1574 }
1575
1576 numofsets = xtmp.get(1).asInt32();
1577
1578 if((0 == numofsets) || (numofsets > _njoints))
1579 {
1580 yError() << "embObjMC BOARD " << _boardname << "Number of jointsets is not correct. it should belong to (1, " << _njoints << ")";
1581 return false;
1582 }
1583
1584
1585
1586 if(!checkAndSetVectorSize(jsets, numofsets, "parseJointsetCfgGroup"))
1587 return false;
1588
1589 if(!checkAndSetVectorSize(joint2set, _njoints, "parseJointsetCfgGroup"))
1590 return false;
1591
1592 for(unsigned int s=0;s<(unsigned)numofsets;s++)
1593 {
1594 char jointset_string[80];
1595 sprintf(jointset_string, "JOINTSET_%d", s);
1596 bool formaterror = false;
1597
1598
1599 Bottle &js_cfg = jointsetcfg.findGroup(jointset_string);
1600 if(js_cfg.isNull())
1601 {
1602 yError() << "embObjMC BOARD " << _boardname << "cannot find " << jointset_string;
1603 return false;
1604 }
1605
1606 //1) id of set
1607 jsets.at(s).id=s;
1608
1609
1610 //2) list of joints
1611 Bottle &b_listofjoints=js_cfg.findGroup("listofjoints", "list of joints");
1612 if (b_listofjoints.isNull())
1613 {
1614 yError() << "embObjMC BOARD " << _boardname << "listofjoints parameter not found";
1615 return false;
1616 }
1617
1618 int numOfJointsInSet = b_listofjoints.size()-1;
1619 if((numOfJointsInSet < 1) || (numOfJointsInSet>_njoints))
1620 {
1621 yError() << "embObjMC BOARD " << _boardname << "numof joints of set " << s << " is not correct";
1622 return false;
1623 }
1624
1625
1626 for (int j = 0; j <numOfJointsInSet; j++)
1627 {
1628 int jointofthisset = b_listofjoints.get(j+1).asInt32();
1629
1630 if((jointofthisset< 0) || (jointofthisset>_njoints))
1631 {
1632 yError() << "embObjMC BOARD " << _boardname << "invalid joint number for set " << s;
1633 return false;
1634 }
1635
1636 jsets.at(s).joints.push_back(jointofthisset);
1637
1638 //2.1) fill map joint to set
1639 joint2set.at(jointofthisset) = s;
1640 }
1641
1642 // 3) constraints
1643 if(!extractGroup(js_cfg, xtmp, "constraint", "type of jointset constraint ", 1))
1644 {
1645 return false;
1646 }
1647
1648 eOmc_jsetconstraint_t constraint;
1649 if(!convert(xtmp.get(1).asString(), constraint, formaterror))
1650 {
1651 return false;
1652 }
1653 jsets.at(s).cfg.constraints.type = constraint;
1654
1655 //param1
1656 if(!extractGroup(js_cfg, xtmp, "param1", "param1 of jointset constraint ", 1))
1657 {
1658 return false;
1659 }
1660 jsets.at(s).cfg.constraints.param1 = (float)xtmp.get(1).asFloat64();
1661
1662 //param2
1663 if(!extractGroup(js_cfg, xtmp, "param2", "param2 of jointset constraint ", 1))
1664 {
1665 return false;
1666 }
1667 jsets.at(s).cfg.constraints.param2 = (float)xtmp.get(1).asFloat64();
1668
1669
1670 }
1671 return true;
1672}
1673
1674bool Parser::parseTimeoutsGroup(yarp::os::Searchable &config, std::vector<timeouts_t> &timeouts, int defaultVelocityTimeout)
1675{
1676 if(!checkAndSetVectorSize(timeouts, _njoints, "parseTimeoutsGroup"))
1677 return false;
1678
1679 unsigned int i;
1680
1681 Bottle timeoutsGroup =config.findGroup("TIMEOUTS");
1682 if(timeoutsGroup.isNull())
1683 {
1684 yError() << "embObjMC BOARD " << _boardname << " no TIMEOUTS group found in config file.";
1685 return false;
1686 }
1687
1688 Bottle xtmp;
1689 xtmp.clear();
1690 if (!extractGroup(timeoutsGroup, xtmp, "velocity", "a list of timeout to be used in the vmo control", _njoints))
1691 {
1692 yError() << "embObjMC BOARD " << _boardname << " no velocity parameter found in TIMEOUTS group in motion control config file.";
1693 return false;
1694 }
1695 else
1696 {
1697 for(i=1; i<xtmp.size(); i++)
1698 timeouts[i-1].velocity = xtmp.get(i).asInt32();
1699 }
1700
1701
1702 return true;
1703
1704}
1705
1706bool Parser::parseCurrentLimits(yarp::os::Searchable &config, std::vector<motorCurrentLimits_t> &currLimits)
1707{
1708 Bottle &limits=config.findGroup("LIMITS");
1709 if (limits.isNull())
1710 {
1711 yError() << "embObjMC BOARD " << _boardname << " detected that Group LIMITS is not found in configuration file";
1712 return false;
1713 }
1714
1715 currLimits.resize(_njoints);
1716 unsigned int i;
1717 Bottle xtmp;
1718
1719 // current limit
1720 if (!extractGroup(limits, xtmp, "motorOverloadCurrents","a list of current limits", _njoints))
1721 return false;
1722 else
1723 for(i=1; i<xtmp.size(); i++) currLimits[i-1].overloadCurrent=xtmp.get(i).asFloat64();
1724
1725 // nominal current
1726 if (!extractGroup(limits, xtmp, "motorNominalCurrents","a list of nominal current limits", _njoints))
1727 return false;
1728 else
1729 for(i=1; i<xtmp.size(); i++) currLimits[i-1].nominalCurrent =xtmp.get(i).asFloat64();
1730
1731 // peak current
1732 if (!extractGroup(limits, xtmp, "motorPeakCurrents","a list of peak current limits", _njoints))
1733 return false;
1734 else
1735 for(i=1; i<xtmp.size(); i++) currLimits[i-1].peakCurrent=xtmp.get(i).asFloat64();
1736
1737 return true;
1738
1739}
1740
1741bool Parser::parseTemperatureLimits(yarp::os::Searchable &config, std::vector<temperatureLimits_t> &temperatureLimits)
1742{
1743 Bottle &limits = config.findGroup("LIMITS");
1744 if (limits.isNull())
1745 {
1746 yError() << "embObjMC BOARD " << _boardname << " detected that Group LIMITS is not found in configuration file";
1747 return false;
1748 }
1749
1750 temperatureLimits.resize(_njoints);
1751 unsigned int i;
1752 Bottle xtmp;
1753
1754 // hardware limit
1755 if(!extractGroup(limits, xtmp, "hardwareTemperatureLimits", "a list of temperature limits", _njoints, false))
1756 {
1757 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());
1758 for (i = 0; i < (unsigned)_njoints; i++)
1759 {
1760 temperatureLimits[i].hardwareTemperatureLimit = 0;
1761 temperatureLimits[i].warningTemperatureLimit = 0;
1762 }
1763 }
1764 else
1765 {
1766 for (i = 1; i < xtmp.size(); i++) temperatureLimits[i - 1].hardwareTemperatureLimit = xtmp.get(i).asFloat64();
1767 if (!extractGroup(limits, xtmp, "warningTemperatureLimits", "a list of warning temperature limits", _njoints, false))
1768 {
1769 // warning limit - parsing it only if hardwareTemperatureLimit available
1770 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());
1771
1772 for (i = 0; i < (unsigned)_njoints; i++) temperatureLimits[i].warningTemperatureLimit = 0;
1773 }
1774 else
1775 {
1776 for (i = 1; i < xtmp.size(); i++) temperatureLimits[i - 1].warningTemperatureLimit = xtmp.get(i).asFloat64();
1777 }
1778 }
1779
1780 //Now I verify that warning temperature limits is < 85% of hardwareTemperatureLimit.
1781 for (i = 0; i < (unsigned)_njoints; i++)
1782 {
1783 if(temperatureLimits[i].warningTemperatureLimit > (0.85 * temperatureLimits[i].hardwareTemperatureLimit))
1784 {
1785 yError() << "In " << _boardname << "joint " << i << ": inconsistent temperature limits. warningTemperatureLimit must be smaller than 85% of hardwareTemperatureLimit" ;
1786 return false;
1787 }
1788 }
1789 return true;
1790}
1791
1792bool Parser::parseJointsLimits(yarp::os::Searchable &config, std::vector<jointLimits_t> &jointsLimits)
1793{
1794 Bottle &limits=config.findGroup("LIMITS");
1795 if (limits.isNull())
1796 {
1797 yError() << "embObjMC BOARD " << _boardname << " detected that Group LIMITS is not found in configuration file";
1798 return false;
1799 }
1800
1801 jointsLimits.resize(_njoints);
1802 unsigned int i;
1803 Bottle xtmp;
1804
1805 // max limit
1806 if (!extractGroup(limits, xtmp, "jntPosMax","a list of user maximum angles (in degrees)", _njoints))
1807 return false;
1808 else
1809 for(i=1; i<xtmp.size(); i++) jointsLimits[i-1].posMax = xtmp.get(i).asFloat64();
1810
1811 // min limit
1812 if (!extractGroup(limits, xtmp, "jntPosMin","a list of user minimum angles (in degrees)", _njoints))
1813 return false;
1814 else
1815 for(i=1; i<xtmp.size(); i++) jointsLimits[i-1].posMin = xtmp.get(i).asFloat64();
1816
1817 // max hardware limit
1818 if (!extractGroup(limits, xtmp, "hardwareJntPosMax","a list of hardware maximum angles (in degrees)", _njoints))
1819 return false;
1820 else
1821 {
1822 for(i=1; i<xtmp.size(); i++) jointsLimits[i-1].posHwMax = xtmp.get(i).asFloat64();
1823
1824 //check hardware limits are bigger then user limits
1825 for(i=0; i<(unsigned)_njoints; i++)
1826 {
1827 if(jointsLimits[i].posMax > jointsLimits[i].posHwMax)
1828 {
1829 yError() << "embObjMotionControl: user has set a limit bigger then hardware limit!. Please check jntPosMax.";
1830 return false;
1831 }
1832 }
1833 }
1834
1835 // min hardware limit
1836 if (!extractGroup(limits, xtmp, "hardwareJntPosMin","a list of hardware minimum angles (in degrees)", _njoints))
1837 {
1838 return false;
1839 }
1840 else
1841 {
1842 for(i=1; i<xtmp.size(); i++) jointsLimits[i-1].posHwMin = xtmp.get(i).asFloat64();
1843
1844 //check hardware limits are bigger then user limits
1845 for(i=0; i<(unsigned)_njoints; i++)
1846 {
1847 if(jointsLimits[i].posMin < jointsLimits[i].posHwMin)
1848 {
1849 yError() << "embObjMotionControl: user has set a limit bigger then hardware limit!. Please check jntPosMin.";
1850 return false;
1851 }
1852 }
1853
1854 }
1855
1856 // joint Velocity command max limit
1857 if (!extractGroup(limits, xtmp, "jntVelMax", "a list of maximum velocities for the joints (in degrees/s)", _njoints))
1858 return false;
1859 else
1860 for (i = 1; i<xtmp.size(); i++) jointsLimits[i - 1].velMax = xtmp.get(i).asFloat64();
1861
1862 return true;
1863}
1864
1865bool Parser::parseRotorsLimits(yarp::os::Searchable &config, std::vector<rotorLimits_t> &rotorsLimits)
1866{
1867 Bottle &limits=config.findGroup("LIMITS");
1868 if (limits.isNull())
1869 {
1870 yError() << "embObjMC BOARD " << _boardname << " detected that Group LIMITS is not found in configuration file";
1871 return false;
1872 }
1873
1874 if(!checkAndSetVectorSize(rotorsLimits, _njoints, "parseRotorsLimits"))
1875 return false;
1876
1877 Bottle xtmp;
1878 unsigned int i;
1879
1880 // Rotor max limit
1881 if (!extractGroup(limits, xtmp, "rotorPosMax","a list of maximum rotor angles (in degrees)", _njoints))
1882 return false;
1883 else
1884 for(i=1; i<xtmp.size(); i++) rotorsLimits[i-1].posMax = xtmp.get(i).asFloat64();
1885
1886
1887
1888 // Rotor min limit
1889 if (!extractGroup(limits, xtmp, "rotorPosMin","a list of minimum roto angles (in degrees)", _njoints))
1890 return false;
1891 else
1892 for(i=1; i<xtmp.size(); i++) rotorsLimits[i-1].posMin = xtmp.get(i).asFloat64();
1893
1894 // Motor pwm limit
1895 if (!extractGroup(limits, xtmp, "motorPwmLimit","a list of motor PWM limits", _njoints))
1896 return false;
1897 else
1898 for(i=1; i<xtmp.size(); i++)
1899 {
1900 rotorsLimits[i-1].pwmMax = xtmp.get(i).asFloat64();
1901 if(rotorsLimits[i-1].pwmMax<0)
1902 {
1903 yError() << "motorPwmLimit should be a positive value";
1904 return false;
1905 }
1906 }
1907
1908 return true;
1909
1910}
1911
1912bool Parser::parseCouplingInfo(yarp::os::Searchable &config, couplingInfo_t &couplingInfo)
1913{
1914 Bottle coupling_bottle = config.findGroup("COUPLINGS");
1915 if (coupling_bottle.isNull())
1916 {
1917 yError() << "embObjMC BOARD " << _boardname << "Missing Coupling group";
1918 return false;
1919 }
1920 Bottle xtmp;
1921 int fixedMatrix4X4Size = 16;
1922 int fixedMatrix4X6Size = 24;
1923 bool formaterror =false;
1924
1925 // matrix J2M
1926 if (!extractGroup(coupling_bottle, xtmp, "matrixJ2M", "matrixJ2M ", fixedMatrix4X4Size))
1927 {
1928 return false;
1929 }
1930
1931 if(false == convert(xtmp, couplingInfo.matrixJ2M, formaterror, fixedMatrix4X4Size))
1932 {
1933 yError() << "embObjMC BOARD " << _boardname << " has detected an illegal format for some of the values of CONTROLLER.matrixJ2M";
1934 return false;
1935 }
1936
1937
1938 // matrix E2J
1939 if (!extractGroup(coupling_bottle, xtmp, "matrixE2J", "matrixE2J ", fixedMatrix4X6Size))
1940 {
1941 return false;
1942 }
1943
1944 formaterror = false;
1945 if(false == convert(xtmp, couplingInfo.matrixE2J, formaterror, fixedMatrix4X6Size))
1946 {
1947 yError() << "embObjMC BOARD " << _boardname << " has detected an illegal format for some of the values of CONTROLLER.matrixE2J";
1948 return false;
1949 }
1950
1951
1952 // matrix M2J
1953 if (!extractGroup(coupling_bottle, xtmp, "matrixM2J", "matrixM2J ", fixedMatrix4X4Size))
1954 {
1955 return false;
1956 }
1957
1958 formaterror = false;
1959 if( false == convert(xtmp, couplingInfo.matrixM2J, formaterror, fixedMatrix4X4Size))
1960 {
1961 yError() << "embObjMC BOARD " << _boardname << " has detected an illegal format for some of the values of CONTROLLER.matrixM2J";
1962 return false;
1963 }
1964
1965 return true;
1966}
1967
1968bool Parser::parseMotioncontrolVersion(yarp::os::Searchable &config, int &version)
1969{
1970 if (!config.findGroup("GENERAL").find("MotioncontrolVersion").isInt32())
1971 {
1972 yError() << "Missing MotioncontrolVersion parameter. RobotInterface cannot start. Please contact icub-support@iit.it";
1973 return false;
1974 }
1975
1976 version = config.findGroup("GENERAL").find("MotioncontrolVersion").asInt32();
1977 return true;
1978
1979}
1980
1981bool Parser::isVerboseEnabled(yarp::os::Searchable &config)
1982{
1983 bool ret = false;
1984 if(!config.findGroup("GENERAL").find("verbose").isBool())
1985 {
1986 yError() << "embObjMotionControl::open() detects that general->verbose bool param is different from accepted values (true / false). Assuming false";
1987 ret = false;
1988 }
1989 else
1990 {
1991 ret = config.findGroup("GENERAL").find("verbose").asBool();
1992 }
1993 _verbosewhenok = ret;
1994 return ret;
1995}
1996
1997bool Parser::parseBehaviourFalgs(yarp::os::Searchable &config, bool &useRawEncoderData, bool &pwmIsLimited)
1998{
1999
2000 // Check useRawEncoderData = do not use calibration data!
2001 Value use_raw = config.findGroup("GENERAL").find("useRawEncoderData");
2002
2003 if(use_raw.isNull())
2004 {
2005 useRawEncoderData = false;
2006 }
2007 else
2008 {
2009 if(!use_raw.isBool())
2010 {
2011 yWarning() << "embObjMotionControl::open() detected that useRawEncoderData bool param is different from accepted values (true / false). Assuming false";
2012 useRawEncoderData = false;
2013 }
2014 else
2015 {
2016 useRawEncoderData = use_raw.asBool();
2017 if(useRawEncoderData)
2018 {
2019 yWarning() << "embObjMotionControl::open() detected that it is using raw data from encoders! Be careful See 'useRawEncoderData' param in config file";
2020 yWarning() << "DO NOT USE OR CALIBRATE THE ROBOT IN THIS CONFIGURATION!";
2021 yWarning() << "CHECK IF THE FAULT BUTTON IS PRESSED and press ENTER to continue";
2022 getchar();
2023 }
2024 }
2025 }
2026
2027 // Check useRawEncoderData = do not use calibration data!
2028 Value use_limitedPWM = config.findGroup("GENERAL").find("useLimitedPWM");
2029 if(use_limitedPWM.isNull())
2030 {
2031 pwmIsLimited = false;
2032 }
2033 else
2034 {
2035 if(!use_limitedPWM.isBool())
2036 {
2037 pwmIsLimited = false;
2038 }
2039 else
2040 {
2041 pwmIsLimited = use_limitedPWM.asBool();
2042 }
2043 }
2044
2045 return true;
2046}
2047
2048bool Parser::parseAxisInfo(yarp::os::Searchable &config, int axisMap[], std::vector<axisInfo_t> &axisInfo)
2049{
2050
2051 Bottle xtmp;
2052 unsigned int i;
2053 axisInfo.resize(_njoints);
2054
2055 Bottle general = config.findGroup("GENERAL");
2056 if (general.isNull())
2057 {
2058 yError() << "embObjMC BOARD " << _boardname << "Missing General group" ;
2059 return false;
2060 }
2061
2062
2063 if (!extractGroup(general, xtmp, "AxisMap", "a list of reordered indices for the axes", _njoints))
2064 return false;
2065
2066 for (i = 1; i < xtmp.size(); i++)
2067 {
2068 int user_joint = xtmp.get(i).asInt32();
2069 if(user_joint>= _njoints)
2070 {
2071 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";
2072 return false;
2073 }
2074 axisMap[i-1] = user_joint;
2075 }
2076
2077
2078 if (!extractGroup(general, xtmp, "AxisName", "a list of strings representing the axes names", _njoints))
2079 return false;
2080
2081 //beware: axis name has to be remapped here because they are not set using the toHw() helper function
2082 for (i = 1; i < xtmp.size(); i++)
2083 {
2084 int mappedto = axisInfo[i-1].mappedto;
2085 axisInfo[axisMap[i - 1]].name = xtmp.get(i).asString();
2086 }
2087
2088 if (!extractGroup(general, xtmp, "AxisType", "a list of strings representing the axes type (revolute/prismatic)", _njoints))
2089 return false;
2090
2091 //beware: axis type has to be remapped here because they are not set using the toHw() helper function
2092 for (i = 1; i < xtmp.size(); i++)
2093 {
2094 string s = xtmp.get(i).asString();
2095 int mappedto = axisInfo[i-1].mappedto;
2096 if (s == "revolute") axisInfo[axisMap[i - 1]].type = VOCAB_JOINTTYPE_REVOLUTE;
2097 else if (s == "prismatic") axisInfo[axisMap[i - 1]].type = VOCAB_JOINTTYPE_PRISMATIC;
2098 else
2099 {
2100 yError("Unknown AxisType value %s!", s.c_str());
2101 axisInfo[axisMap[i - 1]].type = VOCAB_JOINTTYPE_UNKNOWN;
2102 return false;
2103 }
2104 }
2105
2106 return true;
2107}
2108
2109bool Parser::parseEncoderFactor(yarp::os::Searchable &config, double encoderFactor[])
2110{
2111 Bottle general = config.findGroup("GENERAL");
2112 if (general.isNull())
2113 {
2114 yError() << "embObjMC BOARD " << _boardname << "Missing General group" ;
2115 return false;
2116 }
2117 Bottle xtmp;
2118 unsigned int i;
2119 double tmp_A2E;
2120
2121 // Encoder scales
2122 if (!extractGroup(general, xtmp, "Encoder", "a list of scales for the encoders", _njoints))
2123 {
2124 return false;
2125 }
2126
2127 for (i = 1; i < xtmp.size(); i++)
2128 {
2129 tmp_A2E = xtmp.get(i).asFloat64();
2130 if (tmp_A2E<0)
2131 {
2132 yWarning() << "embObjMC BOARD " << _boardname << "Encoder parameter should be positive!";
2133 }
2134 encoderFactor[i - 1] = tmp_A2E;
2135 }
2136
2137 return true;
2138}
2139
2140bool Parser::parsefullscalePWM(yarp::os::Searchable &config, double dutycycleToPWM[])
2141{
2142 Bottle general = config.findGroup("GENERAL");
2143 if (general.isNull())
2144 {
2145 yError() << "embObjMC BOARD " << _boardname << "Missing General group";
2146 return false;
2147 }
2148 Bottle xtmp;
2149 unsigned int i;
2150 double tmpval;
2151
2152 // fullscalePWM
2153 if (!extractGroup(general, xtmp, "fullscalePWM", "a list of scales for the fullscalePWM conversion factor", _njoints))
2154 {
2155 yError("fullscalePWM param not found in config file. Please update robot configuration files or contact https://github.com/robotology/icub-support");
2156 return false;
2157 }
2158
2159 for (i = 1; i < xtmp.size(); i++)
2160 {
2161 tmpval = xtmp.get(i).asFloat64();
2162 if (tmpval<0)
2163 {
2164 yError() << "embObjMC BOARD " << _boardname << "fullscalePWM parameter should be positive!";
2165 return false;
2166 }
2167 dutycycleToPWM[i - 1] = tmpval / 100.0;
2168 }
2169
2170 return true;
2171}
2172
2173bool Parser::parseAmpsToSensor(yarp::os::Searchable &config, double ampsToSensor[])
2174{
2175 Bottle general = config.findGroup("GENERAL");
2176 if (general.isNull())
2177 {
2178 yError() << "embObjMC BOARD " << _boardname << "Missing General group";
2179 return false;
2180 }
2181 Bottle xtmp;
2182 unsigned int i;
2183 double tmpval;
2184
2185 // ampsToSensor
2186 if (!extractGroup(general, xtmp, "ampsToSensor", "a list of scales for the ampsToSensor conversion factor", _njoints))
2187 {
2188 yError("ampsToSensor param not found in config file. Please update robot configuration files or contact https://github.com/robotology/icub-support");
2189 return false;
2190 }
2191
2192 for (i = 1; i < xtmp.size(); i++)
2193 {
2194 tmpval = xtmp.get(i).asFloat64();
2195 if (tmpval<0)
2196 {
2197 yError() << "embObjMC BOARD " << _boardname << "ampsToSensor parameter should be positive!";
2198 return false;
2199 }
2200 ampsToSensor[i - 1] = tmpval;
2201 }
2202
2203 return true;
2204}
2205
2206bool Parser::parseGearboxValues(yarp::os::Searchable &config, double gearbox_M2J[], double gearbox_E2J[])
2207{
2208 Bottle general = config.findGroup("GENERAL");
2209 if (general.isNull())
2210 {
2211 yError() << "embObjMC BOARD " << _boardname << "Missing General group" ;
2212 return false;
2213 }
2214
2215 Bottle xtmp;
2216 unsigned int i;
2217
2218 // Gearbox_M2J
2219 if (!extractGroup(general, xtmp, "Gearbox_M2J", "The gearbox reduction ratio", _njoints))
2220 {
2221 return false;
2222 }
2223
2224 for (i = 1; i < xtmp.size(); i++)
2225 {
2226 gearbox_M2J[i-1] = xtmp.get(i).asFloat64();
2227 if (gearbox_M2J[i-1]==0)
2228 {
2229 yError() << "embObjMC BOARD " << _boardname << "Using a gearbox value = 0 may cause problems! Check your configuration files";
2230 return false;
2231 }
2232 }
2233
2234
2235 //Gearbox_E2J
2236 if (!extractGroup(general, xtmp, "Gearbox_E2J", "The gearbox reduction ratio between encoder and joint", _njoints))
2237 {
2238 return false;
2239 }
2240
2241 int test = xtmp.size();
2242 for (i = 1; i < xtmp.size(); i++)
2243 {
2244 gearbox_E2J[i-1] = xtmp.get(i).asFloat64();
2245 if (gearbox_E2J[i-1]==0)
2246 {
2247 yError() << "embObjMC BOARD " << _boardname << "Using a gearbox value = 0 may cause problems! Check your configuration files";
2248 return false;
2249 }
2250 }
2251
2252
2253 return true;
2254}
2255
2256bool Parser::parseDeadzoneValue(yarp::os::Searchable &config, double deadzone[], bool *found)
2257{
2258// Bottle general = config.findGroup("GENERAL");
2259// if (general.isNull())
2260// {
2261// yError() << "embObjMC BOARD " << _boardname << "Missing General group" ;
2262// return false;
2263// }
2264
2265 Bottle general = config.findGroup("OTHER_CONTROL_PARAMETERS");
2266 if (general.isNull())
2267 {
2268 yWarning() << "embObjMC BOARD " << _boardname << "Missing OTHER_CONTROL_PARAMETERS.DeadZone parameter. I'll use default value. (see documentation for more datails)";
2269 *found = false;
2270 return true;
2271 }
2272 Bottle xtmp;
2273 unsigned int i;
2274
2275 // DeadZone
2276 if (!extractGroup(general, xtmp, "deadZone", "The deadzone of joint", _njoints, false))
2277 {
2278 yWarning() << "embObjMC BOARD " << _boardname << "Missing OTHER_CONTROL_PARAMETERS group.DeadZone parameter. I'll use default value. (see documentation for more datails)";
2279 *found = false;
2280 return true;
2281 }
2282
2283 *found = true;
2284 for (i = 1; i < xtmp.size(); i++)
2285 {
2286 deadzone[i-1] = xtmp.get(i).asFloat64();
2287 }
2288
2289 return true;
2290}
2291
2292bool Parser::parseKalmanFilterParams(yarp::os::Searchable &config, std::vector<kalmanFilterParams_t> &kalmanFilterParams)
2293{
2294 Bottle general = config.findGroup("KALMAN_FILTER");
2295 if (general.isNull())
2296 {
2297 yWarning() << "embObjMC BOARD " << _boardname << "Missing KALMAN_FILTER group. Kalman Filter will be disabled by default.";
2298
2299 // if you don't specify the Kalman Filter group disable the kalmam filter for all the joints.
2300 for(int j=0; j<_njoints; j++)
2301 {
2302 kalmanFilterParams[j].enabled = false;
2303 kalmanFilterParams[j].x0.fill(0.0);
2304 kalmanFilterParams[j].Q.fill(0.0);
2305 kalmanFilterParams[j].R = 0.0;
2306 kalmanFilterParams[j].P0 = 0.0;
2307 }
2308 return true;
2309 }
2310 else
2311 {
2312 Bottle xtmp;
2313
2314 // kalmanFilterEnabled
2315 if (!extractGroup(general, xtmp, "kalmanFilterEnabled", "kalman filter of joint: ", _njoints, true))
2316 {
2317 yWarning() << "embObjMC BOARD " << _boardname << "Missing kalmanFilterEnabled parameter. It will be disabled by default.";
2318 return false;
2319 }
2320 for (int j = 0; j<_njoints; j++) kalmanFilterParams[j].enabled = xtmp.get(j + 1).asBool();
2321
2322 // x0_0
2323 if (!extractGroup(general, xtmp, "x0", "Initial state x0 of kalman filter of joint: ", _njoints, true))
2324 {
2325 yWarning() << "embObjMC BOARD " << _boardname << "Missing initial state x0 parameter. Kalman Filter will be disabled by default.";
2326 return false;
2327 }
2328 for (int j = 0; j<_njoints; j++) kalmanFilterParams[j].x0.at(0) = xtmp.get(j + 1).asFloat32();
2329
2330 // x0_1
2331 if (!extractGroup(general, xtmp, "x1", "Initial state x1 of kalman filter of joint: ", _njoints, true))
2332 {
2333 yWarning() << "embObjMC BOARD " << _boardname << "Missing initial state x1 parameter. Kalman Filter will be disabled by default.";
2334 return false;
2335 }
2336 for (int j = 0; j<_njoints; j++) kalmanFilterParams[j].x0.at(1) = xtmp.get(j + 1).asFloat32();
2337
2338 // x0_2
2339 if (!extractGroup(general, xtmp, "x2", "Initial state x2 of kalman filter of joint: ", _njoints, true))
2340 {
2341 yWarning() << "embObjMC BOARD " << _boardname << "Missing initial state x2 parameter. Kalman Filter will be disabled by default.";
2342 return false;
2343 }
2344 for (int j = 0; j<_njoints; j++) kalmanFilterParams[j].x0.at(2) = xtmp.get(j + 1).asFloat32();
2345
2346 // Q0
2347 if (!extractGroup(general, xtmp, "Q0", "Process Q0 noise covariance matrix of kalman filter of joint: ", _njoints, true))
2348 {
2349 yWarning() << "embObjMC BOARD " << _boardname << "Missing Q0 parameter. Kalman Filter will be disabled by default.";
2350 return false;
2351 }
2352 for (int j = 0; j<_njoints; j++) kalmanFilterParams[j].Q.at(0) = xtmp.get(j + 1).asFloat32();
2353
2354 // Q1
2355 if (!extractGroup(general, xtmp, "Q1", "Process Q1 noise covariance matrix of kalman filter of joint: ", _njoints, true))
2356 {
2357 yWarning() << "embObjMC BOARD " << _boardname << "Missing Q1 parameter. Kalman Filter will be disabled by default.";
2358 return false;
2359 }
2360 for (int j = 0; j<_njoints; j++) kalmanFilterParams[j].Q.at(1) = xtmp.get(j + 1).asFloat32();
2361
2362 // Q2
2363 if (!extractGroup(general, xtmp, "Q2", "Process Q2 noise covariance matrix of kalman filter of joint: ", _njoints, true))
2364 {
2365 yWarning() << "embObjMC BOARD " << _boardname << "Missing Q2 parameter. Kalman Filter will be disabled by default.";
2366 return false;
2367 }
2368 for (int j = 0; j<_njoints; j++) kalmanFilterParams[j].Q.at(2) = xtmp.get(j + 1).asFloat32();
2369
2370 // R
2371 if (!extractGroup(general, xtmp, "R", "Measurement noise covariance matrix of kalman filter for joint: ", _njoints, true))
2372 {
2373 yWarning() << "embObjMC BOARD " << _boardname << "Missing R scalar parameter. Kalman Filter will be disabled by default.";
2374 return false;
2375 }
2376 for (int j = 0; j<_njoints; j++) kalmanFilterParams[j].R = xtmp.get(j + 1).asFloat32();
2377
2378 // P0
2379 if (!extractGroup(general, xtmp, "P0", "Initial state estimation error covariance matrix of kalman filter of joint: ", _njoints, true))
2380 {
2381 yWarning() << "embObjMC BOARD " << _boardname << "Missing P0 scalar parameter. Kalman Filter will be disabled by default.";
2382 return false;
2383 }
2384 for (int j = 0; j<_njoints; j++) kalmanFilterParams[j].P0 = xtmp.get(j + 1).asFloat32();
2385 }
2386
2387 return true;
2388}
2389
2390
2391bool Parser::parseMechanicalsFlags(yarp::os::Searchable &config, int useMotorSpeedFbk[])
2392{
2393 Bottle general = config.findGroup("GENERAL");
2394 if (general.isNull())
2395 {
2396 yError() << "embObjMC BOARD " << _boardname << "Missing General group" ;
2397 return false;
2398 }
2399 Bottle xtmp;
2400 unsigned int i;
2401
2402 if(!extractGroup(general, xtmp, "useMotorSpeedFbk", "Use motor speed feedback", _njoints))
2403 {
2404 return false;
2405 }
2406
2407 for (i = 1; i < xtmp.size(); i++)
2408 {
2409 useMotorSpeedFbk[i-1] = xtmp.get(i).asInt32();
2410 }
2411
2412 return true;
2413
2414}
2415
2416bool Parser::parseImpedanceGroup(yarp::os::Searchable &config,std::vector<impedanceParameters_t> &impedance)
2417{
2418 Bottle impedanceGroup;
2419 impedanceGroup=config.findGroup("IMPEDANCE","IMPEDANCE parameters");
2420
2421 if(impedanceGroup.isNull())
2422 {
2423 yError() <<"embObjMC BOARD " << _boardname << "fromConfig(): Error: no IMPEDANCE group found in config file, returning";
2424 return false;
2425 }
2426
2427
2428
2429 if(_verbosewhenok)
2430 {
2431 yDebug() << "embObjMC BOARD " << _boardname << ":fromConfig() detected that IMPEDANCE parameters section is found";
2432 }
2433
2434 if(!checkAndSetVectorSize(impedance, _njoints, "parseImpedanceGroup"))
2435 return false;
2436
2437
2438 int j=0;
2439 Bottle xtmp;
2440 if (!extractGroup(impedanceGroup, xtmp, "stiffness", "stiffness parameter", _njoints))
2441 return false;
2442
2443 for (j=0; j<_njoints; j++)
2444 impedance[j].stiffness = xtmp.get(j+1).asFloat64();
2445
2446 if (!extractGroup(impedanceGroup, xtmp, "damping", "damping parameter", _njoints))
2447 return false;
2448
2449 for (j=0; j<_njoints; j++)
2450 impedance[j].damping = xtmp.get(j+1).asFloat64();
2451
2452 if(_verbosewhenok)
2453 {
2454 yInfo() << "embObjMC BOARD " << _boardname << "IMPEDANCE section: parameters successfully loaded";
2455 }
2456 return true;
2457
2458}
2459
2460/*
2461typedef struct
2462{
2463 double Km;
2464 double Kw;
2465 double S0;
2466 double S1;
2467 double Vth;
2468 double Fc_pos;
2469 double Fc_neg;
2470 double Fs_pos;
2471 double Fs_neg;
2472 double tbd0;
2473 double tbd1;
2474 double tbd2;
2475} lugreParameters_t;
2476*/
2477
2478bool Parser::parseLugreGroup(yarp::os::Searchable &config,std::vector<lugreParameters_t> &lugre)
2479{
2480 Bottle lugreGroup;
2481 lugreGroup=config.findGroup("LUGRE","LUGRE parameters");
2482
2483 if(lugreGroup.isNull())
2484 {
2485 for (int j = 0; j<_njoints; ++j)
2486 {
2487 lugre[j].Km = -1.0;
2488 }
2489
2490 yWarning() <<"embObjMC BOARD " << _boardname << "fromConfig(): Warning: no LUGRE group found in config file, returning";
2491 return true;
2492 }
2493
2494
2495
2496 if(_verbosewhenok)
2497 {
2498 yDebug() << "embObjMC BOARD " << _boardname << ":fromConfig() detected that LUGRE parameters section is found";
2499 }
2500
2501 if(!checkAndSetVectorSize(lugre, _njoints, "parseLugreGroup"))
2502 return false;
2503
2504
2505 int j=0;
2506 Bottle xtmp;
2507 if (!extractGroup(lugreGroup, xtmp, "Km", "torque constant parameter", _njoints))
2508 return false;
2509
2510 for (j=0; j<_njoints; j++)
2511 lugre[j].Km = xtmp.get(j+1).asFloat64();
2512
2513 if (!extractGroup(lugreGroup, xtmp, "Kw", "viscous friction parameter", _njoints))
2514 return false;
2515
2516 for (j=0; j<_njoints; j++)
2517 lugre[j].Kw = xtmp.get(j+1).asFloat64();
2518
2519 if (!extractGroup(lugreGroup, xtmp, "S0", "hysteresis position parameter", _njoints))
2520 return false;
2521
2522 for (j=0; j<_njoints; j++)
2523 lugre[j].S0 = xtmp.get(j+1).asFloat64();
2524
2525 if (!extractGroup(lugreGroup, xtmp, "S1", "hysteresis velocity parameter", _njoints))
2526 return false;
2527
2528 for (j=0; j<_njoints; j++)
2529 lugre[j].S1 = xtmp.get(j+1).asFloat64();
2530
2531 if (!extractGroup(lugreGroup, xtmp, "Vth", "velocity threshold parameter", _njoints))
2532 return false;
2533
2534 for (j=0; j<_njoints; j++)
2535 lugre[j].Vth = xtmp.get(j+1).asFloat64();
2536
2537 if (!extractGroup(lugreGroup, xtmp, "Fc_pos", "Coulomb force parameter (forward)", _njoints))
2538 return false;
2539
2540 for (j=0; j<_njoints; j++)
2541 lugre[j].Fc_pos = xtmp.get(j+1).asFloat64();
2542
2543 if (!extractGroup(lugreGroup, xtmp, "Fc_neg", "Coulomb force parameter (backward)", _njoints))
2544 return false;
2545
2546 for (j=0; j<_njoints; j++)
2547 lugre[j].Fc_neg = xtmp.get(j+1).asFloat64();
2548
2549 if (!extractGroup(lugreGroup, xtmp, "Fs_pos", "Stribeck force parameter (forward)", _njoints))
2550 return false;
2551
2552 for (j=0; j<_njoints; j++)
2553 lugre[j].Fs_pos = xtmp.get(j+1).asFloat64();
2554
2555 if (!extractGroup(lugreGroup, xtmp, "Fs_neg", "Stribeck force parameter (backward)", _njoints))
2556 return false;
2557
2558 for (j=0; j<_njoints; j++)
2559 lugre[j].Fs_neg = xtmp.get(j+1).asFloat64();
2560
2561 if(_verbosewhenok)
2562 {
2563 yInfo() << "embObjMC BOARD " << _boardname << "LUGRE section: parameters successfully loaded";
2564 }
2565 return true;
2566
2567}
2568
2569
2570bool Parser::parseMaintenanceModeGroup(yarp::os::Searchable &config, bool &skipRecalibrationEnabled)
2571{
2572 // Extract group MaintenanceModeGroup
2573 Bottle &maintenanceGroup=config.findGroup("MAINTENANCE");
2574 if (maintenanceGroup.isNull())
2575 {
2576 skipRecalibrationEnabled = false;
2577 return true;
2578 }
2579
2580 // Check skipRecalibrationEnabled = do not recalibrate joint at yarprobointerface restart! Used by low level motion controller
2581 Value skip_recalibration = maintenanceGroup.find("skipRecalibration");
2582 if (skip_recalibration.isNull())
2583 {
2584 skipRecalibrationEnabled = false;
2585 }
2586 else
2587 {
2588 if (!skip_recalibration.isBool())
2589 {
2590 yError() << "embObjMotionControl::open() detected that skipRecalibration bool param is different from accepted values (true / false). Assuming false";
2591 skipRecalibrationEnabled = false;
2592 }
2593 else
2594 {
2595 skipRecalibrationEnabled = skip_recalibration.asBool();
2596 if(skipRecalibrationEnabled)
2597 {
2598 yWarning() << "embObjMotionControl::open() detected that skipRecalibration is requested! Be careful See 'skipRecalibration' param in config file";
2599 yWarning() << "THE ROBOT WILL SKIP THE RECALIBRATION IN THIS CONFIGURATION!";
2600 }
2601 }
2602 }
2603 return true;
2604}
2605
2606bool Parser::convert(std::string const &fromstring, eOmc_jsetconstraint_t &jsetconstraint, bool& formaterror)
2607{
2608 const char *t = fromstring.c_str();
2609
2610 eObool_t usecompactstring = eobool_false;
2611 jsetconstraint = eomc_string2jsetconstraint(t, usecompactstring);
2612
2613 if(eomc_jsetconstraint_unknown == jsetconstraint)
2614 {
2615 usecompactstring = eobool_true;
2616 jsetconstraint = eomc_string2jsetconstraint(t, usecompactstring);
2617 }
2618
2619 if(eomc_jsetconstraint_unknown == jsetconstraint)
2620 {
2621 yError() << "embObjMC BOARD " << _boardname << "String" << t << "cannot be converted into a proper eOmc_jsetconstraint_t";
2622 formaterror = true;
2623 return false;
2624 }
2625
2626 return true;
2627}
2628
2629bool Parser::convert(Bottle &bottle, vector<double> &matrix, bool &formaterror, int targetsize)
2630{
2631 matrix.resize(0);
2632
2633 int tmp = bottle.size();
2634 int sizeofmatrix = tmp - 1; // first position of bottle contains the tag "matrix"
2635
2636 // check if there are really the target number of elements in matrix.
2637 if(targetsize != sizeofmatrix)
2638 {
2639 yError() << "embObjMC BOARD " << _boardname << " in converting string do matrix.In the matrix there are not" << targetsize << "elements";
2640 return false;
2641 }
2642
2643 formaterror = false;
2644 for(int i=0; i<sizeofmatrix; i++)
2645 {
2646 double item = 0;
2647
2648 // ok, i use the standard converter ... but what if it is not a double format? so far we dont check.
2649 item = bottle.get(i+1).asFloat64();
2650 matrix.push_back(item);
2651 }
2652
2653 // in here we could decide to return false if any previous conversion function has returned error
2654
2655 return true;
2656}
2657
2661void Parser::debugUtil_printControlLaws(void)
2662{
2664 yError() << "position control law: ";
2665 for(int x=0; x<_njoints; x++)
2666 {
2667 yError() << " - j " << x << _positionControlLaw[x].c_str();
2668 }
2669
2670 yError() << "velocity control law: ";
2671 for(int x=0; x<_njoints; x++)
2672 {
2673 yError() << "- j " << x << _velocityControlLaw[x].c_str();
2674 }
2675
2676
2677 yError() << "torque control law: ";
2678 for(int x=0; x<_njoints; x++)
2679 {
2680 yError() << " - j " << x << _torqueControlLaw[x].c_str();
2681 }
2683
2684}
2685
2686/*
2687void PidInfo::dumpdata(void)
2688{
2689
2690 cout << "Is enabled " << enabled;
2691 cout << ". Username pid selected is " << usernamePidSelected;
2692 switch(controlLaw)
2693 {
2694 case PidAlgo_simple:
2695 cout << ". Control law is " << "PidAlgo_simple";
2696 break;
2697
2698 case PIdAlgo_velocityInnerLoop:
2699 cout << ". Control law is " << "PIdAlgo_velocityInnerLoop";
2700 break;
2701
2702 case PidAlgo_currentInnerLoop:
2703 cout << ". Control law is " << "PidAlgo_currentInnerLoop";
2704 break;
2705 default :
2706 cout << ". Control law is " << "unknown";
2707 }
2708
2709 cout << ". PID fbk Unit type is " << (int)fbk_PidUnits;
2710 cout << ". PID out Unit type is " << (int)out_PidUnits;
2711
2712 cout << " kp is " << pid.kp;
2713 cout << endl;
2714
2715}
2716*/
2717void JointsSet::dumpdata(void)
2718{
2719 switch(cfg.constraints.type)
2720 {
2721 case eomc_jsetconstraint_none:
2722 cout << "constraint is " << "eomc_jsetconstraint_none";
2723 break;
2724 case eomc_jsetconstraint_cerhand:
2725 cout << "constraint is " << "eomc_jsetconstraint_cerhand";
2726 break;
2727 case eomc_jsetconstraint_trifid:
2728 cout << "constraint is " << "eomc_jsetconstraint_trifid";
2729 break;
2730 default :
2731 cout << ". constraint is " << "unknown";
2732 }
2733
2734 cout << " param1="<< cfg.constraints.param1 << " param2=" << cfg.constraints.param2 << endl;
2735
2736}
2737
2738bool Parser::checkJointTypes(PidInfo *pids, const std::string &pid_type)
2739{
2740 //Here we check that all joints have same type units in order to create pid_type helper with correct factor.
2741
2742 int firstjoint = -1;
2743
2744 // verify if pid type is torque or some other
2745 if(pid_type == "TORQUE")
2746 {
2747 // since we are working with a torque pid, we first cast it as such
2748 // this allows the loop to correctly point to the corresponding memory
2749 TrqPidInfo* trq_pids = (TrqPidInfo*) pids;
2750
2751 for(int i=0; i<_njoints; i++)
2752 {
2753 // if we already had an enabled PID, compare with current one
2754 if(firstjoint != -1 && !checkSinglePid(trq_pids[firstjoint], trq_pids[i], firstjoint, i, pid_type))
2755 {
2756 return false;
2757 }
2758 // if we haven't found an enabled PID yet, and this one is enabled, save it
2759 if(firstjoint == -1 && trq_pids[i].enabled)
2760 {
2761 firstjoint = i;
2762 }
2763 }
2764 }
2765 else
2766 {
2767 for(int i=0; i<_njoints; i++)
2768 {
2769 // if we already had an enabled PID, compare with current one
2770 if(firstjoint != -1 && !checkSinglePid(pids[firstjoint], pids[i], firstjoint, i, pid_type))
2771 {
2772 return false;
2773 }
2774 // if we haven't found an enabled PID yet, and this one is enabled, save it
2775 if(firstjoint == -1 && pids[i].enabled)
2776 {
2777 firstjoint = i;
2778 }
2779 }
2780 }
2781
2782 return true;
2783}
2784
2785bool Parser::checkSinglePid(PidInfo &firstPid, PidInfo &currentPid, const int &firstjoint, const int &currentjoint, const std::string &pid_type)
2786{
2787 // check if the PID we are checking is enabled
2788 if(currentPid.enabled)
2789 {
2790 // if it has different unit types from the previous enabled PIDs
2791 if(firstPid.fbk_PidUnits != currentPid.fbk_PidUnits ||
2792 firstPid.out_PidUnits != currentPid.out_PidUnits)
2793 {
2794 yError() << "embObjMC BOARD " << _boardname << "all joints with " << pid_type << " enabled should have same controlunits type. Joint " << firstjoint << " differs from joint " << currentjoint;
2795 return false;
2796 }
2797 }
2798 return true;
2799}
2800
2801
2802
2803
2804
yarp::dev::eomc::JointsSet::cfg
eOmc_jointset_configuration_t cfg
Definition eomcParser.h:427
yarp::dev::eomc::Parser::parseJointsetCfgGroup
bool parseJointsetCfgGroup(yarp::os::Searchable &config, std::vector< JointsSet > &jsets, std::vector< int > &jointtoset)
Definition eomcParser.cpp:1558
yarp::dev::eomc::Parser::parseJointsLimits
bool parseJointsLimits(yarp::os::Searchable &config, std::vector< jointLimits_t > &jointsLimits)
Definition eomcParser.cpp:1792
yarp::dev::eomc::Parser::parseMaintenanceModeGroup
bool parseMaintenanceModeGroup(yarp::os::Searchable &config, bool &skipRecalibrationEnabled)
Definition eomcParser.cpp:2570
yarp::dev::eomc::Parser::parseAmpsToSensor
bool parseAmpsToSensor(yarp::os::Searchable &config, double ampsToSensor[])
Definition eomcParser.cpp:2173
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:2391
yarp::dev::eomc::Parser::parseMotioncontrolVersion
bool parseMotioncontrolVersion(yarp::os::Searchable &config, int &version)
Definition eomcParser.cpp:1968
yarp::dev::eomc::Parser::parseLugreGroup
bool parseLugreGroup(yarp::os::Searchable &config, std::vector< lugreParameters_t > &lugre)
Definition eomcParser.cpp:2478
yarp::dev::eomc::Parser::parseAxisInfo
bool parseAxisInfo(yarp::os::Searchable &config, int axisMap[], std::vector< axisInfo_t > &axisInfo)
Definition eomcParser.cpp:2048
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:1912
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:1741
yarp::dev::eomc::Parser::parseDeadzoneValue
bool parseDeadzoneValue(yarp::os::Searchable &config, double deadzone[], bool *found)
Definition eomcParser.cpp:2256
yarp::dev::eomc::Parser::isVerboseEnabled
bool isVerboseEnabled(yarp::os::Searchable &config)
Definition eomcParser.cpp:1981
yarp::dev::eomc::Parser::parseTimeoutsGroup
bool parseTimeoutsGroup(yarp::os::Searchable &config, std::vector< timeouts_t > &timeouts, int defaultVelocityTimeout)
Definition eomcParser.cpp:1674
yarp::dev::eomc::Parser::parseCurrentLimits
bool parseCurrentLimits(yarp::os::Searchable &config, std::vector< motorCurrentLimits_t > &currLimits)
Definition eomcParser.cpp:1706
yarp::dev::eomc::Parser::parseEncoderFactor
bool parseEncoderFactor(yarp::os::Searchable &config, double encoderFactor[])
Definition eomcParser.cpp:2109
yarp::dev::eomc::Parser::parsefullscalePWM
bool parsefullscalePWM(yarp::os::Searchable &config, double dutycycleToPWM[])
Definition eomcParser.cpp:2140
yarp::dev::eomc::Parser::parseRotorsLimits
bool parseRotorsLimits(yarp::os::Searchable &config, std::vector< rotorLimits_t > &rotorsLimits)
Definition eomcParser.cpp:1865
yarp::dev::eomc::Parser::parseImpedanceGroup
bool parseImpedanceGroup(yarp::os::Searchable &config, std::vector< impedanceParameters_t > &impedance)
Definition eomcParser.cpp:2416
yarp::dev::eomc::Parser::parseKalmanFilterParams
bool parseKalmanFilterParams(yarp::os::Searchable &config, std::vector< kalmanFilterParams_t > &kalmanFilterParams)
Definition eomcParser.cpp:2292
yarp::dev::eomc::Parser::parseGearboxValues
bool parseGearboxValues(yarp::os::Searchable &config, double gearbox_M2J[], double gearbox_E2J[])
Definition eomcParser.cpp:2206
yarp::dev::eomc::Parser::parseBehaviourFalgs
bool parseBehaviourFalgs(yarp::os::Searchable &config, bool &useRawEncoderData, bool &pwmIsLimited)
Definition eomcParser.cpp:1997
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:481
yarp::dev::eomc::couplingInfo_t::matrixM2J
std::vector< double > matrixM2J
Definition eomcParser.h:483
yarp::dev::eomc::couplingInfo_t::matrixE2J
std::vector< double > matrixE2J
Definition eomcParser.h:484
yarp::dev::eomc::couplingInfo_t::matrixJ2M
std::vector< double > matrixJ2M
Definition eomcParser.h:482
yarp::dev::eomc::focBasedSpecificInfo_t
Definition eomcParser.h:409
yarp::dev::eomc::focBasedSpecificInfo_t::rotorIndexOffset
int rotorIndexOffset
Definition eomcParser.h:414
yarp::dev::eomc::focBasedSpecificInfo_t::hasTempSensor
bool hasTempSensor
Definition eomcParser.h:411
Generated on Tue May 6 2025 15:42:05 for iCub-main by doxygen 1.9.8