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>
31#include <iCub/iDyn/iDyn.h>
32#include <iCub/iDyn/iDynBody.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{
49 el.data=x;
50 el.time=Time::now();
51
52 return linEstUp->estimate(el);
53}
54
55Vector gravityCompensatorThread::evalVelLow(const Vector &x)
56{
58 el.data=x;
59 el.time=Time::now();
60
61 return linEstLow->estimate(el);
62}
63
64Vector gravityCompensatorThread::evalAccUp(const Vector &x)
65{
67 el.data=x;
68 el.time=Time::now();
69
70 return quadEstUp->estimate(el);
71}
72
73Vector gravityCompensatorThread::evalAccLow(const Vector &x)
74{
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{
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
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
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);
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);
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}
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)
void threadRelease() override
Adaptive window linear fitting to estimate the first derivative.
Basic element for adaptive polynomial fitting.
yarp::sig::Vector estimate()
Execute the algorithm upon the elements list, with the max deviation threshold given by D.
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 ...
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.
yarp::sig::Vector getTorques(const std::string &limbType)
Return the chosen limb torques, as a Nx1 vector.
yarp::sig::Matrix estimateSensorsWrench(const yarp::sig::Matrix &FM, bool afterAttach=false)
Redefinition from iDynSensorNode.
Definition iDynBody.h:1383
iDyn::iDynLimb * left
left limb
Definition iDynBody.h:1079
yarp::sig::Vector setDAng(const std::string &limbType, const yarp::sig::Vector &_dq)
Set joints angle velocity in the chosen limb.
yarp::sig::Vector getTorsoAngAcc() const
Return the torso angular acceleration.
yarp::sig::Vector setD2Ang(const std::string &limbType, const yarp::sig::Vector &_ddq)
Set joints angle acceleration in the chosen limb.
yarp::sig::Vector getTorsoLinAcc() const
Return the torso linear acceleration.
iDyn::iDynLimb * right
right limb
Definition iDynBody.h:1081
yarp::sig::Vector setAng(const std::string &limbType, const yarp::sig::Vector &_q)
Set joints angle position in the chosen limb.
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.
yarp::sig::Vector getTorsoAngVel() const
Return the torso angular velocity.
@ STATUS_OK
@ STATUS_DISCONNECTED
@ GRAVITY_COMPENSATION_ON
@ GRAVITY_COMPENSATION_OFF
@ EXTERNAL_TRQ_ON
constexpr double CTRL_DEG2RAD
PI/180.
Definition math.h:66