iCub-main
controller.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 __CONTROLLER_H__
20 #define __CONTROLLER_H__
21 
22 #include <mutex>
23 #include <condition_variable>
24 #include <string>
25 #include <vector>
26 #include <set>
27 
28 #include <yarp/os/all.h>
29 #include <yarp/sig/all.h>
30 #include <yarp/dev/all.h>
31 #include <yarp/math/Math.h>
32 
33 #include <iCub/ctrl/minJerkCtrl.h>
34 #include <iCub/ctrl/pids.h>
35 #include <iCub/utils.h>
36 
37 constexpr int32_t GAZECTRL_SWOFFCOND_DISABLESLOT = 10; // [-]
38 constexpr double GAZECTRL_MOTIONDONE_NECK_QTHRES = 0.500; // [deg]
39 constexpr double GAZECTRL_MOTIONDONE_EYES_QTHRES = 0.100; // [deg]
40 constexpr double GAZECTRL_CRITICVER_STABILIZATION = 4.0; // [deg]
41 
42 using namespace std;
43 using namespace yarp::os;
44 using namespace yarp::dev;
45 using namespace yarp::sig;
46 using namespace yarp::math;
47 using namespace iCub::ctrl;
48 using namespace iCub::iKin;
49 
50 
51 // The thread launched by the application which is
52 // in charge of computing the control commands.
53 class Controller : public GazeComponent, public PeriodicThread
54 {
55 protected:
58  iKinChain *chainNeck, *chainEyeL, *chainEyeR;
59  PolyDriver *drvTorso, *drvHead;
60  IControlMode *modHead;
61  IPositionControl *posHead;
62  IVelocityControl *velHead;
63  IPositionDirect *posNeck;
65 
71 
72  BufferedPort<Vector> port_x;
73  BufferedPort<Vector> port_q;
74  BufferedPort<Bottle> port_event;
75  BufferedPort<Bottle> port_debug;
76  Stamp txInfo_x;
77  Stamp txInfo_q;
78  Stamp txInfo_pose;
79  Stamp txInfo_event;
80  Stamp txInfo_debug;
81 
82  mutex mutexRun;
83  mutex mutexChain;
84  mutex mutexCtrl;
85  mutex mutexData;
86  mutex mutexLook;
88  condition_variable cv_eventLook;
89  unsigned int period;
92  bool motionDone;
99  double printAccTime;
100  double neckTime;
101  double eyesTime;
102  double pathPerc;
103  double min_abs_vel;
105  double q_stamp;
106  double Ts;
107 
108  Matrix lim;
109  Vector qddeg,qdeg,vdeg;
110  Vector v,vNeck,vEyes;
111  Vector q0,qd,qdNeck,qdEyes;
112  Vector fbTorso,fbHead,fbNeck,fbEyes;
113  vector<int> neckJoints,eyesJoints;
114  vector<int> jointsToSet;
115 
116  multiset<double> motionOngoingEvents;
117  multiset<double> motionOngoingEventsCurrent;
118 
119  Vector computedxFP(const Matrix &H, const Vector &v, const Vector &w, const Vector &x_FP);
120  Vector computeNeckVelFromdxFP(const Vector &fp, const Vector &dfp);
121  Vector computeEyesVelFromdxFP(const Vector &dfp);
122  bool areJointsHealthyAndSet();
123  void setJointsCtrlMode();
124  void stopLimb(const bool execStopPosition=true);
125  void notifyEvent(const string &event, const double checkPoint=-1.0);
126  void motionOngoingEventsHandling();
127  void motionOngoingEventsFlush();
128  void stopControlHelper();
129 
130 public:
131  Controller(PolyDriver *_drvTorso, PolyDriver *_drvHead, ExchangeData *_commData,
132  const double _neckTime, const double _eyesTime,
133  const double _min_abs_vel, const unsigned int _period);
134  virtual ~Controller();
135 
136  void findMinimumAllowedVergence();
137  void minAllowedVergenceChanged() override;
138  void resetCtrlEyes();
139  void doSaccade(const Vector &ang, const Vector &vel);
140  bool look(const Vector &x);
141  void stopControl();
142  void printIter(Vector &xd, Vector &fp, Vector &qd, Vector &q, Vector &v, double printTime);
143  bool threadInit() override;
144  void threadRelease() override;
145  void afterStart(bool s) override;
146  void run() override;
147  void suspend();
148  void resume();
149  double getTneck() const;
150  double getTeyes() const;
151  void setTneck(const double execTime);
152  void setTeyes(const double execTime);
153  bool isMotionDone();
154  void setTrackingMode(const bool f);
155  bool getTrackingMode() const;
156  bool setGazeStabilization(const bool f);
157  bool getGazeStabilization() const;
158  bool getDesired(Vector &des);
159  bool getVelocity(Vector &vel);
160  bool getPose(const string &poseSel, Vector &x, Stamp &stamp);
161  bool registerMotionOngoingEvent(const double checkPoint);
162  bool unregisterMotionOngoingEvent(const double checkPoint);
163  Bottle listMotionOngoingEvents();
164 };
165 
166 
167 #endif
168 
169 
Controller::txInfo_debug
Stamp txInfo_debug
Definition: controller.h:80
Controller::posNeck
IPositionDirect * posNeck
Definition: controller.h:63
Controller::mutexLook
mutex mutexLook
Definition: controller.h:86
Controller::modHead
IControlMode * modHead
Definition: controller.h:60
iCub::ctrl::Integrator
Definition: pids.h:47
Controller::IntPlan
Integrator * IntPlan
Definition: controller.h:69
H
H
Definition: compute_ekf_fast.m:27
Controller::txInfo_q
Stamp txInfo_q
Definition: controller.h:77
Controller::stabilizeGaze
bool stabilizeGaze
Definition: controller.h:94
Controller::neckTime
double neckTime
Definition: controller.h:100
ExchangeData
Definition: utils.h:93
GazeComponent
Definition: utils.h:171
Controller::lim
Matrix lim
Definition: controller.h:108
Controller::jointsToSet
vector< int > jointsToSet
Definition: controller.h:114
Controller::motionOngoingEventsCurrent
multiset< double > motionOngoingEventsCurrent
Definition: controller.h:117
Controller::txInfo_pose
Stamp txInfo_pose
Definition: controller.h:78
Controller::commData
ExchangeData * commData
Definition: controller.h:64
iCub::iKin::iKinChain
Definition: iKinFwd.h:354
Controller::motionDone
bool motionDone
Definition: controller.h:92
GAZECTRL_MOTIONDONE_NECK_QTHRES
constexpr double GAZECTRL_MOTIONDONE_NECK_QTHRES
Definition: controller.h:38
Controller::q_stamp
double q_stamp
Definition: controller.h:105
Controller::drvTorso
PolyDriver * drvTorso
Definition: controller.h:59
fp
static struct bpf_program fp
Definition: pcap_wrapper_linux.cpp:89
iCub::iKin::iCubHeadCenter
Definition: iKinFwd.h:1338
yarp::dev
Definition: DebugInterfaces.h:52
Controller::pathPerc
double pathPerc
Definition: controller.h:102
Controller
Definition: controller.h:53
Controller::mutexChain
mutex mutexChain
Definition: controller.h:83
Controller::imu
iCubInertialSensor * imu
Definition: controller.h:57
Controller::mutexCtrl
mutex mutexCtrl
Definition: controller.h:84
Controller::printAccTime
double printAccTime
Definition: controller.h:99
GAZECTRL_SWOFFCOND_DISABLESLOT
constexpr int32_t GAZECTRL_SWOFFCOND_DISABLESLOT
Definition: controller.h:37
Controller::saccadeStartTime
double saccadeStartTime
Definition: controller.h:98
Controller::chainNeck
iKinChain * chainNeck
Definition: controller.h:58
Controller::ctrlActiveRisingEdgeTime
double ctrlActiveRisingEdgeTime
Definition: controller.h:97
iCub::iKin
Definition: iKinFwd.h:71
Controller::txInfo_x
Stamp txInfo_x
Definition: controller.h:76
Controller::reliableGyro
bool reliableGyro
Definition: controller.h:93
Controller::IntStabilizer
Integrator * IntStabilizer
Definition: controller.h:70
Controller::nJointsTorso
int nJointsTorso
Definition: controller.h:95
Controller::motionOngoingEvents
multiset< double > motionOngoingEvents
Definition: controller.h:116
iCub::ctrl
Definition: adaptWinPolyEstimator.h:37
Controller::unplugCtrlEyes
bool unplugCtrlEyes
Definition: controller.h:90
x
x
Definition: compute_ekf_sym.m:21
Controller::neck
iCubHeadCenter * neck
Definition: controller.h:56
f
f
Definition: compute_ekf_sym.m:22
Controller::posHead
IPositionControl * posHead
Definition: controller.h:61
Controller::port_q
BufferedPort< Vector > port_q
Definition: controller.h:73
Controller::mtx_eventLook
mutex mtx_eventLook
Definition: controller.h:87
Controller::txInfo_event
Stamp txInfo_event
Definition: controller.h:79
Controller::min_abs_vel
double min_abs_vel
Definition: controller.h:103
Controller::nJointsHead
int nJointsHead
Definition: controller.h:96
Controller::velHead
IVelocityControl * velHead
Definition: controller.h:62
Controller::port_debug
BufferedPort< Bottle > port_debug
Definition: controller.h:75
Controller::qdNeck
Vector qdNeck
Definition: controller.h:111
v
static int v
Definition: iCub_Sim.cpp:42
Controller::Ts
double Ts
Definition: controller.h:106
Controller::ctrlInhibited
bool ctrlInhibited
Definition: controller.h:91
minJerkCtrl.h
GAZECTRL_MOTIONDONE_EYES_QTHRES
constexpr double GAZECTRL_MOTIONDONE_EYES_QTHRES
Definition: controller.h:39
Controller::mjCtrlNeck
minJerkVelCtrl * mjCtrlNeck
Definition: controller.h:66
Controller::port_x
BufferedPort< Vector > port_x
Definition: controller.h:72
Controller::mutexRun
mutex mutexRun
Definition: controller.h:82
Controller::period
unsigned int period
Definition: controller.h:89
Controller::mutexData
mutex mutexData
Definition: controller.h:85
Controller::neckJoints
vector< int > neckJoints
Definition: controller.h:113
GAZECTRL_CRITICVER_STABILIZATION
constexpr double GAZECTRL_CRITICVER_STABILIZATION
Definition: controller.h:40
iCub::ctrl::minJerkVelCtrl
Definition: minJerkCtrl.h:46
iCub::iKin::iCubInertialSensor
Definition: iKinFwd.h:1364
Controller::eyesTime
double eyesTime
Definition: controller.h:101
Controller::vdeg
Vector vdeg
Definition: controller.h:109
Controller::cv_eventLook
condition_variable cv_eventLook
Definition: controller.h:88
Controller::mjCtrlEyes
minJerkVelCtrl * mjCtrlEyes
Definition: controller.h:67
pids.h
Controller::port_event
BufferedPort< Bottle > port_event
Definition: controller.h:74
Controller::IntState
Integrator * IntState
Definition: controller.h:68
Controller::startupMinVer
double startupMinVer
Definition: controller.h:104
Controller::fbTorso
Vector fbTorso
Definition: controller.h:112
Controller::vNeck
Vector vNeck
Definition: controller.h:110