iCub-main
solver.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 __SOLVER_H__
20 #define __SOLVER_H__
21 
22 #include <mutex>
23 #include <string>
24 
25 #include <yarp/os/all.h>
26 #include <yarp/sig/all.h>
27 #include <yarp/dev/all.h>
28 #include <yarp/math/Math.h>
29 
30 #include <iCub/ctrl/pids.h>
32 #include <iCub/gazeNlp.h>
33 #include <iCub/utils.h>
34 #include <iCub/localizer.h>
35 #include <iCub/controller.h>
36 
37 constexpr double EYEPINVREFGEN_GAIN = 12.5; // [-]
38 constexpr double SACCADES_VEL = 1000.0; // [deg/s]
39 constexpr double SACCADES_INHIBITION_PERIOD = 0.2; // [s]
40 constexpr double SACCADES_ACTIVATION_ANGLE = 10.0; // [deg]
41 constexpr double NECKSOLVER_ACTIVATIONDELAY = 0.25; // [s]
42 constexpr double NECKSOLVER_ACTIVATIONANGLE_JOINTS = 0.5; // [deg/s]
43 constexpr double NECKSOLVER_ACTIVATIONANGLE = 2.5; // [deg]
44 constexpr double NECKSOLVER_RESTORINGANGLE = 5.0; // [deg]
45 
46 using namespace std;
47 using namespace yarp::os;
48 using namespace yarp::dev;
49 using namespace yarp::sig;
50 using namespace yarp::math;
51 using namespace iCub::ctrl;
52 using namespace iCub::iKin;
53 
54 
55 // The thread launched by the application which computes
56 // the eyes target position relying on the pseudoinverse
57 // method.
58 class EyePinvRefGen : public GazeComponent, public PeriodicThread
59 {
60 protected:
63  iKinChain *chainNeck, *chainEyeL, *chainEyeR;
64  PolyDriver *drvTorso, *drvHead;
68  mutex mtx;
69 
74 
75  unsigned int period;
77  bool genOn;
81  double saccadesClock;
83  double Ts;
84 
85  Matrix orig_lim,lim;
86  Vector fbTorso;
87  Vector fbHead;
88  Vector qd,fp;
89  Matrix eyesJ;
91 
92  Vector getEyesCounterVelocity(const Matrix &eyesJ, const Vector &fp);
93 
94 public:
95  EyePinvRefGen(PolyDriver *_drvTorso, PolyDriver *_drvHead, ExchangeData *_commData,
96  Controller *_ctrl, const Vector &_counterRotGain, const unsigned int _period);
97  virtual ~EyePinvRefGen();
98 
99  void enable() { genOn=true; }
100  void disable() { genOn=false; }
101  Vector getCounterRotGain();
102  void setCounterRotGain(const Vector &gain);
103  void minAllowedVergenceChanged() override;
104  bool bindEyes(const double ver);
105  bool clearEyes();
106  void manageBindEyes(const double ver);
107  bool threadInit() override;
108  void threadRelease() override;
109  void afterStart(bool s) override;
110  void run() override;
111  void suspend();
112  void resume();
113 };
114 
115 
116 // The thread launched by the application which is
117 // in charge of inverting the head kinematic relying
118 // on IPOPT computation.
119 class Solver : public GazeComponent, public PeriodicThread
120 {
121 protected:
124  iKinChain *chainNeck, *chainEyeL, *chainEyeR;
126  PolyDriver *drvTorso, *drvHead;
131  mutex mtx;
132 
133  unsigned int period;
137  double Ts;
138 
139  Vector fbTorso;
140  Vector fbHead;
141  Vector neckPos;
142  Vector gazePos;
143 
145 
146  double neckPitchMin;
147  double neckPitchMax;
148  double neckRollMin;
149  double neckRollMax;
150  double neckYawMin;
151  double neckYawMax;
152 
153  void updateAngles();
154  Vector computeTargetUserTolerance(const Vector &xd);
155 
156 public:
157  Solver(PolyDriver *_drvTorso, PolyDriver *_drvHead, ExchangeData *_commData,
158  EyePinvRefGen *_eyesRefGen, Localizer *_loc, Controller *_ctrl,
159  const unsigned int _period);
160  virtual ~Solver();
161 
162  // Returns a measure of neck angle required to reach the target
163  double neckTargetRotAngle(const Vector &xd);
164  void bindNeckPitch(const double min_deg, const double max_deg);
165  void bindNeckRoll(const double min_deg, const double max_deg);
166  void bindNeckYaw(const double min_deg, const double max_deg);
167  void getCurNeckPitchRange(double &min_deg, double &max_deg);
168  void getCurNeckRollRange(double &min_deg, double &max_deg);
169  void getCurNeckYawRange(double &min_deg, double &max_deg);
170  void clearNeckPitch();
171  void clearNeckRoll();
172  void clearNeckYaw();
173  double getNeckAngleUserTolerance() const;
174  void setNeckAngleUserTolerance(const double angle);
175  bool threadInit() override;
176  void threadRelease() override;
177  void afterStart(bool s) override;
178  void run() override;
179  void suspend();
180  void resume();
181 };
182 
183 
184 #endif
185 
186 
iCubInertialSensor * imu
Definition: solver.h:62
double eyesHalfBaseline
Definition: solver.h:82
Vector counterRotGain
Definition: solver.h:90
mutex mtx
Definition: solver.h:68
double orig_eye_pan_max
Definition: solver.h:73
Integrator * I
Definition: solver.h:67
bool saccadeUnderWayOld
Definition: solver.h:76
void enable()
Definition: solver.h:99
Matrix lim
Definition: solver.h:85
ExchangeData * commData
Definition: solver.h:65
int nJointsTorso
Definition: solver.h:78
Vector fbTorso
Definition: solver.h:86
Vector fbHead
Definition: solver.h:87
double orig_eye_tilt_max
Definition: solver.h:71
Controller * ctrl
Definition: solver.h:66
double orig_eye_tilt_min
Definition: solver.h:70
void disable()
Definition: solver.h:100
Vector fp
Definition: solver.h:88
iKinChain * chainEyeL
Definition: solver.h:63
iCubHeadCenter * neck
Definition: solver.h:61
double saccadesClock
Definition: solver.h:81
bool genOn
Definition: solver.h:77
PolyDriver * drvHead
Definition: solver.h:64
int nJointsHead
Definition: solver.h:79
double orig_eye_pan_min
Definition: solver.h:72
double Ts
Definition: solver.h:83
unsigned int period
Definition: solver.h:75
Matrix eyesJ
Definition: solver.h:89
int saccadesRxTargets
Definition: solver.h:80
Definition: solver.h:120
unsigned int period
Definition: solver.h:133
PolyDriver * drvHead
Definition: solver.h:126
double neckRollMin
Definition: solver.h:148
Controller * ctrl
Definition: solver.h:130
int nJointsHead
Definition: solver.h:135
iCubHeadCenter * neck
Definition: solver.h:122
ExchangeData * commData
Definition: solver.h:127
mutex mtx
Definition: solver.h:131
double neckPitchMin
Definition: solver.h:146
EyePinvRefGen * eyesRefGen
Definition: solver.h:128
double neckYawMin
Definition: solver.h:150
Vector fbHead
Definition: solver.h:140
Vector neckPos
Definition: solver.h:141
iKinChain * chainEyeL
Definition: solver.h:124
double Ts
Definition: solver.h:137
iCubInertialSensor * imu
Definition: solver.h:123
double neckAngleUserTolerance
Definition: solver.h:136
int nJointsTorso
Definition: solver.h:134
Localizer * loc
Definition: solver.h:129
GazeIpOptMin * invNeck
Definition: solver.h:125
Vector fbTorso
Definition: solver.h:139
double neckYawMax
Definition: solver.h:151
double neckRollMax
Definition: solver.h:149
double neckPitchMax
Definition: solver.h:147
Vector gazePos
Definition: solver.h:142
AWLinEstimator * torsoVel
Definition: solver.h:144
Adaptive window linear fitting to estimate the first derivative.
A class for defining a saturated integrator based on Tustin formula: .
Definition: pids.h:48
A class for describing the kinematic of the straight line coming out from the point located between t...
Definition: iKinFwd.h:1451
A class for defining the Inertia Sensor Kinematics of the iCub.
Definition: iKinFwd.h:1477
A Base class for defining a Serial Link Chain.
Definition: iKinFwd.h:355
static float angle
Definition: iCub_Sim.cpp:72
static struct bpf_program fp
constexpr double NECKSOLVER_ACTIVATIONANGLE_JOINTS
Definition: solver.h:42
constexpr double SACCADES_VEL
Definition: solver.h:38
constexpr double SACCADES_ACTIVATION_ANGLE
Definition: solver.h:40
constexpr double NECKSOLVER_RESTORINGANGLE
Definition: solver.h:44
constexpr double EYEPINVREFGEN_GAIN
Definition: solver.h:37
constexpr double SACCADES_INHIBITION_PERIOD
Definition: solver.h:39
constexpr double NECKSOLVER_ACTIVATIONANGLE
Definition: solver.h:43
constexpr double NECKSOLVER_ACTIVATIONDELAY
Definition: solver.h:41