iCub-main
parametricCalibratorEth.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) 20014 iCub Facility, Istituto Italiano di Tecnologia
5  *
6  * Authors: Alberto Cardellino, Marco Randazzo, Valentina Gaggero
7  *
8  * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
9  *
10  */
11 
12 
13 #ifndef __ICUB_PARAMETRIC_CALIBRATOR__
14 #define __ICUB_PARAMETRIC_CALIBRATOR__
15 
16 #include <list>
17 #include <string>
18 #include <atomic>
19 #include <vector>
20 #include <yarp/dev/PolyDriver.h>
21 #include <yarp/dev/CalibratorInterfaces.h>
22 #include <yarp/dev/ControlBoardInterfaces.h>
23 
24 namespace yarp {
25  namespace dev
26  {
27  class parametricCalibratorEth;
28  }
29 }
30 
42 class yarp::dev::parametricCalibratorEth : public ICalibrator, public DeviceDriver, public IRemoteCalibrator
43 {
44 public:
49 
54 
61  bool calibrate(DeviceDriver *device) override;
62 
69  virtual bool open (yarp::os::Searchable& config) override;
70 
75  virtual bool close () override;
76 
77  virtual bool park(DeviceDriver *dd, bool wait=true) override;
78 
79  virtual bool quitPark() override;
80 
81  virtual bool quitCalibrate() override;
82 
83  // IRemoteCalibrator
84 
85  virtual yarp::dev::IRemoteCalibrator *getCalibratorDevice() override;
86 
87  virtual bool calibrateSingleJoint(int j) override;
88 
89  virtual bool calibrateWholePart() override;
90 
91  virtual bool homingSingleJoint(int j) override;
92 
93  virtual bool homingWholePart() override;
94 
95  virtual bool parkSingleJoint(int j, bool _wait=true) override;
96 
97  virtual bool parkWholePart() override;
98 
99 private:
100 
101  // helper struct
102  struct PositionSequence
103  {
104  int seq_num{0}; // progressive number indicating sequence ID
105  std::vector<double> positions; // vector of positions, one for each joint
106  std::vector<double> velocities; // vector of velocities, one for each joint
107  };
108 
109  bool calibrate();
110  bool calibrateJoint(int j);
111  bool goToStartupPosition(int j);
112  bool checkCalibrateJointEnded(std::list<int> set);
113  bool checkGoneToZeroThreshold(int j);
114  bool checkHwFault(int j);
115 
116  yarp::dev::PolyDriver *dev2calibrate;
117  yarp::dev::IControlCalibration *iCalibrate;
118  yarp::dev::IPidControl *iPids;
119  yarp::dev::IEncoders *iEncoders;
120  yarp::dev::IPositionControl *iPosition;
121  yarp::dev::IControlMode *iControlMode;
122  yarp::dev::IAmplifierControl *iAmp;
123 
124  std::list<std::list<int> > joints;
125 
126  int n_joints;
127 
128  yarp::dev::CalibrationParameters* calibParams;
129  int *startupMaxPWM;
130  double *currPos;
131  double *currVel;
132  double *original_max_pwm;
133  double *limited_max_pwm;
134 
135  double *startupPosThreshold;
136  PositionSequence legacyStartupPosition; // upgraded old array to new struct; to be removed when old method will be deprecated
137  PositionSequence legacyParkingPosition; // upgraded old array to new struct; to be removed when old method will be deprecated
138 
139  bool abortCalib;
140  bool abortParking;
141  std::atomic<bool> isCalibrated;
142  bool skipCalibration;
143  int *disableHomeAndPark;
144  int *disableStartupPosCheck;
145  bool clearHwFault;
146 
147  int *timeout_goToZero;
148  int *timeout_calibration;
149 
150  int totJointsToCalibrate;
151  std::string deviceName;
152  yarp::os::Bottle calibJointsString; // joints handled by this caibrator as a string for error messages
153  std::list<int> calibJoints; // joints handled by this caibrator as a list
154 
155  // store the positions for a custom parking sequence
156  int currentParkingSeq_step;
157  std::vector<PositionSequence> parkingSequence;
158  bool useLegacyParking; // calibrator is using new parking sequence
159  bool parseSequenceGroup(yarp::os::Searchable &config, std::string sequence, std::vector<PositionSequence> &seqList);
160  bool moveAndCheck(PositionSequence &data);
161 
162  // to be removed
163  bool moveAndCheck_legacy(PositionSequence &data, std::vector<bool> &cannotPark, bool wait);
164 };
165 #endif
@ data
parametricCalibrator: implement calibration routines for the iCub arm(s) (version 1....
virtual bool park(DeviceDriver *dd, bool wait=true) override
virtual bool open(yarp::os::Searchable &config) override
Open the device driver.
virtual bool homingSingleJoint(int j) override
virtual bool calibrateSingleJoint(int j) override
virtual bool parkSingleJoint(int j, bool _wait=true) override
virtual bool close() override
Close the device driver.
virtual yarp::dev::IRemoteCalibrator * getCalibratorDevice() override
bool calibrate(DeviceDriver *device) override
Calibrate method.
Copyright (C) 2008 RobotCub Consortium.