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