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  // data members that do not need protection
136  string robotName;
138  Vector eyeTiltLim;
140  double eyesBoundVer;
150  bool verbose;
156  ResourceFinder rf_cameras;
157  ResourceFinder rf_tweak;
158  string tweakFile;
160 
161  IThreeAxisGyroscopes* iGyro;
162  IThreeAxisLinearAccelerometers* iAccel;
163 };
164 
165 
166 
167 // This class defines gaze components such as
168 // controller, localizer, solver ...
170 {
171 protected:
172  iCubEye *eyeL{nullptr};
173  iCubEye *eyeR{nullptr};
174 
175 public:
176  virtual bool getExtrinsicsMatrix(const string &type, Matrix &M);
177  virtual bool setExtrinsicsMatrix(const string &type, const Matrix &M);
178  virtual void minAllowedVergenceChanged() { }
179 };
180 
181 
182 // Saturate val between min and max.
183 inline double sat(const double val, const double min, const double max)
184 {
185  return std::min(std::max(val,min),max);
186 }
187 
188 
189 // Allocates Projection Matrix Prj for the camera read from cameras::file;
190 // type is in {"CAMERA_CALIBRATION_LEFT","CAMERA_CALIBRATION_RIGHT"}.
191 // Returns true if correctly configured.
192 bool getCamParams(const ResourceFinder &rf, const string &type, Matrix **Prj, int &w, int &h, const bool verbose=false);
193 
194 
195 // Allocates the two aligning matrices read from cameras::file;
196 // type is in {"ALIGN_KIN_LEFT","ALIGN_KIN_RIGHT"}.
197 // Returns true if correctly configured.
198 bool getAlignHN(const ResourceFinder &rf, const string &type, iKinChain *chain, const bool verbose=false);
199 
200 
201 // Aligns head joints bounds with current onboard bounds.
202 // Returns a matrix containing the actual limits.
203 Matrix alignJointsBounds(iKinChain *chain, PolyDriver *drvTorso, PolyDriver *drvHead,
204  const ExchangeData *commData);
205 
206 
207 // Copies joints bounds from first chain to second chain.
208 void copyJointsBounds(iKinChain *ch1, iKinChain *ch2);
209 
210 
211 // Updates torso blocked joints values within the chain.
212 void updateTorsoBlockedJoints(iKinChain *chain, const Vector &fbTorso);
213 
214 
215 // Updates neck blocked joints values within the chain.
216 void updateNeckBlockedJoints(iKinChain *chain, const Vector &fbNeck);
217 
218 
219 // Reads encoders values.
220 // Returns true if communication with robot is stable, false otherwise.
221 bool getFeedback(Vector &fbTorso, Vector &fbHead, PolyDriver *drvTorso,
222  PolyDriver *drvHead, const ExchangeData *commData,
223  double *timeStamp = nullptr);
224 
225 #endif
226 
227 
Vector qd
Definition: utils.h:97
ResourceFinder rf_cameras
Definition: utils.h:156
double saccadesActivationAngle
Definition: utils.h:145
bool useMASClient
Definition: utils.h:155
string robotName
Definition: utils.h:136
double x_stamp
Definition: utils.h:102
bool stabilizationOn
Definition: utils.h:154
double stabilizationGain
Definition: utils.h:142
bool trackingModeOn
Definition: utils.h:148
IThreeAxisLinearAccelerometers * iAccel
Definition: utils.h:162
Vector q
Definition: utils.h:98
Vector imu
Definition: utils.h:101
Vector counterv
Definition: utils.h:99
double gyro_noise_threshold
Definition: utils.h:141
double saccadesInhibitionPeriod
Definition: utils.h:144
bool tweakOverwrite
Definition: utils.h:151
ResourceFinder rf_tweak
Definition: utils.h:157
Vector eyeTiltLim
Definition: utils.h:138
string tweakFile
Definition: utils.h:158
bool ctrlActive
Definition: utils.h:147
double minAllowedVergence
Definition: utils.h:139
int neckSolveCnt
Definition: utils.h:146
xdPort * port_xd
Definition: utils.h:135
IThreeAxisGyroscopes * iGyro
Definition: utils.h:161
bool debugInfoEnabled
Definition: utils.h:159
bool saccadesOn
Definition: utils.h:152
iKinLimbVersion head_version
Definition: utils.h:143
bool neckPosCtrlOn
Definition: utils.h:153
bool saccadeUnderway
Definition: utils.h:149
double eyesBoundVer
Definition: utils.h:140
bool verbose
Definition: utils.h:150
Matrix S
Definition: utils.h:100
string localStemName
Definition: utils.h:137
virtual void minAllowedVergenceChanged()
Definition: utils.h:178
A class for defining the iCub Eye.
Definition: iKinFwd.h:1386
A Base class for defining a Serial Link Chain.
Definition: iKinFwd.h:355
A class for defining the versions of the iCub limbs.
Definition: iKinFwd.h:1046
Definition: utils.h:56
int get_rx() const
Definition: utils.h:83
bool isNew
Definition: utils.h:66
void lock()
Definition: utils.h:80
bool locked
Definition: utils.h:68
condition_variable cv_triggerNeck
Definition: utils.h:63
bool & get_newDelayed()
Definition: utils.h:85
void unlock()
Definition: utils.h:81
void * slv
Definition: utils.h:58
bool isNewDelayed
Definition: utils.h:67
Vector xdDelayed
Definition: utils.h:65
mutex mutex_1
Definition: utils.h:61
mutex mtx_triggerNeck
Definition: utils.h:62
int rx
Definition: utils.h:70
bool closing
Definition: utils.h:69
mutex mutex_0
Definition: utils.h:60
bool islocked() const
Definition: utils.h:82
bool & get_new()
Definition: utils.h:84
Vector xd
Definition: utils.h:64
static int v
Definition: iCub_Sim.cpp:42
void copyJointsBounds(iKinChain *ch1, iKinChain *ch2)
Definition: utils.cpp:602
bool getAlignHN(const ResourceFinder &rf, const string &type, iKinChain *chain, const bool verbose=false)
Definition: utils.cpp:474
bool getCamParams(const ResourceFinder &rf, const string &type, Matrix **Prj, int &w, int &h, const bool verbose=false)
Definition: utils.cpp:415
bool getFeedback(Vector &fbTorso, Vector &fbHead, PolyDriver *drvTorso, PolyDriver *drvHead, const ExchangeData *commData, double *timeStamp=nullptr)
Definition: utils.cpp:633
void updateNeckBlockedJoints(iKinChain *chain, const Vector &fbNeck)
Definition: utils.cpp:625
void updateTorsoBlockedJoints(iKinChain *chain, const Vector &fbTorso)
Definition: utils.cpp:617
Matrix alignJointsBounds(iKinChain *chain, PolyDriver *drvTorso, PolyDriver *drvHead, const ExchangeData *commData)
Definition: utils.cpp:531
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