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