iCub-main
eomcParser.h
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  * Author: Valentina Gaggero
6  * email: valentina.gaggero@iit.it
7  * Permission is granted to copy, distribute, and/or modify this program
8  * under the terms of the GNU General Public License, version 2 or any
9  * later version published by the Free Software Foundation.
10  *
11  * A copy of the license can be found at
12  * http://www.robotcub.org/icub/license/gpl.txt
13  *
14  * This program is distributed in the hope that it will be useful, but
15  * WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
17  * Public License for more details
18  */
19 
20 #ifndef __mcParserh__
21 #define __mcParserh__
22 
23 
24 
25 #include <string>
26 #include <vector>
27 #include <array>
28 #include <map>
29 
30 // Yarp stuff
31 #include <yarp/os/Bottle.h>
32 //#include <yarp/os/Time.h>
33 #include <yarp/os/Bottle.h>
34 #include <yarp/dev/ControlBoardPid.h>
35 #include <yarp/dev/ControlBoardHelper.h>
36 
37 #include <yarp/dev/PidEnums.h>
38 #include <yarp/dev/ControlBoardInterfacesImpl.h>
39 
40 #include "EoMotionControl.h"
41 #include <yarp/os/LogStream.h>
42 
43 
44 // - public #define --------------------------------------------------------------------------------------------------
45 
46 
47 namespace yarp {
48  namespace dev {
49  namespace eomc {
50 
51 
52 //typedef enum
53 //{
54 // PidAlgo_simple = 0,
55 // PIdAlgo_velocityInnerLoop = 1,
56 // PidAlgo_currentInnerLoop =2
57 //} PidAlgorithmType_t;
58 
59 
61 {
62 public:
63  //PidAlgorithmType_t type;
64 
65  yarp::dev::PidFeedbackUnitsEnum fbk_PidUnits;
66  yarp::dev::PidOutputUnitsEnum out_PidUnits;
67  eOmc_ctrl_out_type_t out_type;
68 
69  virtual ~Pid_Algorithm(){}
70 
71  eOmc_ctrl_out_type_t getOutputType() { return out_type; }
72 
73  void setUnits(yarp::dev::PidFeedbackUnitsEnum fu, yarp::dev::PidOutputUnitsEnum ou)
74  {
75  fbk_PidUnits = fu;
76  out_PidUnits = ou;
77  }
78 
79  virtual yarp::dev::Pid getPID(int joint) = 0;
80 };
81 
82 
84 {
85 public:
86  yarp::dev::Pid *pid;
87 
88  Pid_Algorithm_simple(int nj, eOmc_ctrl_out_type_t ot)
89  {
90  out_type = ot;
91 
92  pid = new yarp::dev::Pid[nj];
93  };
94 
96  {
97  if (pid) delete[] pid;
98  };
99 
100  yarp::dev::Pid getPID(int joint) override
101  {
102  return pid[joint];
103  }
104 };
105 
106 /*
107 class PidAlgorithm_VelocityInnerLoop: public Pid_Algorithm
108 {
109 public:
110  yarp::dev::Pid *extPid; //pos, trq, velocity
111  //yarp::dev::Pid *innerVelPid;
112  PidAlgorithm_VelocityInnerLoop(int nj)
113  {
114  extPid = new yarp::dev::Pid[nj];
115  //innerVelPid = new yarp::dev::Pid[nj];
116  };
117  ~PidAlgorithm_VelocityInnerLoop()
118  {
119  if(extPid)
120  {
121  delete[]extPid;
122  }
123  //if(innerVelPid)
124  //{
125  // delete[]innerVelPid;
126  //}
127  }
128 
129  eOmc_ctrl_out_type_t getOutputType() override
130  {
131  return eomc_ctrl_out_type_vel;
132  }
133 };
134 
135 
136 class PidAlgorithm_CurrentInnerLoop: public Pid_Algorithm
137 {
138  public:
139  yarp::dev::Pid *extPid;
140  yarp::dev::Pid *innerCurrLoop;
141  PidAlgorithm_CurrentInnerLoop(int nj)
142  {
143  extPid = allocAndCheck<yarp::dev::Pid>(nj);
144  innerCurrLoop = allocAndCheck<yarp::dev::Pid>(nj);
145  };
146  ~PidAlgorithm_CurrentInnerLoop()
147  {
148  checkAndDestroy(extPid);
149  checkAndDestroy(innerCurrLoop);
150  }
151 
152  eOmc_ctrl_out_type_t getOutputType() override
153  {
154  return eomc_ctrl_out_type_cur;
155  }
156 };
157 */
158 
159 class PidInfo
160 {
161 public:
162 
163  yarp::dev::Pid pid;
164  yarp::dev::PidFeedbackUnitsEnum fbk_PidUnits;
165  yarp::dev::PidOutputUnitsEnum out_PidUnits;
166  eOmc_ctrl_out_type_t out_type;
167 
168  //PidAlgorithmType_t controlLaw;
169 
170  std::string usernamePidSelected;
171  bool enabled;
172 
173  PidInfo()//:usernamePidSelected("none")
174  {
175  enabled = false;
176  //controlLaw = PidAlgo_simple;
177 
178  out_type = eomc_ctrl_out_type_n_a;
179  fbk_PidUnits = yarp::dev::PidFeedbackUnitsEnum::RAW_MACHINE_UNITS;
180  out_PidUnits = yarp::dev::PidOutputUnitsEnum::RAW_MACHINE_UNITS;
181  }
183  {
184  //delete (usernamePidSelected);
185  }
186 
187  //void dumpdata(void);
188 
189 };
190 
191 class TrqPidInfo : public PidInfo
192 {
193 public:
194  double kbemf;
195  double ktau;
196  double viscousPos;
197  double viscousNeg;
198  double coulombPos;
199  double coulombNeg;
201 };
202 
203 
204 
205 typedef struct
206 {
213  bool hasSpeedEncoder ; //facoltativo
214  bool verbose;
216 
217 
219 {
220 public:
221  int id; //num of set. it can be between 0 and max number of joint (_njoints)
222  std::vector<int> joints; //list of joints belongig to this set2joint
223 
224  eOmc_jointset_configuration_t cfg;
225 
226  JointsSet(int num=0)
227  {
228  id=num;
229  joints.resize(0);
230  cfg.candotorquecontrol=0;
231  cfg.usespeedfeedbackfrommotors=0;
232  cfg.pidoutputtype=eomc_pidoutputtype_unknown;
233  cfg.dummy=0;
234  cfg.constraints.type=eomc_jsetconstraint_unknown;
235  cfg.constraints.param1=0;
236  cfg.constraints.param2=0;
237  }
238 public:
239  int getNumberofJoints(void) {return (joints.size());}
240  eOmc_jointset_configuration_t* getConfiguration(void) {return &cfg;}
241  void setUseSpeedFeedbackFromMotors(bool flag){cfg.usespeedfeedbackfrommotors = flag;}
242  void setPidOutputType(eOmc_pidoutputtype_t type){cfg.pidoutputtype = type;}
243  void setCanDoTorqueControl(bool flag) {cfg.candotorquecontrol = flag;}
244  void dumpdata();
245 };
246 
247 typedef struct
248 {
249  int velocity;
250 } timeouts_t;
251 
252 typedef struct
253 {
255  double peakCurrent;
258 
259 
260 typedef struct
261 {
262  double posMin;
263  double posMax;
264  double posHwMax;
265  double posHwMin;
266  double velMax;
267 } jointLimits_t;
268 
269 typedef struct
270 {
271  double posMin;
272  double posMax;
273  double pwmMax;
274 } rotorLimits_t;
275 
276 
277 typedef struct
278 {
279  std::vector<double> matrixJ2M;
280  std::vector<double> matrixM2J;
281  std::vector<double> matrixE2J;
283 
284 typedef struct
285 {
286  int mappedto;
287  std::string name;
288  JointTypeEnum type;
289 } axisInfo_t;
290 
291 typedef struct
292 {
293  double min_stiff;
294  double max_stiff;
295  double min_damp;
296  double max_damp;
297  double param_a;
298  double param_b;
299  double param_c;
301 
302 
303 typedef struct
304 {
305  double stiffness;
306  double damping;
309 
310 typedef struct
311 {
312  bool enabled;
313  std::array<float32_t, 3> x0;
314  std::array<float32_t, 3> Q;
315  float32_t R;
316  float32_t P0;
318 
319 //template <class T>
320 
321 class Parser
322 {
323 
324 private:
325  int _njoints;
326  std::string _boardname;
327  bool _verbosewhenok;
328 
329  std::map<std::string, Pid_Algorithm*> minjerkAlgoMap;
330  //std::map<std::string, Pid_Algorithm*> directAlgoMap;
331  std::map<std::string, Pid_Algorithm*> torqueAlgoMap;
332 
333  //std::map<std::string, Pid_Algorithm*> currentAlgoMap;
334  //std::map<std::string, Pid_Algorithm*> speedAlgoMap;
335 
336  std::vector<std::string> _positionControlLaw;
337  std::vector<std::string> _velocityControlLaw;
338  std::vector<std::string> _mixedControlLaw;
339  //std::vector<std::string> _posDirectControlLaw;
340  //std::vector<std::string> _velDirectControlLaw;
341  std::vector<std::string> _torqueControlLaw;
342  std::vector<std::string> _currentControlLaw;
343  std::vector<std::string> _speedControlLaw;
344 
345  double *_kbemf;
346  double *_ktau;
347  int *_filterType;
348  double *_viscousPos;
349  double *_viscousNeg;
350  double *_coulombPos;
351  double *_coulombNeg;
352 
353 
354 
355 
356 
357  //PID parsing functions
358  bool parseControlsGroup(yarp::os::Searchable &config);
359 
360  bool parseSelectedPositionControl(yarp::os::Searchable &config);
361  bool parseSelectedVelocityControl(yarp::os::Searchable &config);
362  bool parseSelectedMixedControl(yarp::os::Searchable &config);
363  //bool parseSelectedPosDirectControl(yarp::os::Searchable &config);
364  //bool parseSelectedVelDirectControl(yarp::os::Searchable &config);
365  bool parseSelectedTorqueControl(yarp::os::Searchable &config);
366 
367  bool parseSelectedCurrentPid(yarp::os::Searchable &config, bool pidisMandatory, PidInfo *pids);
368  bool parseSelectedSpeedPid(yarp::os::Searchable &config, bool pidisMandatory, PidInfo *pids);
369 
370  bool parsePid_minJerk_outPwm(yarp::os::Bottle &b_pid, std::string controlLaw);
371  bool parsePid_minJerk_outCur(yarp::os::Bottle &b_pid, std::string controlLaw);
372  bool parsePid_minJerk_outVel(yarp::os::Bottle &b_pid, std::string controlLaw);
373 
374  //bool parsePid_direct_outPwm(yarp::os::Bottle &b_pid, std::string controlLaw);
375  //bool parsePid_direct_outCur(yarp::os::Bottle &b_pid, std::string controlLaw);
376  //bool parsePid_direct_outVel(yarp::os::Bottle &b_pid, std::string controlLaw);
377 
378  bool parsePid_torque_outPwm(yarp::os::Bottle &b_pid, std::string controlLaw);
379  bool parsePid_torque_outCur(yarp::os::Bottle &b_pid, std::string controlLaw);
380  bool parsePid_torque_outVel(yarp::os::Bottle &b_pid, std::string controlLaw);
381 
382  bool parsePidsGroup2FOC(yarp::os::Bottle& pidsGroup, Pid myPid[]);
383  bool parsePidsGroupSimple(yarp::os::Bottle& pidsGroup, Pid myPid[]);
384  bool parsePidsGroupExtended(yarp::os::Bottle& pidsGroup, Pid myPid[]);
385  bool parsePidsGroupDeluxe(yarp::os::Bottle& pidsGroup, Pid myPid[]);
386 
387  bool parsePidsGroup(yarp::os::Bottle& pidsGroup, yarp::dev::Pid myPid[], std::string prefix);
388  bool getCorrectPidForEachJoint(PidInfo *ppids/*, PidInfo *vpids*/, TrqPidInfo *tpids);
389  bool parsePidUnitsType(yarp::os::Bottle &bPid, yarp::dev::PidFeedbackUnitsEnum &fbk_pidunits, yarp::dev::PidOutputUnitsEnum& out_pidunits);
390 
391  bool checkJointTypes(PidInfo *pids, const std::string &pid_type);
392  bool checkSinglePid(PidInfo &firstPid, PidInfo &currentPid, const int &firstjoint, const int &currentjoint, const std::string &pid_type);
393 
394  bool convert(std::string const &fromstring, eOmc_jsetconstraint_t &jsetconstraint, bool& formaterror);
395  bool convert(yarp::os::Bottle &bottle, std::vector<double> &matrix, bool &formaterror, int targetsize);
396 
397  //general utils functions
398  bool extractGroup(yarp::os::Bottle &input, yarp::os::Bottle &out, const std::string &key1, const std::string &txt, int size, bool mandatory=true);
399  template <class T>
400  bool checkAndSetVectorSize(std::vector<T> &vec, int size, const std::string &funcName)
401  {
402  if(size > (int)vec.capacity())
403  {
404  yError() << "embObjMC BOARD " << _boardname << " in " << funcName.c_str() << ": try to insert " << size << "element in vector with " << vec.capacity() << " elements";
405  return false;
406  }
407 
408  vec.resize(size);
409  return true;
410  }
411 
413  void debugUtil_printControlLaws(void);
414 
415 
416 public:
417  Parser(int numofjoints, std::string boardname);
418  ~Parser();
419 
420  bool parsePids(yarp::os::Searchable &config, PidInfo *ppids/*, PidInfo *vpids*/, TrqPidInfo *tpids, PidInfo *cpids, PidInfo *spids, bool lowLevPidisMandatory);
421  bool parseFocGroup(yarp::os::Searchable &config, focBasedSpecificInfo_t *foc_based_info, std::string groupName);
422  //bool parseCurrentPid(yarp::os::Searchable &config, PidInfo *cpids);//deprecated
423  bool parseJointsetCfgGroup(yarp::os::Searchable &config, std::vector<JointsSet> &jsets, std::vector<int> &jointtoset);
424  bool parseTimeoutsGroup(yarp::os::Searchable &config, std::vector<timeouts_t> &timeouts, int defaultVelocityTimeout);
425  bool parseCurrentLimits(yarp::os::Searchable &config, std::vector<motorCurrentLimits_t> &currLimits);
426  bool parseJointsLimits(yarp::os::Searchable &config, std::vector<jointLimits_t> &jointsLimits);
427  bool parseRotorsLimits(yarp::os::Searchable &config, std::vector<rotorLimits_t> &rotorsLimits);
428  bool parseCouplingInfo(yarp::os::Searchable &config, couplingInfo_t &couplingInfo);
429  bool parseMotioncontrolVersion(yarp::os::Searchable &config, int &version);
430  bool parseBehaviourFalgs(yarp::os::Searchable &config, bool &useRawEncoderData, bool &pwmIsLimited );
431  bool isVerboseEnabled(yarp::os::Searchable &config);
432  bool parseAxisInfo(yarp::os::Searchable &config, int axisMap[], std::vector<axisInfo_t> &axisInfo);
433  bool parseEncoderFactor(yarp::os::Searchable &config, double encoderFactor[]);
434  bool parsefullscalePWM(yarp::os::Searchable &config, double dutycycleToPWM[]);
435  bool parseAmpsToSensor(yarp::os::Searchable &config, double ampsToSensor[]);
436  bool parseGearboxValues(yarp::os::Searchable &config, double gearbox_M2J[], double gearbox_E2J[]);
437  bool parseMechanicalsFlags(yarp::os::Searchable &config, int useMotorSpeedFbk[]);
438  bool parseImpedanceGroup(yarp::os::Searchable &config,std::vector<impedanceParameters_t> &impedance);
439  bool parseDeadzoneValue(yarp::os::Searchable &config, double deadzone[], bool *found);
440  bool parseKalmanFilterParams(yarp::os::Searchable &config, std::vector<kalmanFilterParams_t> &kalmanFilterParams);
441 };
442 
443 }}}; //close namespaces
444 
445 #endif // include guard
void setPidOutputType(eOmc_pidoutputtype_t type)
Definition: eomcParser.h:242
eOmc_jointset_configuration_t cfg
Definition: eomcParser.h:224
void setUseSpeedFeedbackFromMotors(bool flag)
Definition: eomcParser.h:241
std::vector< int > joints
Definition: eomcParser.h:222
eOmc_jointset_configuration_t * getConfiguration(void)
Definition: eomcParser.h:240
void setCanDoTorqueControl(bool flag)
Definition: eomcParser.h:243
bool parseJointsetCfgGroup(yarp::os::Searchable &config, std::vector< JointsSet > &jsets, std::vector< int > &jointtoset)
bool parseJointsLimits(yarp::os::Searchable &config, std::vector< jointLimits_t > &jointsLimits)
bool parseAmpsToSensor(yarp::os::Searchable &config, double ampsToSensor[])
Parser(int numofjoints, std::string boardname)
Definition: eomcParser.cpp:38
bool parseMechanicalsFlags(yarp::os::Searchable &config, int useMotorSpeedFbk[])
bool parseMotioncontrolVersion(yarp::os::Searchable &config, int &version)
bool parseAxisInfo(yarp::os::Searchable &config, int axisMap[], std::vector< axisInfo_t > &axisInfo)
bool parseCouplingInfo(yarp::os::Searchable &config, couplingInfo_t &couplingInfo)
bool parsePids(yarp::os::Searchable &config, PidInfo *ppids, TrqPidInfo *tpids, PidInfo *cpids, PidInfo *spids, bool lowLevPidisMandatory)
Definition: eomcParser.cpp:77
bool parseFocGroup(yarp::os::Searchable &config, focBasedSpecificInfo_t *foc_based_info, std::string groupName)
bool parseDeadzoneValue(yarp::os::Searchable &config, double deadzone[], bool *found)
bool isVerboseEnabled(yarp::os::Searchable &config)
bool parseTimeoutsGroup(yarp::os::Searchable &config, std::vector< timeouts_t > &timeouts, int defaultVelocityTimeout)
bool parseCurrentLimits(yarp::os::Searchable &config, std::vector< motorCurrentLimits_t > &currLimits)
bool parseEncoderFactor(yarp::os::Searchable &config, double encoderFactor[])
bool parsefullscalePWM(yarp::os::Searchable &config, double dutycycleToPWM[])
bool parseRotorsLimits(yarp::os::Searchable &config, std::vector< rotorLimits_t > &rotorsLimits)
bool parseImpedanceGroup(yarp::os::Searchable &config, std::vector< impedanceParameters_t > &impedance)
bool parseKalmanFilterParams(yarp::os::Searchable &config, std::vector< kalmanFilterParams_t > &kalmanFilterParams)
bool parseGearboxValues(yarp::os::Searchable &config, double gearbox_M2J[], double gearbox_E2J[])
bool parseBehaviourFalgs(yarp::os::Searchable &config, bool &useRawEncoderData, bool &pwmIsLimited)
yarp::dev::Pid pid
Definition: eomcParser.h:163
std::string usernamePidSelected
Definition: eomcParser.h:170
yarp::dev::PidFeedbackUnitsEnum fbk_PidUnits
Definition: eomcParser.h:164
eOmc_ctrl_out_type_t out_type
Definition: eomcParser.h:166
yarp::dev::PidOutputUnitsEnum out_PidUnits
Definition: eomcParser.h:165
yarp::dev::Pid getPID(int joint) override
Definition: eomcParser.h:100
Pid_Algorithm_simple(int nj, eOmc_ctrl_out_type_t ot)
Definition: eomcParser.h:88
yarp::dev::PidFeedbackUnitsEnum fbk_PidUnits
Definition: eomcParser.h:65
eOmc_ctrl_out_type_t getOutputType()
Definition: eomcParser.h:71
void setUnits(yarp::dev::PidFeedbackUnitsEnum fu, yarp::dev::PidOutputUnitsEnum ou)
Definition: eomcParser.h:73
eOmc_ctrl_out_type_t out_type
Definition: eomcParser.h:67
yarp::dev::PidOutputUnitsEnum out_PidUnits
Definition: eomcParser.h:66
virtual yarp::dev::Pid getPID(int joint)=0
double viscousPos
motor torque constant
Definition: eomcParser.h:196
double ktau
back-emf compensation parameter
Definition: eomcParser.h:195
Copyright (C) 2008 RobotCub Consortium.
out
Definition: sine.m:8
std::vector< double > matrixM2J
Definition: eomcParser.h:280
std::vector< double > matrixE2J
Definition: eomcParser.h:281
std::vector< double > matrixJ2M
Definition: eomcParser.h:279
double posHwMax
user joint limits, min
Definition: eomcParser.h:264
double posHwMin
hardaware joint limits, max
Definition: eomcParser.h:265
double velMax
hardaware joint limits, min
Definition: eomcParser.h:266
double posMax
user joint limits, max
Definition: eomcParser.h:263
std::array< float32_t, 3 > x0
Definition: eomcParser.h:313
std::array< float32_t, 3 > Q
Definition: eomcParser.h:314