iCub-main
Loading...
Searching...
No Matches
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>
30#include <iCub/ctrl/adaptWinPolyEstimator.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
37using namespace yarp::os;
38using namespace yarp::sig;
39using namespace yarp::math;
40using namespace yarp::dev;
41using namespace iCub::ctrl;
42using namespace iCub::iDyn;
43using namespace iCub::skinDynLib;
44using namespace std;
45
46Vector 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
55Vector 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
64Vector 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
73Vector 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
82void 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
123void 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
160void 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
175void 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
189gravityCompensatorThread::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{
203 gravity_mode = GRAVITY_COMPENSATION_ON;
204 external_mode = EXTERNAL_TRQ_ON;
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
257void gravityCompensatorThread::setZeroJntAngVelAcc()
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
274bool gravityCompensatorThread::readAndUpdate(bool waitMeasure)
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
335 b &= getUpperEncodersSpeedAndAcceleration();
336 setUpperMeasure();
337 b &= getLowerEncodersSpeedAndAcceleration();
338 setLowerMeasure();
339
340 return b;
341}
342
343bool gravityCompensatorThread::getLowerEncodersSpeedAndAcceleration()
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
377 setZeroJntAngVelAcc();
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
403bool gravityCompensatorThread::getUpperEncodersSpeedAndAcceleration()
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
437 setZeroJntAngVelAcc();
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
462bool gravityCompensatorThread::threadInit()
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
575void 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
652void gravityCompensatorThread::run()
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
743 if (gravity_mode==GRAVITY_COMPENSATION_ON)
744 {
745 if (external_mode==EXTERNAL_TRQ_ON)
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 {
764 if (external_mode==EXTERNAL_TRQ_ON)
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");
877 setZeroJntAngVelAcc();
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);
885 icub->lowerTorso->setInertialMeasure(icub->upperTorso->getTorsoAngVel(),
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
909void gravityCompensatorThread::threadRelease()
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}
gravityCompensatorThread::readAndUpdate
bool readAndUpdate(bool waitMeasure=false)
Definition gravityThread.cpp:274
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
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
gravityCompensatorThread::threadRelease
void threadRelease() override
Definition gravityThread.cpp:909
iCub::ctrl::AWLinEstimator
Adaptive window linear fitting to estimate the first derivative.
Definition adaptWinPolyEstimator.h:185
iCub::ctrl::AWQuadEstimator
Adaptive window quadratic fitting to estimate the second derivative.
Definition adaptWinPolyEstimator.h:208
iCub::iDyn::iCubWholeBody
A class for connecting UpperTorso and LowerTorso of the iCub, then getting the WholeBody in the dynam...
Definition iDynBody.h:1472
x
x
Definition compute_ekf_sym.m:21
STATUS_OK
@ STATUS_OK
Definition gravityThread.h:41
STATUS_DISCONNECTED
@ STATUS_DISCONNECTED
Definition gravityThread.h:41
EXTERNAL_TRQ_ON
@ EXTERNAL_TRQ_ON
Definition gravityThread.h:43
GRAVITY_COMPENSATION_ON
@ GRAVITY_COMPENSATION_ON
Definition gravityThread.h:42
GRAVITY_COMPENSATION_OFF
@ GRAVITY_COMPENSATION_OFF
Definition gravityThread.h:42
iCub::ctrl::CTRL_DEG2RAD
constexpr double CTRL_DEG2RAD
PI/180.
Definition math.h:66
iCub::iDyn::version_tag
Definition iDynBody.h:109
Generated on for iCub-main by doxygen 1.15.0