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