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