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