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 
EyePinvRefGen::mtx
mutex mtx
Definition: solver.h:68
EyePinvRefGen::fbTorso
Vector fbTorso
Definition: solver.h:86
EYEPINVREFGEN_GAIN
constexpr double EYEPINVREFGEN_GAIN
Definition: solver.h:37
iCub::ctrl::Integrator
Definition: pids.h:47
SACCADES_INHIBITION_PERIOD
constexpr double SACCADES_INHIBITION_PERIOD
Definition: solver.h:39
NECKSOLVER_ACTIVATIONDELAY
constexpr double NECKSOLVER_ACTIVATIONDELAY
Definition: solver.h:41
Solver::commData
ExchangeData * commData
Definition: solver.h:127
Solver::neckPitchMax
double neckPitchMax
Definition: solver.h:147
EyePinvRefGen::eyesJ
Matrix eyesJ
Definition: solver.h:89
EyePinvRefGen::fbHead
Vector fbHead
Definition: solver.h:87
ExchangeData
Definition: utils.h:93
GazeComponent
Definition: utils.h:171
EyePinvRefGen::eyesHalfBaseline
double eyesHalfBaseline
Definition: solver.h:82
EyePinvRefGen::enable
void enable()
Definition: solver.h:99
Solver::chainNeck
iKinChain * chainNeck
Definition: solver.h:124
Solver::neckYawMin
double neckYawMin
Definition: solver.h:150
iCub::iKin::iKinChain
Definition: iKinFwd.h:354
EyePinvRefGen::saccadesRxTargets
int saccadesRxTargets
Definition: solver.h:80
EyePinvRefGen::saccadeUnderWayOld
bool saccadeUnderWayOld
Definition: solver.h:76
Solver::loc
Localizer * loc
Definition: solver.h:129
SACCADES_VEL
constexpr double SACCADES_VEL
Definition: solver.h:38
fp
static struct bpf_program fp
Definition: pcap_wrapper_linux.cpp:89
EyePinvRefGen::chainNeck
iKinChain * chainNeck
Definition: solver.h:63
iCub::iKin::iCubHeadCenter
Definition: iKinFwd.h:1338
Solver::fbTorso
Vector fbTorso
Definition: solver.h:139
controller.h
yarp::dev
Definition: DebugInterfaces.h:52
NECKSOLVER_ACTIVATIONANGLE_JOINTS
constexpr double NECKSOLVER_ACTIVATIONANGLE_JOINTS
Definition: solver.h:42
Controller
Definition: controller.h:53
EyePinvRefGen::genOn
bool genOn
Definition: solver.h:77
Solver::imu
iCubInertialSensor * imu
Definition: solver.h:123
Solver::neckRollMax
double neckRollMax
Definition: solver.h:149
iCub::iKin
Definition: iKinFwd.h:71
EyePinvRefGen::qd
Vector qd
Definition: solver.h:88
Solver::eyesRefGen
EyePinvRefGen * eyesRefGen
Definition: solver.h:128
Solver::neckRollMin
double neckRollMin
Definition: solver.h:148
EyePinvRefGen::orig_eye_pan_min
double orig_eye_pan_min
Definition: solver.h:72
GazeIpOptMin
Definition: gazeNlp.h:40
angle
static float angle
Definition: iCub_Sim.cpp:72
Localizer
Definition: localizer.h:44
EyePinvRefGen::ctrl
Controller * ctrl
Definition: solver.h:66
Solver::neckYawMax
double neckYawMax
Definition: solver.h:151
iCub::ctrl
Definition: adaptWinPolyEstimator.h:37
EyePinvRefGen::counterRotGain
Vector counterRotGain
Definition: solver.h:90
Solver
Definition: solver.h:119
Solver::period
unsigned int period
Definition: solver.h:133
EyePinvRefGen::I
Integrator * I
Definition: solver.h:67
EyePinvRefGen::orig_eye_tilt_min
double orig_eye_tilt_min
Definition: solver.h:70
EyePinvRefGen::nJointsHead
int nJointsHead
Definition: solver.h:79
Solver::nJointsHead
int nJointsHead
Definition: solver.h:135
EyePinvRefGen::imu
iCubInertialSensor * imu
Definition: solver.h:62
EyePinvRefGen::nJointsTorso
int nJointsTorso
Definition: solver.h:78
Solver::neckAngleUserTolerance
double neckAngleUserTolerance
Definition: solver.h:136
EyePinvRefGen::commData
ExchangeData * commData
Definition: solver.h:65
Solver::fbHead
Vector fbHead
Definition: solver.h:140
EyePinvRefGen::orig_lim
Matrix orig_lim
Definition: solver.h:85
NECKSOLVER_RESTORINGANGLE
constexpr double NECKSOLVER_RESTORINGANGLE
Definition: solver.h:44
Solver::neckPitchMin
double neckPitchMin
Definition: solver.h:146
Solver::Ts
double Ts
Definition: solver.h:137
EyePinvRefGen::disable
void disable()
Definition: solver.h:100
adaptWinPolyEstimator.h
iCub::ctrl::AWLinEstimator
Definition: adaptWinPolyEstimator.h:184
Solver::nJointsTorso
int nJointsTorso
Definition: solver.h:134
EyePinvRefGen::drvTorso
PolyDriver * drvTorso
Definition: solver.h:64
Solver::neck
iCubHeadCenter * neck
Definition: solver.h:122
EyePinvRefGen::Ts
double Ts
Definition: solver.h:83
EyePinvRefGen
Definition: solver.h:58
EyePinvRefGen::period
unsigned int period
Definition: solver.h:75
Solver::gazePos
Vector gazePos
Definition: solver.h:142
gazeNlp.h
EyePinvRefGen::saccadesClock
double saccadesClock
Definition: solver.h:81
localizer.h
Solver::mtx
mutex mtx
Definition: solver.h:131
iCub::iKin::iCubInertialSensor
Definition: iKinFwd.h:1364
Solver::invNeck
GazeIpOptMin * invNeck
Definition: solver.h:125
EyePinvRefGen::orig_eye_pan_max
double orig_eye_pan_max
Definition: solver.h:73
pids.h
Solver::drvTorso
PolyDriver * drvTorso
Definition: solver.h:126
EyePinvRefGen::orig_eye_tilt_max
double orig_eye_tilt_max
Definition: solver.h:71
NECKSOLVER_ACTIVATIONANGLE
constexpr double NECKSOLVER_ACTIVATIONANGLE
Definition: solver.h:43
Solver::ctrl
Controller * ctrl
Definition: solver.h:130
SACCADES_ACTIVATION_ANGLE
constexpr double SACCADES_ACTIVATION_ANGLE
Definition: solver.h:40
EyePinvRefGen::neck
iCubHeadCenter * neck
Definition: solver.h:61
Solver::neckPos
Vector neckPos
Definition: solver.h:141
Solver::torsoVel
AWLinEstimator * torsoVel
Definition: solver.h:144