iCub-main
utils.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
3  * Author: Ugo Pattacini, Alessandro Roncone
4  * email: ugo.pattacini@iit.it, alessandro.roncone@iit.it
5  * website: www.robotcub.org
6  * Permission is granted to copy, distribute, and/or modify this program
7  * under the terms of the GNU General Public License, version 2 or any
8  * later version published by the Free Software Foundation.
9  *
10  * A copy of the license can be found at
11  * http://www.robotcub.org/icub/license/gpl.txt
12  *
13  * This program is distributed in the hope that it will be useful, but
14  * WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16  * Public License for more details
17 */
18 
19 #ifndef __UTILS_H__
20 #define __UTILS_H__
21 
22 #include <mutex>
23 #include <condition_variable>
24 #include <string>
25 #include <algorithm>
26 #include <utility>
27 
28 #include <yarp/os/all.h>
29 #include <yarp/dev/all.h>
30 #include <yarp/sig/all.h>
31 #include <yarp/math/Math.h>
32 #include <yarp/math/SVD.h>
33 
34 #include <iCub/gazeNlp.h>
35 
36 using namespace std;
37 using namespace yarp::os;
38 using namespace yarp::dev;
39 using namespace yarp::sig;
40 using namespace yarp::math;
41 using namespace iCub::ctrl;
42 using namespace iCub::iKin;
43 
44 
45 // This class handles the incoming fixation point
46 // xyz coordinates.
47 //
48 // Since it accepts a bottle, it is possible to
49 // issue the command "yarp read /sender /ctrlName/xd:i"
50 // and type the target position manually.
51 //
52 // Moreover, the possibility to delay the received
53 // target is handled as well.
54 class xdPort : public BufferedPort<Bottle>,
55  public Thread
56 {
57 protected:
58  void *slv;
59 
60  mutex mutex_0;
61  mutex mutex_1;
63  condition_variable cv_triggerNeck;
64  Vector xd;
65  Vector xdDelayed;
66  bool isNew;
68  bool locked;
69  bool closing;
70  int rx;
71 
72  void onRead(Bottle &b) override;
73  void run() override;
74 
75 public:
76  explicit xdPort(void *_slv);
77  ~xdPort();
78 
79  void init(const Vector &xd0);
80  void lock() { locked=true; }
81  void unlock() { locked=false; }
82  bool islocked() const { return locked; }
83  int get_rx() const { return rx; }
84  bool &get_new() { return isNew; }
85  bool &get_newDelayed() { return isNewDelayed; }
86  bool set_xd(const Vector &_xd);
87  Vector get_xd();
88  Vector get_xdDelayed();
89 };
90 
91 
92 // This class handles the data exchange among components.
94 {
95 protected:
96  mutex mtx[8];
97  Vector xd,qd;
98  Vector x,q,torso;
99  Vector v,counterv;
100  Matrix S;
101  Vector imu;
102  double x_stamp;
103 
104 public:
105  ExchangeData();
106 
107  void resize_v(const int sz, const double val);
108  void resize_counterv(const int sz, const double val);
109 
110  void set_xd(const Vector &_xd);
111  void set_qd(const Vector &_qd);
112  void set_qd(const int i, const double val);
113  void set_x(const Vector &_x);
114  void set_x(const Vector &_x, const double stamp);
115  void set_q(const Vector &_q);
116  void set_torso(const Vector &_torso);
117  void set_v(const Vector &_v);
118  void set_counterv(const Vector &_counterv);
119  void set_fpFrame(const Matrix &_S);
120 
121  Vector get_xd();
122  Vector get_qd();
123  Vector get_x();
124  Vector get_x(double &stamp);
125  Vector get_q();
126  Vector get_torso();
127  Vector get_v();
128  Vector get_counterv();
129  Matrix get_fpFrame();
130 
131  std::pair<Vector,bool> get_gyro();
132  std::pair<Vector,bool> get_accel();
133 
134  string headVersion2String();
135 
136  // data members that do not need protection
138  string robotName;
140  Vector eyeTiltLim;
142  double eyesBoundVer;
145  double head_version;
152  bool verbose;
158  ResourceFinder rf_cameras;
159  ResourceFinder rf_tweak;
160  string tweakFile;
162 
163  IThreeAxisGyroscopes* iGyro;
164  IThreeAxisLinearAccelerometers* iAccel;
165 };
166 
167 
168 
169 // This class defines gaze components such as
170 // controller, localizer, solver ...
172 {
173 protected:
174  iCubEye *eyeL{nullptr};
175  iCubEye *eyeR{nullptr};
176 
177 public:
178  virtual bool getExtrinsicsMatrix(const string &type, Matrix &M);
179  virtual bool setExtrinsicsMatrix(const string &type, const Matrix &M);
180  virtual void minAllowedVergenceChanged() { }
181 };
182 
183 
184 // Saturate val between min and max.
185 inline double sat(const double val, const double min, const double max)
186 {
187  return std::min(std::max(val,min),max);
188 }
189 
190 
191 // Allocates Projection Matrix Prj for the camera read from cameras::file;
192 // type is in {"CAMERA_CALIBRATION_LEFT","CAMERA_CALIBRATION_RIGHT"}.
193 // Returns true if correctly configured.
194 bool getCamParams(const ResourceFinder &rf, const string &type, Matrix **Prj, int &w, int &h, const bool verbose=false);
195 
196 
197 // Allocates the two aligning matrices read from cameras::file;
198 // type is in {"ALIGN_KIN_LEFT","ALIGN_KIN_RIGHT"}.
199 // Returns true if correctly configured.
200 bool getAlignHN(const ResourceFinder &rf, const string &type, iKinChain *chain, const bool verbose=false);
201 
202 
203 // Aligns head joints bounds with current onboard bounds.
204 // Returns a matrix containing the actual limits.
205 Matrix alignJointsBounds(iKinChain *chain, PolyDriver *drvTorso, PolyDriver *drvHead,
206  const ExchangeData *commData);
207 
208 
209 // Copies joints bounds from first chain to second chain.
210 void copyJointsBounds(iKinChain *ch1, iKinChain *ch2);
211 
212 
213 // Updates torso blocked joints values within the chain.
214 void updateTorsoBlockedJoints(iKinChain *chain, const Vector &fbTorso);
215 
216 
217 // Updates neck blocked joints values within the chain.
218 void updateNeckBlockedJoints(iKinChain *chain, const Vector &fbNeck);
219 
220 
221 // Reads encoders values.
222 // Returns true if communication with robot is stable, false otherwise.
223 bool getFeedback(Vector &fbTorso, Vector &fbHead, PolyDriver *drvTorso,
224  PolyDriver *drvHead, const ExchangeData *commData,
225  double *timeStamp = nullptr);
226 
227 #endif
228 
229 
condition_variable cv_triggerNeck
Definition: utils.h:63
bool locked
Definition: utils.h:68
mutex mutex_1
Definition: utils.h:61
IThreeAxisLinearAccelerometers * iAccel
Definition: utils.h:164
Vector xd
Definition: utils.h:64
bool stabilizationOn
Definition: utils.h:156
bool closing
Definition: utils.h:69
string robotName
Definition: utils.h:138
bool saccadesOn
Definition: utils.h:154
bool islocked() const
Definition: utils.h:82
xdPort * port_xd
Definition: utils.h:137
double eyesBoundVer
Definition: utils.h:142
STL namespace.
Definition: utils.h:54
A Base class for defining a Serial Link Chain.
Definition: iKinFwd.h:354
A class for defining the iCub Eye.
Definition: iKinFwd.h:1272
double gyro_noise_threshold
Definition: utils.h:143
Vector eyeTiltLim
Definition: utils.h:140
double stabilizationGain
Definition: utils.h:144
Matrix alignJointsBounds(iKinChain *chain, PolyDriver *drvTorso, PolyDriver *drvHead, const ExchangeData *commData)
Definition: utils.cpp:539
IThreeAxisGyroscopes * iGyro
Definition: utils.h:163
const FSC max
Definition: strain.h:48
double saccadesInhibitionPeriod
Definition: utils.h:146
double saccadesActivationAngle
Definition: utils.h:147
ResourceFinder rf_tweak
Definition: utils.h:159
int get_rx() const
Definition: utils.h:83
Vector xd
Definition: utils.h:97
const FSC min
Definition: strain.h:49
void updateTorsoBlockedJoints(iKinChain *chain, const Vector &fbTorso)
Definition: utils.cpp:625
double x_stamp
Definition: utils.h:102
void updateNeckBlockedJoints(iKinChain *chain, const Vector &fbNeck)
Definition: utils.cpp:633
bool trackingModeOn
Definition: utils.h:150
bool verbose
Definition: utils.h:152
bool neckPosCtrlOn
Definition: utils.h:155
bool ctrlActive
Definition: utils.h:149
mutex mtx_triggerNeck
Definition: utils.h:62
void unlock()
Definition: utils.h:81
Vector x
Definition: utils.h:98
bool saccadeUnderway
Definition: utils.h:151
bool & get_new()
Definition: utils.h:84
int neckSolveCnt
Definition: utils.h:148
ResourceFinder rf_cameras
Definition: utils.h:158
void * slv
Definition: utils.h:58
virtual void minAllowedVergenceChanged()
Definition: utils.h:180
bool getAlignHN(const ResourceFinder &rf, const string &type, iKinChain *chain, const bool verbose=false)
Definition: utils.cpp:482
string tweakFile
Definition: utils.h:160
double sat(const double val, const double min, const double max)
Definition: utils.h:185
bool debugInfoEnabled
Definition: utils.h:161
bool & get_newDelayed()
Definition: utils.h:85
bool useMASClient
Definition: utils.h:157
bool isNew
Definition: utils.h:66
bool getCamParams(const ResourceFinder &rf, const string &type, Matrix **Prj, int &w, int &h, const bool verbose=false)
Definition: utils.cpp:423
Vector xdDelayed
Definition: utils.h:65
string localStemName
Definition: utils.h:139
void lock()
Definition: utils.h:80
mutex mutex_0
Definition: utils.h:60
void copyJointsBounds(iKinChain *ch1, iKinChain *ch2)
Definition: utils.cpp:610
Matrix S
Definition: utils.h:100
bool isNewDelayed
Definition: utils.h:67
int rx
Definition: utils.h:70
Vector v
Definition: utils.h:99
double head_version
Definition: utils.h:145
Vector imu
Definition: utils.h:101
bool tweakOverwrite
Definition: utils.h:153
bool getFeedback(Vector &fbTorso, Vector &fbHead, PolyDriver *drvTorso, PolyDriver *drvHead, const ExchangeData *commData, double *timeStamp=nullptr)
Definition: utils.cpp:641
double minAllowedVergence
Definition: utils.h:141