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