iCub-main
gravityThread.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 "gravityThread.h"
20 
21 #include <yarp/os/BufferedPort.h>
22 #include <yarp/os/Time.h>
23 #include <yarp/os/Network.h>
24 #include <yarp/os/PeriodicThread.h>
25 #include <yarp/os/Stamp.h>
26 #include <yarp/sig/Vector.h>
27 #include <yarp/dev/PolyDriver.h>
28 #include <yarp/dev/ControlBoardInterfaces.h>
29 #include <iCub/ctrl/math.h>
31 #include <iCub/iDyn/iDyn.h>
32 #include <iCub/iDyn/iDynBody.h>
33 #include <iCub/skinDynLib/common.h>
34 #include <yarp/os/Log.h>
35 #include <yarp/os/LogStream.h>
36 
37 using namespace yarp::os;
38 using namespace yarp::sig;
39 using namespace yarp::math;
40 using namespace yarp::dev;
41 using namespace iCub::ctrl;
42 using namespace iCub::iDyn;
43 using namespace iCub::skinDynLib;
44 using namespace std;
45 
46 Vector gravityCompensatorThread::evalVelUp(const Vector &x)
47 {
48  AWPolyElement el;
49  el.data=x;
50  el.time=Time::now();
51 
52  return linEstUp->estimate(el);
53 }
54 
55 Vector gravityCompensatorThread::evalVelLow(const Vector &x)
56 {
57  AWPolyElement el;
58  el.data=x;
59  el.time=Time::now();
60 
61  return linEstLow->estimate(el);
62 }
63 
64 Vector gravityCompensatorThread::evalAccUp(const Vector &x)
65 {
66  AWPolyElement el;
67  el.data=x;
68  el.time=Time::now();
69 
70  return quadEstUp->estimate(el);
71 }
72 
73 Vector gravityCompensatorThread::evalAccLow(const Vector &x)
74 {
75  AWPolyElement el;
76  el.data=x;
77  el.time=Time::now();
78 
79  return quadEstLow->estimate(el);
80 }
81 
82 void gravityCompensatorThread::init_upper()
83 {
84  //---------------------PARTS-------------------------//
85  // Left_arm variables
86  allJnt = 0;
87  int jnt=16;
88  encoders_arm_left.resize(jnt,0.0);
89  F_LArm.resize(6,0.0);
90  F_iDyn_LArm.resize(6,0.0);
91  Offset_LArm.resize(6,0.0);
92  q_larm.resize(7,0.0);
93  dq_larm.resize(7,0.0);
94  d2q_larm.resize(7,0.0);
95  allJnt+=jnt;
96 
97  // Right_arm variables
98  jnt = 16;
99  encoders_arm_right.resize(jnt,0.0);
100  F_RArm.resize(6,0.0);
101  F_iDyn_RArm.resize(6,0.0);
102  Offset_RArm.resize(6,0.0);
103  q_rarm.resize(7,0.0);
104  dq_rarm.resize(7,0.0);
105  d2q_rarm.resize(7,0.0);
106  allJnt+=jnt;
107 
108  // Head variables
109  jnt = 6;
110  encoders_head.resize(jnt,0.0);
111  q_head.resize(3,0.0);
112  dq_head.resize(3,0.0);
113  d2q_head.resize(3,0.0);
114  allJnt+=jnt;
115 
116  all_q_up.resize(allJnt,0.0);
117  all_dq_up.resize(allJnt,0.0);
118  all_d2q_up.resize(allJnt,0.0);
119  ampli_LA.resize(7);ampli_LA=1.0;
120  ampli_RA.resize(7);ampli_RA=1.0;
121 }
122 
123 void gravityCompensatorThread::init_lower()
124 {
125  //---------------------PARTS-------------------------//
126  // Left_leg variables
127  allJnt = 0;
128  int jnt = 6;
129  encoders_leg_left.resize(jnt,0.0);
130  q_lleg.resize(6,0.0);
131  dq_lleg.resize(6,0.0);
132  d2q_lleg.resize(6,0.0);
133  allJnt+=jnt;
134 
135  // Right_leg variables
136  jnt = 6;
137  encoders_leg_right.resize(jnt,0.0);
138  q_rleg.resize(6,0.0);
139  dq_rleg.resize(6,0.0);
140  d2q_rleg.resize(6,0.0);
141  allJnt+=jnt;
142 
143  // Torso variables
144  jnt = 3;
145  encoders_torso.resize(jnt,0.0);
146  q_torso.resize(3,0.0);
147  dq_torso.resize(3,0.0);
148  d2q_torso.resize(3,0.0);
149  allJnt+=jnt;
150 
151  all_q_low.resize(allJnt,0.0);
152  all_dq_low.resize(allJnt,0.0);
153  all_d2q_low.resize(allJnt,0.0);
154  ampli_TO.resize(3);ampli_TO=1.0;
155  ampli_LL.resize(6);ampli_LL=1.0;
156  ampli_RL.resize(6);ampli_RL=1.0;
157 
158 }
159 
160 void gravityCompensatorThread::setLowerMeasure()
161 {
162  icub->lowerTorso->setAng("torso",CTRL_DEG2RAD * q_torso);
163  icub->lowerTorso->setDAng("torso",CTRL_DEG2RAD * dq_torso);
164  icub->lowerTorso->setD2Ang("torso",CTRL_DEG2RAD * d2q_torso);
165 
166  icub->lowerTorso->setAng("left_leg",CTRL_DEG2RAD * q_lleg);
167  icub->lowerTorso->setDAng("left_leg",CTRL_DEG2RAD * dq_lleg);
168  icub->lowerTorso->setD2Ang("left_leg",CTRL_DEG2RAD * d2q_lleg);
169 
170  icub->lowerTorso->setAng("right_leg",CTRL_DEG2RAD * q_rleg);
171  icub->lowerTorso->setDAng("right_leg",CTRL_DEG2RAD * dq_rleg);
172  icub->lowerTorso->setD2Ang("right_leg",CTRL_DEG2RAD * d2q_rleg);
173 }
174 
175 void gravityCompensatorThread::setUpperMeasure()
176 {
177  icub->upperTorso->setAng("head",CTRL_DEG2RAD * q_head);
178  icub->upperTorso->setAng("left_arm",CTRL_DEG2RAD * q_larm);
179  icub->upperTorso->setAng("right_arm",CTRL_DEG2RAD * q_rarm);
180  icub->upperTorso->setDAng("head",CTRL_DEG2RAD * dq_head);
181  icub->upperTorso->setDAng("left_arm",CTRL_DEG2RAD * dq_larm);
182  icub->upperTorso->setDAng("right_arm",CTRL_DEG2RAD * dq_rarm);
183  icub->upperTorso->setD2Ang("head",CTRL_DEG2RAD * d2q_head);
184  icub->upperTorso->setD2Ang("left_arm",CTRL_DEG2RAD * d2q_larm);
185  icub->upperTorso->setD2Ang("right_arm",CTRL_DEG2RAD * d2q_rarm);
186  icub->upperTorso->setInertialMeasure(w0,dw0,d2p0);
187 }
188 
189 gravityCompensatorThread::gravityCompensatorThread(string _wholeBodyName, int _rate, PolyDriver *_ddLA,
190  PolyDriver *_ddRA, PolyDriver *_ddH, PolyDriver *_ddLL,
191  PolyDriver *_ddRL, PolyDriver *_ddT, version_tag icub_type,
192  bool _inertial_enabled) : PeriodicThread((double)_rate/1000.0), ddLA(_ddLA),
193  ddRA(_ddRA), ddLL(_ddLL), ddRL(_ddRL),
194  ddH(_ddH), ddT(_ddT), gravity_torques_LA(7,0.0),
195  gravity_torques_RA(7,0.0), exec_torques_LA(7,0.0),
196  exec_torques_RA(7,0.0), externalcmd_torques_LA(7,0.0),
197  externalcmd_torques_RA(7,0.0), gravity_torques_TO(3,0.0),
198  gravity_torques_LL(6,0.0), gravity_torques_RL(6,0.0),
199  exec_torques_TO(3,0.0), exec_torques_LL(6,0.0),
200  exec_torques_RL(6,0.0), externalcmd_torques_TO(3,0.0),
201  externalcmd_torques_LL(6,0.0), externalcmd_torques_RL(6,0.0)
202 {
205  wholeBodyName = std::move(_wholeBodyName);
206 
207  //--------------INTERFACE INITIALIZATION-------------//
208  iencs_arm_left = nullptr;
209  iencs_arm_right = nullptr;
210  iencs_head = nullptr;
211  iCtrlMode_arm_left = nullptr;
212  iCtrlMode_arm_right = nullptr;
213  iCtrlMode_torso = nullptr;
214  iIntMode_arm_left = nullptr;
215  iIntMode_arm_right = nullptr;
216  iIntMode_torso = nullptr;
217  iImp_torso = nullptr;
218  iTqs_torso = nullptr;
219  iImp_arm_left = nullptr;
220  iTqs_arm_left = nullptr;
221  iImp_arm_right = nullptr;
222  iTqs_arm_right = nullptr;
223  iencs_leg_left = nullptr;
224  iencs_leg_right = nullptr;
225  iencs_torso = nullptr;
226  iCtrlMode_leg_left = nullptr;
227  iCtrlMode_leg_right = nullptr;
228  iIntMode_leg_left = nullptr;
229  iIntMode_leg_right = nullptr;
230  iImp_leg_left = nullptr;
231  iTqs_leg_left = nullptr;
232  iImp_leg_right = nullptr;
233  iTqs_leg_right = nullptr;
234  isCalibrated = false;
235  inertial_enabled=_inertial_enabled;
236  icub = new iCubWholeBody (icub_type,DYNAMIC, VERBOSE);
237  thread_status=STATUS_DISCONNECTED;
238  port_inertial = nullptr;
239  la_additional_offset = nullptr;
240  ra_additional_offset = nullptr;
241  ll_additional_offset = nullptr;
242  rl_additional_offset = nullptr;
243  to_additional_offset = nullptr;
244  left_arm_exec_torques = nullptr;
245  right_arm_exec_torques = nullptr;
246  left_leg_exec_torques = nullptr;
247  right_leg_exec_torques = nullptr;
248  torso_exec_torques = nullptr;
249  left_arm_gravity_torques = nullptr;
250  right_arm_gravity_torques = nullptr;
251  left_leg_gravity_torques = nullptr;
252  right_leg_gravity_torques = nullptr;
253  torso_gravity_torques = nullptr;
254 
255 }
256 
258 {
259  dq_head = 0.0;
260  d2q_head = 0.0;
261  dq_larm = 0.0;
262  d2q_larm = 0.0;
263  dq_rarm = 0.0;
264  d2q_rarm = 0.0;
265 
266  dq_rleg=0.0;
267  d2q_rleg=0.0;
268  dq_lleg=0.0;
269  d2q_lleg=0.0;
270  dq_torso=0.0;
271  d2q_torso=0.0;
272 }
273 
275 {
276  int sz = 0;
277  bool b = true;
278 
279  /*
280  //offset currently not implemented
281  Vector *offset_input = additional_offset->read(false);
282  if(offset_input!=0)
283  {
284  sz = offset_input->length();
285  if(sz!=ctrlJnt)
286  {
287  yWarning"warning...controlled joint < of offsets size!!!");
288  }
289  else
290  {
291  Vector o=*offset_input;
292  torque_offset = 0.0;
293  for(int i=0;i<ctrlJnt;i++)
294  torque_offset[i] = o[i];
295  }
296  }
297  */
298 
299  if (inertial_enabled)
300  {
301  inertial = port_inertial->read(waitMeasure);
302  if(inertial!=nullptr)
303  {
304  sz = inertial->length();
305  inertial_measurements.resize(sz) ;
306  inertial_measurements= *inertial;
307  d2p0[0] = inertial_measurements[0];
308  d2p0[1] = inertial_measurements[1];
309  d2p0[2] = inertial_measurements[2];
310  w0 [0] = 0;
311  w0 [1] = 0;
312  w0 [2] = 0;
313  dw0 [0] = 0;
314  dw0 [1] = 0;
315  dw0 [2] = 0;
316  }
317  else
318  {
319  b = false;
320  }
321  }
322  else
323  {
324  d2p0[0] = 0;
325  d2p0[1] = 0;
326  d2p0[2] = 9.81;
327  w0 [0] = 0;
328  w0 [1] = 0;
329  w0 [2] = 0;
330  dw0 [0] = 0;
331  dw0 [1] = 0;
332  dw0 [2] = 0;
333  }
334 
336  setUpperMeasure();
338  setLowerMeasure();
339 
340  return b;
341 }
342 
344 {
345  bool b = true;
346  if (iencs_leg_left)
347  {b &= iencs_leg_left->getEncoders(encoders_leg_left.data());}
348  else
349  {encoders_leg_left.zero();}
350 
351  if (iencs_leg_right)
352  {b &= iencs_leg_right->getEncoders(encoders_leg_right.data());}
353  else
354  {encoders_leg_right.zero();}
355 
356  if (iencs_torso)
357  {b &= iencs_torso->getEncoders(encoders_torso.data());}
358  else
359  {encoders_torso.zero();}
360 
361  for (size_t i=0;i<q_torso.length();i++)
362  {
363  q_torso(i) = encoders_torso(2-i);
364  all_q_low(i) = q_torso(i);
365  }
366  for (size_t i=0;i<q_lleg.length();i++)
367  {
368  q_lleg(i) = encoders_leg_left(i);
369  all_q_low(q_torso.length()+i) = q_lleg(i);
370  }
371  for (size_t i=0;i<q_rleg.length();i++)
372  {
373  q_rleg(i) = encoders_leg_right(i);
374  all_q_low(q_torso.length()+q_lleg.length()+i) = q_rleg(i);
375  }
376 
378 
379  /*
380  all_dq_low = evalVelLow(all_q_low);
381  all_d2q_low = evalAccLow(all_q_low);
382 
383  for (int i=0;i<q_torso.length();i++)
384  {
385  dq_torso(i) = all_dq_low(i);
386  d2q_torso(i) = all_d2q_low(i);
387  }
388  for (int i=0;i<q_lleg.length();i++)
389  {
390  dq_lleg(i) = all_dq_low(i+q_torso.length());
391  d2q_lleg(i) = all_d2q_low(i+q_torso.length());
392  }
393  for (int i=0;i<q_rleg.length();i++)
394  {
395  dq_rleg(i) = all_dq_low(i+q_torso.length()+q_lleg.length());
396  d2q_rleg(i) = all_d2q_low(i+q_torso.length()+q_lleg.length());
397  }*/
398 
399  return b;
400 }
401 
402 
404 {
405  bool b = true;
406  if (iencs_arm_left)
407  {b &= iencs_arm_left->getEncoders(encoders_arm_left.data());}
408  else
409  {encoders_arm_left.zero();}
410 
411  if (iencs_arm_right)
412  {b &= iencs_arm_right->getEncoders(encoders_arm_right.data());}
413  else
414  {encoders_arm_right.zero();}
415 
416  if (iencs_head)
417  {b &= iencs_head->getEncoders(encoders_head.data());}
418  else
419  {encoders_head.zero();}
420 
421  for (size_t i=0;i<q_head.length();i++)
422  {
423  q_head(i) = encoders_head(i);
424  all_q_up(i) = q_head(i);
425  }
426  for (size_t i=0;i<q_larm.length();i++)
427  {
428  q_larm(i) = encoders_arm_left(i);
429  all_q_up(q_head.length()+i) = q_larm(i);
430  }
431  for (size_t i=0;i<q_rarm.length();i++)
432  {
433  q_rarm(i) = encoders_arm_right(i);
434  all_q_up(q_head.length()+q_larm.length()+i) = q_rarm(i);
435  }
436 
438 
439  /*
440  all_dq_up = evalVelUp(all_q_up);
441  all_d2q_up = evalAccUp(all_q_up);
442 
443  for (int i=0;i<q_head.length();i++)
444  {
445  dq_head(i) = all_dq_up(i);
446  d2q_head(i) = all_d2q_up(i);
447  }
448  for (int i=0;i<q_larm.length();i++)
449  {
450  dq_larm(i) = all_dq_up(i+q_head.length());
451  d2q_larm(i) = all_d2q_up(i+q_head.length());
452  }
453  for (int i=0;i<q_rarm.length();i++)
454  {
455  dq_rarm(i) = all_dq_up(i+q_head.length()+q_larm.length());
456  d2q_rarm(i) = all_d2q_up(i+q_head.length()+q_larm.length());
457  }*/
458 
459  return b;
460 }
461 
463 {
464  //---------------------PORTS-------------------------//
465  port_inertial=new BufferedPort<Vector>;
466  if (!port_inertial->open("/gravityCompensator/inertial:i")) {yError("Another gravityCompensator module is running? quitting"); return false;}
467 
468  la_additional_offset=new BufferedPort<Vector>;
469  if (!la_additional_offset->open("/gravityCompensator/left_arm/ctrl_offset:i")) {yError("Another gravityCompensator module is running? quitting"); return false;}
470  ra_additional_offset=new BufferedPort<Vector>;
471  if (!ra_additional_offset->open("/gravityCompensator/right_arm/ctrl_offset:i")) {yError("Another gravityCompensator module is running? quitting"); return false;}
472  ll_additional_offset=new BufferedPort<Vector>;
473  if (!ll_additional_offset->open("/gravityCompensator/left_leg/ctrl_offset:i")) {yError("Another gravityCompensator module is running? quitting"); return false;}
474  rl_additional_offset=new BufferedPort<Vector>;
475  if (!rl_additional_offset->open("/gravityCompensator/right_leg/ctrl_offset:i")) {yError("Another gravityCompensator module is running? quitting"); return false;}
476  to_additional_offset=new BufferedPort<Vector>;
477  if (!to_additional_offset->open("/gravityCompensator/torso/ctrl_offset:i")) {yError("Another gravityCompensator module is running? quitting"); return false;}
478 
479  left_arm_exec_torques = new BufferedPort<Vector>;
480  if (!left_arm_exec_torques->open("/gravityCompensator/left_arm/exec_torques:o")) {yError("Another gravityCompensator module is running? quitting"); return false;}
481  right_arm_exec_torques = new BufferedPort<Vector>;
482  if (!right_arm_exec_torques->open("/gravityCompensator/right_arm/exec_torques:o")) {yError("Another gravityCompensator module is running? quitting"); return false;}
483  left_leg_exec_torques = new BufferedPort<Vector>;
484  if (!left_leg_exec_torques->open("/gravityCompensator/left_leg/exec_torques:o")) {yError("Another gravityCompensator module is running? quitting"); return false;}
485  right_leg_exec_torques = new BufferedPort<Vector>;
486  if (!right_leg_exec_torques->open("/gravityCompensator/right_leg/exec_torques:o")) {yError("Another gravityCompensator module is running? quitting"); return false;}
487  torso_exec_torques = new BufferedPort<Vector>;
488  if (!torso_exec_torques->open("/gravityCompensator/torso/exec_torques:o")) {yError("Another gravityCompensator module is running? quitting"); return false;}
489 
490  left_arm_gravity_torques = new BufferedPort<Vector>;
491  if (!left_arm_gravity_torques->open("/gravityCompensator/left_arm/gravity_torques:o")) {yError("Another gravityCompensator module is running? quitting"); return false;}
492  right_arm_gravity_torques = new BufferedPort<Vector>;
493  if (!right_arm_gravity_torques->open("/gravityCompensator/right_arm/gravity_torques:o")) {yError("Another gravityCompensator module is running? quitting"); return false;}
494  left_leg_gravity_torques = new BufferedPort<Vector>;
495  if (!left_leg_gravity_torques->open("/gravityCompensator/left_leg/gravity_torques:o")) {yError("Another gravityCompensator module is running? quitting"); return false;}
496  right_leg_gravity_torques = new BufferedPort<Vector>;
497  if (!right_leg_gravity_torques->open("/gravityCompensator/right_leg/gravity_torques:o")) {yError("Another gravityCompensator module is running? quitting"); return false;}
498  torso_gravity_torques = new BufferedPort<Vector>;
499  if (!torso_gravity_torques->open("/gravityCompensator/torso/gravity_torques:o")) {yError("Another gravityCompensator module is running? quitting"); return false;}
500 
501  //---------------------DEVICES--------------------------//
502  if (ddLA) ddLA->view(iencs_arm_left);
503  if (ddRA) ddRA->view(iencs_arm_right);
504  if (ddH) ddH->view(iencs_head);
505  if (ddLL) ddLL->view(iencs_leg_left);
506  if (ddRL) ddRL->view(iencs_leg_right);
507  if (ddT) ddT->view(iencs_torso);
508 
509  if (ddLA) ddLA->view(iCtrlMode_arm_left);
510  if (ddRA) ddRA->view(iCtrlMode_arm_right);
511  if (ddLA) ddLA->view(iIntMode_arm_left);
512  if (ddRA) ddRA->view(iIntMode_arm_right);
513  if (ddLA) ddLA->view(iImp_arm_left);
514  if (ddLA) ddLA->view(iTqs_arm_left);
515  if (ddRA) ddRA->view(iImp_arm_right);
516  if (ddRA) ddRA->view(iTqs_arm_right);
517 
518  if (ddT) ddT->view(iCtrlMode_torso);
519  if (ddT) ddT->view(iIntMode_torso);
520  if (ddT) ddT->view(iImp_torso);
521  if (ddT) ddT->view(iTqs_torso);
522 
523  if (ddLL) ddLL->view(iCtrlMode_leg_left);
524  if (ddRL) ddRL->view(iCtrlMode_leg_right);
525  if (ddLL) ddLL->view(iIntMode_leg_left);
526  if (ddRL) ddRL->view(iIntMode_leg_right);
527  if (ddLL) ddLL->view(iImp_leg_left);
528  if (ddLL) ddLL->view(iTqs_leg_left);
529  if (ddRL) ddRL->view(iImp_leg_right);
530  if (ddRL) ddRL->view(iTqs_leg_right);
531 
532  linEstUp =new AWLinEstimator(16,1.0);
533  quadEstUp=new AWQuadEstimator(25,1.0);
534  linEstLow =new AWLinEstimator(16,1.0);
535  quadEstLow=new AWQuadEstimator(25,1.0);
536 
537  //-----------parts INIT VARIABLES----------------//
538  init_upper();
539  init_lower();
540 
541  //-----------CARTESIAN INIT VARIABLES----------------//
542  left_arm_ctrlJnt = 5;
543  right_arm_ctrlJnt = 5;
544  left_leg_ctrlJnt = 4;
545  right_leg_ctrlJnt = 4;
546  torso_ctrlJnt = 3;
547  w0.resize(3,0.0);
548  dw0.resize(3,0.0);
549  d2p0.resize(3,0.0);
550  Fend.resize(3,0.0);
551  Muend.resize(3,0.0);
552  F_ext_up.resize(6,3);
553  F_ext_up = 0.0;
554  F_ext_low.resize(6,3);
555  F_ext_low = 0.0;
556  inertial_measurements.resize(12);
557  inertial_measurements.zero();
558 
559  int ctrl_mode = 0;
560 
561  switch(gravity_mode)
562  {
563  case GRAVITY_COMPENSATION_OFF: yInfo("GRAVITY_COMPENSATION_OFF \n"); break;
564  case GRAVITY_COMPENSATION_ON: yInfo("GRAVITY_COMPENSATION_ON \n"); break;
565  default:
566  case VOCAB_CM_UNKNOWN: yError("UNKNOWN \n"); break;
567  }
568 
569  thread_status = STATUS_OK;
570 
571  return true;
572 }
573 
574 
575 void gravityCompensatorThread::feedFwdGravityControl(int part_ctrlJnt, const string& s_part, IControlMode *iCtrlMode, ITorqueControl *iTqs, IImpedanceControl *iImp, IInteractionMode *iIntMode, const Vector &command, bool releasing)
576 {
577  //check if interfaces are still up (icubinterface running)
578  if (iCtrlMode == nullptr)
579  {yError("ControlMode interface already closed, unable to reset compensation offset.\n"); return;}
580  if (iTqs == nullptr)
581  {yError("TorqueControl interface already closed, unable to reset compensation offset.\n"); return;}
582  if (iIntMode == nullptr)
583  {yError("InteractionMode interface already closed, unable to reset compensation offset.\n"); return;}
584  if (iImp == nullptr)
585  {yError("Impedance interface already closed, unable to reset compensation offset.\n"); return;}
586 
587  //set to zero all the offsets if the module is closing
588  if(releasing)
589  {
590  for(int i=0;i<part_ctrlJnt;i++)
591  {
592  iImp->setImpedanceOffset(i,0.0);
593  iTqs->setRefTorque(i,0.0);
594  }
595  return;
596  }
597 
598  //set the appropriate feedforward term (normal operation)
599  for(int i=0;i<part_ctrlJnt;i++)
600  {
601  int ctrl_mode=0;
602  yarp::dev::InteractionModeEnum int_mode;
603  iCtrlMode->getControlMode(i,&ctrl_mode);
604  iIntMode->getInteractionMode(i,&int_mode);
605  switch(ctrl_mode)
606  {
607  //for all this control modes do nothing
608  case VOCAB_CM_CURRENT:
609  case VOCAB_CM_PWM:
610  case VOCAB_CM_IDLE:
611  case VOCAB_CM_UNKNOWN:
612  case VOCAB_CM_HW_FAULT:
613  break;
614 
615  case VOCAB_CM_TORQUE:
616  iTqs->setRefTorque(i,command[i]);
617  break;
618 
619  case VOCAB_CM_POSITION:
620  case VOCAB_CM_POSITION_DIRECT:
621  case VOCAB_CM_MIXED:
622  case VOCAB_CM_VELOCITY:
623  if (int_mode == VOCAB_IM_COMPLIANT)
624  {
625  iImp->setImpedanceOffset(i,command[i]);
626  }
627  else
628  {
629  //stiff or unknown mode, nothing to do
630  }
631  break;
632 
633  case VOCAB_CM_IMPEDANCE_POS:
634  case VOCAB_CM_IMPEDANCE_VEL:
635  iImp->setImpedanceOffset(i,command[i]);
636  break;
637 
638  default:
639  if (s_part=="torso" && i==3)
640  {
641  // do nothing, because joint 3 of the torso is only virtual
642  }
643  else
644  {
645  yError("Unknown control mode (part: %s jnt:%d).\n",s_part.c_str(), i);
646  }
647  break;
648  }
649  }
650 }
651 
653 {
654  thread_status = STATUS_OK;
655  static int delay_check=0;
656  if(isCalibrated)
657  {
658  if (!readAndUpdate(false))
659  {
660  delay_check++;
661  yWarning ("network delays detected (%d/10)\n", delay_check);
662  if (delay_check>=10)
663  {
664  yError ("gravityCompensatorThread lost connection with wholeBodyDynamics.\n");
665  thread_status = STATUS_DISCONNECTED;
666  }
667  }
668  else
669  {
670  delay_check = 0;
671  }
672 
673  Vector F_up(6,0.0);
674  icub->upperTorso->setInertialMeasure(w0,dw0,d2p0);
675  icub->upperTorso->solveKinematics();
676  icub->upperTorso->solveWrench();
677 
678  //compute the arm torques
679  Matrix F_sens_up = icub->upperTorso->estimateSensorsWrench(F_ext_up,false);
680  gravity_torques_LA = icub->upperTorso->left->getTorques();
681  gravity_torques_RA = icub->upperTorso->right->getTorques();
682 
683  //compute the torso torques
684  icub->attachLowerTorso(F_up,F_up);
685  icub->lowerTorso->solveKinematics();
686  icub->lowerTorso->solveWrench();
687  Vector tmp; tmp.resize(3);
688  tmp = icub->lowerTorso->getTorques("torso");
689  gravity_torques_TO[0] = tmp [2];
690  gravity_torques_TO[1] = tmp [1];
691  gravity_torques_TO[2] = tmp [0];
692 
693  //compute the leg torques
694  Matrix F_sens_low = icub->lowerTorso->estimateSensorsWrench(F_ext_low,false);
695  gravity_torques_LL = icub->lowerTorso->getTorques("left_leg");
696  gravity_torques_RL = icub->lowerTorso->getTorques("right_leg");
697 
698 //#define DEBUG_TORQUES
699 #ifdef DEBUG_TORQUES
700  yDebug ("TORQUES: %s *** \n\n", torques_TO.toString().c_str());
701  yDebug ("LL TORQUES: %s *** \n\n", torques_LL.toString().c_str());
702  yDebug ("RL TORQUES: %s *** \n\n", torques_RL.toString().c_str());
703 #endif
704 
705  //read the external command ports
706  Vector *offset_input_la = la_additional_offset->read(false);
707  if(offset_input_la!=nullptr)
708  {
709  auto size = (offset_input_la->size() < 7) ? offset_input_la->size():7;
710  for (size_t i=0; i<size; i++)
711  {externalcmd_torques_LA[i]=(*offset_input_la)[i];}
712  }
713  Vector *offset_input_ra = ra_additional_offset->read(false);
714  if(offset_input_ra!=nullptr)
715  {
716  auto size = (offset_input_ra->size() < 7) ? offset_input_ra->size():7;
717  for (size_t i=0; i<size; i++)
718  {externalcmd_torques_RA[i]=(*offset_input_ra)[i];}
719  }
720  Vector *offset_input_ll = ll_additional_offset->read(false);
721  if(offset_input_ll!=nullptr)
722  {
723  auto size = (offset_input_ll->size() < 6) ? offset_input_ll->size():6;
724  for (size_t i=0; i<size; i++)
725  {externalcmd_torques_LL[i]=(*offset_input_ll)[i];}
726  }
727  Vector *offset_input_rl = rl_additional_offset->read(false);
728  if(offset_input_rl!=nullptr)
729  {
730  auto size = (offset_input_rl->size() < 6) ? offset_input_rl->size():6;
731  for (size_t i=0; i<size; i++)
732  {externalcmd_torques_RL[i]=(*offset_input_rl)[i];}
733  }
734  Vector *offset_input_to = to_additional_offset->read(false);
735  if(offset_input_to!=nullptr)
736  {
737  auto size = (offset_input_to->size() < 3) ? offset_input_to->size():3;
738  for (size_t i=0; i<size; i++)
739  {externalcmd_torques_TO[i]=(*offset_input_to)[i];}
740  }
741 
742  //compute the command to be given to the joint
744  {
746  {
747  exec_torques_LA = ampli_LA*gravity_torques_LA + externalcmd_torques_LA;
748  exec_torques_RA = ampli_RA*gravity_torques_RA + externalcmd_torques_RA;
749  exec_torques_LL = ampli_LL*gravity_torques_LL + externalcmd_torques_LL;
750  exec_torques_RL = ampli_RL*gravity_torques_RL + externalcmd_torques_RL;
751  exec_torques_TO = ampli_TO*gravity_torques_TO + externalcmd_torques_TO;
752  }
753  else
754  {
755  exec_torques_LA = ampli_LA*gravity_torques_LA;
756  exec_torques_RA = ampli_RA*gravity_torques_RA;
757  exec_torques_LL = ampli_LL*gravity_torques_LL;
758  exec_torques_RL = ampli_RL*gravity_torques_RL;
759  exec_torques_TO = ampli_TO*gravity_torques_TO;
760  }
761  }
762  else
763  {
765  {
766  exec_torques_LA = externalcmd_torques_LA;
767  exec_torques_RA = externalcmd_torques_RA;
768  exec_torques_LL = externalcmd_torques_LL;
769  exec_torques_RL = externalcmd_torques_RL;
770  exec_torques_TO = externalcmd_torques_TO;
771  }
772  else
773  {
774  externalcmd_torques_LA.zero();
775  externalcmd_torques_RA.zero();
776  externalcmd_torques_LL.zero();
777  externalcmd_torques_RL.zero();
778  externalcmd_torques_TO.zero();
779  }
780  }
781 
782  //execute the commands
783  static yarp::os::Stamp timestamp;
784  timestamp.update();
785  if (iCtrlMode_arm_left)
786  {
787  feedFwdGravityControl(left_arm_ctrlJnt, "left_arm", iCtrlMode_arm_left,iTqs_arm_left,iImp_arm_left,iIntMode_arm_left,exec_torques_LA);
788  if (left_arm_exec_torques && left_arm_exec_torques->getOutputCount()>0)
789  {
790  left_arm_exec_torques->prepare() = exec_torques_LA;
791  left_arm_exec_torques->setEnvelope(timestamp);
792  left_arm_exec_torques->write();
793  }
794  if (left_arm_gravity_torques && left_arm_gravity_torques->getOutputCount()>0)
795  {
796  left_arm_gravity_torques->prepare() = gravity_torques_LA;
797  left_arm_gravity_torques->setEnvelope(timestamp);
798  left_arm_gravity_torques->write();
799  }
800  }
801  if (iCtrlMode_arm_right)
802  {
803  feedFwdGravityControl(right_arm_ctrlJnt, "right_arm", iCtrlMode_arm_right,iTqs_arm_right,iImp_arm_right,iIntMode_arm_right,exec_torques_RA);
804  if (right_arm_exec_torques && right_arm_exec_torques->getOutputCount()>0)
805  {
806  right_arm_exec_torques->prepare() = exec_torques_RA;
807  right_arm_exec_torques->setEnvelope(timestamp);
808  right_arm_exec_torques->write();
809  }
810  if (right_arm_gravity_torques && right_arm_gravity_torques->getOutputCount()>0)
811  {
812  right_arm_gravity_torques->prepare() = gravity_torques_RA;
813  right_arm_gravity_torques->setEnvelope(timestamp);
814  right_arm_gravity_torques->write();
815  }
816  }
817  if (iCtrlMode_torso)
818  {
819  feedFwdGravityControl(torso_ctrlJnt, "torso", iCtrlMode_torso,iTqs_torso,iImp_torso,iIntMode_torso,exec_torques_TO);
820  if (torso_exec_torques && torso_exec_torques->getOutputCount()>0)
821  {
822  torso_exec_torques->prepare() = exec_torques_TO;
823  torso_exec_torques->setEnvelope(timestamp);
824  torso_exec_torques->write();
825  }
826  if (torso_gravity_torques && torso_gravity_torques->getOutputCount()>0)
827  {
828  torso_gravity_torques->prepare() = gravity_torques_TO;
829  torso_gravity_torques->setEnvelope(timestamp);
830  torso_gravity_torques->write();
831  }
832  }
833  if (iCtrlMode_leg_left)
834  {
835  feedFwdGravityControl(left_leg_ctrlJnt, "left_leg", iCtrlMode_leg_left,iTqs_leg_left,iImp_leg_left,iIntMode_leg_left,exec_torques_LL);
836  if (left_leg_exec_torques && left_leg_exec_torques->getOutputCount()>0)
837  {
838  left_leg_exec_torques->prepare() = exec_torques_LL;
839  left_leg_exec_torques->setEnvelope(timestamp);
840  left_leg_exec_torques->write();
841  }
842  if (left_leg_gravity_torques && left_leg_gravity_torques->getOutputCount()>0)
843  {
844  left_leg_gravity_torques->prepare() = gravity_torques_LL;
845  left_leg_gravity_torques->setEnvelope(timestamp);
846  left_leg_gravity_torques->write();
847  }
848  }
849  if (iCtrlMode_leg_right)
850  {
851  feedFwdGravityControl(right_leg_ctrlJnt, "right_leg", iCtrlMode_leg_right,iTqs_leg_right,iImp_leg_right,iIntMode_leg_right,exec_torques_RL);
852  if (right_leg_exec_torques && right_leg_exec_torques->getOutputCount()>0)
853  {
854  right_leg_exec_torques->prepare() = exec_torques_RL;
855  right_leg_exec_torques->setEnvelope(timestamp);
856  right_leg_exec_torques->write();
857  }
858  if (right_leg_gravity_torques && right_leg_gravity_torques->getOutputCount()>0)
859  {
860  right_leg_gravity_torques->prepare() = gravity_torques_RL;
861  right_leg_gravity_torques->setEnvelope(timestamp);
862  right_leg_gravity_torques->write();
863  }
864  }
865  }
866  else
867  {
868  if(Network::exists("/"+wholeBodyName+"/filtered/inertial:o"))
869  {
870  yInfo("connection exists! starting calibration...\n");
871  //the following delay is required because even if the filtered port exists, may be the
872  //low pass filtered values have not reached yet the correct value.
873  Time::delay(1.0);
874 
875  isCalibrated = true;
876  Network::connect("/"+wholeBodyName+"/filtered/inertial:o","/gravityCompensator/inertial:i");
878  setUpperMeasure();
879  setLowerMeasure();
880 
881  readAndUpdate(true);
882 
883  icub->upperTorso->setInertialMeasure(w0,dw0,d2p0);
884  Matrix F_sens_up = icub->upperTorso->estimateSensorsWrench(F_ext_up,false);
886  icub->upperTorso->getTorsoAngAcc(),
887  icub->upperTorso->getTorsoLinAcc());
888  Matrix F_sens_low = icub->lowerTorso->estimateSensorsWrench(F_ext_low,false);
889  gravity_torques_LA = icub->upperTorso->getTorques("left_arm");
890  gravity_torques_RA = icub->upperTorso->getTorques("right_arm");
891  gravity_torques_LL = icub->lowerTorso->getTorques("left_leg");
892  gravity_torques_RL = icub->lowerTorso->getTorques("right_leg");
893  Vector LATorques = icub->upperTorso->getTorques("left_arm");
894  yDebug("encoders: %.1lf, %.1lf, %.1lf, %.1lf, %.1lf, %.1lf, %.1lf\n", encoders_arm_left(0), encoders_arm_left(1), encoders_arm_left(2), encoders_arm_left(3), encoders_arm_left(4), encoders_arm_left(5), encoders_arm_left(6));
895  yDebug("left arm: %.3lf, %.3lf, %.3lf, %.3lf, %.3lf, %.3lf, %.3lf\n", gravity_torques_LA(0), gravity_torques_LA(1), gravity_torques_LA(2), gravity_torques_LA(3), gravity_torques_LA(4), gravity_torques_LA(5), gravity_torques_LA(6));
896  yDebug("right arm: %.3lf, %.3lf, %.3lf, %.3lf, %.3lf, %.3lf, %.3lf\n", gravity_torques_RA(0), gravity_torques_RA(1), gravity_torques_RA(2), gravity_torques_RA(3), gravity_torques_RA(4), gravity_torques_RA(5), gravity_torques_RA(6));
897  yDebug("left leg: %.3lf, %.3lf, %.3lf, %.3lf, %.3lf, %.3lf\n", gravity_torques_LL(0), gravity_torques_LL(1), gravity_torques_LL(2), gravity_torques_LL(3), gravity_torques_LL(4), gravity_torques_LL(5));
898  yDebug("right leg: %.3lf, %.3lf, %.3lf, %.3lf, %.3lf, %.3lf\n", gravity_torques_RL(0), gravity_torques_RL(1), gravity_torques_RL(2), gravity_torques_RL(3), gravity_torques_RL(4), gravity_torques_RL(5));
899  yDebug("inertial: %.1lf, %.1lf, %.1lf, %.1lf, %.1lf, %.1lf, %.1lf, %.1lf, %.1lf\n", d2p0(0), d2p0(1), d2p0(2), w0(0), w0(1), w0(2), dw0(0), dw0(1), dw0(2));
900  }
901  else
902  {
903  yInfo("waiting for connections from wholeBodyDynamics (port: %s)...\n", wholeBodyName.c_str());
904  Time::delay(1.0);
905  }
906  }
907 }
908 
910 {
911  externalcmd_torques_LA.zero();
912  externalcmd_torques_RA.zero();
913  externalcmd_torques_LL.zero();
914  externalcmd_torques_RL.zero();
915  externalcmd_torques_TO.zero();
916  gravity_torques_LA.zero();
917  gravity_torques_RA.zero();
918  gravity_torques_LL.zero();
919  gravity_torques_RL.zero();
920  gravity_torques_TO.zero();
921  exec_torques_LA.zero();
922  exec_torques_RA.zero();
923  exec_torques_LL.zero();
924  exec_torques_RL.zero();
925  exec_torques_TO.zero();
926 
927  if (iCtrlMode_arm_left)
928  {
929  yInfo("Setting gravity compensation offset to zero, left arm\n");
930  feedFwdGravityControl(left_arm_ctrlJnt, "left_arm",iCtrlMode_arm_left,iTqs_arm_left,iImp_arm_left,iIntMode_arm_left,exec_torques_LA,true);
931  }
932  if (iCtrlMode_arm_right)
933  {
934  yInfo("Setting gravity compensation offset to zero, right arm\n");
935  feedFwdGravityControl(right_arm_ctrlJnt, "right_arm",iCtrlMode_arm_right,iTqs_arm_right,iImp_arm_right,iIntMode_arm_right,exec_torques_RA,true);
936  }
937  if (iCtrlMode_leg_left)
938  {
939  yInfo("Setting gravity compensation offset to zero, left leg\n");
940  feedFwdGravityControl(left_leg_ctrlJnt, "left_leg", iCtrlMode_leg_left,iTqs_leg_left,iImp_leg_left,iIntMode_leg_left,exec_torques_LL,true);
941  }
942  if (iCtrlMode_leg_right)
943  {
944  yInfo("Setting gravity compensation offset to zero, right leg\n");
945  feedFwdGravityControl(right_leg_ctrlJnt, "right_leg",iCtrlMode_leg_right,iTqs_leg_right,iImp_leg_right,iIntMode_leg_right,exec_torques_RL,true);
946  }
947  if (iCtrlMode_torso)
948  {
949  yInfo("Setting gravity compensation offset to zero, torso\n");
950  feedFwdGravityControl(torso_ctrlJnt, "torso",iCtrlMode_torso,iTqs_torso,iImp_torso,iIntMode_torso,exec_torques_TO,true);
951  }
952  Time::delay(0.5);
953 
954  left_arm_exec_torques->interrupt();
955  right_arm_exec_torques->interrupt();
956  left_leg_exec_torques->interrupt();
957  right_leg_exec_torques->interrupt();
958  torso_exec_torques->interrupt();
959  left_arm_exec_torques->close();
960  right_arm_exec_torques->close();
961  left_leg_exec_torques->close();
962  right_leg_exec_torques->close();
963  torso_exec_torques->close();
964 
965  left_arm_gravity_torques->interrupt();
966  right_arm_gravity_torques->interrupt();
967  left_leg_gravity_torques->interrupt();
968  right_leg_gravity_torques->interrupt();
969  torso_gravity_torques->interrupt();
970  left_arm_gravity_torques->close();
971  right_arm_gravity_torques->close();
972  left_leg_gravity_torques->close();
973  right_leg_gravity_torques->close();
974  torso_gravity_torques->close();
975 
976  if (left_arm_exec_torques) {delete left_arm_exec_torques; left_arm_exec_torques = nullptr;}
977  if (right_arm_exec_torques) {delete right_arm_exec_torques; right_arm_exec_torques = nullptr;}
978  if (left_leg_exec_torques) {delete left_leg_exec_torques; left_leg_exec_torques = nullptr;}
979  if (right_leg_exec_torques) {delete right_leg_exec_torques; right_leg_exec_torques = nullptr;}
980  if (torso_exec_torques) {delete torso_exec_torques; torso_exec_torques = nullptr;}
981 
982  if (left_arm_gravity_torques) {delete left_arm_gravity_torques; left_arm_gravity_torques = nullptr;}
983  if (right_arm_gravity_torques) {delete right_arm_gravity_torques; right_arm_gravity_torques = nullptr;}
984  if (left_leg_gravity_torques) {delete left_leg_gravity_torques; left_leg_gravity_torques = nullptr;}
985  if (right_leg_gravity_torques) {delete right_leg_gravity_torques; right_leg_gravity_torques = nullptr;}
986  if (torso_gravity_torques) {delete torso_gravity_torques; torso_gravity_torques = nullptr;}
987 
988  if (linEstUp) {delete linEstUp; linEstUp = nullptr;}
989  if (quadEstUp) {delete quadEstUp; quadEstUp = nullptr;}
990  if (linEstLow) {delete linEstLow; linEstLow = nullptr;}
991  if (quadEstLow) {delete quadEstLow; quadEstLow = nullptr;}
992 
993  //closing ports
994  port_inertial->interrupt();
995  port_inertial->close();
996  la_additional_offset->interrupt();
997  la_additional_offset->close();
998  ra_additional_offset->interrupt();
999  ra_additional_offset->close();
1000  ll_additional_offset->interrupt();
1001  ll_additional_offset->close();
1002  rl_additional_offset->interrupt();
1003  rl_additional_offset->close();
1004  to_additional_offset->interrupt();
1005  to_additional_offset->close();
1006  if (icub) {delete icub; icub=nullptr;}
1007 }
#define CTRL_DEG2RAD
Definition: XSensMTx.cpp:26
bool readAndUpdate(bool waitMeasure=false)
gravityCompensatorThread(std::string _wholeBodyName, int _rate, PolyDriver *_ddLA, PolyDriver *_ddRA, PolyDriver *_ddH, PolyDriver *_ddLL, PolyDriver *_ddRL, PolyDriver *_ddT, version_tag icub_type, bool _inertial_enabled)
void feedFwdGravityControl(int part_ctrlJnt, const std::string &s_part, IControlMode *iCtrlMode, ITorqueControl *iTqs, IImpedanceControl *iImp, IInteractionMode *iIntMode, const Vector &command, bool releasing=false)
bool threadInit() override
void threadRelease() override
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
iCubUpperTorso * upperTorso
pointer to UpperTorso = head + right arm + left arm
Definition: iDynBody.h:1482
iCubLowerTorso * lowerTorso
pointer to LowerTorso = torso + right leg + left leg
Definition: iDynBody.h:1484
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
yarp::sig::Vector getTorques() const
Returns the links torque as a vector.
Definition: iDyn.cpp:875
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.
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
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 getTorsoAngAcc() const
Return the torso angular acceleration.
Definition: iDynBody.cpp:1908
yarp::sig::Vector getTorsoLinAcc() const
Return the torso linear acceleration.
Definition: iDynBody.cpp:1910
iDyn::iDynLimb * right
right limb
Definition: iDynBody.h:1081
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
@ STATUS_OK
Definition: gravityThread.h:41
@ STATUS_DISCONNECTED
Definition: gravityThread.h:41
@ GRAVITY_COMPENSATION_ON
Definition: gravityThread.h:42
@ GRAVITY_COMPENSATION_OFF
Definition: gravityThread.h:42
@ EXTERNAL_TRQ_ON
Definition: gravityThread.h:43
@ DYNAMIC
Definition: iDynInv.h:64