iCub-main
embObjMotionControl.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: Alberto Cardellino
6  * email: alberto.cardellino@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 
21 
22 
23 //
24 // $Id: embObjMotionControl.h,v 1.5 2008/06/25 22:33:53 nat Exp $
25 //
26 
27 #ifndef __embObjMotionControlh__
28 #define __embObjMotionControlh__
29 
30 
31 using namespace std;
32 
33 // system std include
34 #include <string>
35 #include <mutex>
36 #include <math.h>
37 // Yarp stuff
38 #include <yarp/os/Bottle.h>
39 #include <yarp/os/BufferedPort.h>
40 #include <yarp/os/Timer.h>
41 #include <yarp/dev/DeviceDriver.h>
42 #include <yarp/dev/PolyDriver.h>
43 #include <yarp/dev/ControlBoardHelper.h>
44 
45 #include <yarp/dev/IVirtualAnalogSensor.h>
46 
47 #include<yarp/dev/ImplementJointFault.h>
48 
49 #include <abstractEthResource.h>
50 
51 // local include
52 #include "IethResource.h"
53 #include"EoError.h"
54 #include <ethManager.h>
55 
56 #include "serviceParser.h"
57 #include "eomcParser.h"
58 #include "measuresConverter.h"
59 
60 #include "mcEventDownsampler.h"
61 
62 
63 #ifdef NETWORK_PERFORMANCE_BENCHMARK
64 #include <PeriodicEventsVerifier.h>
65 #endif
66 
67 // - public #define --------------------------------------------------------------------------------------------------
68 
69 #undef VERIFY_ROP_SETIMPEDANCE // this macro let you send setimpedence rop with signature.
70  // if you want use this feature, you should compiled ems firmware with same macro.
71 #undef VERIFY_ROP_SETPOSITIONRAW // this macro let you send setposition rop with signature.
72  // if you want use this feature, yuo should compiled ems firmware with same macro.
73 
74 // marco.accame: we dont manage mais anymore from the embObjMotionControl class.
75 // the mais is now initted by the ems board with default params (datarate = 10, mode = eoas_maismode_txdatacontinuously)
76 // and never swicthed off.
77 // only embObjAnalog can override its behaviour
78 
79 #define EMBOBJMC_DONT_USE_MAIS
80 
81 //
82 // helper structures
83 //
84 namespace yarp {
85  namespace dev {
86  namespace eomc {
87 
88 typedef struct
89 {
90  vector<int> joint2set;
91  vector <vector <int> > set2joint;
92  int numofjointsets;
93  vector<eOmc_jointset_configuration_t> jointset_cfgs;
94 } jointsetsInfo_t;
95 
96 typedef struct
97 {
98  eOmc_encoder_t type;
99  double tolerance;
100  int resolution;
101 } encoder_t;
102 
103 typedef struct
104 {
105  bool verbosewhenok;
106  bool useRawEncoderData;
107  bool pwmIsLimited;
108 }behaviour_flags_t;
109 
110 
111 class Watchdog
112 {
113 
114 private:
115 
116 bool _isStarted;
117 uint32_t _count;
118 double _time;
119 uint32_t _threshold; // use 10000 as limit on the watchdog for the error on the temperature sensor receiving of the values -
120  // since the ETH callback timing is 2ms by default so using 10000 we can set a checking threshould of 5 second
121  // in which we can allow the tdb to not respond. If cannot receive response over 1s we trigger the error
122 public:
123 
124 Watchdog(): _count(0), _isStarted(false), _threshold(60000), _time(0){;}
125 Watchdog(uint32_t threshold):_count(0), _isStarted(false), _threshold(threshold), _time(0){;}
126 ~Watchdog() = default;
127 Watchdog(const Watchdog& other) = default;
128 Watchdog(Watchdog&& other) noexcept = default;
129 Watchdog& operator=(const Watchdog& other) = default;
130 Watchdog& operator=(Watchdog&& other) noexcept = default;
131 
132 
133 bool isStarted(){return _isStarted;}
134 void start() {_count = 0; _time = yarp::os::Time::now(); _isStarted = true;}
135 bool isExpired() {return (_count > _threshold);}
136 void increment() {++_count;}
137 void clear(){_isStarted=false;}
138 double getStartTime() {return _time;}
139 uint32_t getCount() {return _count; }
140 void setThreshold(uint8_t txrateOfRegularROPs){ if(txrateOfRegularROPs != 0) _threshold = _threshold / txrateOfRegularROPs;}
141 uint32_t getThreshold(){return _threshold;}
142 
143 };
144 
145 class TemperatureFilter
146 {
147 private:
148  uint32_t _threshold; // threshold for the delta between current and previous temperature --> set to 20 Celsius deg by default --> over 20 deg delta spike
149  double _motorTempPrev; // motor temperature at previous instant for checking positive temperature spikes
150  bool _isStarted;
151  int32_t _initCounter;
152  std::vector<double> _initTempBuffer;
153 public:
154  TemperatureFilter(): _threshold(20), _isStarted(false), _initCounter(50), _initTempBuffer(0), _motorTempPrev(0){;}
155  TemperatureFilter(uint32_t threshold, int32_t initCounter): _threshold(threshold), _isStarted(false), _initCounter(initCounter), _initTempBuffer(0), _motorTempPrev(0){;}
156  ~TemperatureFilter() = default;
157  TemperatureFilter(const TemperatureFilter& other) = default;
158  TemperatureFilter(TemperatureFilter&& other) noexcept = default;
159  TemperatureFilter& operator=(const TemperatureFilter& other) = default;
160  TemperatureFilter& operator=(TemperatureFilter&& other) noexcept = default;
161 
162  bool isStarted(){return _isStarted;}
163  uint32_t getTemperatureThreshold() {return _threshold; }
164  double getPrevTemperature(){return _motorTempPrev;}
165  void updatePrevTemperature(double temperature){_motorTempPrev = temperature;}
166  void start(double temperature)
167  {
168  if(_initCounter < 0)
169  {
170  int median_pos = std::ceil(_initTempBuffer.size() / 2) -1;
171  _motorTempPrev = _initTempBuffer.at(median_pos);
172  _isStarted = true;
173  }
174  else
175  {
176  _initTempBuffer.push_back(temperature);
177  --_initCounter;
178  }
179 
180  }
181 };
182 
183 }}}
184 
185 namespace yarp {
186  namespace dev {
187  class embObjMotionControl;
188  }
189 }
190 
191 
192 struct MCdiagnostics
193 {
194  eOmn_serv_diagn_cfg_t config;
195  std::vector<BufferedPort<Bottle>*> ports;
196 };
197 
198 using namespace yarp::dev;
199 
200 
220 class yarp::dev::embObjMotionControl: public DeviceDriver,
221  public IPidControlRaw,
222  public IControlCalibrationRaw,
223  public IAmplifierControlRaw,
224  public IEncodersTimedRaw,
225  public IMotorEncodersRaw,
226  public ImplementEncodersTimed,
227  public ImplementMotorEncoders,
228  public IMotorRaw,
229  public ImplementMotor,
230  public IPositionControlRaw,
231  public IVelocityControlRaw,
232  public IControlModeRaw,
233  public ImplementControlMode,
234  public IControlLimitsRaw,
235  public IImpedanceControlRaw,
236  public ImplementImpedanceControl,
237  public ImplementControlLimits,
238  public ImplementAmplifierControl,
239  public ImplementPositionControl,
240  public ImplementControlCalibration,
241  public ImplementPidControl,
242  public ImplementVelocityControl,
243  public ITorqueControlRaw,
244  public ImplementTorqueControl,
245  public IVirtualAnalogSensor,
246  public IPositionDirectRaw,
247  public ImplementPositionDirect,
248  public IInteractionModeRaw,
249  public ImplementInteractionMode,
250  public IRemoteVariablesRaw,
251  public ImplementRemoteVariables,
252  public IAxisInfoRaw,
253  public ImplementAxisInfo,
254  public IPWMControlRaw,
255  public ImplementPWMControl,
256  public ICurrentControlRaw,
257  public ImplementCurrentControl,
258  public eth::IethResource,
259  public IJointFaultRaw,
260  public ImplementJointFault
261  {
262 private:
263  eth::TheEthManager* ethManager;
264  eth::AbstractEthResource* res;
265  ServiceParser* parser;
266  eomc::Parser * _mcparser;
267  ControlBoardHelper* _measureConverter;
268  std::mutex _mutex;
269 
270  bool opened; //internal state
271 
272  MCdiagnostics mcdiagnostics;
273 
275  int _njoints;
276  eomc::behaviour_flags_t behFlags;
277  servConfigMC_t serviceConfig;
278  double * _gearbox_M2J;
279  double * _gearbox_E2J;
280  double * _deadzone;
281  std::vector<eomc::kalmanFilterParams_t> _kalman_params;
283  std::vector<std::unique_ptr<eomc::ITemperatureSensor>> _temperatureSensorsVector;
284 
285  eomc::focBasedSpecificInfo_t * _foc_based_info;
286 
287  std::vector<eomc::encoder_t> _jointEncs;
288  std::vector<eomc::encoder_t> _motorEncs;
289 
290  std::vector<eomc::rotorLimits_t> _rotorsLimits;
291  std::vector<eomc::jointLimits_t> _jointsLimits;
292  std::vector<eomc::motorCurrentLimits_t> _currentLimits;
293  std::vector<eomc::temperatureLimits_t> _temperatureLimits;
294  eomc::couplingInfo_t _couplingInfo;
295  std::vector<eomc::JointsSet> _jsets;
296  std::vector<int> _joint2set;
297  std::vector<eomc::timeouts_t> _timeouts;
298 
299  std::vector<eomc::impedanceParameters_t> _impedance_params; // TODO doubled!!! optimize using just one of the 2!!!
300  eomc::impedanceLimits_t * _impedance_limits;
303  eomc::PidInfo * _trj_pids;
304  //eomc::PidInfo * _dir_pids;
305  eomc::TrqPidInfo * _trq_pids;
306  eomc::PidInfo * _cur_pids;
307  eomc::PidInfo * _spd_pids;
308 
309  int * _axisMap;
310  std::vector<eomc::axisInfo_t> _axesInfo;
312 
313  // event downsampler
314  mced::mcEventDownsampler* event_downsampler;
315 
316 #ifdef VERIFY_ROP_SETIMPEDANCE
317  uint32_t *impedanceSignature;
318 #endif
319 
320 #ifdef VERIFY_ROP_SETPOSITIONRAW
321  uint32_t *refRawSignature;
322  bool *sendingDirects;
323 #endif
324 
325 
326 
327  double SAFETY_THRESHOLD;
328 
329 
330  // internal stuff
331  bool *_enabledAmp; // Middle step toward a full enabled motor controller. Amp (pwm) plus Pid enable command must be sent in order to get the joint into an active state.
332  bool *_enabledPid; // Depends on enabledAmp. When both are set, the joint exits the idle mode and goes into position mode. If one of them is disabled, it falls to idle.
333  bool *_calibrated; // Flag to know if the calibrate function has been called for the joint
334  double *_ref_command_positions;// used for position control.
335  double *_ref_speeds; // used for position control.
336  double *_ref_command_speeds; // used for velocity control.
337  double *_ref_positions; // used for direct position control.
338  double *_ref_accs; // for velocity control, in position min jerk eq is used.
339  double *_encodersStamp;
340  bool *checking_motiondone; /* flag telling if I'm already waiting for motion done */
341  #define MAX_POSITION_MOVE_INTERVAL 0.080
342  double *_last_position_move_time;
343  eOmc_impedance_t *_cacheImpedance; /* cache impedance value to split up the 2 sets */
344  std::vector<yarp::dev::eomc::Watchdog> _temperatureSensorErrorWatchdog; /* counter used to filter error coming from tdb reading fromm 2FOC board*/
345  std::vector<yarp::dev::eomc::Watchdog> _temperatureExceededLimitWatchdog; /* counter used to filter the print of the exeded limits*/
346  std::vector<yarp::dev::eomc::TemperatureFilter> _temperatureSpikesFilter;
347 
348 #ifdef NETWORK_PERFORMANCE_BENCHMARK
349  Tools:Emb_RensponseTimingVerifier m_responseTimingVerifier;
350 #endif
351 
352 private:
353 
354  std::string getBoardInfo(void);
355  bool askRemoteValue(eOprotID32_t id32, void* value, uint16_t& size);
356  template <class T>
357  bool askRemoteValues(eOprotEndpoint_t ep, eOprotEntity_t entity, eOprotTag_t tag, std::vector<T>& values);
358  bool checkRemoteControlModeStatus(int joint, int target_mode);
359 
360  bool dealloc();
361 
362 
363  bool verifyUserControlLawConsistencyInJointSet(eomc::PidInfo *ipdInfo);
364  bool verifyUserControlLawConsistencyInJointSet(eomc::TrqPidInfo *pidInfo);
365  bool verifyTorquePidshasSameUnitTypes(yarp::dev::PidFeedbackUnitsEnum &fbk_pidunits, yarp::dev::PidOutputUnitsEnum& out_pidunits);
366  bool verifyUseMotorSpeedFbkInJointSet(int useMotorSpeedFbk []);
367  bool updatedJointsetsCfgWithControlInfo(void);
368  bool saveCouplingsData(void);
369  void debugUtil_printJointsetInfo(void);
370 
371  bool isTorqueControlEnabled(int joint);
372  bool isVelocityControlEnabled(int joint);
373 
374  bool iNeedCouplingsInfo(void); //the device needs coupling info if it manages joints controlled by 2foc and mc4plus.
375 
376  bool getJointConfiguration(int joint, eOmc_joint_config_t *jntCfg_ptr);
377  bool getMotorConfiguration(int axis, eOmc_motor_config_t *motCfg_ptr);
378  bool getGerabox_E2J(int joint, double *gearbox_E2J_ptr);
379  bool getJointEncTolerance(int joint, double *jEncTolerance_ptr);
380  bool getMotorEncTolerance(int axis, double *mEncTolerance_ptr);
381  void updateDeadZoneWithDefaultValues(void);
382  bool getJointDeadZoneRaw(int j, double &jntDeadZone);
383 
384 private:
385 
386  //functions used in init this object
387  bool fromConfig(yarp::os::Searchable &config);
388  int fromConfig_NumOfJoints(yarp::os::Searchable &config);
389  bool fromConfig_getGeneralInfo(yarp::os::Searchable &config); //get general info: useRawEncoderData, useLiitedPwm, etc....
390  bool fromConfig_Step2(yarp::os::Searchable &config);
391  bool fromConfig_readServiceCfg(yarp::os::Searchable &config);
392  bool initializeInterfaces(measureConvFactors &f);
393  bool alloc(int njoints);
394  bool init(void);
395 
396  //function used in the closing this object
397  void cleanup(void);
398 
399  //used in pid interface
400  bool helper_setPosPidRaw( int j, const Pid &pid);
401  bool helper_getPosPidRaw(int j, Pid *pid);
402  bool helper_getPosPidsRaw(Pid *pid);
403 
404  //used in torque control interface
405  bool helper_setTrqPidRaw( int j, const Pid &pid);
406  bool helper_getTrqPidRaw(int j, Pid *pid);
407  bool helper_getTrqPidsRaw(Pid *pid);
408 
409  //used in velocity control interface
410  bool helper_setVelPidRaw( int j, const Pid &pid);
411  bool helper_getVelPidRaw(int j, Pid *pid);
412  bool helper_getVelPidsRaw(Pid *pid);
413 
414  //used in current control interface
415  bool helper_setCurPidRaw(int j, const Pid &pid);
416  bool helper_getCurPidRaw(int j, Pid *pid);
417  bool helper_getCurPidsRaw(Pid *pid);
418 
419  //used in low level speed control interface
420  bool helper_setSpdPidRaw(int j, const Pid &pid);
421  bool helper_getSpdPidRaw(int j, Pid *pid);
422  bool helper_getSpdPidsRaw(Pid *pid);
423 
424  bool checkCalib14RotationParam(int32_t calib_param4);
425 
426 public:
427 
428  embObjMotionControl();
429  ~embObjMotionControl();
430 
431 
432  // Device Driver
433  virtual bool open(yarp::os::Searchable &par);
434  virtual bool close();
435 
436  virtual bool initialised();
437  virtual eth::iethresType_t type();
438  virtual bool update(eOprotID32_t id32, double timestamp, void *rxdata);
439  virtual bool getEntityName(uint32_t entityId, std::string &entityName);
440 
442  virtual bool setPidRaw(const PidControlTypeEnum& pidtype, int j, const Pid &pid) override;
443  virtual bool setPidsRaw(const PidControlTypeEnum& pidtype, const Pid *pids) override;
444  virtual bool setPidReferenceRaw(const PidControlTypeEnum& pidtype, int j, double ref) override;
445  virtual bool setPidReferencesRaw(const PidControlTypeEnum& pidtype, const double *refs) override;
446  virtual bool setPidErrorLimitRaw(const PidControlTypeEnum& pidtype, int j, double limit) override;
447  virtual bool setPidErrorLimitsRaw(const PidControlTypeEnum& pidtype, const double *limits) override;
448  virtual bool getPidErrorRaw(const PidControlTypeEnum& pidtype, int j, double *err) override;
449  virtual bool getPidErrorsRaw(const PidControlTypeEnum& pidtype, double *errs) override;
450  virtual bool getPidOutputRaw(const PidControlTypeEnum& pidtype, int j, double *out) override;
451  virtual bool getPidOutputsRaw(const PidControlTypeEnum& pidtype, double *outs) override;
452  virtual bool getPidRaw(const PidControlTypeEnum& pidtype, int j, Pid *pid) override;
453  virtual bool getPidsRaw(const PidControlTypeEnum& pidtype, Pid *pids) override;
454  virtual bool getPidReferenceRaw(const PidControlTypeEnum& pidtype, int j, double *ref) override;
455  virtual bool getPidReferencesRaw(const PidControlTypeEnum& pidtype, double *refs) override;
456  virtual bool getPidErrorLimitRaw(const PidControlTypeEnum& pidtype, int j, double *limit) override;
457  virtual bool getPidErrorLimitsRaw(const PidControlTypeEnum& pidtype, double *limits) override;
458  virtual bool resetPidRaw(const PidControlTypeEnum& pidtype, int j) override;
459  virtual bool disablePidRaw(const PidControlTypeEnum& pidtype, int j) override;
460  virtual bool enablePidRaw(const PidControlTypeEnum& pidtype, int j) override;
461  virtual bool setPidOffsetRaw(const PidControlTypeEnum& pidtype, int j, double v) override;
462  virtual bool isPidEnabledRaw(const PidControlTypeEnum& pidtype, int j, bool* enabled) override;
463 
464  // POSITION CONTROL INTERFACE RAW
465  virtual bool getAxes(int *ax) override;
466  virtual bool positionMoveRaw(int j, double ref) override;
467  virtual bool positionMoveRaw(const double *refs) override;
468  virtual bool relativeMoveRaw(int j, double delta) override;
469  virtual bool relativeMoveRaw(const double *deltas) override;
470  virtual bool checkMotionDoneRaw(bool *flag) override;
471  virtual bool checkMotionDoneRaw(int j, bool *flag) override;
472  virtual bool setRefSpeedRaw(int j, double sp) override;
473  virtual bool setRefSpeedsRaw(const double *spds) override;
474  virtual bool setRefAccelerationRaw(int j, double acc) override;
475  virtual bool setRefAccelerationsRaw(const double *accs) override;
476  virtual bool getRefSpeedRaw(int j, double *ref) override;
477  virtual bool getRefSpeedsRaw(double *spds) override;
478  virtual bool getRefAccelerationRaw(int j, double *acc) override;
479  virtual bool getRefAccelerationsRaw(double *accs) override;
480  virtual bool stopRaw(int j) override;
481  virtual bool stopRaw() override;
482 
483 
484  // Position Control2 Interface
485  virtual bool positionMoveRaw(const int n_joint, const int *joints, const double *refs) override;
486  virtual bool relativeMoveRaw(const int n_joint, const int *joints, const double *deltas) override;
487  virtual bool checkMotionDoneRaw(const int n_joint, const int *joints, bool *flags) override;
488  virtual bool setRefSpeedsRaw(const int n_joint, const int *joints, const double *spds) override;
489  virtual bool setRefAccelerationsRaw(const int n_joint, const int *joints, const double *accs) override;
490  virtual bool getRefSpeedsRaw(const int n_joint, const int *joints, double *spds) override;
491  virtual bool getRefAccelerationsRaw(const int n_joint, const int *joints, double *accs) override;
492  virtual bool stopRaw(const int n_joint, const int *joints) override;
493  virtual bool getTargetPositionRaw(const int joint, double *ref) override;
494  virtual bool getTargetPositionsRaw(double *refs) override;
495  virtual bool getTargetPositionsRaw(const int n_joint, const int *joints, double *refs) override;
496 
497  // Velocity control interface raw
498  virtual bool velocityMoveRaw(int j, double sp) override;
499  virtual bool velocityMoveRaw(const double *sp) override;
500 
501 
502  // calibration2raw
503  virtual bool setCalibrationParametersRaw(int axis, const CalibrationParameters& params) override;
504  virtual bool calibrateAxisWithParamsRaw(int axis, unsigned int type, double p1, double p2, double p3) override;
505  virtual bool calibrationDoneRaw(int j) override;
506 
507 
509 
510  // ControlMode
511  virtual bool getControlModeRaw(int j, int *v) override;
512  virtual bool getControlModesRaw(int *v) override;
513 
514  // ControlMode 2
515  virtual bool getControlModesRaw(const int n_joint, const int *joints, int *modes) override;
516  virtual bool setControlModeRaw(const int j, const int mode) override;
517  virtual bool setControlModesRaw(const int n_joint, const int *joints, int *modes) override;
518  virtual bool setControlModesRaw(int *modes) override;
519 
521  virtual bool resetEncoderRaw(int j) override;
522  virtual bool resetEncodersRaw() override;
523  virtual bool setEncoderRaw(int j, double val) override;
524  virtual bool setEncodersRaw(const double *vals) override;
525  virtual bool getEncoderRaw(int j, double *v) override;
526  virtual bool getEncodersRaw(double *encs) override;
527  virtual bool getEncoderSpeedRaw(int j, double *sp) override;
528  virtual bool getEncoderSpeedsRaw(double *spds) override;
529  virtual bool getEncoderAccelerationRaw(int j, double *spds) override;
530  virtual bool getEncoderAccelerationsRaw(double *accs) override;
532 
533  virtual bool getEncodersTimedRaw(double *encs, double *stamps) override;
534  virtual bool getEncoderTimedRaw(int j, double *encs, double *stamp) override;
535 
537  virtual bool getNumberOfMotorEncodersRaw(int * num) override;
538  virtual bool resetMotorEncoderRaw(int m) override;
539  virtual bool resetMotorEncodersRaw() override;
540  virtual bool setMotorEncoderRaw(int m, const double val) override;
541  virtual bool setMotorEncodersRaw(const double *vals) override;
542  virtual bool getMotorEncoderRaw(int m, double *v) override;
543  virtual bool getMotorEncodersRaw(double *encs) override;
544  virtual bool getMotorEncoderSpeedRaw(int m, double *sp) override;
545  virtual bool getMotorEncoderSpeedsRaw(double *spds) override;
546  virtual bool getMotorEncoderAccelerationRaw(int m, double *spds) override;
547  virtual bool getMotorEncoderAccelerationsRaw(double *accs) override;
548  virtual bool getMotorEncodersTimedRaw(double *encs, double *stamps) override;
549  virtual bool getMotorEncoderTimedRaw(int m, double *encs, double *stamp) override;
550  virtual bool getMotorEncoderCountsPerRevolutionRaw(int m, double *v) override;
551  virtual bool setMotorEncoderCountsPerRevolutionRaw(int m, const double cpr) override;
553 
555  virtual bool getRemoteVariableRaw(std::string key, yarp::os::Bottle& val) override;
556  virtual bool setRemoteVariableRaw(std::string key, const yarp::os::Bottle& val) override;
557  virtual bool getRemoteVariablesListRaw(yarp::os::Bottle* listOfKeys) override;
559 
561  virtual bool getAxisNameRaw(int axis, std::string& name) override;
562  virtual bool getJointTypeRaw(int axis, yarp::dev::JointTypeEnum& type) override;
564 
565  //Internal use, not exposed by Yarp (yet)
566  bool getRotorEncoderResolutionRaw(int m, double &rotres) ;
567  bool getJointEncoderResolutionRaw(int m, double &jntres) ;
568  bool getJointEncoderTypeRaw(int j, int &type) ;
569  bool getRotorEncoderTypeRaw(int j, int &type) ;
570  bool getKinematicMJRaw(int j, double &rotres) ;
571  bool getTemperatureSensorTypeRaw(int j, std::string& ret) ;
572  bool getHasTempSensorsRaw(int j, int& ret) ;
573  bool getHasHallSensorRaw(int j, int& ret) ;
574  bool getHasRotorEncoderRaw(int j, int& ret) ;
575  bool getHasRotorEncoderIndexRaw(int j, int& ret) ;
576  bool getMotorPolesRaw(int j, int& poles) ;
577  bool getRotorIndexOffsetRaw(int j, double& rotorOffset) ;
578  bool getTorqueControlFilterType(int j, int& type) ;
579  bool getRotorLimitsRaw(int j, double *rotorMin, double *rotorMax) ;
580  bool getWholeImpedanceRaw(int j, eOmc_impedance_t &imped);
581 
583  virtual bool enableAmpRaw(int j) override;
584  virtual bool disableAmpRaw(int j) override;
585  virtual bool getCurrentsRaw(double *vals) override;
586  virtual bool getCurrentRaw(int j, double *val) override;
587  virtual bool setMaxCurrentRaw(int j, double val) override;
588  virtual bool getMaxCurrentRaw(int j, double *val) override;
589  virtual bool getAmpStatusRaw(int *st) override;
590  virtual bool getAmpStatusRaw(int j, int *st) override;
591  virtual bool getPWMRaw(int j, double* val) override;
592  virtual bool getPWMLimitRaw(int j, double* val) override;
593  virtual bool setPWMLimitRaw(int j, const double val) override;
594  virtual bool getPowerSupplyVoltageRaw(int j, double* val) override;
595 
597 
598  // virtual analog sensor
599  virtual yarp::dev::VAS_status getVirtualAnalogSensorStatus(int ch) override;
600  virtual int getVirtualAnalogSensorChannels() override;
601  virtual bool updateVirtualAnalogSensorMeasure(yarp::sig::Vector &fTorques) override;
602  virtual bool updateVirtualAnalogSensorMeasure(int j, double &fTorque) override;
603 
604 #ifdef IMPLEMENT_DEBUG_INTERFACE
605  //----------------------------------------------
606  // Debug interface
607  //----------------------------------------------
608  virtual bool setParameterRaw(int j, unsigned int type, double value) override;
609  virtual bool getParameterRaw(int j, unsigned int type, double *value) override;
610  virtual bool setDebugParameterRaw(int j, unsigned int index, double value) override;
611  virtual bool setDebugReferencePositionRaw(int j, double value) override;
612  virtual bool getDebugParameterRaw(int j, unsigned int index, double *value) override;
613  virtual bool getDebugReferencePositionRaw(int j, double *value) override;
614 #endif
615 
616  // Limits
617  virtual bool setLimitsRaw(int axis, double min, double max) override;
618  virtual bool getLimitsRaw(int axis, double *min, double *max) override;
619  // Limits 2
620  virtual bool setVelLimitsRaw(int axis, double min, double max) override;
621  virtual bool getVelLimitsRaw(int axis, double *min, double *max) override;
622 
623  // Torque control
624  virtual bool getTorqueRaw(int j, double *t) override;
625  virtual bool getTorquesRaw(double *t) override;
626  virtual bool getTorqueRangeRaw(int j, double *min, double *max) override;
627  virtual bool getTorqueRangesRaw(double *min, double *max) override;
628  virtual bool setRefTorquesRaw(const double *t) override;
629  virtual bool setRefTorqueRaw(int j, double t) override;
630  virtual bool setRefTorquesRaw(const int n_joint, const int *joints, const double *t) override;
631  virtual bool getRefTorquesRaw(double *t) override;
632  virtual bool getRefTorqueRaw(int j, double *t) override;
633  virtual bool getMotorTorqueParamsRaw(int j, MotorTorqueParameters *params) override;
634  virtual bool setMotorTorqueParamsRaw(int j, const MotorTorqueParameters params) override;
635 
636 
637  // IVelocityControl2
638  virtual bool velocityMoveRaw(const int n_joint, const int *joints, const double *spds) override;
639  virtual bool getRefVelocityRaw(const int joint, double *ref) override;
640  virtual bool getRefVelocitiesRaw(double *refs) override;
641  virtual bool getRefVelocitiesRaw(const int n_joint, const int *joints, double *refs) override;
642 
643 
644  // Impedance interface
645  virtual bool getImpedanceRaw(int j, double *stiffness, double *damping) override;
646  virtual bool setImpedanceRaw(int j, double stiffness, double damping) override;
647  virtual bool setImpedanceOffsetRaw(int j, double offset) override;
648  virtual bool getImpedanceOffsetRaw(int j, double *offset) override;
649  virtual bool getCurrentImpedanceLimitRaw(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp) override;
650 
651  // PositionDirect Interface
652  virtual bool setPositionRaw(int j, double ref) override;
653  virtual bool setPositionsRaw(const int n_joint, const int *joints, const double *refs) override;
654  virtual bool setPositionsRaw(const double *refs) override;
655  virtual bool getRefPositionRaw(const int joint, double *ref) override;
656  virtual bool getRefPositionsRaw(double *refs) override;
657  virtual bool getRefPositionsRaw(const int n_joint, const int *joints, double *refs) override;
658 
659  // InteractionMode interface
660  virtual bool getInteractionModeRaw(int j, yarp::dev::InteractionModeEnum* _mode) override;
661  virtual bool getInteractionModesRaw(int n_joints, int *joints, yarp::dev::InteractionModeEnum* modes) override;
662  virtual bool getInteractionModesRaw(yarp::dev::InteractionModeEnum* modes) override;
663  virtual bool setInteractionModeRaw(int j, yarp::dev::InteractionModeEnum _mode) override;
664  virtual bool setInteractionModesRaw(int n_joints, int *joints, yarp::dev::InteractionModeEnum* modes) override;
665  virtual bool setInteractionModesRaw(yarp::dev::InteractionModeEnum* modes) override;
666 
667  // IMotor interface
668  virtual bool getNumberOfMotorsRaw(int * num) override;
669  virtual bool getTemperatureRaw(int m, double* val) override;
670  virtual bool getTemperaturesRaw(double *vals) override;
671  virtual bool getTemperatureLimitRaw(int m, double *temp) override;
672  virtual bool setTemperatureLimitRaw(int m, const double temp) override;
673  virtual bool getPeakCurrentRaw(int m, double *val) override;
674  virtual bool setPeakCurrentRaw(int m, const double val) override;
675  virtual bool getNominalCurrentRaw(int m, double *val) override;
676  virtual bool setNominalCurrentRaw(int m, const double val) override;
677  virtual bool getGearboxRatioRaw(int m, double *gearbox) override;
678 
679  // PWMControl
680  virtual bool setRefDutyCycleRaw(int j, double v) override;
681  virtual bool setRefDutyCyclesRaw(const double *v) override;
682  virtual bool getRefDutyCycleRaw(int j, double *v) override;
683  virtual bool getRefDutyCyclesRaw(double *v) override;
684  virtual bool getDutyCycleRaw(int j, double *v) override;
685  virtual bool getDutyCyclesRaw(double *v) override;
686 
687  // CurrentControl
688  // virtual bool getAxes(int *ax) override;
689  //virtual bool getCurrentRaw(int j, double *t) override;
690  //virtual bool getCurrentsRaw(double *t) override;
691  virtual bool getCurrentRangeRaw(int j, double *min, double *max) override;
692  virtual bool getCurrentRangesRaw(double *min, double *max) override;
693  virtual bool setRefCurrentsRaw(const double *t) override;
694  virtual bool setRefCurrentRaw(int j, double t) override;
695  virtual bool setRefCurrentsRaw(const int n_joint, const int *joints, const double *t) override;
696  virtual bool getRefCurrentsRaw(double *t) override;
697  virtual bool getRefCurrentRaw(int j, double *t) override;
698 
699  // Used in joint faults interface
700  // Teturns true if it was successful and writes the fault code in the fault parameter
701  // with the associated string in message. If no fault is detected the fault parameters is set to -1.
702  // Returns false and fault is set to -2 if retrieval was unsuccessful.
703  virtual bool getLastJointFaultRaw(int j, int& fault, std::string& message) override;
704 
705 };
706 
707 #endif // include guard
yarp::dev::embObjMotionControl
embObjMotionControl : driver for iCub motor control boards EMS on a ETH bus.
Definition: embObjMotionControl.h:261
yarp::dev::eomc::TemperatureFilter::updatePrevTemperature
void updatePrevTemperature(double temperature)
Definition: embObjMotionControl.h:165
yarp::dev::eomc::TemperatureFilter::TemperatureFilter
TemperatureFilter(TemperatureFilter &&other) noexcept=default
yarp::dev::eomc::TemperatureFilter::TemperatureFilter
TemperatureFilter(const TemperatureFilter &other)=default
yarp::dev::eomc::TemperatureFilter::TemperatureFilter
TemperatureFilter(uint32_t threshold, int32_t initCounter)
Definition: embObjMotionControl.h:155
yarp::dev::eomc::TemperatureFilter::operator=
TemperatureFilter & operator=(TemperatureFilter &&other) noexcept=default
yarp::dev::eomc::TemperatureFilter::operator=
TemperatureFilter & operator=(const TemperatureFilter &other)=default
yarp::dev::eomc::Watchdog::setThreshold
void setThreshold(uint8_t txrateOfRegularROPs)
Definition: embObjMotionControl.h:140
yarp::dev::eomc::Watchdog::Watchdog
Watchdog(uint32_t threshold)
Definition: embObjMotionControl.h:125
yarp::dev::eomc::Watchdog::operator=
Watchdog & operator=(Watchdog &&other) noexcept=default
yarp::dev::eomc::Watchdog::Watchdog
Watchdog(const Watchdog &other)=default
yarp::dev::eomc::Watchdog::operator=
Watchdog & operator=(const Watchdog &other)=default
yarp::dev::eomc::Watchdog::Watchdog
Watchdog(Watchdog &&other) noexcept=default
f
f
Definition: compute_ekf_sym.m:22
acc
_3f_vect_t acc
Definition: dataTypes.h:1
Tools
In the Tools namespace there are classes useful to check some kinds of performance on robot.
Definition: PeriodicEventsVerifier.h:32
eth::iethresType_t
iethresType_t
Definition: IethResource.h:62
strain::dsp::fsc::max
const FSC max
Definition: strain.h:48
strain::dsp::fsc::min
const FSC min
Definition: strain.h:49
yarp
Copyright (C) 2008 RobotCub Consortium.
Definition: DebugInterfaces.h:51
out
out
Definition: sine.m:8
offset
degrees offset
Definition: sine.m:4
MCdiagnostics::ports
std::vector< BufferedPort< Bottle > * > ports
Definition: embObjMotionControl.h:195
MCdiagnostics::config
eOmn_serv_diagn_cfg_t config
Definition: embObjMotionControl.h:194
servConfigMC_t
Definition: serviceParser.h:89
yarp::dev::eomc::behaviour_flags_t
Definition: embObjMotionControl.h:104
yarp::dev::eomc::behaviour_flags_t::verbosewhenok
bool verbosewhenok
Definition: embObjMotionControl.h:105
yarp::dev::eomc::behaviour_flags_t::useRawEncoderData
bool useRawEncoderData
its value depends on environment variable "ETH_VERBOSEWHENOK"
Definition: embObjMotionControl.h:106
yarp::dev::eomc::behaviour_flags_t::pwmIsLimited
bool pwmIsLimited
if true than do not use calibration data
Definition: embObjMotionControl.h:107
yarp::dev::eomc::couplingInfo_t
Definition: eomcParser.h:481
yarp::dev::eomc::encoder_t
Definition: embObjMotionControl.h:97
yarp::dev::eomc::encoder_t::resolution
int resolution
Num of error bits passable for joint encoder.
Definition: embObjMotionControl.h:100
yarp::dev::eomc::encoder_t::tolerance
double tolerance
joint encoder type
Definition: embObjMotionControl.h:99
yarp::dev::eomc::encoder_t::type
eOmc_encoder_t type
Definition: embObjMotionControl.h:98
yarp::dev::eomc::focBasedSpecificInfo_t
Definition: eomcParser.h:409
yarp::dev::eomc::impedanceLimits_t
Definition: eomcParser.h:495
yarp::dev::eomc::jointsetsInfo_t
Definition: embObjMotionControl.h:89
yarp::dev::eomc::jointsetsInfo_t::jointset_cfgs
vector< eOmc_jointset_configuration_t > jointset_cfgs
Definition: embObjMotionControl.h:93
yarp::dev::eomc::jointsetsInfo_t::set2joint
vector< vector< int > > set2joint
Definition: embObjMotionControl.h:91
yarp::dev::eomc::jointsetsInfo_t::numofjointsets
int numofjointsets
Definition: embObjMotionControl.h:92
yarp::dev::eomc::jointsetsInfo_t::joint2set
vector< int > joint2set
Definition: embObjMotionControl.h:90
Generated on Fri Jul 26 2024 09:37:01 for iCub-main by doxygen 1.9.1