YARP
Yet Another Robot Platform
fakeMotionControl.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2019 Istituto Italiano di Tecnologia (IIT)
3  * All rights reserved.
4  *
5  * This software may be modified and distributed under the terms of the
6  * BSD-3-Clause license. See the accompanying LICENSE file for details.
7  */
8 
9 /*
10  * The aim of this device is to mimic the expected behaviour of a
11  * real motion control device to help testing the high level software.
12  *
13  * This device is implementing last version of interfaces and it is compatible
14  * with ControlBoardWrapper2 device.
15  *
16  * WIP - it is very basic now, not all interfaces are implemented yet.
17  */
18 
19 #ifndef YARP_DEVICE_FAKE_MOTIONCONTROL
20 #define YARP_DEVICE_FAKE_MOTIONCONTROL
21 
22 #include <yarp/os/Time.h>
23 #include <yarp/os/Bottle.h>
24 #include <yarp/sig/Vector.h>
25 #include <yarp/os/PeriodicThread.h>
26 #include <yarp/dev/DeviceDriver.h>
31 
32 #include <mutex>
33 
34 
36 {
37  double min_stiff;
38  double max_stiff;
39  double min_damp;
40  double max_damp;
41  double param_a;
42  double param_b;
43  double param_c;
44 
45 public:
47  {
48  min_stiff=0;
49  max_stiff=0;
50  min_damp=0;
51  max_damp=0;
52  param_a=0;
53  param_b=0;
54  param_c=0;
55  }
56 
57  double get_min_stiff()
58  {
59  return min_stiff;
60  }
61  double get_max_stiff()
62  {
63  return max_stiff;
64  }
65  double get_min_damp()
66  {
67  return min_damp;
68  }
69  double get_max_damp()
70  {
71  return max_damp;
72  }
73 };
74 
76 {
77  double stiffness;
78  double damping;
80  ImpedanceParameters() {stiffness=0; damping=0;}
81 };
82 
85 // public yarp::dev::DeviceResponder,
92  public yarp::dev::IMotorRaw,
123 {
124 private:
125  enum VerboseLevel
126  {
127  MUTE = 0, // only errors that prevent device from working
128  QUIET = 1, // adds errors that can cause malfunctioning
129  DEFAULT = 2, // adds warnings // DEFAULT // show noisy messages about back-compatible changes
130  CHATTY = 3, // adds info messages
131  VERBOSE = 4, // adds debug messages
132  VERY_VERBOSE = 5, // adds trace of events (shows thread running and catch if they get stuck)
133  VERY_VERY_VERBOSE = 6 // adds messages printed every cycle, so too much verbose for usage, only for deep debugging
134  };
135 
136  std::mutex _mutex;
137  int _njoints;
138  int *_axisMap;
139  double *_angleToEncoder;
140  double *_encodersStamp;
141  double *_ampsToSensor;
142  double *_dutycycleToPWM;
143  float *_DEPRECATED_encoderconversionfactor;
144  float *_DEPRECATED_encoderconversionoffset;
145 // uint8_t *_jointEncoderType; /** joint encoder type*/
146  int *_jointEncoderRes;
147  int *_rotorEncoderRes;
148 // uint8_t *_rotorEncoderType; /** rotor encoder type*/
149  double *_gearbox;
150  bool *_hasHallSensor;
151  bool *_hasTempSensor;
152  bool *_hasRotorEncoder;
153  bool *_hasRotorEncoderIndex;
154  int *_rotorIndexOffset;
155  int *_motorPoles;
156  double *_rotorlimits_max;
157  double *_rotorlimits_min;
158  yarp::dev::Pid *_ppids;
159  yarp::dev::Pid *_tpids;
160  yarp::dev::Pid *_cpids;
161  yarp::dev::Pid *_vpids;
162  bool *_ppids_ena;
163  bool *_tpids_ena;
164  bool *_cpids_ena;
165  bool *_vpids_ena;
166  double *_ppids_lim;
167  double *_tpids_lim;
168  double *_cpids_lim;
169  double *_vpids_lim;
170  double *_ppids_ref;
171  double *_tpids_ref;
172  double *_cpids_ref;
173  double *_vpids_ref;
174 
175  std::string *_axisName;
176  yarp::dev::JointTypeEnum *_jointType;
177 // ImpedanceLimits *_impedance_limits; /** impedance limits */
178  double *_limitsMin;
179  double *_limitsMax;
180  double *_kinematic_mj;
181  //double *_currentLimits; /** current limits */
182 // MotorCurrentLimits *_currentLimits;
183  double *_maxJntCmdVelocity;
184  double *_maxMotorVelocity;
185  int *_velocityShifts;
186  int *_velocityTimeout;
187  double *_kbemf;
188  double *_ktau;
189  int *_kbemf_scale;
190  int *_ktau_scale;
191  int * _filterType;
192  int *_torqueSensorId;
193  int *_torqueSensorChan;
194  double *_maxTorque;
195  double *_newtonsToSensor;
196  bool *checking_motiondone; /* flag telling if I'm already waiting for motion done */
197  double *_last_position_move_time;
198  double *_motorPwmLimits;
199  double *_torques;
201 // ImpedanceParameters *_impedance_params; /** impedance parameters */
202 
203  bool verbosewhenok;
204  bool useRawEncoderData;
205  bool _pwmIsLimited;
206  bool _torqueControlEnabled;
208  enum torqueControlUnitsType {T_MACHINE_UNITS=0, T_METRIC_UNITS=1};
209  torqueControlUnitsType _torqueControlUnits;
210 
211  enum positionControlUnitsType {P_MACHINE_UNITS=0, P_METRIC_UNITS=1};
212  positionControlUnitsType _positionControlUnits;
213 
214  // internal stuff
215  int *_controlModes;
216  int *_interactMode;
217  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.
218  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.
219  bool *_calibrated; // Flag to know if the calibrate function has been called for the joint
220  double *_posCtrl_references; // used for position control.
221  double *_posDir_references; // used for position Direct control.
222  double *_ref_speeds; // used for position control.
223  double *_command_speeds; // used for velocity control.
224  double *_ref_accs; // for velocity control, in position min jerk eq is used.
225  double *_ref_torques; // for torque control.
226  double *_ref_currents;
227  yarp::sig::Vector current, nominalCurrent, maxCurrent, peakCurrent;
228  yarp::sig::Vector pwm, pwmLimit, refpwm, supplyVoltage,last_velocity_command, last_pwm_command;
229  yarp::sig::Vector pos, dpos, vel, speed, acc, loc, amp;
230  double prev_time;
231  bool opened;
232 
233  // debugging
234  VerboseLevel verbose;
235 public:
236 
239 
240  // Device Driver
241  bool open(yarp::os::Searchable &par) override;
242  bool close() override;
243  bool fromConfig(yarp::os::Searchable &config);
244 
245  virtual bool initialised();
246 
250  bool alloc(int njoints);
251 
255  void resizeBuffers();
256 
257  bool threadInit() override;
258  void threadRelease() override;
259 
261  bool setPidRaw(const yarp::dev::PidControlTypeEnum& pidtype,int j, const yarp::dev::Pid &pid) override;
262  bool setPidsRaw(const yarp::dev::PidControlTypeEnum& pidtype,const yarp::dev::Pid *pids) override;
263  bool setPidReferenceRaw(const yarp::dev::PidControlTypeEnum& pidtype,int j, double ref) override;
264  bool setPidReferencesRaw(const yarp::dev::PidControlTypeEnum& pidtype,const double *refs) override;
265  bool setPidErrorLimitRaw(const yarp::dev::PidControlTypeEnum& pidtype,int j, double limit) override;
266  bool setPidErrorLimitsRaw(const yarp::dev::PidControlTypeEnum& pidtype,const double *limits) override;
267  bool getPidErrorRaw(const yarp::dev::PidControlTypeEnum& pidtype,int j, double *err) override;
268  bool getPidErrorsRaw(const yarp::dev::PidControlTypeEnum& pidtype, double *errs) override;
269  bool getPidOutputRaw(const yarp::dev::PidControlTypeEnum& pidtype,int j, double *out) override;
270  bool getPidOutputsRaw(const yarp::dev::PidControlTypeEnum& pidtype,double *outs) override;
271  bool getPidRaw(const yarp::dev::PidControlTypeEnum& pidtype,int j, yarp::dev::Pid *pid) override;
272  bool getPidsRaw(const yarp::dev::PidControlTypeEnum& pidtype, yarp::dev::Pid *pids) override;
273  bool getPidReferenceRaw(const yarp::dev::PidControlTypeEnum& pidtype,int j, double *ref) override;
274  bool getPidReferencesRaw(const yarp::dev::PidControlTypeEnum& pidtype,double *refs) override;
275  bool getPidErrorLimitRaw(const yarp::dev::PidControlTypeEnum& pidtype,int j, double *limit) override;
276  bool getPidErrorLimitsRaw(const yarp::dev::PidControlTypeEnum& pidtype,double *limits) override;
277  bool resetPidRaw(const yarp::dev::PidControlTypeEnum& pidtype,int j) override;
278  bool disablePidRaw(const yarp::dev::PidControlTypeEnum& pidtype,int j) override;
279  bool enablePidRaw(const yarp::dev::PidControlTypeEnum& pidtype,int j) override;
280  bool setPidOffsetRaw(const yarp::dev::PidControlTypeEnum& pidtype,int j, double v) override;
281  bool isPidEnabledRaw(const yarp::dev::PidControlTypeEnum& pidtype, int j, bool* enabled) override;
282 
283  // POSITION CONTROL INTERFACE RAW
284  bool getAxes(int *ax) override;
285  bool positionMoveRaw(int j, double ref) override;
286  bool positionMoveRaw(const double *refs) override;
287  bool relativeMoveRaw(int j, double delta) override;
288  bool relativeMoveRaw(const double *deltas) override;
289  bool checkMotionDoneRaw(bool *flag) override;
290  bool checkMotionDoneRaw(int j, bool *flag) override;
291  bool setRefSpeedRaw(int j, double sp) override;
292  bool setRefSpeedsRaw(const double *spds) override;
293  bool setRefAccelerationRaw(int j, double acc) override;
294  bool setRefAccelerationsRaw(const double *accs) override;
295  bool getRefSpeedRaw(int j, double *ref) override;
296  bool getRefSpeedsRaw(double *spds) override;
297  bool getRefAccelerationRaw(int j, double *acc) override;
298  bool getRefAccelerationsRaw(double *accs) override;
299  bool stopRaw(int j) override;
300  bool stopRaw() override;
301 
302  // Position Control2 Interface
303  bool positionMoveRaw(const int n_joint, const int *joints, const double *refs) override;
304  bool relativeMoveRaw(const int n_joint, const int *joints, const double *deltas) override;
305  bool checkMotionDoneRaw(const int n_joint, const int *joints, bool *flags) override;
306  bool setRefSpeedsRaw(const int n_joint, const int *joints, const double *spds) override;
307  bool setRefAccelerationsRaw(const int n_joint, const int *joints, const double *accs) override;
308  bool getRefSpeedsRaw(const int n_joint, const int *joints, double *spds) override;
309  bool getRefAccelerationsRaw(const int n_joint, const int *joints, double *accs) override;
310  bool stopRaw(const int n_joint, const int *joints) override;
311  bool getTargetPositionRaw(const int joint, double *ref) override;
312  bool getTargetPositionsRaw(double *refs) override;
313  bool getTargetPositionsRaw(const int n_joint, const int *joints, double *refs) override;
314 
315  // Velocity control interface raw
316  bool velocityMoveRaw(int j, double sp) override;
317  bool velocityMoveRaw(const double *sp) override;
318 
319 
320  // calibration2raw
321  bool setCalibrationParametersRaw(int axis, const yarp::dev::CalibrationParameters& params) override;
322  bool calibrateAxisWithParamsRaw(int axis, unsigned int type, double p1, double p2, double p3) override;
323  bool calibrationDoneRaw(int j) override;
324 
325 
327 
328  // ControlMode
329  bool getControlModeRaw(int j, int *v) override;
330  bool getControlModesRaw(int *v) override;
331 
332  // ControlMode 2
333  bool getControlModesRaw(const int n_joint, const int *joints, int *modes) override;
334  bool setControlModeRaw(const int j, const int mode) override;
335  bool setControlModesRaw(const int n_joint, const int *joints, int *modes) override;
336  bool setControlModesRaw(int *modes) override;
337 
339  bool resetEncoderRaw(int j) override;
340  bool resetEncodersRaw() override;
341  bool setEncoderRaw(int j, double val) override;
342  bool setEncodersRaw(const double *vals) override;
343  bool getEncoderRaw(int j, double *v) override;
344  bool getEncodersRaw(double *encs) override;
345  bool getEncoderSpeedRaw(int j, double *sp) override;
346  bool getEncoderSpeedsRaw(double *spds) override;
347  bool getEncoderAccelerationRaw(int j, double *spds) override;
348  bool getEncoderAccelerationsRaw(double *accs) override;
350 
351  bool getEncodersTimedRaw(double *encs, double *stamps) override;
352  bool getEncoderTimedRaw(int j, double *encs, double *stamp) override;
353 
355  bool getNumberOfMotorEncodersRaw(int * num) override;
356  bool resetMotorEncoderRaw(int m) override;
357  bool resetMotorEncodersRaw() override;
358  bool setMotorEncoderRaw(int m, const double val) override;
359  bool setMotorEncodersRaw(const double *vals) override;
360  bool getMotorEncoderRaw(int m, double *v) override;
361  bool getMotorEncodersRaw(double *encs) override;
362  bool getMotorEncoderSpeedRaw(int m, double *sp) override;
363  bool getMotorEncoderSpeedsRaw(double *spds) override;
364  bool getMotorEncoderAccelerationRaw(int m, double *spds) override;
365  bool getMotorEncoderAccelerationsRaw(double *accs) override;
366  bool getMotorEncodersTimedRaw(double *encs, double *stamps) override;
367  bool getMotorEncoderTimedRaw(int m, double *encs, double *stamp) override;\
368  bool getMotorEncoderCountsPerRevolutionRaw(int m, double *v) override;
369  bool setMotorEncoderCountsPerRevolutionRaw(int m, const double cpr) override;
371 
373  bool getAxisNameRaw(int axis, std::string& name) override;
374  bool getJointTypeRaw(int axis, yarp::dev::JointTypeEnum& type) override;
376 
377  //Internal use, not exposed by Yarp (yet)
378  bool getGearboxRatioRaw(int m, double *gearbox) override;
379  virtual bool getRotorEncoderResolutionRaw(int m, double &rotres);
380  virtual bool getJointEncoderResolutionRaw(int m, double &jntres);
381  virtual bool getJointEncoderTypeRaw(int j, int &type);
382  virtual bool getRotorEncoderTypeRaw(int j, int &type);
383  virtual bool getKinematicMJRaw(int j, double &rotres);
384  virtual bool getHasTempSensorsRaw(int j, int& ret);
385  virtual bool getHasHallSensorRaw(int j, int& ret);
386  virtual bool getHasRotorEncoderRaw(int j, int& ret);
387  virtual bool getHasRotorEncoderIndexRaw(int j, int& ret);
388  virtual bool getMotorPolesRaw(int j, int& poles);
389  virtual bool getRotorIndexOffsetRaw(int j, double& rotorOffset);
390  virtual bool getTorqueControlFilterType(int j, int& type);
391 
393  bool enableAmpRaw(int j) override;
394  bool disableAmpRaw(int j) override;
395  bool getCurrentsRaw(double *vals) override;
396  bool getCurrentRaw(int j, double *val) override;
397  bool getNominalCurrentRaw(int m, double *val) override;
398  bool setNominalCurrentRaw(int m, const double val) override;
399  bool setMaxCurrentRaw(int j, double val) override;
400  bool getMaxCurrentRaw(int j, double *val) override;
401  bool getPeakCurrentRaw(int m, double *val) override;
402  bool setPeakCurrentRaw(int m, const double val) override;
403  bool getAmpStatusRaw(int *st) override;
404  bool getAmpStatusRaw(int j, int *st) override;
405  bool getPWMRaw(int j, double* val) override;
406  bool getPWMLimitRaw(int j, double* val) override;
407  bool setPWMLimitRaw(int j, const double val) override;
408  bool getPowerSupplyVoltageRaw(int j, double* val) override;
410 
411  // Limits
412  bool setLimitsRaw(int axis, double min, double max) override;
413  bool getLimitsRaw(int axis, double *min, double *max) override;
414  // Limits 2
415  bool setVelLimitsRaw(int axis, double min, double max) override;
416  bool getVelLimitsRaw(int axis, double *min, double *max) override;
417 
418  // Torque control
419  bool getTorqueRaw(int j, double *t) override;
420  bool getTorquesRaw(double *t) override;
421  bool getTorqueRangeRaw(int j, double *min, double *max) override;
422  bool getTorqueRangesRaw(double *min, double *max) override;
423  bool setRefTorquesRaw(const double *t) override;
424  bool setRefTorqueRaw(int j, double t) override;
425  bool setRefTorquesRaw(const int n_joint, const int *joints, const double *t) override;
426  bool getRefTorquesRaw(double *t) override;
427  bool getRefTorqueRaw(int j, double *t) override;
428  bool getMotorTorqueParamsRaw(int j, yarp::dev::MotorTorqueParameters *params) override;
429  bool setMotorTorqueParamsRaw(int j, const yarp::dev::MotorTorqueParameters params) override;
430 // int32_t getRefSpeedInTbl(uint8_t boardNum, int j, eOmeas_position_t pos) override;
431 
432  // IVelocityControl2
433  bool velocityMoveRaw(const int n_joint, const int *joints, const double *spds) override;
434  bool getRefVelocityRaw(const int joint, double *ref) override;
435  bool getRefVelocitiesRaw(double *refs) override;
436  bool getRefVelocitiesRaw(const int n_joint, const int *joints, double *refs) override;
437 
438  // Impedance interface
439  bool getImpedanceRaw(int j, double *stiffness, double *damping) override;
440  bool setImpedanceRaw(int j, double stiffness, double damping) override;
441  bool setImpedanceOffsetRaw(int j, double offset) override;
442  bool getImpedanceOffsetRaw(int j, double *offset) override;
443  bool getCurrentImpedanceLimitRaw(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp) override;
444 
445  // PositionDirect Interface
446  bool setPositionRaw(int j, double ref) override;
447  bool setPositionsRaw(const int n_joint, const int *joints, const double *refs) override;
448  bool setPositionsRaw(const double *refs) override;
449  bool getRefPositionRaw(const int joint, double *ref) override;
450  bool getRefPositionsRaw(double *refs) override;
451  bool getRefPositionsRaw(const int n_joint, const int *joints, double *refs) override;
452 
453  // InteractionMode interface
454  bool getInteractionModeRaw(int j, yarp::dev::InteractionModeEnum* _mode) override;
455  bool getInteractionModesRaw(int n_joints, int *joints, yarp::dev::InteractionModeEnum* modes) override;
456  bool getInteractionModesRaw(yarp::dev::InteractionModeEnum* modes) override;
457  bool setInteractionModeRaw(int j, yarp::dev::InteractionModeEnum _mode) override;
458  bool setInteractionModesRaw(int n_joints, int *joints, yarp::dev::InteractionModeEnum* modes) override;
459  bool setInteractionModesRaw(yarp::dev::InteractionModeEnum* modes) override;
460 
461  // IMotor interface
462  bool getNumberOfMotorsRaw(int * num) override;
463  bool getTemperatureRaw(int m, double* val) override;
464  bool getTemperaturesRaw(double *vals) override;
465  bool getTemperatureLimitRaw(int m, double *temp) override;
466  bool setTemperatureLimitRaw(int m, const double temp) override;
467 
468  // PWM interface
469  bool setRefDutyCycleRaw(int j, double v) override;
470  bool setRefDutyCyclesRaw(const double *v) override;
471  bool getRefDutyCycleRaw(int j, double *v) override;
472  bool getRefDutyCyclesRaw(double *v) override;
473  bool getDutyCycleRaw(int j, double *v) override;
474  bool getDutyCyclesRaw(double *v) override;
475 
476  // Current interface
477  //bool getAxes(int *ax) override;
478  //bool getCurrentRaw(int j, double *t) override;
479  //bool getCurrentsRaw(double *t) override;
480  bool getCurrentRangeRaw(int j, double *min, double *max) override;
481  bool getCurrentRangesRaw(double *min, double *max) override;
482  bool setRefCurrentsRaw(const double *t) override;
483  bool setRefCurrentRaw(int j, double t) override;
484  bool setRefCurrentsRaw(const int n_joint, const int *joints, const double *t) override;
485  bool getRefCurrentsRaw(double *t) override;
486  bool getRefCurrentRaw(int j, double *t) override;
487 
488  yarp::dev::VAS_status getVirtualAnalogSensorStatusRaw(int ch) override;
489  int getVirtualAnalogSensorChannelsRaw() override;
490  bool updateVirtualAnalogSensorMeasureRaw(yarp::sig::Vector &measure) override;
491  bool updateVirtualAnalogSensorMeasureRaw(int ch, double &measure) override;
492 
493  void run() override;
494 private:
495  void cleanup(void);
496  bool dealloc();
497  bool parsePositionPidsGroup(yarp::os::Bottle& pidsGroup, yarp::dev::Pid myPid[]);
498  bool parseTorquePidsGroup(yarp::os::Bottle& pidsGroup, yarp::dev::Pid myPid[], double kbemf[], double ktau[], int filterType[]);
499  bool parseImpedanceGroup_NewFormat(yarp::os::Bottle& pidsGroup, ImpedanceParameters vals[]);
500 
501  bool extractGroup(yarp::os::Bottle &input, yarp::os::Bottle &out, const std::string &key1, const std::string &txt, int size);
502 };
503 
504 #endif // YARP_DEVICE_FAKE_MOTIONCONTROL
Interface implemented by all device drivers.
Definition: DeviceDriver.h:37
bool ret
A base class for nested structures that can be searched.
Definition: Searchable.h:68
Contains the parameters for a PID.
Interface for control devices, amplifier commands.
Default implementation of the IPositionDirect interface.
Interface for control boards implementing current control.
Interface settings the way the robot interacts with the environment: basic interaction types are Stif...
Provides:
Definition: Vector.h:32
Control board, extend encoder raw interface adding timestamps.
Interface for a generic control board device implementing position control in encoder coordinates...
Interface for a generic control board device implementing position control in encoder coordinates...
An abstraction for a periodic thread.
float t
class ImplementControlLimits; class StubImplControlLimitsRaw;
Interface for a generic control board device implementing a PID controller.
Definition: IPidControl.h:31
ImpedanceLimits limits
Interface for control devices, calibration commands.
Default implementation of the IPositionControl interface.
A simple collection of objects that can be described and transmitted in a portable way...
Definition: Bottle.h:72
virtual analog sensor interface
Interface for setting control mode in control board.
Definition: IControlMode.h:103
bool verbose
Definition: yarpros.cpp:23
A generic interface to a virtual sensors.
Control board, encoder interface.
define control board standard interfaces
contains the definition of a Vector type
PidControlTypeEnum
Definition: PidEnums.h:20
Control board, encoder interface.
Definition: IMotor.h:28
Interface for control boards implementing torque control.
Interface for control devices.
Interface for control boards implementig velocity control in encoder coordinates. ...
Interface settings the way the robot interacts with the environment: basic interaction types are Stif...
Interface for controlling an axis, by sending directly a PWM reference signal to a motor...
Definition: IPWMControl.h:87
Interface for control boards implementing impedance control.
Interface for getting information about specific axes, if available.
Definition: IAxisInfo.h:68