iCub-main
observerThread.cpp
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 #include <cmath>
20 #include <iostream>
21 #include <iomanip>
22 #include <string>
23 
24 #include <yarp/os/all.h>
25 #include <yarp/sig/all.h>
26 #include <yarp/dev/all.h>
27 #include <iCub/ctrl/math.h>
29 #include <iCub/iDyn/iDyn.h>
30 #include <iCub/iDyn/iDynBody.h>
32 
33 #include "observerThread.h"
34 
35 using namespace yarp::os;
36 using namespace yarp::sig;
37 using namespace yarp::math;
38 using namespace yarp::dev;
39 using namespace iCub::ctrl;
40 using namespace iCub::iDyn;
41 using namespace iCub::skinDynLib;
42 using namespace std;
43 
44 #define TEST_LEG_SENSOR
45 #define MEASURE_FROM_FOOT
46 double lpf_ord1_3hz(double input, int j)
47 {
48  if (j<0 || j>= MAX_JN)
49  {
50  cout<<"Received an invalid joint index to filter"<<endl;
51  return 0;
52  }
53 
54  static double xv[MAX_FILTER_ORDER][MAX_JN];
55  static double yv[MAX_FILTER_ORDER][MAX_JN];
56  xv[0][j] = xv[1][j] ;
57  xv[1][j] = input / 1.157889499e+01;
58  yv[0][j] = yv[1][j] ;
59  yv[1][j] = (xv[0][j] + xv[1][j] ) + ( 0.8272719460 * yv[0][j] );
60  return (yv[1][j]);
61 }
62 
63 Vector inverseDynamics::evalVelUp(const Vector &x)
64 {
65  AWPolyElement el;
66  el.data=x;
67  el.time=Time::now();
68 
69  return linEstUp->estimate(el);
70 }
71 
72 Vector inverseDynamics::evalVelLow(const Vector &x)
73 {
74  AWPolyElement el;
75  el.data=x;
76  el.time=Time::now();
77 
78  return linEstLow->estimate(el);
79 }
80 
81 Vector inverseDynamics::eval_domega(const Vector &x)
82 {
83  AWPolyElement el;
84  el.data=x;
85  el.time=Time::now();
86 
87  return InertialEst->estimate(el);
88 }
89 
90 Vector inverseDynamics::evalAccUp(const Vector &x)
91 {
92  AWPolyElement el;
93  el.data=x;
94  el.time=Time::now();
95 
96  return quadEstUp->estimate(el);
97 }
98 
99 Vector inverseDynamics::evalAccLow(const Vector &x)
100 {
101  AWPolyElement el;
102  el.data=x;
103  el.time=Time::now();
104 
105  return quadEstLow->estimate(el);
106 }
107 
108 void inverseDynamics::init_upper()
109 {
110  //---------------------PARTS-------------------------//
111  // Left_arm variables
112  allJnt = 0;
113  int jnt=16;
114  encoders_arm_left.resize(jnt,0.0);
115  F_LArm.resize(6,0.0);
116  F_iDyn_LArm.resize(6,0.0);
117  Offset_LArm.resize(6,0.0);
118  allJnt+=jnt;
119 
120  // Right_arm variables
121  jnt = 16;
122  encoders_arm_right.resize(jnt,0.0);
123  F_RArm.resize(6,0.0);
124  F_iDyn_RArm.resize(6,0.0);
125  Offset_RArm.resize(6,0.0);
126  allJnt+=jnt;
127 
128  // Head variables
129  jnt = 6;
130  encoders_head.resize(jnt,0.0);
131  allJnt+=jnt;
132 
133  current_status.all_q_up.resize(allJnt,0.0);
134  current_status.all_dq_up.resize(allJnt,0.0);
135  current_status.all_d2q_up.resize(allJnt,0.0);
136  F_sens_up.zero();
137  FM_sens_up.resize(6,2); FM_sens_up.zero();
138 }
139 
140 void inverseDynamics::init_lower()
141 {
142  //---------------------PARTS-------------------------//
143  // Left_leg variables
144  allJnt = 0;
145  int jnt=6;
146  encoders_leg_left.resize(jnt,0.0);
147  F_LLeg.resize(6,0.0);
148  F_iDyn_LLeg.resize(6,0.0);
149  Offset_LLeg.resize(6,0.0);
150  F_LFoot.resize(6,0.0);
151  F_iDyn_LFoot.resize(6,0.0);
152  Offset_LFoot.resize(6,0.0);
153  allJnt+=jnt;
154 
155  // Right_leg variables
156  jnt = 6;
157  encoders_leg_right.resize(jnt,0.0);
158  F_RLeg.resize(6,0.0);
159  F_iDyn_RLeg.resize(6,0.0);
160  Offset_RLeg.resize(6,0.0);
161  F_RFoot.resize(6,0.0);
162  F_iDyn_RFoot.resize(6,0.0);
163  Offset_RFoot.resize(6,0.0);
164  allJnt+=jnt;
165 
166  // Torso variables
167  jnt = 3;
168  encoders_torso.resize(jnt,0.0);
169  allJnt+=jnt;
170 
171  current_status.all_q_low.resize(allJnt,0.0);
172  current_status.all_dq_low.resize(allJnt,0.0);
173  current_status.all_d2q_low.resize(allJnt,0.0);
174 
175  F_sens_low.zero();
176  FM_sens_low.resize(6,2); FM_sens_low.zero();
177 }
178 
180 {
181  if (iint_arm_left)
182  {
183  for (int i=0; i<7; i++)
184  iint_arm_left->setInteractionMode(i,VOCAB_IM_STIFF);
185  }
186  if (iint_arm_right)
187  {
188  for (int i=0; i<7; i++)
189  iint_arm_right->setInteractionMode(i,VOCAB_IM_STIFF);
190  }
191  if (iint_leg_left)
192  {
193  for (int i=0; i<6; i++)
194  iint_leg_left->setInteractionMode(i,VOCAB_IM_STIFF);
195  }
196  if (iint_leg_right)
197  {
198  for (int i=0; i<6; i++)
199  iint_leg_right->setInteractionMode(i,VOCAB_IM_STIFF);
200  }
201  if (iint_torso)
202  {
203  for (int i=0; i<3; i++)
204  iint_torso->setInteractionMode(i,VOCAB_IM_STIFF);
205  }
206 
207  if (icmd_arm_left)
208  {
209  for (int i=0; i<7; i++)
210  {
211  int mode =0; icmd_arm_left->getControlMode(i, &mode);
212  if (mode==VOCAB_CM_TORQUE) icmd_arm_left->setControlMode(i, VOCAB_CM_POSITION);
213  }
214  }
215 
216  if (icmd_arm_right)
217  {
218  for (int i=0; i<7; i++)
219  {
220  int mode =0; icmd_arm_right->getControlMode(i, &mode);
221  if (mode==VOCAB_CM_TORQUE) icmd_arm_right->setControlMode(i, VOCAB_CM_POSITION);
222  }
223  }
224 
225  if (icmd_leg_left)
226  {
227  for (int i=0; i<6; i++)
228  {
229  int mode =0; icmd_leg_left->getControlMode(i, &mode);
230  if (mode==VOCAB_CM_TORQUE) icmd_leg_left->setControlMode(i, VOCAB_CM_POSITION);
231  }
232  }
233 
234  if (icmd_leg_right)
235  {
236  for (int i=0; i<6; i++)
237  {
238  int mode =0; icmd_leg_right->getControlMode(i, &mode);
239  if (mode==VOCAB_CM_TORQUE) icmd_leg_right->setControlMode(i, VOCAB_CM_POSITION);
240  }
241  }
242 
243  if (icmd_torso)
244  {
245  for (int i=0; i<3; i++)
246  {
247  int mode =0; icmd_torso->getControlMode(i, &mode);
248  if (mode==VOCAB_CM_TORQUE) icmd_torso->setControlMode(i, VOCAB_CM_POSITION);
249  }
250  }
251 }
252 
253 inverseDynamics::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) : PeriodicThread((double)_rate/1000.0), ddAL(_ddAL), ddAR(_ddAR), ddH(_ddH), ddLL(_ddLL), ddLR(_ddLR), ddT(_ddT), robot_name(_robot_name), icub_type(_icub_type), local_name(_local_name), zero_sens_tolerance (1e-12)
254 {
255  status_queue_size = 10;
256  autoconnect = _autoconnect;
257  com_enabled = true;
258  com_vel_enabled = false;
259  dummy_ft = false;
260  w0_dw0_enabled = false;
261  dumpvel_enabled = false;
262  auto_drift_comp = false;
263  add_legs_once = false;
264 
265  icub = new iCubWholeBody(icub_type, DYNAMIC, VERBOSE);
266  icub_sens = new iCubWholeBody(icub_type, DYNAMIC, VERBOSE);
267  first = true;
268  skinContactsTimestamp = 0.0;
269 
270  //--------------INTERFACE INITIALIZATION-------------//
271  iencs_arm_left = 0;
272  iencs_arm_right= 0;
273  iencs_head = 0;
274  iencs_leg_left = 0;
275  iencs_leg_right= 0;
276  iencs_torso = 0;
277  iint_arm_left = 0;
278  iint_arm_right = 0;
279  iint_head = 0;
280  iint_leg_left = 0;
281  iint_leg_right = 0;
282  iint_torso = 0;
283  icmd_arm_left = 0;
284  icmd_arm_right = 0;
285  icmd_head = 0;
286  icmd_leg_left = 0;
287  icmd_leg_right = 0;
288  icmd_torso = 0;
289 
290  //---------------------PORT--------------------------//
291 
292  port_inertial_thread=new BufferedPort<Vector>;
293  port_ft_arm_left=new BufferedPort<Vector>;
294  port_ft_arm_right=new BufferedPort<Vector>;
295  port_ft_leg_left=new BufferedPort<Vector>;
296  port_ft_leg_right=new BufferedPort<Vector>;
297  port_ft_foot_left=new BufferedPort<Vector>;
298  port_ft_foot_right=new BufferedPort<Vector>;
299  port_RATorques = new BufferedPort<Bottle>;
300  port_LATorques = new BufferedPort<Bottle>;
301  port_RLTorques = new BufferedPort<Bottle>;
302  port_LLTorques = new BufferedPort<Bottle>;
303  port_RWTorques = new BufferedPort<Bottle>;
304  port_LWTorques = new BufferedPort<Bottle>;
305  port_TOTorques = new BufferedPort<Bottle>;
306  port_HDTorques = new BufferedPort<Bottle>;
307  port_external_wrench_RA = new BufferedPort<Vector>;
308  port_external_wrench_LA = new BufferedPort<Vector>;
309  port_external_wrench_RL = new BufferedPort<Vector>;
310  port_external_wrench_LL = new BufferedPort<Vector>;
311  port_external_wrench_RF = new BufferedPort<Vector>;
312  port_external_wrench_LF = new BufferedPort<Vector>;
313 #ifdef TEST_LEG_SENSOR
314  port_sensor_wrench_RL = new BufferedPort<Vector>;
315  port_sensor_wrench_LL = new BufferedPort<Vector>;
316  port_model_wrench_RL = new BufferedPort<Vector>;
317  port_model_wrench_LL = new BufferedPort<Vector>;
318 #endif
319  port_external_wrench_TO = new BufferedPort<Vector>;
320  port_external_cartesian_wrench_RA = new BufferedPort<Vector>;
321  port_external_cartesian_wrench_LA = new BufferedPort<Vector>;
322  port_external_cartesian_wrench_RL = new BufferedPort<Vector>;
323  port_external_cartesian_wrench_LL = new BufferedPort<Vector>;
324  port_external_cartesian_wrench_RF = new BufferedPort<Vector>;
325  port_external_cartesian_wrench_LF = new BufferedPort<Vector>;
326  port_skin_contacts = new BufferedPort<skinContactList>;
327  port_com_all = new BufferedPort<Vector>;
328  port_com_lb = new BufferedPort<Vector>;
329  port_com_ub = new BufferedPort<Vector>;
330  port_com_la = new BufferedPort<Vector>;
331  port_com_ra = new BufferedPort<Vector>;
332  port_com_ll = new BufferedPort<Vector>;
333  port_com_rl = new BufferedPort<Vector>;
334  port_com_to = new BufferedPort<Vector>;
335  port_com_hd = new BufferedPort<Vector>;
336  port_com_all_foot = new BufferedPort<Vector>;
337  port_monitor = new BufferedPort<Vector>;
338  port_contacts = new BufferedPort<skinContactList>;
339  port_dumpvel = new BufferedPort<Vector>;
340  port_external_ft_arm_left = new BufferedPort<Vector>;
341  port_external_ft_arm_right = new BufferedPort<Vector>;
342  port_external_ft_leg_left = new BufferedPort<Vector>;
343  port_external_ft_leg_right = new BufferedPort<Vector>;
344  port_COM_vel = new BufferedPort<Vector>;
345  port_COM_Jacobian = new BufferedPort<Matrix>;
346  port_all_velocities = new BufferedPort<Vector>;
347  port_all_positions = new BufferedPort<Vector>;
348  port_root_position_mat = new BufferedPort<Matrix>;
349  port_root_position_vec = new BufferedPort<Vector>;
350 
351  port_inertial_thread->open(string("/"+local_name+"/inertial:i").c_str());
352  port_ft_arm_left->open(string("/"+local_name+"/left_arm/FT:i").c_str());
353  port_ft_arm_right->open(string("/"+local_name+"/right_arm/FT:i").c_str());
354  port_ft_leg_left->open(string("/"+local_name+"/left_leg/FT:i").c_str());
355  port_ft_leg_right->open(string("/"+local_name+"/right_leg/FT:i").c_str());
356  port_ft_foot_left->open(string("/"+local_name+"/left_foot/FT:i").c_str());
357  port_ft_foot_right->open(string("/"+local_name+"/right_foot/FT:i").c_str());
358  port_RATorques->open(string("/"+local_name+"/right_arm/Torques:o").c_str());
359  port_LATorques->open(string("/"+local_name+"/left_arm/Torques:o").c_str());
360  port_RLTorques->open(string("/"+local_name+"/right_leg/Torques:o").c_str());
361  port_LLTorques->open(string("/"+local_name+"/left_leg/Torques:o").c_str());
362  port_RWTorques->open(string("/"+local_name+"/right_wrist/Torques:o").c_str());
363  port_LWTorques->open(string("/"+local_name+"/left_wrist/Torques:o").c_str());
364  port_TOTorques->open(string("/"+local_name+"/torso/Torques:o").c_str());
365  port_HDTorques->open(string("/"+local_name+"/head/Torques:o").c_str());
366  port_external_wrench_RA->open(string("/"+local_name+"/right_arm/endEffectorWrench:o").c_str());
367  port_external_wrench_LA->open(string("/"+local_name+"/left_arm/endEffectorWrench:o").c_str());
368  port_external_wrench_RL->open(string("/"+local_name+"/right_leg/endEffectorWrench:o").c_str());
369  port_external_wrench_LL->open(string("/"+local_name+"/left_leg/endEffectorWrench:o").c_str());
370  port_external_wrench_RF->open(string("/"+local_name+"/right_foot/endEffectorWrench:o").c_str());
371  port_external_wrench_LF->open(string("/"+local_name+"/left_foot/endEffectorWrench:o").c_str());
372 #ifdef TEST_LEG_SENSOR
373  port_sensor_wrench_RL->open(string("/"+local_name+"/right_leg/sensorWrench:o").c_str());
374  port_sensor_wrench_LL->open(string("/"+local_name+"/left_leg/sensorWrench:o").c_str());
375  port_model_wrench_RL->open(string("/"+local_name+"/right_leg/modelWrench:o").c_str());
376  port_model_wrench_LL->open(string("/"+local_name+"/left_leg/modelWrench:o").c_str());
377 #endif
378  port_external_cartesian_wrench_RA->open(string("/"+local_name+"/right_arm/cartesianEndEffectorWrench:o").c_str());
379  port_external_cartesian_wrench_LA->open(string("/"+local_name+"/left_arm/cartesianEndEffectorWrench:o").c_str());
380  port_external_cartesian_wrench_RL->open(string("/"+local_name+"/right_leg/cartesianEndEffectorWrench:o").c_str());
381  port_external_cartesian_wrench_LL->open(string("/"+local_name+"/left_leg/cartesianEndEffectorWrench:o").c_str());
382  port_external_cartesian_wrench_RF->open(string("/"+local_name+"/right_foot/cartesianEndEffectorWrench:o").c_str());
383  port_external_cartesian_wrench_LF->open(string("/"+local_name+"/left_foot/cartesianEndEffectorWrench:o").c_str());
384  port_external_wrench_TO->open(string("/"+local_name+"/torso/Wrench:o").c_str());
385  port_com_all->open(string("/"+local_name+"/com:o").c_str());
386  port_com_lb->open (string("/"+local_name+"/lower_body/com:o").c_str());
387  port_com_ub->open (string("/"+local_name+"/upper_body/com:o").c_str());
388  port_com_la ->open(string("/"+local_name+"/left_arm/com:o").c_str());
389  port_com_ra ->open(string("/"+local_name+"/right_arm/com:o").c_str());
390  port_com_ll ->open(string("/"+local_name+"/left_leg/com:o").c_str());
391  port_com_rl ->open(string("/"+local_name+"/right_leg/com:o").c_str());
392  port_com_hd ->open(string("/"+local_name+"/head/com:o").c_str());
393  port_com_to ->open(string("/"+local_name+"/torso/com:o").c_str());
394  port_com_all_foot->open(string("/"+local_name+"/com_foot:o").c_str());
395  port_skin_contacts->open(string("/"+local_name+"/skin_contacts:i").c_str());
396  port_monitor->open(string("/"+local_name+"/monitor:o").c_str());
397  port_contacts->open(string("/"+local_name+"/contacts:o").c_str());
398  port_dumpvel->open(string("/"+local_name+"/va:o").c_str());
399  port_external_ft_arm_left->open(string("/"+local_name+"/left_arm/ext_ft_sens:o").c_str());
400  port_external_ft_arm_right->open(string("/"+local_name+"/right_arm/ext_ft_sens:o").c_str());
401  port_external_ft_leg_left->open(string("/"+local_name+"/left_leg/ext_ft_sens:o").c_str());
402  port_external_ft_leg_right->open(string("/"+local_name+"/right_leg/ext_ft_sens:o").c_str());
403  port_COM_vel->open(string("/"+local_name+"/com_vel:o").c_str());
404  port_COM_Jacobian->open(string("/"+local_name+"/com_jacobian:o").c_str());
405  port_all_velocities->open(string("/"+local_name+"/all_velocities:o").c_str());
406  port_all_positions->open(string("/"+local_name+"/all_positions:o").c_str());
407  port_root_position_mat->open(string("/"+local_name+"/root_position_mat:o").c_str());
408  port_root_position_vec->open(string("/"+local_name+"/root_position_vec:o").c_str());
409 
410  yInfo ("Waiting for port connections");
411  if (autoconnect)
412  {
413  //from iCub to wholeBodyDynamics
414  Network::connect(string("/"+local_name+"/filtered/inertial:o").c_str(),string("/"+local_name+"/inertial:i").c_str(),"tcp",false);
415  Network::connect(string("/"+robot_name+"/inertial").c_str(), string("/"+local_name+"/unfiltered/inertial:i").c_str(),"tcp",false);
416  Network::connect(string("/"+robot_name+"/left_arm/analog:o").c_str(), string("/"+local_name+"/left_arm/FT:i").c_str(),"tcp",false);
417  Network::connect(string("/"+robot_name+"/right_arm/analog:o").c_str(), string("/"+local_name+"/right_arm/FT:i").c_str(),"tcp",false);
418  Network::connect(string("/"+robot_name+"/left_leg/analog:o").c_str(), string("/"+local_name+"/left_leg/FT:i").c_str(),"tcp",false);
419  Network::connect(string("/"+robot_name+"/right_leg/analog:o").c_str(), string("/"+local_name+"/right_leg/FT:i").c_str(),"tcp",false);
420  Network::connect(string("/"+robot_name+"/left_foot/analog:o").c_str(), string("/"+local_name+"/left_foot/FT:i").c_str(),"tcp",false);
421  Network::connect(string("/"+robot_name+"/right_foot/analog:o").c_str(), string("/"+local_name+"/right_foot/FT:i").c_str(),"tcp",false);
422  //from wholeBodyDynamics to iCub (mandatory)
423  Network::connect(string("/"+local_name+"/left_arm/Torques:o").c_str(), string("/"+robot_name+"/joint_vsens/left_arm:i").c_str(),"tcp",false);
424  Network::connect(string("/"+local_name+"/right_arm/Torques:o").c_str(),string("/"+robot_name+"/joint_vsens/right_arm:i").c_str(),"tcp",false);
425  Network::connect(string("/"+local_name+"/left_leg/Torques:o").c_str(), string("/"+robot_name+"/joint_vsens/left_leg:i").c_str(),"tcp",false);
426  Network::connect(string("/"+local_name+"/right_leg/Torques:o").c_str(),string("/"+robot_name+"/joint_vsens/right_leg:i").c_str(),"tcp",false);
427  Network::connect(string("/"+local_name+"/torso/Torques:o").c_str(), string("/"+robot_name+"/joint_vsens/torso:i").c_str(),"tcp",false);
428  //from wholeBodyDynamics to iCub (optional)
429  if (Network::exists(string("/"+robot_name+"/joint_vsens/left_wrist:i").c_str()))
430  Network::connect(string("/"+local_name+"/left_wrist/Torques:o").c_str(), string("/"+robot_name+"/joint_vsens/left_wrist:i").c_str(),"tcp",false);
431  if (Network::exists(string("/"+robot_name+"/joint_vsens/right_wrist:i").c_str()))
432  Network::connect(string("/"+local_name+"/right_wrist/Torques:o").c_str(),string("/"+robot_name+"/joint_vsens/right_wrist:i").c_str(),"tcp",false);
433  }
434  yInfo ("Ports connected");
435 
436  //---------------------DEVICES--------------------------//
437  if (ddAL) {ddAL->view(iencs_arm_left); ddAL->view(iint_arm_left); ddAL->view(icmd_arm_left);}
438  if (ddAR) {ddAR->view(iencs_arm_right); ddAR->view(iint_arm_right); ddAR->view(icmd_arm_right);}
439  if (ddH) {ddH->view(iencs_head); ddH ->view(iint_head); ddH ->view(icmd_head);}
440  if (ddLL) {ddLL->view(iencs_leg_left); ddLL->view(iint_leg_left); ddLL->view(icmd_leg_left);}
441  if (ddLR) {ddLR->view(iencs_leg_right); ddLR->view(iint_leg_right); ddLR->view(icmd_leg_right);}
442  if (ddT) {ddT->view(iencs_torso); ddT ->view(iint_torso); ddT ->view(icmd_torso);}
443 
444  linEstUp =new AWLinEstimator(16,1.0);
445  quadEstUp=new AWQuadEstimator(25,1.0);
446  linEstLow =new AWLinEstimator(16,1.0);
447  quadEstLow=new AWQuadEstimator(25,1.0);
448  InertialEst = new AWLinEstimator(16,1.0);
449 
450  //-----------parts INIT VARIABLES----------------//
451  init_upper();
452  init_lower();
453 
454  //-----------CARTESIAN INIT VARIABLES----------------//
455  Fend.resize(3,0.0);
456  Muend.resize(3,0.0);
457  F_ext_up.resize(6,3);
458  F_ext_up = 0.0;
459  F_ext_low.resize(6,3);
460  F_ext_low = 0.0;
461  F_ext_left_arm.resize(6,0.0);
462  F_ext_right_arm.resize(6,0.0);
463  F_ext_cartesian_left_arm.resize(6,0.0);
464  F_ext_cartesian_right_arm.resize(6,0.0);
465  F_ext_left_leg.resize(6,0.0);
466  F_ext_right_leg.resize(6,0.0);
467  F_ext_cartesian_left_leg.resize(6,0.0);
468  F_ext_cartesian_right_leg.resize(6,0.0);
469  F_ext_left_foot.resize(6,0.0);
470  F_ext_right_foot.resize(6,0.0);
471  F_ext_cartesian_left_foot.resize(6,0.0);
472  F_ext_cartesian_right_foot.resize(6,0.0);
473  com_jac.resize(6,32);
474 
475 }
476 
478 {
479  yInfo("threadInit: waiting for port connections... \n\n");
480  if (!dummy_ft)
481  {
482  Vector *dummy = port_inertial_thread->read(true); //blocking call: waits for ports connection
483  }
484 
485  // N trials to get a more accurate estimation
486  for(size_t i=0; i<status_queue_size; i++)
487  {
488  //read joints and ft sensor
489  bool ret = readAndUpdate(true,true);
490  if (!ret)
491  {
492  yError("A problem occured during the initial readAndUpdate(), stopping... \n");
493  thread_status = STATUS_DISCONNECTED;
494  return false;
495  }
496  }
497 
498  // the queue previous_status now contains status_queue_size elements, and we can calibrate
499  calibrateOffset();
500 
501  thread_status = STATUS_OK;
502  return true;
503 }
504 
506 {
507  bool ret = true;
508  for (size_t i=0; i<this->all_dq_low.size(); i++)
509  if (fabs(all_dq_low[i])>0.7)
510  {
511  ret = false;
512  }
513 
514  for (size_t i=0; i<this->all_dq_up.size(); i++)
515  if (fabs(all_dq_up[i])>0.7)
516  {
517  ret = false;
518  }
519 
520  return ret;
521 }
522 
523 void iCubStatus::dump (FILE* f)
524 {
525  fprintf (f, "%f ", timestamp);
526  fprintf (f, "%s ", all_q_up.toString().c_str());
527  fprintf (f, "%s ", all_dq_up.toString().c_str());
528  fprintf (f, "%s ", all_d2q_up.toString().c_str());
529  fprintf (f, "%s ", all_q_low.toString().c_str());
530  fprintf (f, "%s ", all_dq_low.toString().c_str());
531  fprintf (f, "%s ", all_d2q_low.toString().c_str());
532  fprintf (f, "%s ", ft_arm_left.toString().c_str());
533  fprintf (f, "%s ", ft_arm_right.toString().c_str());
534  fprintf (f, "%s ", ft_leg_left.toString().c_str());
535  fprintf (f, "%s ", ft_leg_right.toString().c_str());
536  fprintf (f, "%s ", inertial_w0.toString().c_str());
537  fprintf (f, "%s ", inertial_dw0.toString().c_str());
538  fprintf (f, "%s \n", inertial_d2p0.toString().c_str());
539 }
540 
542 {
543  timestamp.update();
544 
545  thread_status = STATUS_OK;
546  static int delay_check=0;
547  if(!readAndUpdate(false))
548  {
549  delay_check++;
550  yWarning ("network delays detected (%d/10)\n", delay_check);
551  if (delay_check>=10)
552  {
553  yError ("inverseDynamics thread lost connection with iCubInterface.\n");
554  thread_status = STATUS_DISCONNECTED;
555  }
556  }
557  else
558  {
559  delay_check = 0;
560  }
561 
562  //remove the offset from the FT sensors measurements
563  F_LArm = -1.0 * (current_status.ft_arm_left-Offset_LArm);
564  F_RArm = -1.0 * (current_status.ft_arm_right-Offset_RArm);
565  F_LLeg = -1.0 * (current_status.ft_leg_left-Offset_LLeg);
566  F_RLeg = -1.0 * (current_status.ft_leg_right-Offset_RLeg);
567  F_LFoot = -1.0 * (current_status.ft_foot_left-Offset_LFoot);
568  F_RFoot = -1.0 * (current_status.ft_foot_right-Offset_RFoot);
569  //F_LFoot = 1.0 * (current_status.ft_foot_left);
570  //F_RFoot = 1.0 * (current_status.ft_foot_right);
571 
572  //check if iCub is currently moving. If not, put the current iCub positions in the status queue
573  current_status.iCub_not_moving = current_status.checkIcubNotMoving();
574  if (current_status.iCub_not_moving)
575  {
576  not_moving_status.push_front(current_status);
577  if (not_moving_status.size()>status_queue_size)
578  {
579  not_moving_status.pop_back();
580  if (auto_drift_comp) yDebug ("drift_comp: buffer full\n"); //@@@DEBUG
581  }
582  }
583  else
584  {
585  //efficient way to clear the queue
586  not_moving_status.clear();
587  if (auto_drift_comp) yDebug ("drift_comp: clearing buffer\n"); //@@@DEBUG
588  }
589 
590  // THIS BLOCK SHOULD BE NOW DEPRECATED
591  if (!w0_dw0_enabled)
592  {
593  //if w0 and dw0 are too noisy, you can disable them using 'no_w0_dw0' option
594  current_status.inertial_w0.zero();
595  current_status.inertial_dw0.zero();
596  }
597 
598  Vector F_up(6, 0.0);
599  icub->upperTorso->setInertialMeasure(current_status.inertial_w0,current_status.inertial_dw0,current_status.inertial_d2p0);
600  icub->upperTorso->setSensorMeasurement(F_RArm,F_LArm,F_up);
601 
602 //#define DEBUG_PERFORMANCE
603 #ifdef DEBUG_PERFORMANCE
604  static double meanTime = 0.0;
605  static double startTime = 0;
606  startTime = Time::now();
607 #endif
608  icub->upperTorso->solveKinematics();
609  addSkinContacts();
610  icub->upperTorso->solveWrench();
611 #ifdef DEBUG_PERFORMANCE
612  meanTime += Time::now()-startTime;
613  yDebug("Mean uppertorso NE time: %.4f\n", meanTime/getIterations());
614 #endif
615 
616 //#define DEBUG_KINEMATICS
617 #ifdef DEBUG_KINEMATICS
618  // DEBUG ONLY
619  yDebug ("\nHEAD: %s \n", d2p0.toString().c_str());
620 
621  yDebug ("UPTORSO: %s \n", icub->upperTorso->getTorsoLinAcc().toString().c_str());
622 #endif
623 
624  icub->attachLowerTorso(F_RLeg,F_LLeg);
625  icub->lowerTorso->solveKinematics();
626  icub->lowerTorso->solveWrench();
627 
628 //#define DEBUG_KINEMATICS
629 #ifdef DEBUG_KINEMATICS
630  //DEBUG ONLY
631  yDebug ("LOWTORSO->UP: %s *** %s *** %s\n",
632  icub->lowerTorso->up->getLinAcc(0).toString().c_str(),
633  icub->lowerTorso->up->getLinAcc(1).toString().c_str(),
634  cub->lowerTorso->up->getLinAcc(2).toString().c_str());
635 
636  yDebug ("LOWTORSO: %s \n", icub->lowerTorso->getLinAcc().toString().c_str());
637 
638  yDebug ("LOWTORSO->RI: %s *** %s *** %s *** %s\n",
639  icub->lowerTorso->right->getLinAcc(0).toString().c_str(),
640  icub->lowerTorso->right->getLinAcc(1).toString().c_str(),
641  icub->lowerTorso->right->getLinAcc(2).toString().c_str(),
642  icub->lowerTorso->right->getLinAcc(3).toString().c_str());
643 
644  yDebug ("LOWTORSO->LE: %s *** %s *** %s *** %s\n",
645  icub->lowerTorso->left->getLinAcc(0).toString().c_str(),
646  icub->lowerTorso->left->getLinAcc(1).toString().c_str(),
647  icub->lowerTorso->left->getLinAcc(2).toString().c_str(),
648  icub->lowerTorso->left->getLinAcc(3).toString().c_str());
649 #endif
650 
651 //#define DEBUG_WRENCH
652 #ifdef DEBUG_WRENCH
653 
654  yDebug ("LOWTORSO->UP: %s *** %s \n",
655  icub->lowerTorso->up->getForce(0).toString().c_str(),
656  icub->lowerTorso->up->getMoment(0).toString().c_str());
657 
658  yDebug ("LOWTORSO->RO: %s *** %s \n",
659  icub->lowerTorso->up->getForce(2).toString().c_str(),
660  icub->lowerTorso->up->getMoment(2).toString().c_str());
661 #endif
662 
663  Vector LATorques = icub->upperTorso->getTorques("left_arm");
664  Vector RATorques = icub->upperTorso->getTorques("right_arm");
665  Vector HDtmp = icub->upperTorso->getTorques("head");
666 
667  Vector LLTorques = icub->lowerTorso->getTorques("left_leg");
668  Vector RLTorques = icub->lowerTorso->getTorques("right_leg");
669  Vector TOtmp = icub->lowerTorso->getTorques("torso");
670  Vector TOTorques(3);
671  Vector HDTorques(3);
672 
673  //head torques
674  HDTorques[0] = HDtmp [0];
675  HDTorques[1] = HDtmp [1];
676  HDTorques[2] = HDtmp [2];
677  //torso torques
678  TOTorques[0] = TOtmp [2];
679  TOTorques[1] = TOtmp [1];
680  TOTorques[2] = TOtmp [0];
681 
682 //#define DEBUG_TORQUES
683 #ifdef DEBUG_TORQUES
684  yDebug ("TORQUES: %s *** \n\n", TOTorques.toString().c_str());
685 #endif
686 
687  writeTorque(RATorques, 1, port_RATorques); //arm
688  writeTorque(LATorques, 1, port_LATorques); //arm
689  writeTorque(TOTorques, 4, port_TOTorques); //torso
690  writeTorque(HDTorques, 0, port_HDTorques); //head
691 
692  if (ddLR) writeTorque(RLTorques, 2, port_RLTorques); //leg
693  if (ddLL) writeTorque(LLTorques, 2, port_LLTorques); //leg
694  writeTorque(RATorques, 3, port_RWTorques); //wrist
695  writeTorque(LATorques, 3, port_LWTorques); //wrist
696 
697  Vector com_all(7), com_ll(7), com_rl(7), com_la(7),com_ra(7), com_hd(7), com_to(7), com_lb(7), com_ub(7);
698  double mass_all , mass_ll , mass_rl , mass_la ,mass_ra , mass_hd, mass_to, mass_lb, mass_ub;
699  Vector com_v; com_v.resize(3); com_v.zero();
700  Vector all_dq; all_dq.resize(32,1); all_dq.zero();
701  Vector all_q; all_q.resize(32,1); all_q.zero();
702 
703  // For balancing purposes
704  yarp::sig::Vector com_all_foot; com_all_foot.resize(3); com_all_foot.zero();
705  yarp::sig::Matrix rTf; rTf.resize(4,4); rTf.zero();
706  yarp::sig::Matrix fTr; fTr.resize(4,4); fTr.zero();
707  yarp::sig::Matrix lastRotTrans; lastRotTrans.resize(4,4); lastRotTrans.zero();
708  lastRotTrans(2,0)=lastRotTrans(3,3)=lastRotTrans(0,2)=1;
709  lastRotTrans(1,1)=-1;
710 
711  rTf = icub->lowerTorso->right->getH();
712 
713  rTf = (icub->lowerTorso->getHRight()) * rTf*lastRotTrans; //Until the world reference frame
714  fTr.setSubmatrix(rTf.submatrix(0,2,0,2).transposed(), 0, 0); //CHECKED
715  fTr.setSubcol(-1*fTr.submatrix(0,2,0,2)*rTf.subcol(0,3,3), 0, 3); //CHECKED
716  fTr(3,3) = 1;
717 
718  //filling distance from root projection onto the floor to the right foot.
719  lastRotTrans(1,3)=-rTf(1,3);
720  lastRotTrans(2,3)= -rTf(0,3);
721 
722 
723  if (com_enabled)
724  {
725  icub->computeCOM();
726 
727  if (com_vel_enabled)
728  {
731  icub->EXPERIMENTAL_getCOMvelocity(BODY_PART_ALL,com_v,all_dq);
732  icub->getAllPositions(all_q);
733  }
734 
735  icub->getCOM(BODY_PART_ALL, com_all, mass_all);
736  icub->getCOM(LOWER_BODY_PARTS, com_lb, mass_lb);
737  icub->getCOM(UPPER_BODY_PARTS, com_ub, mass_ub);
738  icub->getCOM(LEFT_LEG, com_ll, mass_ll);
739  icub->getCOM(RIGHT_LEG, com_rl, mass_rl);
740  icub->getCOM(LEFT_ARM, com_la, mass_la);
741  icub->getCOM(RIGHT_ARM, com_ra, mass_ra);
742  icub->getCOM(HEAD, com_hd, mass_hd);
743  icub->getCOM(TORSO, com_to, mass_to);
744  com_lb.push_back(mass_lb);
745  com_ub.push_back(mass_ub);
746  com_all.push_back(mass_all);
747  com_ll.push_back (mass_ll);
748  com_rl.push_back (mass_rl);
749  com_la.push_back (mass_la);
750  com_ra.push_back (mass_ra);
751  com_hd.push_back (mass_hd);
752  com_to.push_back (mass_to);
753 
754  if (com_vel_enabled)
755  {
756  com_all.push_back(com_v[0]);
757  com_all.push_back(com_v[1]);
758  com_all.push_back(com_v[2]);
759  }
760  else
761  {
762  com_all.push_back(0);
763  com_all.push_back(0);
764  com_all.push_back(0);
765  }
766 
767  #ifdef MEASURE_FROM_FOOT
768  com_all_foot.setSubvector(0,com_all.subVector(0,2));
769  com_all_foot.push_back(1);
770  com_all_foot = fTr*com_all_foot;
771  #endif
772 
773  }
774  else
775  {
776  mass_all=mass_ll=mass_rl=mass_la=mass_ra=mass_hd=mass_to=0.0;
777  com_all.zero(); com_ll.zero(); com_rl.zero(); com_la.zero(); com_ra.zero(); com_hd.zero(); com_to.zero();
778  }
779 
780  // DYN/SKIN CONTACTS
781  dynContacts = icub->upperTorso->leftSensor->getContactList();
782  const dynContactList& contactListR = icub->upperTorso->rightSensor->getContactList();
783  dynContacts.insert(dynContacts.begin(), contactListR.begin(), contactListR.end());
784  // for each dynContact find the related skinContact (if any) and set the wrench in it
785  unsigned long cId;
786  bool contactFound=false;
787  for(unsigned int i=0; i<dynContacts.size(); i++)
788  {
789  cId = dynContacts[i].getId();
790  for(unsigned int j=0; j<skinContacts.size(); j++)
791  {
792  if(cId == skinContacts[j].getId())
793  {
794  skinContacts[j].setForceMoment( dynContacts[i].getForceMoment() );
795  contactFound = true;
796  j = skinContacts.size(); // break from the inside for loop
797  }
798  }
799  // if there is no associated skin contact, create one
800  if(!contactFound)
801  skinContacts.push_back(skinContact(dynContacts[i]));
802  contactFound = false;
803  }
804 
805  //*********************************************** add the legs contacts JUST TEMP FIX!! *******************
806  skinContact left_leg_contact;
807  left_leg_contact.setLinkNumber(5);
808  left_leg_contact.setBodyPart(iCub::skinDynLib::LEFT_LEG);
809  left_leg_contact.setForceMoment(F_ext_left_leg);
810  skinContact right_leg_contact;
811  right_leg_contact.setLinkNumber(5);
812  right_leg_contact.setBodyPart(iCub::skinDynLib::RIGHT_LEG);
813  right_leg_contact.setForceMoment(F_ext_right_leg);
814 
815  if (!add_legs_once)
816  {
817  skinContacts.push_back(left_leg_contact);
818  skinContacts.push_back(right_leg_contact);
819  add_legs_once = true;
820  }
821 
822  bool skin_lleg_found = false;
823  bool skin_rleg_found = false;
824  for(unsigned int j=0; j<skinContacts.size(); j++)
825  {
826  if (skinContacts[j].getBodyPart()==iCub::skinDynLib::LEFT_LEG)
827  {
828  skinContacts[j].setForceMoment(F_ext_left_leg);
829  skin_lleg_found = true;
830  }
831  if (skinContacts[j].getBodyPart()==iCub::skinDynLib::RIGHT_LEG)
832  {
833  skinContacts[j].setForceMoment(F_ext_right_leg);
834  skin_rleg_found = true;
835  }
836  }
837  if (!skin_rleg_found) {skinContacts.push_back(right_leg_contact);}
838  if (!skin_lleg_found) {skinContacts.push_back(left_leg_contact);}
839 
840  //*********************************************** add the legs contacts JUST TEMP FIX!! *******************
841 
842  F_ext_cartesian_left_arm = F_ext_cartesian_right_arm = zeros(6);
843  F_ext_cartesian_left_leg = F_ext_cartesian_right_leg = zeros(6);
844  F_ext_left_arm = icub->upperTorso->leftSensor->getForceMomentEndEff();
845  F_ext_right_arm = icub->upperTorso->rightSensor->getForceMomentEndEff();
846  F_ext_left_leg = icub->lowerTorso->leftSensor->getForceMomentEndEff();
847  F_ext_right_leg = icub->lowerTorso->rightSensor->getForceMomentEndEff();
848 
849  // EXTERNAL DYNAMICS AT THE F/T SENSORS
850  Matrix F_sensor_up = icub_sens->upperTorso->estimateSensorsWrench(zeros(6,3));
851  F_ext_sens_right_arm = F_RArm - F_sensor_up.getCol(0); // measured wrench - internal wrench = external wrench
852  F_ext_sens_left_arm = F_LArm - F_sensor_up.getCol(1); // measured wrench - internal wrench = external wrench
853 
854 #ifdef TEST_LEG_SENSOR
855  setUpperMeasure(true);
856  setLowerMeasure(true);
857 #endif
858 
859  Matrix F_sensor_low = icub_sens->lowerTorso->estimateSensorsWrench(F_ext_low,false);
860  F_ext_sens_right_leg = F_RLeg - F_sensor_low.getCol(0); // measured wrench - internal wrench = external wrench
861  F_ext_sens_left_leg = F_LLeg - F_sensor_low.getCol(1); // measured wrench - internal wrench = external wrench
862 
863 #ifdef TEST_LEG_SENSOR
864  F_mdl_right_leg = F_sensor_low.getCol(0);
865  F_mdl_left_leg = F_sensor_low.getCol(1);
866  F_sns_right_leg = F_RLeg;
867  F_sns_left_leg = F_LLeg;
868 #endif
869 
870  yarp::sig::Matrix ht = icub->upperTorso->getHUp() * icub->upperTorso->up->getH();
871  yarp::sig::Matrix ahl = ht * icub->upperTorso->getHLeft() * icub->upperTorso->left->getH();
872  yarp::sig::Matrix ahr = ht * icub->upperTorso->getHRight() * icub->upperTorso->right->getH();
873  yarp::sig::Matrix lhl = icub->lowerTorso->getHLeft() * icub->lowerTorso->left->getH();
874  yarp::sig::Matrix lhr = icub->lowerTorso->getHRight() * icub->lowerTorso->right->getH();
875 
876  yarp::sig::Vector tmp1,tmp2;
877  tmp1 = F_ext_left_arm.subVector(0,2); tmp1.push_back(0.0); tmp1 = ahl * tmp1;
878  tmp2 = F_ext_left_arm.subVector(3,5); tmp2.push_back(0.0); tmp2 = ahl * tmp2;
879  for (int i=0; i<3; i++) F_ext_cartesian_left_arm[i] = tmp1[i];
880  for (int i=3; i<6; i++) F_ext_cartesian_left_arm[i] = tmp2[i-3];
881  double n1=norm(F_ext_cartesian_left_arm.subVector(0,2));
882  double n2=norm(F_ext_cartesian_left_arm.subVector(3,5));
883  F_ext_cartesian_left_arm.push_back(n1);
884  F_ext_cartesian_left_arm.push_back(n2);
885 
886  tmp1 = F_ext_right_arm.subVector(0,2); tmp1.push_back(0.0); tmp1 = ahr * tmp1;
887  tmp2 = F_ext_right_arm.subVector(3,5); tmp2.push_back(0.0); tmp2 = ahr * tmp2;
888  for (int i=0; i<3; i++) F_ext_cartesian_right_arm[i] = tmp1[i];
889  for (int i=3; i<6; i++) F_ext_cartesian_right_arm[i] = tmp2[i-3];
890  n1=norm(F_ext_cartesian_right_arm.subVector(0,2));
891  n2=norm(F_ext_cartesian_right_arm.subVector(3,5));
892  F_ext_cartesian_right_arm.push_back(n1);
893  F_ext_cartesian_right_arm.push_back(n2);
894 
895  tmp1 = F_ext_left_leg.subVector(0,2); tmp1.push_back(0.0); tmp1 = lhl * tmp1;
896  tmp2 = F_ext_left_leg.subVector(3,5); tmp2.push_back(0.0); tmp2 = lhl * tmp2;
897  for (int i=0; i<3; i++) F_ext_cartesian_left_leg[i] = tmp1[i];
898  for (int i=3; i<6; i++) F_ext_cartesian_left_leg[i] = tmp2[i-3];
899 
900  tmp1 = F_ext_right_leg.subVector(0,2); tmp1.push_back(0.0); tmp1 = lhr * tmp1;
901  tmp2 = F_ext_right_leg.subVector(3,5); tmp2.push_back(0.0); tmp2 = lhr * tmp2;
902  for (int i=0; i<3; i++) F_ext_cartesian_right_leg[i] = tmp1[i];
903  for (int i=3; i<6; i++) F_ext_cartesian_right_leg[i] = tmp2[i-3];
904 
905  //computation of the root
906  yarp::sig::Vector angles (3);
907  angles[0] = 0;
908  angles[1] = -90/180.0*M_PI;
909  angles[2] = 0;
910  yarp::sig::Matrix r1 = yarp::math::euler2dcm(angles);
911  yarp::sig::Matrix ilhl = yarp::math::SE3inv(lhl);
912  yarp::sig::Matrix foot_root_mat = r1*ilhl;
913 
914  yarp::sig::Vector foot_tmp = yarp::math::dcm2rpy(foot_root_mat);
915  //yDebug ("before\n %s\n", foot_root_mat.toString().c_str());
916  yarp::sig::Vector foot_root_vec (6,0.0);
917  foot_root_vec[3] = foot_root_mat[0][3]*1000;
918  foot_root_vec[4] = foot_root_mat[1][3]*1000;
919  foot_root_vec[5] = foot_root_mat[2][3]*1000;
920  foot_root_vec[0] = foot_tmp[0]*180.0/M_PI;
921  foot_root_vec[1] = foot_tmp[1]*180.0/M_PI;
922  foot_root_vec[2] = foot_tmp[2]*180.0/M_PI;
923  //yarp::sig::Matrix test = iCub::ctrl::rpy2dcm(foot_tmp);
924  //yDebug ("afer\n %s\n", test.toString().c_str());
925  //yDebug ("angles %+.2f %+.2f %+.2f\n", foot_tmp[0]*180.0/M_PI, foot_tmp[1]*180.0/M_PI, foot_tmp[2]*180.0/M_PI);
926 
927  //computation for the foot
928  Matrix foot_hn(4,4); foot_hn.zero();
929  foot_hn(0,2)=1;foot_hn(0,3)=-7.75;
930  foot_hn(1,1)=-1;
931  foot_hn(2,0)=-1;
932  foot_hn(3,3)=1;
933 
934  tmp1 = F_LFoot.subVector(0,2); tmp1.push_back(0.0); tmp1 = foot_hn * tmp1;
935  tmp2 = F_LFoot.subVector(3,5); tmp2.push_back(0.0); tmp2 = foot_hn * tmp2;
936  for (int i=0; i<3; i++) F_ext_left_foot[i] = tmp1[i];
937  for (int i=3; i<6; i++) F_ext_left_foot[i] = tmp2[i-3];
938 
939  tmp1 = F_RFoot.subVector(0,2); tmp1.push_back(0.0); tmp1 = foot_hn * tmp1;
940  tmp2 = F_RFoot.subVector(3,5); tmp2.push_back(0.0); tmp2 = foot_hn * tmp2;
941  for (int i=0; i<3; i++) F_ext_right_foot[i] = tmp1[i];
942  for (int i=3; i<6; i++) F_ext_right_foot[i] = tmp2[i-3];
943 
944  tmp1 = F_ext_left_foot.subVector(0,2); tmp1.push_back(0.0); tmp1 = lhl * tmp1;
945  tmp2 = F_ext_left_foot.subVector(3,5); tmp2.push_back(0.0); tmp2 = lhl * tmp2;
946  for (int i=0; i<3; i++) F_ext_cartesian_left_foot[i] = tmp1[i];
947  for (int i=3; i<6; i++) F_ext_cartesian_left_foot[i] = tmp2[i-3];
948 
949  tmp1 = F_ext_right_foot.subVector(0,2); tmp1.push_back(0.0); tmp1 = lhr * tmp1;
950  tmp2 = F_ext_right_foot.subVector(3,5); tmp2.push_back(0.0); tmp2 = lhr * tmp2;
951  for (int i=0; i<3; i++) F_ext_cartesian_right_foot[i] = tmp1[i];
952  for (int i=3; i<6; i++) F_ext_cartesian_right_foot[i] = tmp2[i-3];
953 
954  // *** MONITOR DATA ***
955  //sendMonitorData();
956 
957  // *** DUMP VEL DATA ***
958  //sendVelAccData();
959 
960  if (com_vel_enabled)
961  {
962  // com_jac = M_PI/180.0 * (com_jac);
963  // all_dq = 180.0/M_PI * all_dq;
964  // all_q = 180.0/M_PI * all_q;
965  broadcastData<Vector> (com_v, port_COM_vel);
966  broadcastData<Vector> (all_dq, port_all_velocities);
967  broadcastData<Vector> (all_q, port_all_positions);
968  broadcastData<Matrix> (com_jac, port_COM_Jacobian);
969  }
970  broadcastData<Vector> (com_all, port_com_all);
971  broadcastData<Vector> (com_lb, port_com_lb);
972  broadcastData<Vector> (com_ub, port_com_ub);
973  broadcastData<Vector> (com_ll, port_com_ll);
974  broadcastData<Vector> (com_rl, port_com_rl);
975  broadcastData<Vector> (com_la, port_com_la);
976  broadcastData<Vector> (com_ra, port_com_ra);
977  broadcastData<Vector> (com_hd, port_com_hd);
978  broadcastData<Vector> (com_to, port_com_to);
979  broadcastData<Vector> (com_all_foot, port_com_all_foot);
980 
981  broadcastData<Vector> (F_up, port_external_wrench_TO);
982  broadcastData<Vector> (F_ext_right_arm, port_external_wrench_RA);
983  broadcastData<Vector> (F_ext_left_arm, port_external_wrench_LA);
984  broadcastData<Vector> (F_ext_cartesian_right_arm, port_external_cartesian_wrench_RA);
985  broadcastData<Vector> (F_ext_cartesian_left_arm, port_external_cartesian_wrench_LA);
986  broadcastData<Vector> (F_ext_right_leg, port_external_wrench_RL);
987  broadcastData<Vector> (F_ext_left_leg, port_external_wrench_LL);
988  broadcastData<Vector> (F_ext_right_foot, port_external_wrench_RF);
989  broadcastData<Vector> (F_ext_left_foot, port_external_wrench_LF);
990 #ifdef TEST_LEG_SENSOR
991  broadcastData<Vector> (F_sns_right_leg, port_sensor_wrench_RL);
992  broadcastData<Vector> (F_sns_left_leg, port_sensor_wrench_LL);
993  broadcastData<Vector> (F_mdl_right_leg, port_model_wrench_RL);
994  broadcastData<Vector> (F_mdl_left_leg, port_model_wrench_LL);
995 #endif
996  broadcastData<Vector> (F_ext_cartesian_right_leg, port_external_cartesian_wrench_RL);
997  broadcastData<Vector> (F_ext_cartesian_left_leg, port_external_cartesian_wrench_LL);
998  broadcastData<Vector> (F_ext_cartesian_right_foot, port_external_cartesian_wrench_RF);
999  broadcastData<Vector> (F_ext_cartesian_left_foot, port_external_cartesian_wrench_LF);
1000  broadcastData<skinContactList>( skinContacts, port_contacts);
1001  broadcastData<Vector> (F_ext_sens_right_arm, port_external_ft_arm_right);
1002  broadcastData<Vector> (F_ext_sens_left_arm, port_external_ft_arm_left);
1003  broadcastData<Vector> (F_ext_sens_right_leg, port_external_ft_leg_right);
1004  broadcastData<Vector> (F_ext_sens_left_leg, port_external_ft_leg_left);
1005 
1006  broadcastData<Matrix> (foot_root_mat, port_root_position_mat);
1007  broadcastData<Vector> (foot_root_vec, port_root_position_vec);
1008 }
1009 
1011 {
1012  yInfo( "Closing the linear estimator\n");
1013  if(linEstUp)
1014  {
1015  delete linEstUp;
1016  linEstUp = 0;
1017  }
1018  if(linEstLow)
1019  {
1020  delete linEstLow;
1021  linEstLow = 0;
1022  }
1023  yInfo( "Closing the quadratic estimator\n");
1024  if(quadEstUp)
1025  {
1026  delete quadEstUp;
1027  quadEstUp = 0;
1028  }
1029  if(quadEstLow)
1030  {
1031  delete quadEstLow;
1032  quadEstLow = 0;
1033  }
1034  yInfo( "Closing the inertial estimator\n");
1035  if(InertialEst)
1036  {
1037  delete InertialEst;
1038  InertialEst = 0;
1039  }
1040 
1041  yInfo( "Closing RATorques port\n");
1042  closePort(port_RATorques);
1043  yInfo( "Closing LATorques port\n");
1044  closePort(port_LATorques);
1045  yInfo( "Closing RLTorques port\n");
1046  closePort(port_RLTorques);
1047  yInfo( "Closing LLTorques port\n");
1048  closePort(port_LLTorques);
1049  yInfo( "Closing RWTorques port\n");
1050  closePort(port_RWTorques);
1051  yInfo( "Closing LWTorques port\n");
1052  closePort(port_LWTorques);
1053  yInfo( "Closing TOTorques port\n");
1054  closePort(port_TOTorques);
1055  yInfo( "Closing HDTorques port\n");
1056  closePort(port_HDTorques);
1057  yInfo( "Closing external_wrench_RA port\n");
1058  closePort(port_external_wrench_RA);
1059  yInfo( "Closing external_wrench_LA port\n");
1060  closePort(port_external_wrench_LA);
1061 #ifdef TEST_LEG_SENSOR
1062  closePort(port_sensor_wrench_RL);
1063  closePort(port_sensor_wrench_LL);
1064  closePort(port_model_wrench_RL);
1065  closePort(port_model_wrench_LL);
1066 #endif
1067  yInfo( "Closing cartesian_external_wrench_RA port\n");
1068  closePort(port_external_cartesian_wrench_RA);
1069  yInfo( "Closing cartesian_external_wrench_LA port\n");
1070  closePort(port_external_cartesian_wrench_LA);
1071  yInfo( "Closing external_wrench_RL port\n");
1072  closePort(port_external_wrench_RL);
1073  yInfo( "Closing external_wrench_LL port\n");
1074  closePort(port_external_wrench_LL);
1075  yInfo( "Closing cartesian_external_wrench_RL port\n");
1076  closePort(port_external_cartesian_wrench_RL);
1077  yInfo( "Closing cartesian_external_wrench_LL port\n");
1078  closePort(port_external_cartesian_wrench_LL);
1079  yInfo( "Closing external_wrench_RF port\n");
1080  closePort(port_external_wrench_RF);
1081  yInfo( "Closing external_wrench_LF port\n");
1082  closePort(port_external_wrench_LF);
1083  yInfo( "Closing cartesian_external_wrench_RF port\n");
1084  closePort(port_external_cartesian_wrench_RF);
1085  yInfo( "Closing cartesian_external_wrench_LF port\n");
1086  closePort(port_external_cartesian_wrench_LF);
1087  yInfo( "Closing external_wrench_TO port\n");
1088  closePort(port_external_wrench_TO);
1089  yInfo( "Closing COM ports\n");
1090  closePort(port_com_all);
1091  closePort(port_com_lb);
1092  closePort(port_com_ub);
1093  closePort(port_com_ra);
1094  closePort(port_com_rl);
1095  closePort(port_com_la);
1096  closePort(port_com_ll);
1097  closePort(port_com_hd);
1098  closePort(port_com_to);
1099  closePort(port_com_all_foot);
1100 
1101  yInfo( "Closing inertial port\n");
1102  closePort(port_inertial_thread);
1103  yInfo( "Closing ft_arm_right port\n");
1104  closePort(port_ft_arm_right);
1105  yInfo( "Closing ft_arm_left port\n");
1106  closePort(port_ft_arm_left);
1107  yInfo( "Closing ft_leg_right port\n");
1108  closePort(port_ft_leg_right);
1109  yInfo( "Closing ft_leg_left port\n");
1110  closePort(port_ft_leg_left);
1111  yInfo( "Closing ft_foot_right port\n");
1112  closePort(port_ft_foot_right);
1113  yInfo( "Closing ft_foot_left port\n");
1114  closePort(port_ft_foot_left);
1115  yInfo( "Closing skin_contacts port\n");
1116  closePort(port_skin_contacts);
1117  yInfo( "Closing monitor port\n");
1118  closePort(port_monitor);
1119  yInfo( "Closing dump port\n");
1120  closePort(port_dumpvel);
1121  yInfo( "Closing contacts port\n");
1122  closePort(port_contacts);
1123  yInfo( "Closing external_ft_arm_left port\n");
1124  closePort(port_external_ft_arm_left);
1125  yInfo( "Closing external_ft_arm_right port\n");
1126  closePort(port_external_ft_arm_right);
1127  yInfo("Closing external_ft_leg_left port\n");
1128  closePort(port_external_ft_leg_left);
1129  yInfo("Closing external_ft_leg_right port\n");
1130  closePort(port_external_ft_leg_right);
1131  yInfo("Close COM velocity port\n");
1132  closePort(port_COM_vel);
1133  yInfo("Closing COM Jacobian port\n");
1134  closePort(port_COM_Jacobian);
1135  yInfo("Closing All Velocities port\n");
1136  closePort(port_all_velocities);
1137  yInfo("Closing All Positions port\n");
1138  closePort(port_all_positions);
1139  yInfo("Closing Foot/Root port\n");
1140  closePort(port_root_position_mat);
1141  closePort(port_root_position_vec);
1142 
1143  if (icub) {delete icub; icub=0;}
1144  if (icub_sens) {delete icub_sens; icub=0;}
1145 }
1146 
1147 void inverseDynamics::closePort(Contactable *_port)
1148 {
1149  if (_port)
1150  {
1151  _port->interrupt();
1152  _port->close();
1153 
1154  delete _port;
1155  _port = nullptr;
1156  }
1157 }
1158 
1159 template <class T> void inverseDynamics::broadcastData(T& _values, BufferedPort<T> *_port)
1160 {
1161  if (_port && _port->getOutputCount()>0)
1162  {
1163  _port->setEnvelope(this->timestamp);
1164  _port->prepare() = _values ;
1165  _port->write();
1166  }
1167 }
1168 
1169 void inverseDynamics::writeTorque(Vector _values, int _address, BufferedPort<Bottle> *_port)
1170 {
1171  Bottle a;
1172  a.addInt32(_address);
1173  for(size_t i=0;i<_values.length();i++)
1174  a.addFloat64(_values(i));
1175  _port->prepare() = a;
1176  _port->write();
1177 }
1178 
1180 {
1181  yInfo("calibrateOffset: starting calibration... \n");
1182 
1183  Offset_LArm.zero();
1184  Offset_RArm.zero();
1185  Offset_LLeg.zero();
1186  Offset_RLeg.zero();
1187  Offset_LFoot.zero();
1188  Offset_RFoot.zero();
1189  F_ext_up.zero();
1190  F_ext_low.zero();
1191  setUpperMeasure(true);
1192  setLowerMeasure(true);
1193 
1194  size_t Ntrials = previous_status.size();
1195  list<iCubStatus>::iterator it=previous_status.begin();
1196 
1197  icub_sens->upperTorso->setInertialMeasure(it->inertial_w0,it->inertial_dw0,it->inertial_d2p0);
1198  Matrix F_sensor_up = icub_sens->upperTorso->estimateSensorsWrench(F_ext_up,false);
1199  icub_sens->lowerTorso->setInertialMeasure(icub_sens->upperTorso->getTorsoAngVel(),icub_sens->upperTorso->getTorsoAngAcc(),icub_sens->upperTorso->getTorsoLinAcc());
1200  Matrix F_sensor_low = icub_sens->lowerTorso->estimateSensorsWrench(F_ext_low,false);
1201 
1202  for (it=previous_status.begin() ; it != previous_status.end(); it++ )
1203  {
1204  /*
1205  // TO BE VERIFIED IF USEFUL
1206  setZeroJntAngVelAcc();
1207  */
1208 
1209  F_iDyn_LArm = -1.0 * F_sensor_up.getCol(1);
1210  F_iDyn_RArm = -1.0 * F_sensor_up.getCol(0);
1211  F_iDyn_LLeg = -1.0 * F_sensor_low.getCol(1);
1212  F_iDyn_RLeg = -1.0 * F_sensor_low.getCol(0);
1213  F_iDyn_LFoot = Vector(6,0.0); //@@@ TO BE COMPLETED
1214  F_iDyn_RFoot = Vector(6,0.0); //@@@ TO BE COMPLETED
1215 
1216  Offset_LArm = Offset_LArm + (it->ft_arm_left-F_iDyn_LArm);
1217  Offset_RArm = Offset_RArm + (it->ft_arm_right-F_iDyn_RArm);
1218  Offset_LLeg = Offset_LLeg + (it->ft_leg_left-F_iDyn_LLeg);
1219  Offset_RLeg = Offset_RLeg + (it->ft_leg_right-F_iDyn_RLeg);
1220  Offset_LFoot = Offset_LFoot + (it->ft_foot_left-F_iDyn_LFoot);
1221  Offset_RFoot = Offset_RFoot + (it->ft_foot_right-F_iDyn_RFoot);
1222  }
1223 
1224  Offset_LArm = 1.0/(double)Ntrials * Offset_LArm;
1225  Offset_RArm = 1.0/(double)Ntrials * Offset_RArm;
1226  Offset_LLeg = 1.0/(double)Ntrials * Offset_LLeg;
1227  Offset_RLeg = 1.0/(double)Ntrials * Offset_RLeg;
1228  Offset_LFoot = 1.0/(double)Ntrials * Offset_LFoot;
1229  Offset_RFoot = 1.0/(double)Ntrials * Offset_RFoot;
1230 
1231  it=previous_status.begin();
1232  yDebug("\n");
1233  yDebug( "Ntrials: %d\n", (int)Ntrials);
1234  yDebug( "F_LArm: %s\n", it->ft_arm_left.toString().c_str());
1235  yDebug( "F_idyn_LArm: %s\n", F_iDyn_LArm.toString().c_str());
1236  yDebug( "F_RArm: %s\n", it->ft_arm_right.toString().c_str());
1237  yDebug( "F_idyn_RArm: %s\n", F_iDyn_RArm.toString().c_str());
1238  yDebug( "F_LLeg: %s\n", it->ft_leg_left.toString().c_str());
1239  yDebug( "F_idyn_LLeg: %s\n", F_iDyn_LLeg.toString().c_str());
1240  yDebug( "F_RLeg: %s\n", it->ft_leg_right.toString().c_str());
1241  yDebug( "F_idyn_RLeg: %s\n", F_iDyn_RLeg.toString().c_str());
1242  yDebug( "\n");
1243  yDebug( "Left Arm: %s\n", Offset_LArm.toString().c_str());
1244  yDebug( "Right Arm: %s\n", Offset_RArm.toString().c_str());
1245  yDebug( "Left Leg: %s\n", Offset_LLeg.toString().c_str());
1246  yDebug( "Right Leg: %s\n", Offset_RLeg.toString().c_str());
1247  yDebug( "Left Foot: %s\n", Offset_LFoot.toString().c_str());
1248  yDebug( "Right Foot: %s\n", Offset_RFoot.toString().c_str());
1249  yDebug( "\n");
1250 }
1251 
1252 bool inverseDynamics::readAndUpdate(bool waitMeasure, bool _init)
1253 {
1254  bool b = true;
1255 
1256  // arms
1257  if (ddAL)
1258  {
1259  Vector* tmp= nullptr;
1260  if (waitMeasure) yInfo("Trying to connect to left arm sensor...");
1261  if (!dummy_ft)
1262  {
1263  tmp = port_ft_arm_left->read(waitMeasure);
1264  if (tmp != nullptr)
1265  {
1266  current_status.ft_arm_left = *tmp;
1267  }
1268  }
1269  else
1270  {
1271  current_status.ft_arm_left.zero();
1272  }
1273  if (waitMeasure) yDebug("done. \n");
1274  }
1275 
1276  if (ddAR)
1277  {
1278  Vector* tmp= nullptr;
1279  if (waitMeasure) yInfo("Trying to connect to right arm sensor...");
1280  if (!dummy_ft)
1281  {
1282  tmp = port_ft_arm_right->read(waitMeasure);
1283  if (tmp != nullptr)
1284  {
1285  current_status.ft_arm_right = *tmp;
1286  }
1287  }
1288  else
1289  {
1290  current_status.ft_arm_right.zero();
1291  }
1292  if (waitMeasure) yInfo("done. \n");
1293  }
1295  setUpperMeasure(_init);
1296 
1297  // legs
1298  if (ddLL)
1299  {
1300  Vector* tmp= nullptr;
1301  if (waitMeasure) yInfo("Trying to connect to left leg sensor...");
1302  if (!dummy_ft)
1303  {
1304  tmp = port_ft_leg_left->read(waitMeasure);
1305  if (tmp != nullptr)
1306  {
1307  current_status.ft_leg_left = *tmp;
1308  }
1309  }
1310  else
1311  {
1312  current_status.ft_leg_left.zero();
1313  }
1314  if (waitMeasure) yInfo("done. \n");
1315  }
1316  if (ddLR)
1317  {
1318  Vector* tmp= nullptr;
1319  if (waitMeasure) yInfo("Trying to connect to right leg sensor...");
1320  if (!dummy_ft)
1321  {
1322  tmp = port_ft_leg_right->read(waitMeasure);
1323  if (tmp != nullptr)
1324  {
1325  current_status.ft_leg_right = *tmp;
1326  }
1327  }
1328  else
1329  {
1330  current_status.ft_leg_right.zero();
1331  }
1332  if (waitMeasure) yInfo("done. \n");
1333  }
1334 
1335  // feet
1336  if (ddLL)
1337  {
1338  Vector* tmp= nullptr;
1339  if (waitMeasure) yInfo("Trying to connect to left foot sensor...");
1340  if (!dummy_ft)
1341  {
1342  tmp = port_ft_foot_left->read(false); //not all the robot versions have the FT sensors installed in the feet
1343  if (tmp != nullptr)
1344  {
1345  current_status.ft_foot_left = *tmp;
1346  }
1347  }
1348  else
1349  {
1350  current_status.ft_foot_left.zero();
1351  }
1352  if (waitMeasure) yInfo("done. \n");
1353  }
1354  if (ddLR)
1355  {
1356  Vector* tmp= nullptr;
1357  if (waitMeasure) yInfo("Trying to connect to right foot sensor...");
1358  if (!dummy_ft)
1359  {
1360  tmp = port_ft_foot_right->read(false); //not all the robot versions have the FT sensors installed in the feet
1361  if (tmp != nullptr)
1362  {
1363  current_status.ft_foot_right = *tmp;
1364  }
1365  }
1366  else
1367  {
1368  current_status.ft_foot_right.zero();
1369  }
1370  if (waitMeasure) yInfo("done. \n");
1371  }
1372 
1374  setLowerMeasure(_init);
1375 
1376  //inertial sensor
1377  if (waitMeasure) yInfo("Trying to connect to inertial sensor...");
1378  Vector *inertial = port_inertial_thread->read(waitMeasure);
1379  if (waitMeasure) yInfo("done. \n");
1380 
1381  int sz = 0;
1382  if(inertial!=nullptr)
1383  {
1384 //#define DEBUG_FIXED_INERTIAL
1385 #ifdef DEBUG_FIXED_INERTIAL
1386  (*inertial)[0] = 0;
1387  (*inertial)[1] = 0;
1388  (*inertial)[2] = 9.81;
1389  (*inertial)[3] = 0;
1390  (*inertial)[4] = 0;
1391  (*inertial)[5] = 0;
1392 #endif
1393  current_status.inertial_d2p0[0] = (*inertial)[0];
1394  current_status.inertial_d2p0[1] = (*inertial)[1];
1395  current_status.inertial_d2p0[2] = (*inertial)[2];
1396  current_status.inertial_w0 [0] = (*inertial)[3]*CTRL_DEG2RAD;
1397  current_status.inertial_w0 [1] = (*inertial)[4]*CTRL_DEG2RAD;
1398  current_status.inertial_w0 [2] = (*inertial)[5]*CTRL_DEG2RAD;
1399  current_status.inertial_dw0 = this->eval_domega(current_status.inertial_w0);
1400  //yDebug ("%3.3f, %3.3f, %3.3f \n",current_status.inertial_d2p0[0],current_status.inertial_d2p0[1],current_status.inertial_d2p0[2]);
1401 #ifdef DEBUG_PRINT_INERTIAL
1402  yDebug ("meas_w (rad/s): %3.3f, %3.3f, %3.3f \n", w0[0], w0[1], w0[2]);
1403  yDebug ("meas_dwo(rad/s): %3.3f, %3.3f, %3.3f \n", dw0[0], dw0[1], dw0[2]);
1404 #endif
1405  }
1406 
1407  //update the status memory
1408  current_status.timestamp=Time::now();
1409  previous_status.push_front(current_status);
1410  if (previous_status.size()>status_queue_size) previous_status.pop_back();
1411 
1412  return b;
1413 }
1414 
1416 {
1417  bool b = true;
1418  if (iencs_leg_left)
1419  {b &= iencs_leg_left->getEncoders(encoders_leg_left.data());}
1420  else
1421  {encoders_leg_left.zero();}
1422 
1423  if (iencs_leg_right)
1424  {b &= iencs_leg_right->getEncoders(encoders_leg_right.data());}
1425  else
1426  {encoders_leg_right.zero();}
1427 
1428  if (iencs_torso)
1429  {b &= iencs_torso->getEncoders(encoders_torso.data());}
1430  else
1431  {encoders_torso.zero();}
1432 
1433  for (size_t i=0;i<3;i++)
1434  {
1435  current_status.all_q_low(i) = encoders_torso(2-i);
1436  }
1437  for (size_t i=0;i<6;i++)
1438  {
1439  current_status.all_q_low(3+i) = encoders_leg_left(i);
1440  }
1441  for (size_t i=0;i<6;i++)
1442  {
1443  current_status.all_q_low(3+6+i) = encoders_leg_right(i);
1444  }
1445  current_status.all_dq_low = evalVelLow(current_status.all_q_low);
1446  current_status.all_d2q_low = evalAccLow(current_status.all_q_low);
1447 
1448  return b;
1449 }
1450 
1451 
1453 {
1454  bool b = true;
1455  if (iencs_arm_left) b &= iencs_arm_left->getEncoders(encoders_arm_left.data());
1456  else encoders_arm_left.zero();
1457  if (iencs_arm_right) b &= iencs_arm_right->getEncoders(encoders_arm_right.data());
1458  else encoders_arm_right.zero();
1459  if (iencs_head) b &= iencs_head->getEncoders(encoders_head.data());
1460  else encoders_head.zero();
1461 
1462  for (size_t i=0;i<3;i++)
1463  {
1464  current_status.all_q_up(i) = encoders_head(i);
1465  }
1466  for (size_t i=0;i<7;i++)
1467  {
1468  current_status.all_q_up(3+i) = encoders_arm_left(i);
1469  }
1470  for (size_t i=0;i<7;i++)
1471  {
1472  current_status.all_q_up(3+7+i) = encoders_arm_right(i);
1473  }
1474  current_status.all_dq_up = evalVelUp(current_status.all_q_up);
1475  current_status.all_d2q_up = evalAccUp(current_status.all_q_up);
1476 
1477  return b;
1478 }
1479 
1480 void inverseDynamics::setLowerMeasure(bool _init)
1481 {
1482  if(!_init)
1483  {
1484  icub->lowerTorso->setAng("torso",CTRL_DEG2RAD * current_status.get_q_torso());
1485  icub->lowerTorso->setDAng("torso",CTRL_DEG2RAD * current_status.get_dq_torso());
1486  icub->lowerTorso->setD2Ang("torso",CTRL_DEG2RAD * current_status.get_d2q_torso());
1487 
1488  icub->lowerTorso->setAng("left_leg",CTRL_DEG2RAD * current_status.get_q_lleg());
1489  icub->lowerTorso->setDAng("left_leg",CTRL_DEG2RAD * current_status.get_dq_lleg());
1490  icub->lowerTorso->setD2Ang("left_leg",CTRL_DEG2RAD * current_status.get_d2q_lleg());
1491 
1492  icub->lowerTorso->setAng("right_leg",CTRL_DEG2RAD * current_status.get_q_rleg());
1493  icub->lowerTorso->setDAng("right_leg",CTRL_DEG2RAD * current_status.get_dq_rleg());
1494  icub->lowerTorso->setD2Ang("right_leg",CTRL_DEG2RAD * current_status.get_d2q_rleg());
1495  }
1496  else
1497  {
1498  icub_sens->lowerTorso->setAng("torso",CTRL_DEG2RAD * current_status.get_q_torso());
1499  icub_sens->lowerTorso->setDAng("torso",CTRL_DEG2RAD * current_status.get_dq_torso());
1500  icub_sens->lowerTorso->setD2Ang("torso",CTRL_DEG2RAD * current_status.get_d2q_torso());
1501 
1502  icub_sens->lowerTorso->setAng("left_leg",CTRL_DEG2RAD * current_status.get_q_lleg());
1503  icub_sens->lowerTorso->setDAng("left_leg",CTRL_DEG2RAD * current_status.get_dq_lleg());
1504  icub_sens->lowerTorso->setD2Ang("left_leg",CTRL_DEG2RAD * current_status.get_d2q_lleg());
1505 
1506  icub_sens->lowerTorso->setAng("right_leg",CTRL_DEG2RAD * current_status.get_q_rleg());
1507  icub_sens->lowerTorso->setDAng("right_leg",CTRL_DEG2RAD * current_status.get_dq_rleg());
1508  icub_sens->lowerTorso->setD2Ang("right_leg",CTRL_DEG2RAD * current_status.get_d2q_rleg());
1509  }
1510 }
1511 
1512 void inverseDynamics::setUpperMeasure(bool _init)
1513 {
1514  if(!_init)
1515  {
1516  icub->upperTorso->setAng("head",CTRL_DEG2RAD * current_status.get_q_head());
1517  icub->upperTorso->setAng("left_arm",CTRL_DEG2RAD * current_status.get_q_larm());
1518  icub->upperTorso->setAng("right_arm",CTRL_DEG2RAD * current_status.get_q_rarm());
1519  icub->upperTorso->setDAng("head",CTRL_DEG2RAD * current_status.get_dq_head());
1520  icub->upperTorso->setDAng("left_arm",CTRL_DEG2RAD * current_status.get_dq_larm());
1521  icub->upperTorso->setDAng("right_arm",CTRL_DEG2RAD * current_status.get_dq_rarm());
1522  icub->upperTorso->setD2Ang("head",CTRL_DEG2RAD * current_status.get_d2q_head());
1523  icub->upperTorso->setD2Ang("left_arm",CTRL_DEG2RAD * current_status.get_d2q_larm());
1524  icub->upperTorso->setD2Ang("right_arm",CTRL_DEG2RAD * current_status.get_d2q_rarm());
1525  icub->upperTorso->setInertialMeasure(current_status.inertial_w0,current_status.inertial_dw0,current_status.inertial_d2p0);
1526  }
1527  else
1528  {
1529  icub_sens->upperTorso->setAng("head",CTRL_DEG2RAD * current_status.get_q_head());
1530  icub_sens->upperTorso->setAng("left_arm",CTRL_DEG2RAD * current_status.get_q_larm());
1531  icub_sens->upperTorso->setAng("right_arm",CTRL_DEG2RAD * current_status.get_q_rarm());
1532  icub_sens->upperTorso->setDAng("head",CTRL_DEG2RAD * current_status.get_dq_head());
1533  icub_sens->upperTorso->setDAng("left_arm",CTRL_DEG2RAD * current_status.get_dq_larm());
1534  icub_sens->upperTorso->setDAng("right_arm",CTRL_DEG2RAD * current_status.get_dq_rarm());
1535  icub_sens->upperTorso->setD2Ang("head",CTRL_DEG2RAD * current_status.get_d2q_head());
1536  icub_sens->upperTorso->setD2Ang("left_arm",CTRL_DEG2RAD * current_status.get_d2q_larm());
1537  icub_sens->upperTorso->setD2Ang("right_arm",CTRL_DEG2RAD *current_status.get_d2q_rarm());
1538  icub_sens->upperTorso->setInertialMeasure(current_status.inertial_w0,current_status.inertial_dw0,current_status.inertial_d2p0);
1539  }
1540 }
1541 
1543 {
1544  current_status.all_dq_up.zero();
1545  current_status.all_dq_low.zero();
1546  current_status.all_d2q_up.zero();
1547  current_status.all_d2q_low.zero();
1548 }
1549 
1550 void inverseDynamics::addSkinContacts()
1551 {
1552  skinContactList *scl = port_skin_contacts->read(false);
1553  if(scl)
1554  {
1555  skinContactsTimestamp = Time::now();
1556  if(scl->empty() && !default_ee_cont) // if no skin contacts => leave the old contacts but reset pressure and contact list
1557  {
1558  for(auto & skinContact : skinContacts)
1559  {
1560  skinContact.setPressure(0.0);
1562  }
1563  return;
1564  }
1565 
1566  map<BodyPart, skinContactList> contactsPerBp = scl->splitPerBodyPart();
1567  // if there are more than 1 contact and less than 10 taxels are active then suppose zero moment
1568  for(auto & it : contactsPerBp)
1569  if(it.second.size()>1)
1570  for(auto & c : it.second)
1571  if(c.getActiveTaxels()<10)
1572  c.fixMoment();
1573 
1574  icub->upperTorso->clearContactList();
1575  icub->upperTorso->leftSensor->addContacts(contactsPerBp[LEFT_ARM].toDynContactList());
1576  icub->upperTorso->rightSensor->addContacts(contactsPerBp[RIGHT_ARM].toDynContactList());
1577  skinContacts = contactsPerBp[LEFT_ARM];
1578  skinContacts.insert(skinContacts.end(), contactsPerBp[RIGHT_ARM].begin(), contactsPerBp[RIGHT_ARM].end());
1579  }
1580  else if(Time::now()-skinContactsTimestamp>SKIN_EVENTS_TIMEOUT && skinContactsTimestamp!=0.0)
1581  {
1582  // if time is up, remove all the contacts
1583  icub->upperTorso->clearContactList();
1584  skinContacts.clear();
1585  add_legs_once = true;
1586  }
1587 }
1588 
1590 {
1591  Vector monitorData(0);
1592  monitorData = current_status.inertial_w0 * CTRL_RAD2DEG; // w inertial sensor
1593  Vector temp = current_status.inertial_dw0 * CTRL_RAD2DEG; // dw inertial sensor
1594  for(int i=0;i<3;i++) monitorData.push_back(temp(i));
1595  temp = icub->upperTorso->getAngVel() * CTRL_RAD2DEG; // w upper node
1596  for(int i=0;i<3;i++) monitorData.push_back(temp(i));
1597  temp = icub->upperTorso->getAngAcc() * CTRL_RAD2DEG; // dw upper node
1598  for(int i=0;i<3;i++) monitorData.push_back(temp(i));
1599  monitorData.push_back(norm(icub->upperTorso->getLinAcc())); // lin acc norm upper node
1600  monitorData.push_back(norm(icub->upperTorso->getTorsoForce())); // force norm upper node
1601  monitorData.push_back(norm(icub->upperTorso->getTorsoMoment()));// moment norm upper node
1602  for(size_t i=0;i<3;i++){ // w torso
1603  temp = icub->lowerTorso->up->getAngVel(i) * CTRL_RAD2DEG;
1604  for(double j : temp) monitorData.push_back(j);
1605  }
1606  for(size_t i=0;i<3;i++){ // dw torso
1607  temp = icub->lowerTorso->up->getAngAcc(i) * CTRL_RAD2DEG;
1608  for(double j : temp) monitorData.push_back(j);
1609  }
1610  //for(int j=0;j<TOTorques.size();j++) // torso torques
1611  // monitorData.push_back(TOTorques[j]);
1612  for(size_t i=0;i<3;i++) // norm of COM ddp torso
1613  monitorData.push_back(norm(icub->lowerTorso->up->getLinAccCOM(i)));
1614  for(size_t i=0;i<3;i++) // norm of forces torso
1615  monitorData.push_back(norm(icub->lowerTorso->up->getForce(i)));
1616  for(size_t i=0;i<3;i++){ // moments torso
1617  temp = icub->lowerTorso->up->getMoment(i);
1618  for(size_t j=0;j<temp.size();j++)
1619  monitorData.push_back(temp(j));
1620  }
1621  for(int i=0;i<3;i++) // norm of moments head
1622  monitorData.push_back(norm(icub->upperTorso->up->getMoment(i)));
1623  port_monitor->prepare() = monitorData;
1624  port_monitor->write();
1625 }
1626 
1628 {
1629  Vector dump(0);
1630  size_t i;
1631  if(dumpvel_enabled)
1632  {
1633  for(i=0; i< current_status.all_q_up.size(); i++)
1634  dump.push_back(current_status.all_q_up[i]);
1635  for(i=0; i< current_status.all_dq_up.size(); i++)
1636  dump.push_back(current_status.all_dq_up[i]);
1637  for(i=0; i< current_status.all_d2q_up.size(); i++)
1638  dump.push_back(current_status.all_d2q_up[i]);
1639  for(i=0; i< current_status.all_q_low.size(); i++)
1640  dump.push_back(current_status.all_q_low[i]);
1641  for(i=0; i< current_status.all_dq_low.size(); i++)
1642  dump.push_back(current_status.all_dq_low[i]);
1643  for(i=0; i< current_status.all_d2q_low.size(); i++)
1644  dump.push_back(current_status.all_d2q_low[i]);
1645 
1646  port_dumpvel->prepare() = dump;
1647  port_dumpvel->write();
1648  }
1649 }
1650 
#define CTRL_RAD2DEG
Definition: XSensMTx.cpp:25
#define CTRL_DEG2RAD
Definition: XSensMTx.cpp:26
#define M_PI
Definition: XSensMTx.cpp:24
Vector get_dq_torso()
Vector ft_leg_left
Vector ft_arm_left
Vector inertial_w0
Vector get_q_larm()
Vector ft_foot_right
Vector all_q_low
Vector get_dq_larm()
bool checkIcubNotMoving()
Vector get_dq_rarm()
Vector ft_foot_left
Vector inertial_d2p0
Vector all_d2q_low
Vector get_d2q_rleg()
Vector all_dq_low
Vector all_q_up
Vector ft_arm_right
Vector all_d2q_up
Vector get_q_torso()
bool iCub_not_moving
Vector get_dq_rleg()
Vector get_q_lleg()
Vector get_d2q_rarm()
Vector all_dq_up
Vector get_q_rleg()
Vector get_d2q_head()
Vector get_q_rarm()
Vector inertial_dw0
Vector get_d2q_lleg()
Vector get_d2q_larm()
Vector ft_leg_right
void dump(FILE *f)
Vector get_dq_lleg()
double timestamp
Vector get_d2q_torso()
Vector get_q_head()
Vector get_dq_head()
Adaptive window linear fitting to estimate the first derivative.
Basic element for adaptive polynomial fitting.
Adaptive window quadratic fitting to estimate the second derivative.
A class for connecting UpperTorso and LowerTorso of the iCub, then getting the WholeBody in the dynam...
Definition: iDynBody.h:1472
bool EXPERIMENTAL_getCOMvelocity(iCub::skinDynLib::BodyPart which_part, yarp::sig::Vector &vel, yarp::sig::Vector &dq)
Retrieves a 3x1 vector containing the velocity of the robot COM.
Definition: iDynBody.cpp:3016
iCubUpperTorso * upperTorso
pointer to UpperTorso = head + right arm + left arm
Definition: iDynBody.h:1482
bool getAllPositions(yarp::sig::Vector &pos)
Retrieves a vector containing the anglular position of all the iCub joints, ordered in this way: left...
Definition: iDynBody.cpp:2607
bool EXPERIMENTAL_computeCOMjacobian()
Performs the computation of the center of mass jacobian of the whole iCub.
Definition: iDynBody.cpp:2675
bool computeCOM()
Performs the computation of the center of mass (COM) of the whole iCub.
Definition: iDynBody.cpp:2473
iCubLowerTorso * lowerTorso
pointer to LowerTorso = torso + right leg + left leg
Definition: iDynBody.h:1484
bool getCOM(iCub::skinDynLib::BodyPart which_part, yarp::sig::Vector &COM, double &mass)
Retrieves the result of the last COM computation.
Definition: iDynBody.cpp:2511
void attachLowerTorso(const yarp::sig::Vector &FM_right_leg, const yarp::sig::Vector &FM_left_leg)
Connect upper and lower torso: this procedure handles the exchange of kinematic and wrench variables ...
Definition: iDynBody.cpp:2421
bool EXPERIMENTAL_getCOMjacobian(iCub::skinDynLib::BodyPart which_part, yarp::sig::Matrix &jac)
Retrieves the result of the last COM jacobian computation.
Definition: iDynBody.cpp:2746
yarp::sig::Vector getAngAcc(const unsigned int i) const
Returns the i-th link angular acceleration.
Definition: iDyn.cpp:744
const yarp::sig::Vector & getLinAccCOM(const unsigned int i) const
Returns the i-th link linear acceleration of the COM.
Definition: iDyn.cpp:722
const yarp::sig::Vector & getForce(const unsigned int iLink) const
Returns the i-th link force.
Definition: iDyn.cpp:766
const yarp::sig::Vector & getMoment(const unsigned int iLink) const
Returns the i-th link moment.
Definition: iDyn.cpp:777
yarp::sig::Vector getLinAcc(const unsigned int i) const
Returns the i-th link linear acceleration.
Definition: iDyn.cpp:711
yarp::sig::Vector getAngVel(const unsigned int i) const
Returns the i-th link angular velocity.
Definition: iDyn.cpp:733
const iCub::skinDynLib::dynContactList & getContactList() const
bool addContacts(const iCub::skinDynLib::dynContactList &contacts)
Add the specified elements to the contact list.
Definition: iDynContact.cpp:69
yarp::sig::Vector getForceMomentEndEff() const
Returns the end effector force-moment as a single (6x1) vector.
yarp::sig::Vector getAngAcc() const
Return the node angular acceleration.
Definition: iDynBody.cpp:823
bool solveKinematics(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0)
Main function to manage the exchange of kinematic information among the limbs attached to the node.
yarp::sig::Vector getAngVel() const
Return the node angular velocity.
Definition: iDynBody.cpp:821
yarp::sig::Vector getLinAcc() const
Return the node linear acceleration.
Definition: iDynBody.cpp:825
virtual bool solveWrench()
Main function to manage the exchange of wrench information among the limbs attached to the node.
Definition: iDynBody.cpp:1352
yarp::sig::Vector getTorques(const std::string &limbType)
Return the chosen limb torques, as a Nx1 vector.
Definition: iDynBody.cpp:1890
iDyn::iDynContactSolver * leftSensor
Build the node.
Definition: iDynBody.h:1073
yarp::sig::Matrix estimateSensorsWrench(const yarp::sig::Matrix &FM, bool afterAttach=false)
Redefinition from iDynSensorNode.
Definition: iDynBody.h:1383
iDyn::iDynLimb * left
left limb
Definition: iDynBody.h:1079
yarp::sig::Vector setDAng(const std::string &limbType, const yarp::sig::Vector &_dq)
Set joints angle velocity in the chosen limb.
yarp::sig::Vector getTorsoAngAcc() const
Return the torso angular acceleration.
Definition: iDynBody.cpp:1908
bool setSensorMeasurement(const yarp::sig::Vector &FM_right, const yarp::sig::Vector &FM_left)
Set the FT sensor measurements on the sensor in right and left limb.
yarp::sig::Vector setD2Ang(const std::string &limbType, const yarp::sig::Vector &_ddq)
Set joints angle acceleration in the chosen limb.
yarp::sig::Vector getTorsoLinAcc() const
Return the torso linear acceleration.
Definition: iDynBody.cpp:1910
yarp::sig::Matrix getHLeft()
Return HLeft, i.e.
Definition: iDynBody.h:1151
yarp::sig::Vector getTorsoForce() const
Return the torso force.
Definition: iDynBody.cpp:1902
iDyn::iDynLimb * right
right limb
Definition: iDynBody.h:1081
iDyn::iDynContactSolver * rightSensor
right FT sensor and solver
Definition: iDynBody.h:1075
iDyn::iDynLimb * up
central-up limb
Definition: iDynBody.h:1083
yarp::sig::Vector setAng(const std::string &limbType, const yarp::sig::Vector &_q)
Set joints angle position in the chosen limb.
yarp::sig::Matrix getHUp()
Return HUp, i.e.
Definition: iDynBody.h:1159
yarp::sig::Vector getTorsoMoment() const
Return the torso moment.
Definition: iDynBody.cpp:1904
bool setInertialMeasure(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0)
Set the inertial sensor measurements on the central-up limb.
Definition: iDynBody.cpp:1753
yarp::sig::Vector getTorsoAngVel() const
Return the torso angular velocity.
Definition: iDynBody.cpp:1906
yarp::sig::Matrix getHRight()
Return HRight, i.e.
Definition: iDynBody.h:1155
yarp::sig::Matrix getH(const unsigned int i, const bool allLink=false)
Returns the rigid roto-translation matrix from the root reference frame to the ith frame in Denavit-H...
Definition: iKinFwd.cpp:732
Class representing a list of external contacts.
virtual void setBodyPart(BodyPart _bodyPart)
Set the body part of this contact.
Definition: dynContact.cpp:161
virtual void setLinkNumber(unsigned int _linkNum)
Set the contact link number (0 is the first link)
Definition: dynContact.cpp:157
virtual bool setForceMoment(const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu)
Set the contact force and moment.
Definition: dynContact.cpp:139
Class representing a list of external contacts acting on the iCub' skin.
virtual std::map< BodyPart, skinContactList > splitPerBodyPart()
Split the list in N lists dividing the contacts per body part.
Class representing an external contact acting on the iCub' skin.
Definition: skinContact.h:50
void setActiveTaxels(unsigned int _activeTaxels)
Set the number of active taxels.
Definition: skinContact.cpp:83
bool setPressure(double _pressure)
Set the average contact pressure.
Definition: skinContact.cpp:76
void threadRelease() override
void broadcastData(T &_values, BufferedPort< T > *_port)
bool getLowerEncodersSpeedAndAcceleration()
void closePort(Contactable *_port)
void run() override
bool readAndUpdate(bool waitMeasure=false, bool _init=false)
void calibrateOffset(calib_enum calib_code=CALIB_ALL)
bool threadInit() override
void writeTorque(Vector _values, int _address, BufferedPort< Bottle > *_port)
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)
bool getUpperEncodersSpeedAndAcceleration()
zeros(2, 2) eye(2
@ _init
Definition: dc1394thread.h:14
@ STATUS_OK
Definition: gravityThread.h:41
@ STATUS_DISCONNECTED
Definition: gravityThread.h:41
double norm(const yarp::sig::Matrix &M, int col)
Returns the norm of the vector given in the form: matrix(:,col).
BodyPart getBodyPart(SkinPart s)
Get the body part associated to the specified skin part.
Definition: common.cpp:26
@ DYNAMIC
Definition: iDynInv.h:64
@ UPPER_BODY_PARTS
Definition: common.h:47
@ LOWER_BODY_PARTS
Definition: common.h:47
double lpf_ord1_3hz(double input, int j)
constexpr int8_t MAX_FILTER_ORDER
constexpr int8_t MAX_JN
calib_enum
constexpr float_t SKIN_EVENTS_TIMEOUT
fprintf(fid,'\n')