iCub-main
CanBusMotionControl.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 // $Id: CanBusMotionControl.h,v 1.16 2009/07/29 13:12:29 nat Exp $
5 //
6 //
7 
8 // Copyright: (C) 2010-2017 iCub Facility, Istituto Italiano di Tecnologia
9 // Authors: Lorenzo Natale <lorenzo.natale@iit.it>
10 // CopyPolicy: Released under the terms of the GNU GPL v2.0.
11 
12 #ifndef __CanBusMotionControlh__
13 #define __CanBusMotionControlh__
14 
15 #include <yarp/dev/DeviceDriver.h>
16 #include <yarp/dev/ControlBoardHelper.h>
17 #include <yarp/dev/ControlBoardInterfaces.h>
18 #include <yarp/dev/ControlBoardInterfacesImpl.h>
19 #include <yarp/dev/IAnalogSensor.h>
20 #include <yarp/dev/CanBusInterface.h>
21 #include <yarp/dev/IPreciselyTimed.h>
22 #include <yarp/os/PeriodicThread.h>
23 #include <string>
24 #include <list>
25 #include <mutex>
26 
27 #include <iCub/FactoryInterface.h>
28 #include <iCub/LoggerInterfaces.h>
29 #include <messages.h>
30 
31 namespace yarp{
32  namespace dev{
33  class CanBusMotionControl;
34  class CanBusMotionControlParameters;
35  }
36 }
37 
38 class ThreadPool2;
39 class RequestsQueue;
41 {
46 
48  {
53  }
54 };
55 
57 {
58  double min_stiff;
59  double max_stiff;
60  double min_damp;
61  double max_damp;
62  double param_a;
63  double param_b;
64  double param_c;
65 
66  public:
68  {
69  min_stiff=0; max_stiff=0;
70  min_damp=0; max_damp=0;
71  param_a=0; param_a=0; param_c=0;
72  }
73 
74  double get_min_stiff() {return min_stiff;}
75  double get_max_stiff() {return max_stiff;}
76  double get_min_damp() {return min_damp;}
77  double get_max_damp() {return max_damp;}
78 };
99 {
100 private:
102  void operator= (const CanBusMotionControlParameters&);
103 
104 
105 
106 public:
113 
118 
120  {
121  double data[8];
122  bool enabled;
123  DebugParameters() {for (int i=0; i<8; i++) data[i]=0; enabled=false;}
124  };
125 
127  {
128  double stiffness;
129  double damping;
132  };
133 
134  bool parsePosPidsGroup_OldFormat(yarp::os::Bottle& pidsGroup, int nj, Pid myPid[]);
135  bool parseTrqPidsGroup_OldFormat(yarp::os::Bottle& pidsGroup, int nj, Pid myPid[]);
136  bool parsePidsGroup_NewFormat(yarp::os::Bottle& pidsGroup, Pid myPid[]);
137  bool parseImpedanceGroup_NewFormat(yarp::os::Bottle& pidsGroup, ImpedanceParameters vals[]);
138  bool parseDebugGroup_NewFormat(yarp::os::Bottle& pidsGroup, DebugParameters vals[]);
139 
140  bool setBroadCastMask(yarp::os::Bottle &list, int MASK);
141 
142  bool fromConfig(yarp::os::Searchable &config);
143  bool alloc(int nj);
144 
150 
151  int _networkN;
152  std::string _networkName;
153  int _njoints;
154  unsigned char *_destinations;
155  unsigned char _my_address;
157  int _timeout;
159  std::string *_axisName;
160  std::string *_axisType;
161  int *_axisMap;
162  double *_angleToEncoder;
163  double *_rotToEncoder;
164  double *_zeros;
165  Pid *_pids;
166  Pid *_tpids;
172  double *_bemfGain;
173  double *_ktau;
175  double *_limitsMin;
176  double *_limitsMax;
177  double *_currentLimits;
178  double *_motorPwmLimits;
181  double *_maxStep;
183  double *_optical_factor;
186  double *_maxTorque;
188  double *_ampsToSensor;
193 };
194 
196 {
197 private:
198  double *_data;
199  int _size;
200  int _bufferSize;
201 public:
202  TBR_AnalogData(int ch, int buffsize): _data(0), _size(ch), _bufferSize(buffsize)
203  {
204  _data=new double[_bufferSize];
205  for(int k=0;k<_bufferSize;k++)
206  _data[k]=0;
207  }
209  {
210  delete [] _data;
211  }
212 
213  inline double &operator[](int i)
214  { return _data[i]; }
215 
216  inline int size()
217  { return _size; }
218 
219  inline double *getBuffer()
220  {return _data;}
221 };
222 
223 typedef int AnalogDataFormat;
224 
225 class TBR_CanBackDoor;
226 
227 class TBR_AnalogSensor: public yarp::dev::IAnalogSensor,
228  public yarp::dev::DeviceDriver
229 {
230 public:
232  {
235  };
236 
238  {
244  };
245 
246 private:
247  // debug messages
248  unsigned int counterSat;
249  unsigned int counterError;
250  unsigned int counterTimeout;
251  int rate;
252 
254  TBR_AnalogData *data;
255  short status;
256  double timeStamp;
257  double* scaleFactor;
258  std::mutex mtx;
259  AnalogDataFormat dataFormat;
260  yarp::os::Bottle initMsg;
261  yarp::os::Bottle speedMsg;
262  yarp::os::Bottle closeMsg;
263  std::string deviceIdentifier;
264  short boardId;
265  short useCalibration;
266  bool isVirtualSensor; //RANDAZ
267 
268  bool decode8(const unsigned char *msg, int id, double *data);
269  bool decode16(const unsigned char *msg, int id, double *data);
270 
271 public:
273 
276  bool handleAnalog(void *);
277 
279  {
280  counterSat=0;
281  counterError=0;
282  counterTimeout=0;
283  }
284 
285  void getCounters(unsigned int &sat, unsigned int &err, unsigned int &to)
286  {
287  sat=counterSat;
288  err=counterError;
289  to=counterTimeout;
290  }
291 
292  void setDeviceId(std::string id)
293  {
294  deviceIdentifier=id;
295  }
296 
297  std::string getDeviceId()
298  {
299  return deviceIdentifier;
300  }
301 
302  short getId()
303  { return boardId;}
304 
305  short getStatus()
306  { return status;}
307 
308  bool isOpen()
309  {
310  if (data)
311  return true;
312  else
313  return false;
314  }
315 
317  {return useCalibration;}
318  double* getScaleFactor()
319  {return scaleFactor;}
320  double getScaleFactor(int chan)
321  {
322  if (chan>=0 && chan<data->size())
323  return scaleFactor[chan];
324  else
325  return 0;
326  }
327 
328  bool open(int channels, AnalogDataFormat f, short bId, short useCalib, bool isVirtualSensor);
329 
330  //IAnalogSensor interface
331  virtual int read(yarp::sig::Vector &out);
332  virtual int getState(int ch);
333  virtual int getChannels();
334  virtual int calibrateChannel(int ch, double v);
335  virtual int calibrateSensor();
336  virtual int calibrateSensor(const yarp::sig::Vector& value)
337  {
338  return calibrateSensor();
339  }
340 
341  virtual int calibrateChannel(int ch)
342  {
343  return calibrateChannel(ch, 0);
344  }
346 };
347 
349 {
350  int jointsNum;
351  SpeedEstimationParameters *estim_params;
352 
353  public:
354  speedEstimationHelper (int njoints, SpeedEstimationParameters* estim_parameters );
355  inline int getNumberOfJoints ()
356  {
357  return jointsNum;
358  }
360  {
361  if (jnt>=0 && jnt<jointsNum) return estim_params[jnt];
362  SpeedEstimationParameters empty_params;
363  return empty_params;
364  }
366  {
367  delete [] estim_params;
368  }
369 };
370 
371 #define BOARD_TYPE_4DC 0x03
372 #define BOARD_TYPE_BLL 0x04 // Note: Also BLL2DC firmware is identified BLL. This is intentional.
373 
375 {
376  int major;
377  int minor;
378 };
379 
381 {
382  int joint;
384  std::string network_name;
387  int fw_major;
389  int fw_build;
391  int ack;
392 
393  inline void print_info()
394  {
395  if (board_type==0)
396  {
397  yWarning("%s [%d] joint: %d can_address: %2d Unable to detect firmware version. Old firmware running?",network_name.c_str(),network_number,joint,board_can_id);
398  }
399  else
400  {
402  {yInfo("%s [%d] joint: %d can_address: %2d board type: 3 (4DC) version:%2x.%2x build:%3d CAN_protocol:%d.%d", network_name.c_str(),network_number,joint,board_can_id, fw_major, fw_version, fw_build,can_protocol.major,can_protocol.minor);}
404  {yInfo("%s [%d] joint: %d can_address: %2d board type: 4 (BLL) version:%2x.%2x build:%3d CAN_protocol:%d.%d", network_name.c_str(),network_number,joint,board_can_id, fw_major, fw_version, fw_build,can_protocol.major,can_protocol.minor);}
405  }
406  }
407 
408 };
409 
411 {
412  int jointsNum;
413 
414  public:
417 
418  firmwareVersionHelper(int joints, firmware_info* f_infos, can_protocol_info& protocol)
419  {
420  icub_protocol = protocol;
421  jointsNum=joints;
422  infos = new firmware_info [jointsNum];
423  for (int i=0; i<jointsNum; i++)
424  {
425  infos[i] = f_infos[i];
426  }
427  }
428  inline int getNumberOfJoints ()
429  {
430  return jointsNum;
431  }
433  {
434  bool printed = false;
435  for (int j=0; j<jointsNum; j++)
436  {
437  if (infos[j].board_type==0)
438  {
440  return false;
441  }
442 
443  if (infos[j].ack==0)
444  {
446  return false;
447  }
448 
449  if (infos[j].board_type==BOARD_TYPE_BLL)
450  {
451  // Note: Also BLL2DC firmware is identified BLL. This is intentional.
452  if (infos[j].fw_build<LAST_BLL_BUILD)
453  {
454  if (!printed) printMessagePleaseUpgradeFirmware();
455  printed = true;
456  }
457  if (infos[j].fw_build>LAST_BLL_BUILD)
458  {
459  if (!printed) printMessagePleaseUpgradeiCub();
460  printed = true;
461  }
462  }
463 
464  if (infos[j].board_type==BOARD_TYPE_4DC)
465  {
466  if (infos[j].fw_build<LAST_MC4_BUILD)
467  {
468  if (!printed) printMessagePleaseUpgradeFirmware();
469  printed = true;
470  }
471  if (infos[j].fw_build>LAST_MC4_BUILD)
472  {
473  if (!printed) printMessagePleaseUpgradeiCub();
474  printed = true;
475  }
476  }
477  }
478  return true;
479  }
480  inline void printFirmwareVersions()
481  {
482  yInfo("\n");
483  yInfo("**********************************\n");
484  yInfo("yarprobotinterface CAN protocol: %d.%d\n",icub_protocol.major,icub_protocol.minor);
485  yInfo("Firmware report:\n");
486  for (int j=0; j<jointsNum; j++)
487  {
488  infos[j].print_info();
489  }
490  yInfo("**********************************\n");
491  yInfo("\n");
492  }
494  {
495  yWarning("\n");
496  yWarning("###################################################################################\n");
497  yWarning("###################################################################################\n");
498  yWarning("\n");
499  yWarning(" yarprobotinterface detected that your control boards are not running the latest\n");
500  yWarning(" available firmware version, although it is still compatible with it.\n");
501  yWarning(" Upgrading your iCub firmware to build %d is highly recommended.\n", LAST_BLL_BUILD);
502  yWarning(" For further information please visit: https://icub-tech-iit.github.io/documentation/icub_firmware\n");
503  yWarning("\n");
504  yWarning("###################################################################################\n");
505  yWarning("###################################################################################\n");
506  yWarning("\n");
507  }
509  {
510  yWarning("\n");
511  yWarning("#################################################################################################\n");
512  yWarning("#################################################################################################\n");
513  yWarning("\n");
514  yWarning(" yarprobotinterface detected that your control boards are running a firmware version\n");
515  yWarning(" which is newer than the recommended version (build %d), although it is still compatible with it.\n", LAST_BLL_BUILD);
516  yWarning(" It may also be that you are running an experimental firmware version. \n");
517  yWarning(" An update of Yarp/iCub SW is recommended. Proceed only if you are aware of what you are doing.\n");
518  yWarning("\n");
519  yWarning("#################################################################################################\n");
520  yWarning("#################################################################################################\n");
521  yWarning("\n");
522  }
524  {
525  yError("\n");
526  yError("###################################################################################\n");
527  yError("###################################################################################\n");
528  yError("\n");
529  yError(" It has been detected that your control boards are not using the same\n");
530  yError(" CAN protocol used by yarprobotinterface. yarprobotinterface cannot continue.\n");
531  yError(" Please update your system (yarprobotinterface and/or your control board firmware.\n");
532  yError(" For further information please visit: https://icub-tech-iit.github.io/documentation/icub_firmware\n");
533  yError("\n");
534  yError("###################################################################################\n");
535  yError("###################################################################################\n");
536  yError("\n");
537  }
539  {
540  delete [] infos;
541  infos = 0;
542  }
543 };
544 
546 {
547  int jointsNum;
548  ImpedanceLimits* impLimits;
549 
550  public:
551  axisImpedanceHelper(int njoints, ImpedanceLimits* imped_limits );
552 
554  {
555  delete [] impLimits;
556  impLimits=0;
557  }
558 
559  inline ImpedanceLimits* getImpedanceLimits () {return impLimits;}
560 };
561 
563 {
564  int jointsNum;
565  double* maxHwStep;
566  double* maxUserStep;
567  yarp::dev::ControlBoardHelper* helper;
568 
569  public:
570  axisPositionDirectHelper(int njoints, const int *aMap, const double *angToEncs, double* _maxStep);
571 
573  {
574  delete [] maxHwStep;
575  maxHwStep=0;
576  delete [] maxUserStep;
577  maxUserStep=0;
578  delete helper;
579  helper = 0;
580  }
581 
582  inline double posA2E (double ang, int j) {return helper->posA2E(ang, j);}
583  inline double posE2A (double ang, int j) {return helper->posE2A(ang, j);}
584  inline double getMaxHwStep (int j) {return maxHwStep[j];}
585  inline double getMaxUserStep (int j) {return maxUserStep[j];}
586  inline double getSaturatedValue (int j, double curr_value, double ref_value);
587 };
588 
590 {
591  int jointsNum;
592  int* torqueSensorId;
593  int* torqueSensorChan;
594  double* maximumTorque;
595  double* newtonsToSensor;
596 
597  public:
598  axisTorqueHelper(int njoints, int* id, int* chan, double* maxTrq, double* newtons2sens );
599  inline int getTorqueSensorId (int jnt)
600  {
601  if (jnt>=0 && jnt<jointsNum) return torqueSensorId[jnt];
602  return 0;
603  }
604  inline int getTorqueSensorChan (int jnt)
605  {
606  if (jnt>=0 && jnt<jointsNum) return torqueSensorChan[jnt];
607  return 0;
608  }
609  inline double getMaximumTorque (int jnt)
610  {
611  if (jnt>=0 && jnt<jointsNum) return maximumTorque[jnt];
612  return 0;
613  }
614  inline double getNewtonsToSensor (int jnt)
615  {
616  if (jnt>=0 && jnt<jointsNum) return newtonsToSensor[jnt];
617  return 0;
618  }
619  inline int getNumberOfJoints ()
620  {
621  return jointsNum;
622  }
624  {
625  if (torqueSensorId) delete [] torqueSensorId;
626  if (torqueSensorChan) delete [] torqueSensorChan;
627  if (maximumTorque) delete [] maximumTorque;
628  if (newtonsToSensor) delete [] newtonsToSensor;
629  torqueSensorId=0;
630  torqueSensorChan=0;
631  maximumTorque=0;
632  newtonsToSensor=0;
633  }
634 };
635 
654 class yarp::dev::CanBusMotionControl:public DeviceDriver,
655  public os::PeriodicThread,
656  public IPidControlRaw,
657  public IPositionControlRaw,
658  public IPositionDirectRaw,
659  public IVelocityControlRaw,
660  public IAmplifierControlRaw,
661  public IControlCalibrationRaw,
662  public IControlLimitsRaw,
663  public ITorqueControlRaw,
664  public IImpedanceControlRaw,
665  public IControlModeRaw,
666  public IPreciselyTimed,
667  public ImplementPositionControl,
668  public ImplementPositionDirect,
669  public ImplementVelocityControl,
670  public ImplementPidControl,
671  public IEncodersTimedRaw,
672  public ImplementEncodersTimed,
673  public IMotorEncodersRaw,
674  public ImplementMotorEncoders,
675  public IMotorRaw,
676  public ImplementMotor,
677  public ImplementControlCalibration,
678  public ImplementAmplifierControl,
679  public ImplementControlLimits,
680  public ImplementTorqueControl,
681  public ImplementImpedanceControl,
682  public ImplementControlMode,
683  public IInteractionModeRaw,
684  public ImplementInteractionMode,
685  public IRemoteVariablesRaw,
686  public ImplementRemoteVariables,
687  public IAxisInfoRaw,
688  public ImplementAxisInfo,
689  public IPWMControlRaw,
690  public ImplementPWMControl,
691  public ICurrentControlRaw,
692  public ImplementCurrentControl,
693  public IFactoryInterface,
694  public IClientLogger
695 {
696  class torqueControlHelper
697  {
698  int jointsNum;
699  double* newtonsToSensor;
700  double* angleToEncoders;
701 
702  public:
703  torqueControlHelper(int njoints, double* angleToEncoders, double* newtons2sens);
704  inline ~torqueControlHelper()
705  {
706  if (newtonsToSensor) delete [] newtonsToSensor;
707  if (angleToEncoders) delete [] angleToEncoders;
708  newtonsToSensor=0;
709  angleToEncoders=0;
710  }
711  inline double getNewtonsToSensor (int jnt)
712  {
713  if (jnt>=0 && jnt<jointsNum) return newtonsToSensor[jnt];
714  return 0;
715  }
716  inline double getAngleToEncoders (int jnt)
717  {
718  if (jnt>=0 && jnt<jointsNum) return angleToEncoders[jnt];
719  return 0;
720  }
721  inline int getNumberOfJoints ()
722  {
723  return jointsNum;
724  }
725  };
726 
727 private:
729  void operator=(const CanBusMotionControl&);
730 
731  void handleBroadcasts();
732 
733  double previousRun;
734  double averagePeriod;
735  double averageThreadTime;
736  double currentRun;
737  int myCount;
738  double lastReportTime;
739  os::Stamp stampEncoders;
740 
741  char _buff[256];
742  std::string errorstring;
743  static constexpr size_t errorstringsize = 512;
744 
745  std::list<TBR_AnalogSensor *> analogSensors;
746 
747  std::string canDevName;
748  std::string networkName;
749 
750  IServerLogger *mServerLogger;
751 
752  bool readFullScaleAnalog(int analog_can_address, int channel, double* fullScale);
753  TBR_AnalogSensor *instantiateAnalog(yarp::os::Searchable& config, std::string id);
754  void finiAnalog(TBR_AnalogSensor *s);
755 
756 public:
762 
766  virtual ~CanBusMotionControl();
767 
773  virtual bool open(yarp::os::Searchable& config);
774 
781 
782 
787  virtual bool close(void);
788 
790  yarp::dev::DeviceDriver *createDevice(yarp::os::Searchable& config);
791 
793  void setServerLogger(const IServerLogger *server)
794  {
795  mServerLogger=(IServerLogger*)server;
796  }
797 
798  #ifdef _USE_INTERFACEGUI
799 
800  void logNetworkData(const char *devName,int network,int index,const yarp::os::Value& data)
801  {
802  if (mServerLogger)
803  {
804  sprintf(_buff,"%s %d,network,%d",devName,network,index);
805  mServerLogger->log(std::string(_buff),data);
806  }
807  }
808 
809  void logJointData(const char *devName,int network,int joint,int index,const yarp::os::Value& data)
810  {
811  if (mServerLogger)
812  {
813  sprintf(_buff,"%s %d,BLL,%d,%d",devName,network,joint,index);
814  mServerLogger->log(std::string(_buff),data);
815  }
816  }
817 
818  void logAnalogData(const char *devName,int network,int board,int index,const yarp::os::Value& data)
819  {
820  if (mServerLogger)
821  {
822  sprintf(_buff,"%s %d,analog,%d,%d",devName,network,board,index);
823  mServerLogger->log(std::string(_buff),data);
824  }
825  }
826  #else
827  #define logJointData(a,b,c,d,e)
828  #define logNetworkData(a,b,c,d)
829  #define logBoardData(a,b,c,d,e)
830  #endif
831 
833  //
834  virtual bool setPidRaw(const PidControlTypeEnum& pidtype, int j, const Pid &pid) override;
835  virtual bool setPidsRaw(const PidControlTypeEnum& pidtype, const Pid *pids) override;
836  virtual bool setPidReferenceRaw(const PidControlTypeEnum& pidtype, int j, double ref) override;
837  virtual bool setPidReferencesRaw(const PidControlTypeEnum& pidtype, const double *refs) override;
838  virtual bool setPidErrorLimitRaw(const PidControlTypeEnum& pidtype, int j, double limit) override;
839  virtual bool setPidErrorLimitsRaw(const PidControlTypeEnum& pidtype, const double *limits) override;
840  virtual bool getPidErrorRaw(const PidControlTypeEnum& pidtype, int j, double *err) override;
841  virtual bool getPidErrorsRaw(const PidControlTypeEnum& pidtype, double *errs) override;
842  virtual bool getPidOutputRaw(const PidControlTypeEnum& pidtype, int j, double *out) override;
843  virtual bool getPidOutputsRaw(const PidControlTypeEnum& pidtype, double *outs) override;
844  virtual bool getPidRaw(const PidControlTypeEnum& pidtype, int j, Pid *pid) override;
845  virtual bool getPidsRaw(const PidControlTypeEnum& pidtype, Pid *pids) override;
846  virtual bool getPidReferenceRaw(const PidControlTypeEnum& pidtype, int j, double *ref) override;
847  virtual bool getPidReferencesRaw(const PidControlTypeEnum& pidtype, double *refs) override;
848  virtual bool getPidErrorLimitRaw(const PidControlTypeEnum& pidtype, int j, double *limit) override;
849  virtual bool getPidErrorLimitsRaw(const PidControlTypeEnum& pidtype, double *limits) override;
850  virtual bool resetPidRaw(const PidControlTypeEnum& pidtype, int j) override;
851  virtual bool disablePidRaw(const PidControlTypeEnum& pidtype, int j) override;
852  virtual bool enablePidRaw(const PidControlTypeEnum& pidtype, int j) override;
853  virtual bool setPidOffsetRaw(const PidControlTypeEnum& pidtype, int j, double v) override;
854  virtual bool isPidEnabledRaw(const PidControlTypeEnum& pidtype, int j, bool* enabled);
855 
856  //
858 
859  //
861  virtual bool getAxes(int *ax) override;
862  virtual bool positionMoveRaw(int j, double ref) override;
863  virtual bool positionMoveRaw(const double *refs) override;
864  virtual bool relativeMoveRaw(int j, double delta) override;
865  virtual bool relativeMoveRaw(const double *deltas) override;
866  virtual bool checkMotionDoneRaw(bool *flag) override;
867  virtual bool checkMotionDoneRaw(int j, bool *flag) override;
868  virtual bool setRefSpeedRaw(int j, double sp) override;
869  virtual bool setRefSpeedsRaw(const double *spds) override;
870  virtual bool setRefAccelerationRaw(int j, double acc) override;
871  virtual bool setRefAccelerationsRaw(const double *accs) override;
872  virtual bool getRefSpeedRaw(int j, double *ref) override;
873  virtual bool getRefSpeedsRaw(double *spds) override;
874  virtual bool getRefAccelerationRaw(int j, double *acc) override;
875  virtual bool getRefAccelerationsRaw(double *accs) override;
876  virtual bool stopRaw(int j) override;
877  virtual bool stopRaw() override;
878  //helpers
879  bool helper_setPosPidRaw( int j, const Pid &pid);
880  bool helper_getPosPidRaw(int j, Pid *pid);
881 
882  //
884 
885  //
887 // virtual bool getAxes(int *ax) override;
888  virtual bool getRefTorqueRaw(int j, double *ref_trq) override;
889  virtual bool getRefTorquesRaw(double *ref_trqs) override;
890  virtual bool setRefTorqueRaw(int j, double ref_trq) override;
891  virtual bool setRefTorquesRaw(const double *ref_trqs) override;
892  virtual bool setRefTorquesRaw(const int n_joint, const int *joints, const double *t) override;
893  virtual bool getTorqueRaw(int j, double *trq) override;
894  virtual bool getTorquesRaw(double *trqs) override;
895  virtual bool getTorqueRangeRaw(int j, double *min, double *max) override;
896  virtual bool getTorqueRangesRaw(double *min, double *max) override;
897  virtual bool getMotorTorqueParamsRaw(int j, MotorTorqueParameters *params) override;
898  virtual bool setMotorTorqueParamsRaw(int j, const MotorTorqueParameters params) override;
899  bool getFilterTypeRaw(int j, int *type) ;
900  bool setFilterTypeRaw(int j, int type) ;
901  //helper
902  bool helper_setTrqPidRaw(int j, const Pid &pid);
903  bool helper_getTrqPidRaw(int j, Pid *pid);
904 
905  //
907 
908  //
910  virtual bool getImpedanceRaw(int j, double *stiff, double *damp) override;
911  virtual bool setImpedanceRaw(int j, double stiff, double damp) override;
912  virtual bool getImpedanceOffsetRaw(int j, double *offs) override;
913  virtual bool setImpedanceOffsetRaw(int j, double offs) override;
914  virtual bool getCurrentImpedanceLimitRaw(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp) override;
915 
916  //
918 
919  // ControlMode
920  virtual bool getControlModeRaw(int j, int *v) override;
921  virtual bool getControlModesRaw(int* v) override;
922 
923  // ControlMode 2
924  virtual bool getControlModesRaw(const int n_joint, const int *joints, int *modes) override;
925  virtual bool setControlModeRaw(const int j, const int mode) override;
926  virtual bool setControlModesRaw(const int n_joint, const int *joints, int *modes) override;
927  virtual bool setControlModesRaw(int *modes) override;
928 
930  virtual bool getRemoteVariableRaw(std::string key, yarp::os::Bottle& val) override;
931  virtual bool setRemoteVariableRaw(std::string key, const yarp::os::Bottle& val) override;
932  virtual bool getRemoteVariablesListRaw(yarp::os::Bottle* listOfKeys) override;
934 
937  virtual bool velocityMoveRaw(int j, double sp) override;
938  virtual bool velocityMoveRaw(const double *sp) override;
939  //
941 
942  //Shift factors for velocity control
943  bool setVelocityShiftRaw(int j, double val) ;
944  //Timeout factors for velocity control
945  bool setVelocityTimeoutRaw(int j, double val) ;
946  //Shift factors for speed / acceleration estimation
947  bool setSpeedEstimatorShiftRaw(int j, double jnt_speed, double jnt_acc, double mot_speed, double mot_acc) ;
948 
950  //
951  virtual bool resetEncoderRaw(int j) override;
952  virtual bool resetEncodersRaw() override;
953  virtual bool setEncoderRaw(int j, double val) override;
954  virtual bool setEncodersRaw(const double *vals) override;
955  virtual bool getEncoderRaw(int j, double *v) override;
956  virtual bool getEncodersRaw(double *encs) override;
957  virtual bool getEncoderSpeedRaw(int j, double *sp) override;
958  virtual bool getEncoderSpeedsRaw(double *spds) override;
959  virtual bool getEncoderAccelerationRaw(int j, double *spds) override;
960  virtual bool getEncoderAccelerationsRaw(double *accs) override;
961  //
963 
964  virtual bool getEncodersTimedRaw(double *v, double *t) override;
965  virtual bool getEncoderTimedRaw(int j, double *v, double *t) override;
966 
968  //
969  virtual bool getNumberOfMotorEncodersRaw(int* num) override;
970  virtual bool resetMotorEncoderRaw(int m) override;
971  virtual bool resetMotorEncodersRaw() override;
972  virtual bool setMotorEncoderRaw(int m, const double val) override;
973  virtual bool setMotorEncodersRaw(const double *vals) override;
974  virtual bool getMotorEncoderRaw(int m, double *v) override;
975  virtual bool getMotorEncodersRaw(double *encs) override;
976  virtual bool getMotorEncoderCountsPerRevolutionRaw(int m, double *cpr) override;
977  virtual bool setMotorEncoderCountsPerRevolutionRaw(int m, const double cpr) override;
978  virtual bool getMotorEncoderSpeedRaw(int m, double *sp) override;
979  virtual bool getMotorEncoderSpeedsRaw(double *spds) override;
980  virtual bool getMotorEncoderAccelerationRaw(int m, double *spds) override;
981  virtual bool getMotorEncoderAccelerationsRaw(double *accs) override;
982  virtual bool getMotorEncodersTimedRaw(double *v, double *t) override;
983  virtual bool getMotorEncoderTimedRaw(int m, double *v, double *t) override;
985 
987  //
988  virtual bool enableAmpRaw(int j) override;
989  virtual bool disableAmpRaw(int j) override;
990  virtual bool getCurrentsRaw(double *vals) override;
991  virtual bool getCurrentRaw(int j, double *val) override;
992  virtual bool setMaxCurrentRaw(int j, double val) override;
993  virtual bool getMaxCurrentRaw(int j, double *val) override;
994  virtual bool getAmpStatusRaw(int *st) override;
995  virtual bool getAmpStatusRaw(int j, int *st) override;
996  virtual bool getPWMRaw(int j, double* val) override;
997  virtual bool getPWMLimitRaw(int j, double* val) override;
998  virtual bool setPWMLimitRaw(int j, const double val) override;
999  virtual bool getPowerSupplyVoltageRaw(int j, double* val) override;
1000 
1001  //
1003 
1005  virtual bool getNumberOfMotorsRaw(int* m) override;
1006  virtual bool getTemperatureRaw(int m, double* val) override;
1007  virtual bool getTemperaturesRaw(double *vals) override;
1008  virtual bool getTemperatureLimitRaw(int m, double *temp) override;
1009  virtual bool setTemperatureLimitRaw(int m, const double temp) override;
1010  virtual bool getPeakCurrentRaw(int m, double *val) override;
1011  virtual bool setPeakCurrentRaw(int m, const double val) override;
1012  virtual bool getNominalCurrentRaw(int m, double *val) override;
1013  virtual bool setNominalCurrentRaw(int m, const double val) override;
1014 
1016  virtual bool getAxisNameRaw(int axis, std::string& name) override;
1017  virtual bool getJointTypeRaw(int axis, yarp::dev::JointTypeEnum& type) override;
1018 
1020  virtual bool calibrationDoneRaw(int j) override;
1021  virtual bool calibrateAxisWithParamsRaw(int axis, unsigned int type, double p1, double p2, double p3) override;
1022  virtual bool setCalibrationParametersRaw(int j, const CalibrationParameters& params) override;
1023 
1025  virtual bool setPrintFunction(int (*f) (const char *fmt, ...));
1026  virtual bool loadBootMemory();
1027  virtual bool saveBootMemory();
1028 
1029  // IDebug Interface
1030  virtual bool setParameterRaw(int j, unsigned int type, double value) ;
1031  virtual bool getParameterRaw(int j, unsigned int type, double* value) ;
1032  virtual bool setDebugParameterRaw(int j, unsigned int index, double value) ;
1033  virtual bool getDebugParameterRaw(int j, unsigned int index, double* value) ;
1034  virtual bool setDebugReferencePositionRaw(int j, double value) ;
1035  virtual bool getDebugReferencePositionRaw(int j, double *value) ;
1036 
1038  virtual bool setLimitsRaw(int axis, double min, double max) override;
1039  virtual bool getLimitsRaw(int axis, double *min, double *max) override;
1040  // Limits 2
1041  virtual bool setVelLimitsRaw(int axis, double min, double max) override;
1042  virtual bool getVelLimitsRaw(int axis, double *min, double *max) override;
1043 
1045  virtual yarp::os::Stamp getLastInputStamp() override;
1046 
1047 
1048  // Position Control2 Interface
1049  virtual bool positionMoveRaw(const int n_joint, const int *joints, const double *refs) override;
1050  virtual bool relativeMoveRaw(const int n_joint, const int *joints, const double *deltas) override;
1051  virtual bool checkMotionDoneRaw(const int n_joint, const int *joints, bool *flags) override;
1052  virtual bool setRefSpeedsRaw(const int n_joint, const int *joints, const double *spds) override;
1053  virtual bool setRefAccelerationsRaw(const int n_joint, const int *joints, const double *accs) override;
1054  virtual bool getRefSpeedsRaw(const int n_joint, const int *joints, double *spds) override;
1055  virtual bool getRefAccelerationsRaw(const int n_joint, const int *joints, double *accs) override;
1056  virtual bool stopRaw(const int n_joint, const int *joints) override;
1057  virtual bool getTargetPositionRaw(const int joint, double *ref) override;
1058  virtual bool getTargetPositionsRaw(double *refs) override;
1059  virtual bool getTargetPositionsRaw(const int n_joint, const int *joints, double *refs) override;
1060 
1061  // IVelocityControl2
1062  virtual bool velocityMoveRaw(const int n_joint, const int *joints, const double *spds) override;
1063  virtual bool getRefVelocityRaw(const int joint, double *ref) override;
1064  virtual bool getRefVelocitiesRaw(double *refs) override;
1065  virtual bool getRefVelocitiesRaw(const int n_joint, const int *joints, double *refs) override;
1066  //helper
1067  bool helper_getVelPidRaw(int j, Pid *pid);
1068  bool helper_setVelPidRaw(int j, const Pid &pid);
1069 
1070  // Firmware version
1071  virtual bool getFirmwareVersionRaw(int axis, can_protocol_info const& icub_interface_protocol, firmware_info *info);
1072  // Torque measurement selection
1073  virtual bool setTorqueSource(int axis, char board_id, char board_chan);
1074 
1075  // PositionDirect Interface
1076  virtual bool setPositionRaw(int j, double ref) override;
1077  virtual bool setPositionsRaw(const int n_joint, const int *joints, const double *refs) override;
1078  virtual bool setPositionsRaw(const double *refs) override;
1079  virtual bool getRefPositionRaw(const int joint, double *ref) override;
1080  virtual bool getRefPositionsRaw(double *refs) override;
1081  virtual bool getRefPositionsRaw(const int n_joint, const int *joints, double *refs) override;
1082 
1083  // InteractionMode interface
1084  virtual bool getInteractionModeRaw(int axis, yarp::dev::InteractionModeEnum* mode) override;
1085  virtual bool getInteractionModesRaw(int n_joints, int *joints, yarp::dev::InteractionModeEnum* modes) override;
1086  virtual bool getInteractionModesRaw(yarp::dev::InteractionModeEnum* modes) override;
1087  virtual bool setInteractionModeRaw(int axis, yarp::dev::InteractionModeEnum mode) override;
1088  virtual bool setInteractionModesRaw(int n_joints, int *joints, yarp::dev::InteractionModeEnum* modes) override;
1089  virtual bool setInteractionModesRaw(yarp::dev::InteractionModeEnum* modes) override;
1090 
1091  // PWMControl
1092  virtual bool setRefDutyCycleRaw(int j, double v) override;
1093  virtual bool setRefDutyCyclesRaw(const double *v) override;
1094  virtual bool getRefDutyCycleRaw(int j, double *v) override;
1095  virtual bool getRefDutyCyclesRaw(double *v) override;
1096  virtual bool getDutyCycleRaw(int j, double *v) override;
1097  virtual bool getDutyCyclesRaw(double *v) override;
1098 
1099  // CurrentControl
1100  // virtual bool getAxes(int *ax) override;
1101  //virtual bool getCurrentRaw(int j, double *t) override;
1102  //virtual bool getCurrentsRaw(double *t) override;
1103  virtual bool getCurrentRangeRaw(int j, double *min, double *max) override;
1104  virtual bool getCurrentRangesRaw(double *min, double *max) override;
1105  virtual bool setRefCurrentsRaw(const double *t) override;
1106  virtual bool setRefCurrentRaw(int j, double t) override;
1107  virtual bool setRefCurrentsRaw(const int n_joint, const int *joints, const double *t) override;
1108  virtual bool getRefCurrentsRaw(double *t) override;
1109  virtual bool getRefCurrentRaw(int j, double *t) override;
1110  //helper
1111  bool helper_setCurPidRaw(int j, const Pid &pid);
1112  bool helper_getCurPidRaw(int j, Pid *pid);
1113 
1114 protected:
1115  bool setBCastMessages (int axis, unsigned int v);
1116 
1117 protected:
1119  std::recursive_mutex _mutex;
1120  ICanBus *canController;
1121 
1123  bool _noreply;
1124  bool _opened;
1126 
1130  int _filter;
1131 
1138  inline bool ENABLED (int axis);
1139 
1140  virtual void run(void);
1141  virtual bool threadInit();
1142  virtual void threadRelease();
1143 
1144  // helper functions
1145  bool _writeWord16 (int msg, int axis, short s);
1146  bool _writeWord16Ex (int msg, int axis, short s1, short s2, bool check=true);
1147  bool _readWord16 (int msg, int axis, short& value);
1148  bool _readWord16Ex (int msg, int axis, short& value1, short& value2);
1149  bool _readWord16Array (int msg, double *out);
1150  bool _readDWord (int msg, int axis, int& value);
1151  bool _readDWordArray (int msg, double *out);
1152  bool _writeDWord (int msg, int axis, int value);
1153  bool _writeNone (int msg, int axis);
1154  bool _writeByte8 (int msg, int axis, int value);
1155  bool _readByte8(int msg, int axis, int& value);
1156  bool _writeByteWords16(int msg, int axis, unsigned char value, short s1, short s2, short s3);
1162 
1163  inline icubCanProto_controlmode_t from_modevocab_to_modeint (int modevocab);
1164  inline int from_modeint_to_modevocab (unsigned char modeint);
1165  inline unsigned char from_interactionvocab_to_interactionint (int interactionvocab);
1166  inline int from_interactionint_to_interactionvocab (unsigned char interactionint);
1167 
1168  // internal stuff.
1169  double *_ref_speeds; // used for position control.
1170  double *_ref_command_speeds; // used for velocity control.
1171  double *_ref_accs; // for velocity control, in position min jerk eq is used.
1172  double *_ref_torques; // for torque control.
1173  double *_ref_command_positions; // for position control.
1174  double *_ref_positions; // for direct position control
1175  double *_max_vel_jnt_cmd; // maximum velocity command for a joint, internally cached, not sent to the fw yet
1177  #define MAX_POSITION_MOVE_INTERVAL 0.080
1180  enum { MAX_SHORT = 32767, MIN_SHORT = -32768, MAX_INT = 0x7fffffff, MIN_INT = 0x80000000 };
1181  enum { CAN_SKIP_ADDR = 0x80 };
1182 
1183  inline short S_16(double x) const
1184  {
1185  if (x <= double(-(MAX_SHORT))-1)
1186  return MIN_SHORT;
1187  else
1188  if (x >= double(MAX_SHORT))
1189  return MAX_SHORT;
1190  else
1191  if (x>0)
1192  return short(x + .5);
1193  else
1194  return short(x - .5);
1195  }
1196 
1197  inline int S_32(double x) const
1198  {
1199  if (x <= double(-(MAX_INT))-1.0)
1200  return MIN_INT;
1201  else
1202  if (x >= double(MAX_INT))
1203  return MAX_INT;
1204  else
1205  if (x>0)
1206  return int(x + .5);
1207  else
1208  return int(x - .5);
1209  }
1210 };
1211 
1212 #endif
1213 
1214 
#define logNetworkData(a, b, c, d)
int AnalogDataFormat
#define BOARD_TYPE_4DC
#define BOARD_TYPE_BLL
#define logJointData(a, b, c, d, e)
Interface for a factory device; a device that can create objects.
@ data
@ ack
double & operator[](int i)
TBR_AnalogData(int ch, int buffsize)
virtual int calibrateChannel(int ch, double v)
std::string getDeviceId()
double getScaleFactor(int chan)
bool open(int channels, AnalogDataFormat f, short bId, short useCalib, bool isVirtualSensor)
virtual int getState(int ch)
TBR_CanBackDoor * backDoor
virtual int read(yarp::sig::Vector &out)
void getCounters(unsigned int &sat, unsigned int &err, unsigned int &to)
void setDeviceId(std::string id)
virtual int calibrateSensor(const yarp::sig::Vector &value)
virtual int calibrateSensor()
virtual int calibrateChannel(int ch)
axisImpedanceHelper(int njoints, ImpedanceLimits *imped_limits)
ImpedanceLimits * getImpedanceLimits()
double posA2E(double ang, int j)
double getSaturatedValue(int j, double curr_value, double ref_value)
axisPositionDirectHelper(int njoints, const int *aMap, const double *angToEncs, double *_maxStep)
double posE2A(double ang, int j)
double getMaximumTorque(int jnt)
double getNewtonsToSensor(int jnt)
axisTorqueHelper(int njoints, int *id, int *chan, double *maxTrq, double *newtons2sens)
int getTorqueSensorChan(int jnt)
int getTorqueSensorId(int jnt)
firmwareVersionHelper(int joints, firmware_info *f_infos, can_protocol_info &protocol)
can_protocol_info icub_protocol
speedEstimationHelper(int njoints, SpeedEstimationParameters *estim_parameters)
SpeedEstimationParameters getEstimationParameters(int jnt)
The PlxCan motion controller device driver.
double * _maxTorque
Channel of associated Joint Torque Sensor.
int _timeout
thread polling interval [ms]
std::string * _axisName
number of cycles before timing out
DebugParameters * _debug_params
parameters for speed/acceleration estimation
double * _optical_factor
max velocity command for a joint
bool parseImpedanceGroup_NewFormat(yarp::os::Bottle &pidsGroup, ImpedanceParameters vals[])
int * _torqueSensorId
reduction ratio of the optical encoder on motor axis
double * _ktau
bemf compensation gain
double * _ampsToSensor
Newtons to force sensor units conversion factors.
double * _rotToEncoder
angle to encoder conversion factors
double * _zeros
angle to rotor conversion factors
bool parseTrqPidsGroup_OldFormat(yarp::os::Bottle &pidsGroup, int nj, Pid myPid[])
bool setBroadCastMask(yarp::os::Bottle &list, int MASK)
SpeedEstimationParameters * _estim_params
set to true if pwm is limited
unsigned char _my_address
destination addresses
bool parsePosPidsGroup_OldFormat(yarp::os::Bottle &pidsGroup, int nj, Pid myPid[])
unsigned char * _destinations
number of joints/axes/controlled motors
bool fromConfig(yarp::os::Searchable &config)
double * _angleToEncoder
axis remapping lookup-table
~CanBusMotionControlParameters()
Destructor, with memory deallocation.
double * _maxJntCmdVelocity
max size of a positionDirect step
int * _torqueSensorChan
Id of associated Joint Torque Sensor.
double * _newtonsToSensor
Max torque of a joint.
ImpedanceParameters * _impedance_params
debug parameters
CanBusMotionControlParameters()
Constructor (please make sure you use the constructor to allocate memory).
bool parsePidsGroup_NewFormat(yarp::os::Bottle &pidsGroup, Pid myPid[])
ImpedanceLimits * _impedance_limits
impedance parameters
bool parseDebugGroup_NewFormat(yarp::os::Bottle &pidsGroup, DebugParameters vals[])
canbusmotioncontrol : driver for motor control boards on a CAN bus.
virtual bool setInteractionModeRaw(int axis, yarp::dev::InteractionModeEnum mode) override
bool _readDWordArray(int msg, double *out)
reads an array of double words.
virtual bool enablePidRaw(const PidControlTypeEnum &pidtype, int j) override
virtual bool getTemperatureLimitRaw(int m, double *temp) override
virtual bool setEncoderRaw(int j, double val) override
bool _readByte8(int msg, int axis, int &value)
virtual bool getRefVelocitiesRaw(double *refs) override
virtual bool setRefCurrentsRaw(const double *t) override
bool _writeWord16(int msg, int axis, short s)
to send a Word16.
virtual bool getMotorEncodersTimedRaw(double *v, double *t) override
virtual bool getCurrentImpedanceLimitRaw(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp) override
bool _readWord16Ex(int msg, int axis, short &value1, short &value2)
virtual bool getEncoderAccelerationsRaw(double *accs) override
virtual bool setRefSpeedsRaw(const double *spds) override
virtual bool getMotorEncoderRaw(int m, double *v) override
icubCanProto_controlmode_t from_modevocab_to_modeint(int modevocab)
virtual bool setPrintFunction(int(*f)(const char *fmt,...))
IControlDebug Interface.
void setServerLogger(const IServerLogger *server)
virtual bool getRemoteVariableRaw(std::string key, yarp::os::Bottle &val) override
virtual bool getNominalCurrentRaw(int m, double *val) override
int from_interactionint_to_interactionvocab(unsigned char interactionint)
virtual bool setPidReferenceRaw(const PidControlTypeEnum &pidtype, int j, double ref) override
cmd is a SingleAxis poitner with 1 double arg
virtual bool getCurrentRaw(int j, double *val) override
virtual bool setRefAccelerationRaw(int j, double acc) override
virtual bool getEncoderRaw(int j, double *v) override
virtual bool setParameterRaw(int j, unsigned int type, double value)
axisPositionDirectHelper * _axisPositionDirectHelper
firmwareVersionHelper * _firmwareVersionHelper
virtual bool getPidRaw(const PidControlTypeEnum &pidtype, int j, Pid *pid) override
virtual bool getEncodersRaw(double *encs) override
virtual bool getLimitsRaw(int axis, double *min, double *max) override
virtual bool disableAmpRaw(int j) override
virtual bool setTorqueSource(int axis, char board_id, char board_chan)
virtual bool getRefVelocityRaw(const int joint, double *ref) override
virtual bool setInteractionModesRaw(int n_joints, int *joints, yarp::dev::InteractionModeEnum *modes) override
virtual bool setMotorEncoderCountsPerRevolutionRaw(int m, const double cpr) override
virtual bool setPidOffsetRaw(const PidControlTypeEnum &pidtype, int j, double v) override
virtual bool getTorqueRangesRaw(double *min, double *max) override
virtual bool getDutyCyclesRaw(double *v) override
bool helper_getTrqPidRaw(int j, Pid *pid)
virtual bool getRefTorqueRaw(int j, double *ref_trq) override
TORQUE CONTROL INTERFACE RAW.
virtual bool setRefTorquesRaw(const double *ref_trqs) override
cmd is an array of double (LATER: to be optimized).
bool _writeWord16Ex(int msg, int axis, short s1, short s2, bool check=true)
two shorts in a single Can message (both must belong to the same control card).
virtual bool setPositionsRaw(const int n_joint, const int *joints, const double *refs) override
bool helper_setTrqPidRaw(int j, const Pid &pid)
virtual bool getPidOutputRaw(const PidControlTypeEnum &pidtype, int j, double *out) override
speedEstimationHelper * _speedEstimationHelper
virtual bool resetEncodersRaw() override
yarp::dev::DeviceDriver * createDevice(yarp::os::Searchable &config)
bool _writeDWord(int msg, int axis, int value)
write a DWord
CanBusMotionControl()
Default constructor.
virtual bool getImpedanceOffsetRaw(int j, double *offs) override
bool _readWord16(int msg, int axis, short &value)
virtual yarp::os::Stamp getLastInputStamp() override
virtual bool getEncoderSpeedsRaw(double *spds) override
virtual bool setMotorEncodersRaw(const double *vals) override
bool helper_getVelPidRaw(int j, Pid *pid)
virtual bool calibrationDoneRaw(int j) override
virtual bool getMotorEncoderAccelerationRaw(int m, double *spds) override
virtual bool getControlModesRaw(int *v) override
virtual bool setVelLimitsRaw(int axis, double min, double max) override
virtual bool setMaxCurrentRaw(int j, double val) override
virtual bool checkMotionDoneRaw(bool *flag) override
ret is a pointer to a bool
virtual bool getVelLimitsRaw(int axis, double *min, double *max) override
virtual bool getCurrentsRaw(double *vals) override
bool getFilterTypeRaw(int j, int *type)
virtual bool getCurrentRangeRaw(int j, double *min, double *max) override
bool open(const CanBusMotionControlParameters &par)
Open the device driver.
virtual bool getInteractionModesRaw(int n_joints, int *joints, yarp::dev::InteractionModeEnum *modes) override
virtual bool getRefPositionsRaw(double *refs) override
bool ENABLED(int axis)
helper function to check whether the enabled flag is on or off.
bool helper_setPosPidRaw(int j, const Pid &pid)
virtual bool setControlModesRaw(const int n_joint, const int *joints, int *modes) override
virtual bool getPidReferencesRaw(const PidControlTypeEnum &pidtype, double *refs) override
virtual bool setImpedanceRaw(int j, double stiff, double damp) override
virtual bool setPidErrorLimitRaw(const PidControlTypeEnum &pidtype, int j, double limit) override
virtual bool getEncoderTimedRaw(int j, double *v, double *t) override
virtual bool getCurrentRangesRaw(double *min, double *max) override
virtual bool getMotorEncoderTimedRaw(int m, double *v, double *t) override
virtual bool getJointTypeRaw(int axis, yarp::dev::JointTypeEnum &type) override
axisImpedanceHelper * _axisImpedanceHelper
virtual bool getMotorEncoderSpeedRaw(int m, double *sp) override
virtual bool setTemperatureLimitRaw(int m, const double temp) override
virtual bool getEncodersTimedRaw(double *v, double *t) override
virtual bool resetMotorEncodersRaw() override
virtual bool getPidErrorsRaw(const PidControlTypeEnum &pidtype, double *errs) override
virtual bool getInteractionModeRaw(int axis, yarp::dev::InteractionModeEnum *mode) override
virtual bool setImpedanceOffsetRaw(int j, double offs) override
virtual bool getControlModeRaw(int j, int *v) override
virtual bool getRefCurrentsRaw(double *t) override
bool helper_getPosPidRaw(int j, Pid *pid)
bool _readWord16Array(int msg, double *out)
reads an array.
virtual bool getPowerSupplyVoltageRaw(int j, double *val) override
virtual bool getTargetPositionRaw(const int joint, double *ref) override
virtual bool getNumberOfMotorEncodersRaw(int *num) override
bool helper_setCurPidRaw(int j, const Pid &pid)
int from_modeint_to_modevocab(unsigned char modeint)
bool _writeByteWords16(int msg, int axis, unsigned char value, short s1, short s2, short s3)
virtual bool getAxes(int *ax) override
POSITION CONTROL INTERFACE RAW.
virtual bool setMotorEncoderRaw(int m, const double val) override
virtual bool getRefCurrentRaw(int j, double *t) override
virtual bool getRefAccelerationRaw(int j, double *acc) override
cmd is an array of double (LATER: to be optimized).
virtual bool setPWMLimitRaw(int j, const double val) override
bool _writeNone(int msg, int axis)
WRITE functions sends a message without parameters.
bool setBCastMessages(int axis, unsigned int v)
sets the broadcast policy for a given board (don't need to be called twice).
virtual bool calibrateAxisWithParamsRaw(int axis, unsigned int type, double p1, double p2, double p3) override
virtual bool getRefSpeedsRaw(double *spds) override
cmd is an array of double (LATER: to be optimized).
virtual bool close(void)
Closes the device driver.
virtual bool setPositionRaw(int j, double ref) override
virtual bool open(yarp::os::Searchable &config)
Open the device driver and start communication with the hardware.
bool _readDWord(int msg, int axis, int &value)
READ functions sends a message and gets a dword back.
virtual bool setDebugReferencePositionRaw(int j, double value)
virtual bool getPWMLimitRaw(int j, double *val) override
unsigned char from_interactionvocab_to_interactionint(int interactionvocab)
virtual bool getAxisNameRaw(int axis, std::string &name) override
IAxisInfo.
virtual bool positionMoveRaw(int j, double ref) override
virtual bool setPidErrorLimitsRaw(const PidControlTypeEnum &pidtype, const double *limits) override
virtual bool setRefCurrentRaw(int j, double t) override
virtual bool getEncoderAccelerationRaw(int j, double *spds) override
virtual bool getTorqueRangeRaw(int j, double *min, double *max) override
virtual bool setPidReferencesRaw(const PidControlTypeEnum &pidtype, const double *refs) override
cmd is an array of double (LATER: to be optimized).
virtual bool setPidRaw(const PidControlTypeEnum &pidtype, int j, const Pid &pid) override
virtual bool setPeakCurrentRaw(int m, const double val) override
virtual bool setLimitsRaw(int axis, double min, double max) override
virtual bool getFirmwareVersionRaw(int axis, can_protocol_info const &icub_interface_protocol, firmware_info *info)
virtual bool getTemperatureRaw(int m, double *val) override
virtual bool getDutyCycleRaw(int j, double *v) override
virtual bool relativeMoveRaw(int j, double delta) override
virtual bool getNumberOfMotorsRaw(int *m) override
IMotor.
virtual bool getDebugParameterRaw(int j, unsigned int index, double *value)
virtual bool getRefDutyCycleRaw(int j, double *v) override
virtual bool getMaxCurrentRaw(int j, double *val) override
int _filter
filter for recurrent messages.
virtual bool getRefDutyCyclesRaw(double *v) override
virtual bool isPidEnabledRaw(const PidControlTypeEnum &pidtype, int j, bool *enabled)
virtual bool getMotorTorqueParamsRaw(int j, MotorTorqueParameters *params) override
virtual bool getMotorEncodersRaw(double *encs) override
virtual bool setRefDutyCyclesRaw(const double *v) override
virtual bool getMotorEncoderSpeedsRaw(double *spds) override
virtual bool getRefAccelerationsRaw(double *accs) override
cmd is an array of double (LATER: to be optimized).
virtual bool getMotorEncoderCountsPerRevolutionRaw(int m, double *cpr) override
virtual bool velocityMoveRaw(int j, double sp) override
Velocity control interface raw.
bool setVelocityShiftRaw(int j, double val)
virtual bool getPidsRaw(const PidControlTypeEnum &pidtype, Pid *pids) override
virtual bool getPeakCurrentRaw(int m, double *val) override
virtual bool getTargetPositionsRaw(double *refs) override
bool setVelocityTimeoutRaw(int j, double val)
virtual bool resetEncoderRaw(int j) override
virtual bool getRemoteVariablesListRaw(yarp::os::Bottle *listOfKeys) override
virtual bool setCalibrationParametersRaw(int j, const CalibrationParameters &params) override
bool helper_setVelPidRaw(int j, const Pid &pid)
bool setSpeedEstimatorShiftRaw(int j, double jnt_speed, double jnt_acc, double mot_speed, double mot_acc)
virtual bool setRefTorqueRaw(int j, double ref_trq) override
cmd is a SingleAxis poitner with 1 double arg
virtual bool setControlModeRaw(const int j, const int mode) override
virtual bool getParameterRaw(int j, unsigned int type, double *value)
virtual bool getPWMRaw(int j, double *val) override
bool _writeByte8(int msg, int axis, int value)
write a byte
virtual bool setDebugParameterRaw(int j, unsigned int index, double value)
virtual bool getAmpStatusRaw(int *st) override
virtual bool getRefTorquesRaw(double *ref_trqs) override
cmd is an array of double (LATER: to be optimized).
virtual bool getPidOutputsRaw(const PidControlTypeEnum &pidtype, double *outs) override
virtual bool getPidErrorRaw(const PidControlTypeEnum &pidtype, int j, double *err) override
virtual bool setRemoteVariableRaw(std::string key, const yarp::os::Bottle &val) override
virtual bool setRefAccelerationsRaw(const double *accs) override
virtual bool getRefPositionRaw(const int joint, double *ref) override
virtual bool getPidReferenceRaw(const PidControlTypeEnum &pidtype, int j, double *ref) override
bool helper_getCurPidRaw(int j, Pid *pid)
virtual bool getImpedanceRaw(int j, double *stiff, double *damp) override
IMPEDANCE CONTROL INTERFACE RAW.
virtual bool getPidErrorLimitsRaw(const PidControlTypeEnum &pidtype, double *limits) override
virtual bool setRefSpeedRaw(int j, double sp) override
virtual bool getTorqueRaw(int j, double *trq) override
cmd is a SingleAxis pointer with 1 double arg
virtual bool resetMotorEncoderRaw(int m) override
virtual bool getDebugReferencePositionRaw(int j, double *value)
virtual bool getTemperaturesRaw(double *vals) override
virtual bool setEncodersRaw(const double *vals) override
virtual bool getEncoderSpeedRaw(int j, double *sp) override
virtual bool disablePidRaw(const PidControlTypeEnum &pidtype, int j) override
virtual bool getTorquesRaw(double *trqs) override
cmd is an array of double (LATER: to be optimized).
virtual bool setMotorTorqueParamsRaw(int j, const MotorTorqueParameters params) override
virtual bool resetPidRaw(const PidControlTypeEnum &pidtype, int j) override
virtual bool setRefDutyCycleRaw(int j, double v) override
virtual bool getPidErrorLimitRaw(const PidControlTypeEnum &pidtype, int j, double *limit) override
virtual bool setPidsRaw(const PidControlTypeEnum &pidtype, const Pid *pids) override
virtual bool enableAmpRaw(int j) override
virtual bool getRefSpeedRaw(int j, double *ref) override
virtual bool setNominalCurrentRaw(int m, const double val) override
virtual bool getMotorEncoderAccelerationsRaw(double *accs) override
Interface for a client that logs status messages to a server.
Interface for a factory device; a device that can create objects.
Interface for a server that logs status messages.
virtual bool log(const std::string &key, const yarp::os::Value &data)=0
Get a reference to log server data.
_3f_vect_t acc
Definition: dataTypes.h:1
uint8_t board
static int v
Definition: iCub_Sim.cpp:42
#define LAST_MC4_BUILD
Definition: messages.h:25
#define LAST_BLL_BUILD
Definition: messages.h:24
double sat(const double val, const double min, const double max)
Definition: utils.h:183
const FSC max
Definition: strain.h:48
const FSC min
Definition: strain.h:49
Copyright (C) 2008 RobotCub Consortium.
GLenum mode
Definition: rendering.cpp:48
out
Definition: sine.m:8
std::string network_name
can_protocol_info can_protocol