iCub-main
observerThread.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2011 RobotCub Consortium, European Commission FP6 Project IST-004370
3  * Author: Marco Randazzo, Matteo Fumagalli
4  * email: marco.randazzo@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 OBSERVER_THREAD
20 #define OBSERVER_THREAD
21 
22 #include <yarp/os/all.h>
23 #include <yarp/sig/all.h>
24 #include <yarp/dev/all.h>
25 #include <iCub/ctrl/math.h>
27 #include <iCub/iDyn/iDyn.h>
28 #include <iCub/iDyn/iDynBody.h>
30 
31 #include <iostream>
32 #include <iomanip>
33 #include <cstring>
34 #include <list>
35 
36 using namespace yarp::os;
37 using namespace yarp::sig;
38 using namespace yarp::math;
39 using namespace yarp::dev;
40 using namespace iCub::ctrl;
41 using namespace iCub::iDyn;
42 using namespace std;
43 
44 constexpr int8_t MAX_JN = 12;
45 constexpr int8_t MAX_FILTER_ORDER = 6;
46 constexpr float_t SKIN_EVENTS_TIMEOUT = 0.2; // max time (in sec) a contact is kept without reading anything from the skin events port
47 
50 
51 // struct version
52 // {
53 // int head_version;
54 // int legs_version;
55 // };
56 
57 // filter
58 double lpf_ord1_3hz(double input, int j);
59 
61 {
62  public:
63  double timestamp;
64  Vector all_q_up, all_dq_up, all_d2q_up;
65  Vector all_q_low, all_dq_low, all_d2q_low;
66 
67  Vector ft_arm_left;
68  Vector ft_arm_right;
69  Vector ft_leg_left;
70  Vector ft_leg_right;
71  Vector ft_foot_left;
72  Vector ft_foot_right;
73 
74  Vector inertial_w0,inertial_dw0,inertial_d2p0;
75 
76  bool iCub_not_moving{};
77 
79  {
80  timestamp=0;
81  ft_arm_left=0;
82  ft_arm_right=0;
83  ft_leg_left=0;
84  ft_leg_right=0;
85  ft_foot_left=0;
86  ft_foot_right=0;
87  inertial_w0.resize(3); inertial_w0.zero();
88  inertial_dw0.resize(3); inertial_dw0.zero();
89  inertial_d2p0.resize(3); inertial_d2p0.zero();
90  all_q_up.resize(3+7+7); all_q_up.zero();
91  all_dq_up.resize(3+7+7); all_dq_up.zero();
92  all_d2q_up.resize(3+7+7); all_d2q_up.zero();
93  all_q_low.resize(3+6+6); all_q_low.zero();
94  all_dq_low.resize(3+6+6); all_dq_low.zero();
95  all_d2q_low.resize(3+6+6); all_d2q_low.zero();
96  ft_arm_left.resize(6); ft_arm_left.zero();
97  ft_arm_right.resize(6); ft_arm_right.zero();
98  ft_leg_left.resize(6); ft_leg_left.zero();
99  ft_leg_right.resize(6); ft_leg_right.zero();
100  ft_foot_left.resize(6); ft_foot_left.zero();
101  ft_foot_right.resize(6); ft_foot_right.zero();
102  }
103 
104  Vector get_q_head() {return all_q_up.subVector(0,2);}
105  Vector get_dq_head() {return all_dq_up.subVector(0,2);}
106  Vector get_d2q_head() {return all_d2q_up.subVector(0,2);}
107  Vector get_q_larm() {return all_q_up.subVector(3,9);}
108  Vector get_dq_larm() {return all_dq_up.subVector(3,9);}
109  Vector get_d2q_larm() {return all_d2q_up.subVector(3,9);}
110  Vector get_q_rarm() {return all_q_up.subVector(10,16);}
111  Vector get_dq_rarm() {return all_dq_up.subVector(10,16);}
112  Vector get_d2q_rarm() {return all_d2q_up.subVector(10,16);}
113  Vector get_q_torso() {return all_q_low.subVector(0,2);}
114  Vector get_dq_torso() {return all_dq_low.subVector(0,2);}
115  Vector get_d2q_torso() {return all_d2q_low.subVector(0,2);}
116  Vector get_q_lleg() {return all_q_low.subVector(3,8);}
117  Vector get_dq_lleg() {return all_dq_low.subVector(3,8);}
118  Vector get_d2q_lleg() {return all_d2q_low.subVector(3,8);}
119  Vector get_q_rleg() {return all_q_low.subVector(9,14);}
120  Vector get_dq_rleg() {return all_dq_low.subVector(9,14);}
121  Vector get_d2q_rleg() {return all_d2q_low.subVector(9,14);}
122 
123  bool checkIcubNotMoving();
124  void dump (FILE* f);
125 
126 };
127 
128 // class inverseDynamics: class for reading from Vrow and providing FT on an output port
129 class inverseDynamics: public PeriodicThread
130 {
131 public:
134  bool dummy_ft;
140 
141 private:
142  string robot_name;
143  string local_name;
144  bool autoconnect;
145  version_tag icub_type;
146 
147  PolyDriver *ddAL;
148  PolyDriver *ddAR;
149  PolyDriver *ddH;
150  IEncoders *iencs_arm_left;
151  IEncoders *iencs_arm_right;
152  IEncoders *iencs_head;
153  IInteractionMode *iint_arm_left;
154  IInteractionMode *iint_arm_right;
155  IInteractionMode *iint_head;
156  IControlMode *icmd_arm_left;
157  IControlMode *icmd_arm_right;
158  IControlMode *icmd_head;
159 
160  PolyDriver *ddLL;
161  PolyDriver *ddLR;
162  PolyDriver *ddT;
163  IEncoders *iencs_leg_left;
164  IEncoders *iencs_leg_right;
165  IEncoders *iencs_torso;
166  IInteractionMode *iint_leg_left;
167  IInteractionMode *iint_leg_right;
168  IInteractionMode *iint_torso;
169  IControlMode *icmd_leg_left;
170  IControlMode *icmd_leg_right;
171  IControlMode *icmd_torso;
172 
173  string part;
174 
175  const long double zero_sens_tolerance;
176  double skinContactsTimestamp;
177 
178  //input ports
179  BufferedPort<Vector> *port_ft_arm_left;
180  BufferedPort<Vector> *port_ft_arm_right;
181  BufferedPort<Vector> *port_ft_leg_left;
182  BufferedPort<Vector> *port_ft_leg_right;
183  BufferedPort<Vector> *port_ft_foot_left;
184  BufferedPort<Vector> *port_ft_foot_right;
185  BufferedPort<Vector> *port_inertial_thread;
186  BufferedPort<iCub::skinDynLib::skinContactList> *port_skin_contacts;
187 
188  //output ports
189  BufferedPort<Bottle> *port_RATorques;
190  BufferedPort<Bottle> *port_RLTorques;
191  BufferedPort<Bottle> *port_RWTorques;
192  BufferedPort<Bottle> *port_LATorques;
193  BufferedPort<Bottle> *port_LLTorques;
194  BufferedPort<Bottle> *port_LWTorques;
195  BufferedPort<Bottle> *port_TOTorques;
196  BufferedPort<Bottle> *port_HDTorques;
197  BufferedPort<Vector> *port_external_wrench_RA;
198  BufferedPort<Vector> *port_external_wrench_LA;
199  BufferedPort<Vector> *port_external_wrench_RL;
200  BufferedPort<Vector> *port_external_wrench_LL;
201  BufferedPort<Vector> *port_external_wrench_RF;
202  BufferedPort<Vector> *port_external_wrench_LF;
203  BufferedPort<Vector> *port_external_cartesian_wrench_RA;
204  BufferedPort<Vector> *port_external_cartesian_wrench_LA;
205  BufferedPort<Vector> *port_external_cartesian_wrench_RL;
206  BufferedPort<Vector> *port_external_cartesian_wrench_LL;
207  BufferedPort<Vector> *port_external_cartesian_wrench_RF;
208  BufferedPort<Vector> *port_external_cartesian_wrench_LF;
209  BufferedPort<Vector> *port_sensor_wrench_RL;
210  BufferedPort<Vector> *port_sensor_wrench_LL;
211  BufferedPort<Vector> *port_model_wrench_RL;
212  BufferedPort<Vector> *port_model_wrench_LL;
213  BufferedPort<Vector> *port_external_wrench_TO;
214  BufferedPort<Vector> *port_com_all;
215  BufferedPort<Vector> *port_com_all_foot;
216  BufferedPort<Vector> *port_com_lb;
217  BufferedPort<Vector> *port_com_ub;
218  BufferedPort<Vector> *port_com_la;
219  BufferedPort<Vector> *port_com_ra;
220  BufferedPort<Vector> *port_com_ll;
221  BufferedPort<Vector> *port_com_rl;
222  BufferedPort<Vector> *port_com_hd;
223  BufferedPort<Vector> *port_com_to;
224  BufferedPort<Vector> *port_monitor;
225  BufferedPort<iCub::skinDynLib::skinContactList> *port_contacts;
226  BufferedPort<Vector> *port_dumpvel;
227  BufferedPort<Vector> *port_COM_vel;
228  BufferedPort<Matrix> *port_COM_Jacobian;
229  BufferedPort<Vector> *port_all_velocities;
230  BufferedPort<Vector> *port_all_positions;
231  BufferedPort<Matrix> *port_root_position_mat;
232  BufferedPort<Vector> *port_root_position_vec;
233 
234  // ports outputing the external dynamics seen at the F/T sensor
235  BufferedPort<Vector> *port_external_ft_arm_left;
236  BufferedPort<Vector> *port_external_ft_arm_right;
237  BufferedPort<Vector> *port_external_ft_leg_left;
238  BufferedPort<Vector> *port_external_ft_leg_right;
239  yarp::os::Stamp timestamp;
240 
241  bool first;
242  thread_status_enum thread_status;
243 
244  AWLinEstimator *InertialEst;
245  AWLinEstimator *linEstUp;
246  AWQuadEstimator *quadEstUp;
247  AWLinEstimator *linEstLow;
248  AWQuadEstimator *quadEstLow;
249 
250  int ctrlJnt;
251  int allJnt;
252  iCubWholeBody *icub;
253  iCubWholeBody *icub_sens;
254  iCubStatus current_status;
255 
256  size_t status_queue_size;
257  list<iCubStatus> previous_status;
258  list<iCubStatus> not_moving_status;
259 
260  Vector encoders_arm_left;
261  Vector encoders_arm_right;
262  Vector encoders_head;
263 
264  Vector encoders_leg_left;
265  Vector encoders_leg_right;
266  Vector encoders_torso;
267 
268  Vector Fend,Muend;
269  Vector F_LArm, F_RArm;
270  Vector F_iDyn_LArm, F_iDyn_RArm, Offset_LArm, Offset_RArm;
271  Vector F_ext_left_arm, F_ext_right_arm, F_ext_torso;
272  Vector F_ext_cartesian_left_arm, F_ext_cartesian_right_arm;
273  Vector F_ext_left_leg, F_ext_right_leg;
274  Vector F_ext_left_foot, F_ext_right_foot;
275  Vector F_sns_left_leg, F_sns_right_leg;
276  Vector F_mdl_left_leg, F_mdl_right_leg;
277  Vector F_ext_cartesian_left_leg, F_ext_cartesian_right_leg;
278  Vector F_ext_cartesian_left_foot, F_ext_cartesian_right_foot;
279  Vector F_LLeg, F_RLeg;
280  Vector F_LFoot, F_RFoot;
281  Vector F_iDyn_LLeg, F_iDyn_RLeg, Offset_LLeg, Offset_RLeg;
282  Vector F_iDyn_LFoot, F_iDyn_RFoot, Offset_LFoot, Offset_RFoot;
283  Matrix F_sens_up, F_sens_low, F_ext_up, F_ext_low;
284  Vector F_ext_sens_right_arm, F_ext_sens_left_arm; // external wrench seen at the F/T sensors
285  Vector F_ext_sens_right_leg, F_ext_sens_left_leg; // external wrench seen at the F/T sensors
286 
289 
290 
291  // icub model
292  int comp;
293  Matrix FM_sens_up,FM_sens_low;
294 
295  //COM Jacobian Matrix
296  Matrix com_jac;
297 
298  Vector evalVelUp(const Vector &x);
299  Vector evalVelLow(const Vector &x);
300  Vector eval_domega(const Vector &x);
301  Vector evalAccUp(const Vector &x);
302  Vector evalAccLow(const Vector &x);
303 
304  void init_upper();
305  void init_lower();
306  void setUpperMeasure(bool _init=false);
307  void setLowerMeasure(bool _init=false);
308 
309  void addSkinContacts();
310 
311 public:
312  inverseDynamics(int _rate, PolyDriver *_ddAL, PolyDriver *_ddAR, PolyDriver *_ddH, PolyDriver *_ddLL, PolyDriver *_ddLR, PolyDriver *_ddT, string _robot_name, string _local_name, version_tag icub_type, bool _autoconnect=false );
313  bool threadInit() override;
314  void setStiffMode();
316  {
317  return thread_status;
318  }
319 
320  void run() override;
321  void threadRelease() override;
322  void closePort(Contactable *_port);
323  void writeTorque(Vector _values, int _address, BufferedPort<Bottle> *_port);
324  template <class T> void broadcastData(T& _values, BufferedPort<T> *_port);
325  void calibrateOffset(calib_enum calib_code=CALIB_ALL);
326  bool readAndUpdate(bool waitMeasure=false, bool _init=false);
327  bool getLowerEncodersSpeedAndAcceleration();
328  bool getUpperEncodersSpeedAndAcceleration();
329  void setZeroJntAngVelAcc();
330  void sendMonitorData();
331  void sendVelAccData();
332 
333 };
334 
335 #endif
336 
337 
iCubStatus::ft_arm_left
Vector ft_arm_left
Definition: observerThread.h:67
iCub::skinDynLib::skinContactList
Definition: skinContactList.h:44
iCub::ctrl::AWQuadEstimator
Definition: adaptWinPolyEstimator.h:207
T
T
Definition: compute_ekf_sym.m:12
iCubStatus
Definition: observerThread.h:60
iCubStatus::ft_foot_left
Vector ft_foot_left
Definition: observerThread.h:71
iCubStatus::all_q_up
Vector all_q_up
Definition: observerThread.h:64
iCubStatus::get_dq_larm
Vector get_dq_larm()
Definition: observerThread.h:108
iCubStatus::ft_foot_right
Vector ft_foot_right
Definition: observerThread.h:72
inverseDynamics::getThreadStatus
thread_status_enum getThreadStatus()
Definition: observerThread.h:315
inverseDynamics::w0_dw0_enabled
bool w0_dw0_enabled
Definition: observerThread.h:135
iCubStatus::iCubStatus
iCubStatus()
Definition: observerThread.h:78
STATUS_OK
@ STATUS_OK
Definition: observerThread.h:48
iCubStatus::get_d2q_rleg
Vector get_d2q_rleg()
Definition: observerThread.h:121
iDyn.h
iCubStatus::ft_leg_left
Vector ft_leg_left
Definition: observerThread.h:69
iCubStatus::get_d2q_larm
Vector get_d2q_larm()
Definition: observerThread.h:109
iCubStatus::get_dq_rleg
Vector get_dq_rleg()
Definition: observerThread.h:120
thread_status_enum
thread_status_enum
Definition: gravityThread.h:41
lpf_ord1_3hz
double lpf_ord1_3hz(double input, int j)
Definition: observerThread.cpp:46
iCubStatus::get_d2q_lleg
Vector get_d2q_lleg()
Definition: observerThread.h:118
yarp::dev
Definition: DebugInterfaces.h:52
inverseDynamics::auto_drift_comp
bool auto_drift_comp
Definition: observerThread.h:137
math.h
CALIB_FEET
@ CALIB_FEET
Definition: observerThread.h:49
inverseDynamics::com_vel_enabled
bool com_vel_enabled
Definition: observerThread.h:133
iCubStatus::get_d2q_torso
Vector get_d2q_torso()
Definition: observerThread.h:115
iCub::iDyn
Definition: iDyn.h:81
iCubStatus::get_d2q_head
Vector get_d2q_head()
Definition: observerThread.h:106
CALIB_LEGS
@ CALIB_LEGS
Definition: observerThread.h:49
inverseDynamics::default_ee_cont
bool default_ee_cont
Definition: observerThread.h:138
SKIN_EVENTS_TIMEOUT
constexpr float_t SKIN_EVENTS_TIMEOUT
Definition: observerThread.h:46
iCub::ctrl
Definition: adaptWinPolyEstimator.h:37
STATUS_DISCONNECTED
@ STATUS_DISCONNECTED
Definition: observerThread.h:48
inverseDynamics
Definition: observerThread.h:129
iCubStatus::get_q_lleg
Vector get_q_lleg()
Definition: observerThread.h:116
iCubStatus::all_q_low
Vector all_q_low
Definition: observerThread.h:65
x
x
Definition: compute_ekf_sym.m:21
iCubStatus::inertial_w0
Vector inertial_w0
Definition: observerThread.h:74
iCubStatus::get_dq_head
Vector get_dq_head()
Definition: observerThread.h:105
inverseDynamics::dumpvel_enabled
bool dumpvel_enabled
Definition: observerThread.h:136
skinContactList.h
CALIB_ALL
@ CALIB_ALL
Definition: observerThread.h:49
inverseDynamics::com_enabled
bool com_enabled
Definition: observerThread.h:132
iCubStatus::get_d2q_rarm
Vector get_d2q_rarm()
Definition: observerThread.h:112
MAX_JN
constexpr int8_t MAX_JN
Definition: observerThread.h:44
_init
@ _init
Definition: dc1394thread.h:14
f
f
Definition: compute_ekf_sym.m:22
iCub::iDyn::version_tag
Definition: iDynBody.h:108
iCubStatus::get_dq_lleg
Vector get_dq_lleg()
Definition: observerThread.h:117
iCubStatus::get_q_torso
Vector get_q_torso()
Definition: observerThread.h:113
iCubStatus::ft_leg_right
Vector ft_leg_right
Definition: observerThread.h:70
iDynBody.h
iCub::skinDynLib::dynContactList
Definition: dynContactList.h:42
CALIB_ARMS
@ CALIB_ARMS
Definition: observerThread.h:49
iCubStatus::ft_arm_right
Vector ft_arm_right
Definition: observerThread.h:68
iCubStatus::get_q_rleg
Vector get_q_rleg()
Definition: observerThread.h:119
iCubStatus::timestamp
double timestamp
Definition: observerThread.h:63
adaptWinPolyEstimator.h
iCub::ctrl::AWLinEstimator
Definition: adaptWinPolyEstimator.h:184
iCub::iDyn::iCubWholeBody
Definition: iDynBody.h:1471
iCubStatus::get_q_larm
Vector get_q_larm()
Definition: observerThread.h:107
calib_enum
calib_enum
Definition: observerThread.h:49
iCubStatus::get_dq_rarm
Vector get_dq_rarm()
Definition: observerThread.h:111
iCubStatus::get_q_head
Vector get_q_head()
Definition: observerThread.h:104
inverseDynamics::add_legs_once
bool add_legs_once
Definition: observerThread.h:139
iCubStatus::get_dq_torso
Vector get_dq_torso()
Definition: observerThread.h:114
iCubStatus::get_q_rarm
Vector get_q_rarm()
Definition: observerThread.h:110
inverseDynamics::dummy_ft
bool dummy_ft
Definition: observerThread.h:134
MAX_FILTER_ORDER
constexpr int8_t MAX_FILTER_ORDER
Definition: observerThread.h:45