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 }
iCub::iDyn::iDynSensorTorsoNode::getTorsoAngAcc
yarp::sig::Vector getTorsoAngAcc() const
Return the torso angular acceleration.
Definition: iDynBody.cpp:1908
python-motor-control.tmp
tmp
Definition: python-motor-control.py:43
iCub::ctrl::AWQuadEstimator
Definition: adaptWinPolyEstimator.h:207
gravityThread.h
iCub::skinDynLib
Definition: common.h:35
STATUS_DISCONNECTED
@ STATUS_DISCONNECTED
Definition: gravityThread.h:41
gravityCompensatorThread::threadInit
bool threadInit() override
Definition: gravityThread.cpp:462
GRAVITY_COMPENSATION_ON
@ GRAVITY_COMPENSATION_ON
Definition: gravityThread.h:42
iCub::iDyn::iDynNode::solveKinematics
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.
EXTERNAL_TRQ_ON
@ EXTERNAL_TRQ_ON
Definition: gravityThread.h:43
gravityCompensatorThread::setZeroJntAngVelAcc
void setZeroJntAngVelAcc()
Definition: gravityThread.cpp:257
gravityCompensatorThread::getUpperEncodersSpeedAndAcceleration
bool getUpperEncodersSpeedAndAcceleration()
Definition: gravityThread.cpp:403
iDyn.h
gravityCompensatorThread::external_mode
int external_mode
Definition: gravityThread.h:167
iCub::iDyn::iDynSensorTorsoNode::right
iDyn::iDynLimb * right
right limb
Definition: iDynBody.h:1081
CTRL_DEG2RAD
#define CTRL_DEG2RAD
Definition: XSensMTx.cpp:26
iCub::iDyn::iDynSensorTorsoNode::estimateSensorsWrench
yarp::sig::Matrix estimateSensorsWrench(const yarp::sig::Matrix &FM, bool afterAttach=false)
Redefinition from iDynSensorNode.
Definition: iDynBody.h:1383
iCub::iDyn::iDynSensorTorsoNode::getTorques
yarp::sig::Vector getTorques(const std::string &limbType)
Return the chosen limb torques, as a Nx1 vector.
Definition: iDynBody.cpp:1890
yarp::dev
Definition: DebugInterfaces.h:52
iCub::iDyn::iDynSensorNode::solveWrench
virtual bool solveWrench()
Main function to manage the exchange of wrench information among the limbs attached to the node.
Definition: iDynBody.cpp:1352
math.h
iCub::ctrl::AWPolyElement
Definition: adaptWinPolyEstimator.h:45
iCub::iDyn::iCubWholeBody::attachLowerTorso
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
gravityCompensatorThread::feedFwdGravityControl
void feedFwdGravityControl(int part_ctrlJnt, const std::string &s_part, IControlMode *iCtrlMode, ITorqueControl *iTqs, IImpedanceControl *iImp, IInteractionMode *iIntMode, const Vector &command, bool releasing=false)
Definition: gravityThread.cpp:575
iCub::iDyn::DYNAMIC
@ DYNAMIC
Definition: iDynInv.h:64
iCub::iDyn
Definition: iDyn.h:81
iCub::iDyn::iDynSensorTorsoNode::getTorsoAngVel
yarp::sig::Vector getTorsoAngVel() const
Return the torso angular velocity.
Definition: iDynBody.cpp:1906
iCub::iDyn::iCubWholeBody::lowerTorso
iCubLowerTorso * lowerTorso
pointer to LowerTorso = torso + right leg + left leg
Definition: iDynBody.h:1484
iCub::ctrl::AWPolyElement::time
double time
Definition: adaptWinPolyEstimator.h:49
iCub::iDyn::iDynSensorTorsoNode::getTorsoLinAcc
yarp::sig::Vector getTorsoLinAcc() const
Return the torso linear acceleration.
Definition: iDynBody.cpp:1910
iCub::iDyn::iDynSensorTorsoNode::left
iDyn::iDynLimb * left
left limb
Definition: iDynBody.h:1079
iCub::ctrl
Definition: adaptWinPolyEstimator.h:37
x
x
Definition: compute_ekf_sym.m:21
STATUS_OK
@ STATUS_OK
Definition: gravityThread.h:41
iCub::skinDynLib::VERBOSE
@ VERBOSE
Definition: common.h:38
iCub::ctrl::AWPolyElement::data
yarp::sig::Vector data
Definition: adaptWinPolyEstimator.h:48
iCub::iDyn::version_tag
Definition: iDynBody.h:108
gravityCompensatorThread::readAndUpdate
bool readAndUpdate(bool waitMeasure=false)
Definition: gravityThread.cpp:274
common.h
gravityCompensatorThread::gravity_mode
int gravity_mode
Definition: gravityThread.h:166
iCub::iDyn::iCubWholeBody::upperTorso
iCubUpperTorso * upperTorso
pointer to UpperTorso = head + right arm + left arm
Definition: iDynBody.h:1482
iDynBody.h
gravityCompensatorThread::gravityCompensatorThread
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)
Definition: gravityThread.cpp:189
adaptWinPolyEstimator.h
iCub::ctrl::AWLinEstimator
Definition: adaptWinPolyEstimator.h:184
iCub::iDyn::iCubWholeBody
Definition: iDynBody.h:1471
iCub::iDyn::iDynSensorTorsoNode::setInertialMeasure
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
gravityCompensatorThread::threadRelease
void threadRelease() override
Definition: gravityThread.cpp:909
gravityCompensatorThread::run
void run() override
Definition: gravityThread.cpp:652
iCub::iDyn::iDynChain::getTorques
yarp::sig::Vector getTorques() const
Returns the links torque as a vector.
Definition: iDyn.cpp:875
GRAVITY_COMPENSATION_OFF
@ GRAVITY_COMPENSATION_OFF
Definition: gravityThread.h:42
gravityCompensatorThread::getLowerEncodersSpeedAndAcceleration
bool getLowerEncodersSpeedAndAcceleration()
Definition: gravityThread.cpp:343