search

robotology-legacy / codyco-modules

Whole-body Compliant Dynamical Contacts in Cognitive Humanoids
www.codyco.eu
Other
19 stars 13 forks source link

Investigate random bug of wholeBodyDynamicsTree #96

Closed traversaro closed 9 years ago

traversaro commented 9 years ago 
iDynTree WARNING: base_residual_f.force.Norm() is 0.0021643 instead of zero.
iDynTree WARNING: base_residual_f.torque.Norm() is 0.00101268 instead of zero.
traversaro commented 9 years ago

More info:

528     39.728312               wholeBodyDynamicsTree: double support calibration for allrequested
529     39.728390               wholeBodyDynamicsThread::calibrateOffsetOnDoubleSupport called with code all
530     39.728425               wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 0 is -nan -nan -nan -nan -nan -nan
531     39.728456               wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 1 is -nan -nan -nan -nan -nan -nan
532     39.728486               wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 2 is -nan -nan -nan -nan -nan -nan
533     39.728515               wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 3 is 49.8407 5.2716 15.6721 -0.789865 0.13584 -0.280371
534     39.728545               wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 4 is -nan -nan -nan -nan -nan -nan
535     39.728573               wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 5 is -nan -nan -nan -nan -nan -nan
536     39.728601               wholeBodyDynamicsThread::calibrateOffset all called successfully, starting calibration.
551     43.103568               wholeBodyDynamicsTree: double support calibration for allrequested
552     43.103646               wholeBodyDynamicsThread::calibrateOffsetOnDoubleSupport called with code all
553     43.103680               wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 0 is 0 0 0 0 0 0
554     43.103708               wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 1 is 0 0 0 0 0 0
555     43.103734               wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 2 is 0 0 0 0 0 0
556     43.103760               wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 3 is 0 0 0 0 0 0
557     43.103786               wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 4 is 0 0 0 0 0 0
558     43.103812               wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 5 is 0 0 0 0 0 0
707     67.776289               yarp: Removing output from /wholeBodyDynamicsTree/contacts:o to /iCubGui/forces
709     68.048372               yarp: Removing output from /wholeBodyDynamicsTree/base:o to /iCubGui/base:i
730     69.015886               iDynTree WARNING: base_residual_f.force.Norm() is 0.226746 instead of zero.
731     69.015927               iDynTree WARNING: base_residual_f.torque.Norm() is 0.0605821 instead of zero.

It is quite dangerous, because it corrupts all torque estimates.

traversaro commented 9 years ago 
5292        1931.365924             ||| clearing context
5293        1931.371788             ||| adding context [wholeBodyDynamicsTree]
5294        1931.371849             ||| configuring
5295        1931.371908             ||| no policy found
5296        1931.371938             ||| default config file specified as wholeBodyDynamicsTree.ini
5297        1931.371969             ||| checking [/tmp/wholeBodyDynamicsTree.ini] (pwd)
5298        1931.371997             ||| checking [/home/icub/.config/yarp/robots/iCubParis01] (robot YARP_CONFIG_HOME)
5299        1931.372027             ||| checking [/home/icub/.local/share/yarp/robots/iCubParis01] (robot YARP_DATA_HOME)
5300        1931.372056             ||| found /home/icub/.local/share/yarp/robots/iCubParis01
5301        1931.372091             ||| checking [/etc/yarp/robots/iCubParis01] (robot YARP_CONFIG_DIRS)
5302        1931.372121             ||| checking [/home/icub/software/src/yarp/_build/share/yarp/robots/iCubParis01] (robot YARP_DATA_DIRS)
5303        1931.372152             ||| checking [/home/icub/software/src/icub-main/_build/share/iCub/robots/iCubParis01] (robot YARP_DATA_DIRS)
5304        1931.372184             ||| found /home/icub/software/src/icub-main/_build/share/iCub/robots/iCubParis01
5305        1931.372249             ||| checking [/home/icub/software/src/codyco-superbuild/_build/install/share/codyco/robots/iCubParis01] (robot YARP_DATA_DIRS)
5306        1931.372281             ||| found /home/icub/software/src/codyco-superbuild/_build/install/share/codyco/robots/iCubParis01
5307        1931.372312             ||| checking [/home/icub/software/src/yarp/_build/share/yarp/config/path.d] (robot path.d YARP_DATA_DIRS)
5308        1931.372352             ||| checking [/home/icub/software/src/icub-main/_build/share/iCub/config/path.d] (robot path.d YARP_DATA_DIRS)
5309        1931.372384             ||| checking [/home/icub/software/src/codyco-superbuild/_build/install/share/codyco/config/path.d] (robot path.d YARP_DATA_DIRS)
5310        1931.372413             ||| checking [/home/icub/.local/share/yarp/robots/iCubParis01/wholeBodyDynamicsTree.ini] (robot)
5311        1931.372443             ||| found /home/icub/.local/share/yarp/robots/iCubParis01/wholeBodyDynamicsTree.ini
5312        1931.372492             assume_fixed option found, using l_sole as fixed link as a kinematic root instead of the imu.
5313        1931.372529             assume_fixed_base_calibration option found, using l_sole as fixed link as a kinematic root instead of the imu for calibration.
5314        1931.372560             yarp: Port /wholeBodyDynamicsTree/rpc:i active at tcp://10.0.0.13:10175
5315        1931.372590             ||| finding file [wbi_conf_file]
5316        1931.372617             ||| checking [/tmp/yarpWholeBodyInterface.ini] (pwd)
5317        1931.372645             ||| checking [/home/icub/.local/share/yarp/robots/iCubParis01/yarpWholeBodyInterface.ini] (robot)
5318        1931.372674             ||| found /home/icub/.local/share/yarp/robots/iCubParis01/yarpWholeBodyInterface.ini
5319        1931.372703              loadIdListFromConfig error: requested list ROBOT_DYNAMIC_MODEL_JOINTS not found
5323        1931.386079             yarp: Port /wholeBodyDynamicsTree/skin_contacts:i active at tcp://10.0.0.13:10176
5333        1931.486926             yarp: Port /wholeBodyDynamicsTree/torso/rpc:o active at tcp://10.0.0.13:10177
5482        1931.547580             yarp: Port /wholeBodyDynamicsTree/torso/command:o active at tcp://10.0.0.13:10178
5577        1932.496406             yarp: Port /wholeBodyDynamicsTree/torso/state:i active at tcp://10.0.0.13:10179
5694        1932.627378             yarp: Port /wholeBodyDynamicsTree/torso/stateExt:i active at tcp://10.0.0.13:10180
5699        1932.768583             yarp: Sending output from /wholeBodyDynamicsTree/torso/rpc:o to /icub/torso/rpc:i using tcp
5715        1932.890215             yarp: Sending output from /wholeBodyDynamicsTree/torso/command:o to /icub/torso/command:i using tcp
5724        1933.101843             yarp: Receiving input from /icub/torso/state:o to /wholeBodyDynamicsTree/torso/state:i using udp
5733        1933.152326             yarp: Receiving input from /icub/torso/stateExt:o to /wholeBodyDynamicsTree/torso/stateExt:i using udp
5734        1933.162262             RemoteControlBoard is ENABLING the writeStrict option for all commands
5735        1933.162332     INFO        created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
5739        1933.242947             yarp: Port /wholeBodyDynamicsTree/left_arm/rpc:o active at tcp://10.0.0.13:10185
5743        1933.283331             yarp: Port /wholeBodyDynamicsTree/left_arm/command:o active at tcp://10.0.0.13:10186
5745        1933.313577             yarp: Port /wholeBodyDynamicsTree/left_arm/state:i active at tcp://10.0.0.13:10187
5747        1933.384231             yarp: Port /wholeBodyDynamicsTree/left_arm/stateExt:i active at tcp://10.0.0.13:10188
5756        1933.404636             yarp: Sending output from /wholeBodyDynamicsTree/left_arm/rpc:o to /icub/left_arm/rpc:i using tcp
5764        1933.414649             yarp: Sending output from /wholeBodyDynamicsTree/left_arm/command:o to /icub/left_arm/command:i using tcp
5771        1933.434650             yarp: Receiving input from /icub/left_arm/state:o to /wholeBodyDynamicsTree/left_arm/state:i using udp
5773        1933.434676             yarp: Receiving input from /icub/left_arm/stateExt:o to /wholeBodyDynamicsTree/left_arm/stateExt:i using udp
5774        1933.434690             RemoteControlBoard is ENABLING the writeStrict option for all commands
5775        1933.434704     INFO        created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
5776        1933.464797             yarp: Port /wholeBodyDynamicsTree/right_arm/rpc:o active at tcp://10.0.0.13:10189
5777        1933.495038             yarp: Port /wholeBodyDynamicsTree/right_arm/command:o active at tcp://10.0.0.13:10190
5800        1933.535315             yarp: Port /wholeBodyDynamicsTree/right_arm/state:i active at tcp://10.0.0.13:10191
5802        1933.555437             yarp: Port /wholeBodyDynamicsTree/right_arm/stateExt:i active at tcp://10.0.0.13:10192
5810        1933.555706             yarp: Sending output from /wholeBodyDynamicsTree/right_arm/rpc:o to /icub/right_arm/rpc:i using tcp
5811        1933.565519             yarp: Sending output from /wholeBodyDynamicsTree/right_arm/command:o to /icub/right_arm/command:i using tcp
5819        1933.575580             yarp: Receiving input from /icub/right_arm/state:o to /wholeBodyDynamicsTree/right_arm/state:i using udp
5827        1933.575851             yarp: Receiving input from /icub/right_arm/stateExt:o to /wholeBodyDynamicsTree/right_arm/stateExt:i using udp
5828        1933.585642             RemoteControlBoard is ENABLING the writeStrict option for all commands
5829        1933.585709     INFO        created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
5837        1933.605767             yarp: Port /wholeBodyDynamicsTree/left_leg/rpc:o active at tcp://10.0.0.13:10193
5852        1933.615827             yarp: Port /wholeBodyDynamicsTree/left_leg/command:o active at tcp://10.0.0.13:10194
5860        1933.676215             yarp: Port /wholeBodyDynamicsTree/left_leg/state:i active at tcp://10.0.0.13:10195
5861        1933.726555             yarp: Port /wholeBodyDynamicsTree/left_leg/stateExt:i active at tcp://10.0.0.13:10196
5862        1933.736625             yarp: Sending output from /wholeBodyDynamicsTree/left_leg/rpc:o to /icub/left_leg/rpc:i using tcp
5863        1933.776866             yarp: Sending output from /wholeBodyDynamicsTree/left_leg/command:o to /icub/left_leg/command:i using tcp
5864        1933.786963             yarp: Receiving input from /icub/left_leg/state:o to /wholeBodyDynamicsTree/left_leg/state:i using udp
5865        1933.817190             yarp: Receiving input from /icub/left_leg/stateExt:o to /wholeBodyDynamicsTree/left_leg/stateExt:i using udp
5866        1933.817264             RemoteControlBoard is ENABLING the writeStrict option for all commands
5867        1933.817296     INFO        created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
5868        1933.837311             yarp: Port /wholeBodyDynamicsTree/right_leg/rpc:o active at tcp://10.0.0.13:10197
5869        1933.857432             yarp: Port /wholeBodyDynamicsTree/right_leg/command:o active at tcp://10.0.0.13:10198
5870        1933.877639             yarp: Port /wholeBodyDynamicsTree/right_leg/state:i active at tcp://10.0.0.13:10199
5871        1933.937999             yarp: Port /wholeBodyDynamicsTree/right_leg/stateExt:i active at tcp://10.0.0.13:10200
5872        1933.948059             yarp: Sending output from /wholeBodyDynamicsTree/right_leg/rpc:o to /icub/right_leg/rpc:i using tcp
5873        1933.958118             yarp: Sending output from /wholeBodyDynamicsTree/right_leg/command:o to /icub/right_leg/command:i using tcp
5874        1933.968177             yarp: Receiving input from /icub/right_leg/state:o to /wholeBodyDynamicsTree/right_leg/state:i using udp
5875        1933.978237             yarp: Receiving input from /icub/right_leg/stateExt:o to /wholeBodyDynamicsTree/right_leg/stateExt:i using udp
5876        1933.978294             RemoteControlBoard is ENABLING the writeStrict option for all commands
5877        1933.978327     INFO        created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
5878        1933.998351             yarp: Port /wholeBodyDynamicsTree/ftSens/l_arm_ft_sensor:i active at tcp://10.0.0.13:10201
5879        1934.018529             yarp: Receiving input from /icub/left_arm/analog:o to /wholeBodyDynamicsTree/ftSens/l_arm_ft_sensor:i using udp
5880        1934.038659             yarp: Port /wholeBodyDynamicsTree/ftSens/r_arm_ft_sensor:i active at tcp://10.0.0.13:10202
5882        1934.048791             yarp: Receiving input from /icub/right_arm/analog:o to /wholeBodyDynamicsTree/ftSens/r_arm_ft_sensor:i using udp
5883        1934.078910             yarp: Port /wholeBodyDynamicsTree/ftSens/l_leg_ft_sensor:i active at tcp://10.0.0.13:10203
5884        1934.088968             yarp: Receiving input from /icub/left_leg/analog:o to /wholeBodyDynamicsTree/ftSens/l_leg_ft_sensor:i using udp
5885        1934.119242             yarp: Port /wholeBodyDynamicsTree/ftSens/l_foot_ft_sensor:i active at tcp://10.0.0.13:10204
5886        1934.139395             yarp: Receiving input from /icub/left_foot/analog:o to /wholeBodyDynamicsTree/ftSens/l_foot_ft_sensor:i using udp
5887        1934.159552             yarp: Port /wholeBodyDynamicsTree/ftSens/r_leg_ft_sensor:i active at tcp://10.0.0.13:10222
5888        1934.169623             yarp: Receiving input from /icub/right_leg/analog:o to /wholeBodyDynamicsTree/ftSens/r_leg_ft_sensor:i using udp
5889        1934.189761             yarp: Port /wholeBodyDynamicsTree/ftSens/r_foot_ft_sensor:i active at tcp://10.0.0.13:10223
5890        1934.209927             yarp: Receiving input from /icub/right_foot/analog:o to /wholeBodyDynamicsTree/ftSens/r_foot_ft_sensor:i using udp
5891        1934.219992             yarp: Port /wholeBodyDynamicsTree/imu/imu_frame:i active at tcp://10.0.0.13:10224
5892        1934.240187             yarp: Receiving input from /icub/inertial to /wholeBodyDynamicsTree/imu/imu_frame:i using udp
5893        1934.240261     DEBUG        Create TorqueEstimationTree with 6 ft sensors
5895        1934.260301             ft_sens0
5896        1934.260335             Found ft sensor l_arm_ft_sensor in urdf 
5897        1934.260350             ft_sens0
5898        1934.260364             ft_sens1
5899        1934.260376             Found ft sensor r_arm_ft_sensor in urdf 
5900        1934.260389             ft_sens0
5901        1934.260402             ft_sens1
5902        1934.260414             ft_sens2
5903        1934.260426             Found ft sensor l_leg_ft_sensor in urdf 
5904        1934.260439             ft_sens0
5905        1934.260452             ft_sens1
5906        1934.260464             ft_sens2
5907        1934.260476             ft_sens3
5908        1934.260488             ft_sens4
5909        1934.260500             Found ft sensor l_foot_ft_sensor in urdf 
5910        1934.260512             ft_sens0
5911        1934.260525             ft_sens1
5912        1934.260536             ft_sens2
5913        1934.260548             ft_sens3
5914        1934.260560             Found ft sensor r_leg_ft_sensor in urdf 
5915        1934.260573             ft_sens0
5916        1934.260584             ft_sens1
5917        1934.260596             ft_sens2
5918        1934.260608             ft_sens3
5919        1934.260621             ft_sens4
5920        1934.260633             ft_sens5
5921        1934.260645             Found ft sensor r_foot_ft_sensor in urdf 
5922        1934.260657     INFO         TorqueEstimationTree constructor: loaded urdf with 29dofs and 6 fts ( 6) 
5923        1934.260671     DEBUG        TorqueEstimationTree: Setting id of dof torso_pitch to 0
5924        1934.260685     DEBUG        TorqueEstimationTree: Setting id of dof torso_roll to 1
5925        1934.260699     DEBUG        TorqueEstimationTree: Setting id of dof torso_yaw to 2
5926        1934.260712     DEBUG        TorqueEstimationTree: Setting id of dof l_shoulder_pitch to 3
5927        1934.260726     DEBUG        TorqueEstimationTree: Setting id of dof l_shoulder_roll to 4
5928        1934.260739     DEBUG        TorqueEstimationTree: Setting id of dof l_shoulder_yaw to 5
5929        1934.260753     DEBUG        TorqueEstimationTree: Setting id of dof l_elbow to 6
5930        1934.260778     DEBUG        TorqueEstimationTree: Setting id of dof l_wrist_prosup to 7
5931        1934.260791     DEBUG        TorqueEstimationTree: Setting id of dof l_wrist_pitch to 8
5932        1934.260806     DEBUG        TorqueEstimationTree: Setting id of dof l_wrist_yaw to 9
5933        1934.260819     DEBUG        TorqueEstimationTree: Setting id of dof r_shoulder_pitch to 10
5934        1934.260833     DEBUG        TorqueEstimationTree: Setting id of dof r_shoulder_roll to 11
5935        1934.260846     DEBUG        TorqueEstimationTree: Setting id of dof r_shoulder_yaw to 12
5936        1934.260860     DEBUG        TorqueEstimationTree: Setting id of dof r_elbow to 13
5937        1934.260873     DEBUG        TorqueEstimationTree: Setting id of dof r_wrist_prosup to 14
5938        1934.260887     DEBUG        TorqueEstimationTree: Setting id of dof r_wrist_pitch to 15
5939        1934.260900     DEBUG        TorqueEstimationTree: Setting id of dof r_wrist_yaw to 16
5940        1934.260913     DEBUG        TorqueEstimationTree: Setting id of dof l_hip_pitch to 17
5941        1934.260927     DEBUG        TorqueEstimationTree: Setting id of dof l_hip_roll to 18
5942        1934.260940     DEBUG        TorqueEstimationTree: Setting id of dof l_hip_yaw to 19
5943        1934.260953     DEBUG        TorqueEstimationTree: Setting id of dof l_knee to 20
5944        1934.260966     DEBUG        TorqueEstimationTree: Setting id of dof l_ankle_pitch to 21
5945        1934.260980     DEBUG        TorqueEstimationTree: Setting id of dof l_ankle_roll to 22
5946        1934.260993     DEBUG        TorqueEstimationTree: Setting id of dof r_hip_pitch to 23
5947        1934.261006     DEBUG        TorqueEstimationTree: Setting id of dof r_hip_roll to 24
5948        1934.261019     DEBUG        TorqueEstimationTree: Setting id of dof r_hip_yaw to 25
5949        1934.261033     DEBUG        TorqueEstimationTree: Setting id of dof r_knee to 26
5950        1934.261046     DEBUG        TorqueEstimationTree: Setting id of dof r_ankle_pitch to 27
5951        1934.261059     DEBUG        TorqueEstimationTree: Setting id of dof r_ankle_roll to 28
5952        1934.261073     INFO         TorqueEstimationTree constructor: loaded urdf with 29dofs and 6 fts ( 6) 
5953        1934.261086             
5954        1934.261098     INFO         IDYNTREE_SKINDYNLIB_LINKS correctly loaded
5955        1934.261111     DEBUG        robot_estimation_model->getSubTreeInternalDynamics().size() : 7
5956        1934.261124     DEBUG        torque_estimation_subtrees.size(): 7
5957        1934.261137     INFO         WBD_SUBTREES correctly loaded with 7subtrees
5958        1934.261152     DEBUG        yarpWholeBodyDynamicsEstimator::threadInit() terminet successfully
5959        1934.261165     INFO         autoconnect option found, enabling the autoconnection.
5960        1934.261178             Launching wholeBodyDynamicsThread with name : wholeBodyDynamicsTree and robotName icub and period 10
5961        1934.270355             ft_sens0
5962        1934.270400             Found ft sensor l_arm_ft_sensor in urdf 
5963        1934.270427             ft_sens0
5964        1934.270441             ft_sens1
5965        1934.270480             Found ft sensor r_arm_ft_sensor in urdf 
5966        1934.270505             ft_sens0
5967        1934.270518             ft_sens1
5968        1934.270531             ft_sens2
5969        1934.270544             Found ft sensor l_leg_ft_sensor in urdf 
5970        1934.270556             ft_sens0
5971        1934.270569             ft_sens1
5972        1934.270581             ft_sens2
5973        1934.270594             ft_sens3
5974        1934.270606             ft_sens4
5975        1934.270619             Found ft sensor l_foot_ft_sensor in urdf 
5976        1934.270632             ft_sens0
5977        1934.270644             ft_sens1
5978        1934.270657             ft_sens2
5979        1934.270669             ft_sens3
5980        1934.270681             Found ft sensor r_leg_ft_sensor in urdf 
5981        1934.270694             ft_sens0
5982        1934.270706             ft_sens1
5983        1934.270719             ft_sens2
5984        1934.270731             ft_sens3
5985        1934.270744             ft_sens4
5986        1934.270757             ft_sens5
5987        1934.270769             Found ft sensor r_foot_ft_sensor in urdf 
5988        1934.270782     INFO         TorqueEstimationTree constructor: loaded urdf with 29dofs and 6 fts ( 6) 
5989        1934.270796     DEBUG        TorqueEstimationTree: Setting id of dof torso_pitch to 0
5990        1934.270810     DEBUG        TorqueEstimationTree: Setting id of dof torso_roll to 1
5991        1934.270825     DEBUG        TorqueEstimationTree: Setting id of dof torso_yaw to 2
5992        1934.270839     DEBUG        TorqueEstimationTree: Setting id of dof l_shoulder_pitch to 3
5993        1934.270853     DEBUG        TorqueEstimationTree: Setting id of dof l_shoulder_roll to 4
5994        1934.270867     DEBUG        TorqueEstimationTree: Setting id of dof l_shoulder_yaw to 5
5995        1934.270881     DEBUG        TorqueEstimationTree: Setting id of dof l_elbow to 6
5996        1934.270895     DEBUG        TorqueEstimationTree: Setting id of dof l_wrist_prosup to 7
5997        1934.270909     DEBUG        TorqueEstimationTree: Setting id of dof l_wrist_pitch to 8
5998        1934.270922     DEBUG        TorqueEstimationTree: Setting id of dof l_wrist_yaw to 9
5999        1934.270936     DEBUG        TorqueEstimationTree: Setting id of dof r_shoulder_pitch to 10
6000        1934.270950     DEBUG        TorqueEstimationTree: Setting id of dof r_shoulder_roll to 11
6001        1934.270964     DEBUG        TorqueEstimationTree: Setting id of dof r_shoulder_yaw to 12
6002        1934.270977     DEBUG        TorqueEstimationTree: Setting id of dof r_elbow to 13
6003        1934.270991     DEBUG        TorqueEstimationTree: Setting id of dof r_wrist_prosup to 14
6004        1934.271006     DEBUG        TorqueEstimationTree: Setting id of dof r_wrist_pitch to 15
6005        1934.271034     DEBUG        TorqueEstimationTree: Setting id of dof r_wrist_yaw to 16
6006        1934.271062     DEBUG        TorqueEstimationTree: Setting id of dof l_hip_pitch to 17
6007        1934.271090     DEBUG        TorqueEstimationTree: Setting id of dof l_hip_roll to 18
6008        1934.271119     DEBUG        TorqueEstimationTree: Setting id of dof l_hip_yaw to 19
6009        1934.271147     DEBUG        TorqueEstimationTree: Setting id of dof l_knee to 20
6010        1934.271176     DEBUG        TorqueEstimationTree: Setting id of dof l_ankle_pitch to 21
6011        1934.271204     DEBUG        TorqueEstimationTree: Setting id of dof l_ankle_roll to 22
6012        1934.271232     DEBUG        TorqueEstimationTree: Setting id of dof r_hip_pitch to 23
6013        1934.271259     DEBUG        TorqueEstimationTree: Setting id of dof r_hip_roll to 24
6014        1934.271287     DEBUG        TorqueEstimationTree: Setting id of dof r_hip_yaw to 25
6015        1934.271314     DEBUG        TorqueEstimationTree: Setting id of dof r_knee to 26
6016        1934.271342     DEBUG        TorqueEstimationTree: Setting id of dof r_ankle_pitch to 27
6017        1934.271371     DEBUG        TorqueEstimationTree: Setting id of dof r_ankle_roll to 28
6018        1934.271398     INFO         TorqueEstimationTree constructor: loaded urdf with 29dofs and 6 fts ( 6) 
6019        1934.320747             yarp: Port /wholeBodyDynamicsTree/contacts:o active at tcp://10.0.0.13:10225
6021        1934.361001             yarp: Port /wholeBodyDynamicsTree/right_arm/endEffectorWrench:o active at tcp://10.0.0.13:10226
6022        1934.381132             yarp: Port /wholeBodyDynamicsTree/left_arm/endEffectorWrench:o active at tcp://10.0.0.13:10227
6023        1934.401269             yarp: Port /wholeBodyDynamicsTree/right_leg/endEffectorWrench:o active at tcp://10.0.0.13:10228
6024        1934.421396             yarp: Port /wholeBodyDynamicsTree/left_leg/endEffectorWrench:o active at tcp://10.0.0.13:10229
6025        1934.431457             yarp: Port /wholeBodyDynamicsTree/right_arm/cartesianEndEffectorWrench:o active at tcp://10.0.0.13:10230
6027        1934.461666             yarp: Port /wholeBodyDynamicsTree/left_arm/cartesianEndEffectorWrench:o active at tcp://10.0.0.13:10231
6028        1934.481798             yarp: Port /wholeBodyDynamicsTree/right_leg/cartesianEndEffectorWrench:o active at tcp://10.0.0.13:10232
6029        1934.501921             yarp: Port /wholeBodyDynamicsTree/left_leg/cartesianEndEffectorWrench:o active at tcp://10.0.0.13:10233
6030        1934.511990             yarp: Port /wholeBodyDynamicsTree/base:o active at tcp://10.0.0.13:10234
6071        1936.476183             yarp: Port /wholeBodyDynamicsTree/filtered/inertial:o active at tcp://10.0.0.13:10235
6072        1936.476260     INFO         calibration_support_link is root_link
6073        1936.476295     INFO         wholeBodyDynamicsThread: autoconnect option enabled, autoconnecting.
6074        1936.506358             yarp: Port /wholeBodyDynamicsTree/left_leg/Torques:o active at tcp://10.0.0.13:10236
6075        1936.516419             yarp: Sending output from /wholeBodyDynamicsTree/left_leg/Torques:o to /icub/joint_vsens/left_leg:i using tcp
6076        1936.516480             Success: Added connection from /wholeBodyDynamicsTree/left_leg/Torques:o to tcp://icub/joint_vsens/left_leg:i
6077        1936.536539             yarp: Port /wholeBodyDynamicsTree/torso/Torques:o active at tcp://10.0.0.13:10238
6079        1936.546636             yarp: Sending output from /wholeBodyDynamicsTree/torso/Torques:o to /icub/joint_vsens/torso:i using tcp
6080        1936.546707             Success: Added connection from /wholeBodyDynamicsTree/torso/Torques:o to tcp://icub/joint_vsens/torso:i
6081        1936.566778             yarp: Port /wholeBodyDynamicsTree/right_wrist/Torques:o active at tcp://10.0.0.13:10239
6085        1936.607016             yarp: Port /wholeBodyDynamicsTree/left_arm/Torques:o active at tcp://10.0.0.13:10240
6086        1936.617127             yarp: Sending output from /wholeBodyDynamicsTree/left_arm/Torques:o to /icub/joint_vsens/left_arm:i using tcp
6087        1936.617191             Success: Added connection from /wholeBodyDynamicsTree/left_arm/Torques:o to tcp://icub/joint_vsens/left_arm:i
6089        1936.627187             yarp: Port /wholeBodyDynamicsTree/left_wrist/Torques:o active at tcp://10.0.0.13:10241
6090        1936.667522             yarp: Port /wholeBodyDynamicsTree/right_leg/Torques:o active at tcp://10.0.0.13:10242
6091        1936.687648             yarp: Sending output from /wholeBodyDynamicsTree/right_leg/Torques:o to /icub/joint_vsens/right_leg:i using tcp
6092        1936.687715             Success: Added connection from /wholeBodyDynamicsTree/right_leg/Torques:o to tcp://icub/joint_vsens/right_leg:i
6093        1936.727898             yarp: Port /wholeBodyDynamicsTree/right_arm/Torques:o active at tcp://10.0.0.13:10243
6095        1936.737960             yarp: Sending output from /wholeBodyDynamicsTree/right_arm/Torques:o to /icub/joint_vsens/right_arm:i using tcp
6096        1936.738030             Success: Added connection from /wholeBodyDynamicsTree/right_arm/Torques:o to tcp://icub/joint_vsens/right_arm:i
6097        1936.738063     INFO         wholeBodyDynamicsThread::calibrateOffset called with code all
6098        1936.738096     INFO         wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 0 is 0 0 0 0 0 0
6099        1936.738143     INFO         wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 1 is 0 0 0 0 0 0
6100        1936.738175     INFO         wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 2 is 0 0 0 0 0 0
6101        1936.738205     INFO         wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 3 is 0 0 0 0 0 0
6102        1936.738234     INFO         wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 4 is 0 0 0 0 0 0
6103        1936.738263     INFO         wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 5 is 0 0 0 0 0 0
6104        1936.738292             wholeBodyDynamicsThread::calibrateOffset all called successfully, starting calibration.
6105        1936.738320             wholeBodyDynamicsThread started
6110        1938.671525             yarp: Sending output from /wholeBodyDynamicsTree/contacts:o to /iCubGui/forces using udp
6112        1938.701699             yarp: Sending output from /wholeBodyDynamicsTree/base:o to /iCubGui/base:i using udp
6113        1938.741896             
6114        1938.741937             
6115        1938.741957     INFO         wholeBodyDynamicsThread: complete calibration at system time : 02-03-2015 02:32:04
6116        1938.741980     INFO         wholeBodyDynamicsThread: new calibration for FT l_arm_ft_sensor is 91.1028 -52.7174 -68.7064 -1.91131 -2.10423 -0.279053
6117        1938.742001     INFO         wholeBodyDynamicsThread: new calibration for FT r_arm_ft_sensor is 33.661 -31.9156 33.186 -0.355178 -1.32133 0.289624
6118        1938.742022     INFO         wholeBodyDynamicsThread: new calibration for FT l_leg_ft_sensor is -46.8179 87.8542 -33.5182 1.51166 2.66414 0.163925
6119        1938.742042     INFO         wholeBodyDynamicsThread: new calibration for FT l_foot_ft_sensor is 58.6568 -4.35052 1.64551 -0.194919 -0.805118 -0.00366789
6120        1938.742064     INFO         wholeBodyDynamicsThread: new calibration for FT r_leg_ft_sensor is 2.48172 -18.4639 -4.75735 -0.0826843 1.77764 -0.176346
6121        1938.742084     INFO         wholeBodyDynamicsThread: new calibration for FT r_foot_ft_sensor is -70.7107 -27.9261 2.22172 -0.971011 1.30368 -0.0680085
6209        1940.000552             iDynTree WARNING: base_residual_f.force.Norm() is 0.0343111 instead of zero.
6210        1940.000591             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0458203 instead of zero.
6868        2041.234355             wholeBodyDynamicsTree: double support calibration for allrequested
6869        2041.234418             wholeBodyDynamicsThread::calibrateOffsetOnDoubleSupport called with code all
6870        2041.234455             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 0 is 91.1028 -52.7174 -68.7064 -1.91131 -2.10423 -0.279053
6871        2041.234485             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 1 is 33.661 -31.9156 33.186 -0.355178 -1.32133 0.289624
6872        2041.234515             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 2 is -46.8179 87.8542 -33.5182 1.51166 2.66414 0.163925
6873        2041.234545             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 3 is 58.6568 -4.35052 1.64551 -0.194919 -0.805118 -0.00366789
6874        2041.234573             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 4 is 2.48172 -18.4639 -4.75735 -0.0826843 1.77764 -0.176346
6875        2041.234603             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 5 is -70.7107 -27.9261 2.22172 -0.971011 1.30368 -0.0680085
6876        2041.234632             wholeBodyDynamicsThread::calibrateOffset all called successfully, starting calibration.
6914        2047.279113             wholeBodyDynamicsThread: new calibration for FT l_arm_ft_sensor is 88.0644 -53.205 -69.5055 -1.82014 -2.33024 -0.285872
6915        2047.279179             wholeBodyDynamicsThread: new calibration for FT r_arm_ft_sensor is 30.8511 -31.5332 31.1677 -0.410836 -1.55604 0.289628
6916        2047.279215             wholeBodyDynamicsThread: new calibration for FT l_leg_ft_sensor is -67.5977 103.514 -24.6752 -2.59894 1.9004 -1.0788
6917        2047.279245             wholeBodyDynamicsThread: new calibration for FT l_foot_ft_sensor is 47.0691 10.4442 11.1176 -0.0815548 0.436936 -0.68926
6918        2047.279274             wholeBodyDynamicsThread: new calibration for FT r_leg_ft_sensor is -20.0596 -29.0349 8.44354 3.05609 -0.599277 0.63224
6919        2047.279302             wholeBodyDynamicsThread: new calibration for FT r_foot_ft_sensor is -84.5001 -38.4989 17.279 -1.30366 2.03186 0.294189
6920        2047.279330             wholeBodyDynamicsThread::waitCalibrationDone() returning: calibration finished with success
7995        2184.475413             wholeBodyDynamicsTree: double support calibration for allrequested
7996        2184.475484             wholeBodyDynamicsThread::calibrateOffsetOnDoubleSupport called with code all
7997        2184.475521             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 0 is 88.0644 -53.205 -69.5055 -1.82014 -2.33024 -0.285872
7998        2184.475554             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 1 is 30.8511 -31.5332 31.1677 -0.410836 -1.55604 0.289628
7999        2184.475585             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 2 is -67.5977 103.514 -24.6752 -2.59894 1.9004 -1.0788
8000        2184.485471             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 3 is 47.0691 10.4442 11.1176 -0.0815548 0.436936 -0.68926
8001        2184.485522             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 4 is -20.0596 -29.0349 8.44354 3.05609 -0.599277 0.63224
8002        2184.485556             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 5 is -84.5001 -38.4989 17.279 -1.30366 2.03186 0.294189
8003        2184.485587             wholeBodyDynamicsThread::calibrateOffset all called successfully, starting calibration.
8040        2190.522651             wholeBodyDynamicsThread: new calibration for FT l_arm_ft_sensor is 87.9432 -53.1808 -69.4791 -1.82415 -2.32523 -0.280507
8041        2190.522728             wholeBodyDynamicsThread: new calibration for FT r_arm_ft_sensor is 30.7176 -31.7362 31.102 -0.420121 -1.54859 0.286744
8042        2190.522768             wholeBodyDynamicsThread: new calibration for FT l_leg_ft_sensor is -70.9365 99.6913 -25.4705 -2.40044 0.499882 -0.838334
8043        2190.522799             wholeBodyDynamicsThread: new calibration for FT l_foot_ft_sensor is 46.1861 8.63518 10.111 -0.213449 -0.531381 -0.536147
8044        2190.522828             wholeBodyDynamicsThread: new calibration for FT r_leg_ft_sensor is -16.5206 -29.2788 7.96299 2.45994 0.591832 0.736375
8045        2190.522857             wholeBodyDynamicsThread: new calibration for FT r_foot_ft_sensor is -83.8359 -38.7742 16.271 -2.11016 2.39607 0.455284
8046        2190.522888             wholeBodyDynamicsThread::waitCalibrationDone() returning: calibration finished with success
8122        2199.875776             wholeBodyDynamicsTree: double support calibration for allrequested
8123        2199.875846             wholeBodyDynamicsThread::calibrateOffsetOnDoubleSupport called with code all
8124        2199.875883             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 0 is 87.9432 -53.1808 -69.4791 -1.82415 -2.32523 -0.280507
8125        2199.875914             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 1 is 30.7176 -31.7362 31.102 -0.420121 -1.54859 0.286744
8126        2199.875944             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 2 is -70.9365 99.6913 -25.4705 -2.40044 0.499882 -0.838334
8127        2199.875975             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 3 is 46.1861 8.63518 10.111 -0.213449 -0.531381 -0.536147
8128        2199.876004             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 4 is -16.5206 -29.2788 7.96299 2.45994 0.591832 0.736375
8129        2199.876033             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 5 is -83.8359 -38.7742 16.271 -2.11016 2.39607 0.455284
8130        2199.876061             wholeBodyDynamicsThread::calibrateOffset all called successfully, starting calibration.
8167        2205.909726             wholeBodyDynamicsThread: new calibration for FT l_arm_ft_sensor is 87.7366 -53.115 -69.3648 -1.8188 -2.3214 -0.28113
8168        2205.909779             wholeBodyDynamicsThread: new calibration for FT r_arm_ft_sensor is 30.6108 -31.6748 30.9877 -0.41997 -1.54649 0.286842
8169        2205.909797             wholeBodyDynamicsThread: new calibration for FT l_leg_ft_sensor is -71.026 99.2556 -25.1733 -2.43201 0.569262 -0.661171
8170        2205.909826             wholeBodyDynamicsThread: new calibration for FT l_foot_ft_sensor is 46.1612 8.13349 10.413 -0.0355885 -0.43981 -0.352161
8171        2205.909856             wholeBodyDynamicsThread: new calibration for FT r_leg_ft_sensor is -16.3299 -29.025 7.56776 2.59738 0.516309 0.575016
8172        2205.909891             wholeBodyDynamicsThread: new calibration for FT r_foot_ft_sensor is -83.3604 -38.4449 15.9075 -2.09434 2.22857 0.295197
8173        2205.909904             wholeBodyDynamicsThread::waitCalibrationDone() returning: calibration finished with success
10280       2539.964340             wholeBodyDynamicsTree: double support calibration for allrequested
10281       2539.964412             wholeBodyDynamicsThread::calibrateOffsetOnDoubleSupport called with code all
10282       2539.964449             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 0 is 87.7366 -53.115 -69.3648 -1.8188 -2.3214 -0.28113
10283       2539.964482             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 1 is 30.6108 -31.6748 30.9877 -0.41997 -1.54649 0.286842
10284       2539.964513             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 2 is -71.026 99.2556 -25.1733 -2.43201 0.569262 -0.661171
10285       2539.964543             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 3 is 46.1612 8.13349 10.413 -0.0355885 -0.43981 -0.352161
10286       2539.964573             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 4 is -16.3299 -29.025 7.56776 2.59738 0.516309 0.575016
10287       2539.964602             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 5 is -83.3604 -38.4449 15.9075 -2.09434 2.22857 0.295197
10288       2539.964631             wholeBodyDynamicsThread::calibrateOffset all called successfully, starting calibration.
10326       2546.005250             wholeBodyDynamicsThread: new calibration for FT l_arm_ft_sensor is 87.0239 -52.8015 -68.7374 -1.80903 -2.28887 -0.284209
10327       2546.005322             wholeBodyDynamicsThread: new calibration for FT r_arm_ft_sensor is 30.5302 -32.0236 30.0272 -0.422248 -1.52667 0.288364
10328       2546.005356             wholeBodyDynamicsThread: new calibration for FT l_leg_ft_sensor is -71.0156 98.2011 -27.4324 -2.50149 0.413867 -0.478831
10329       2546.005385             wholeBodyDynamicsThread: new calibration for FT l_foot_ft_sensor is 46.6538 7.97054 9.06645 -0.0601097 -0.683076 -0.181356
10330       2546.005414             wholeBodyDynamicsThread: new calibration for FT r_leg_ft_sensor is -16.2101 -28.9667 7.52897 2.6964 0.434997 0.385001
10331       2546.005442             wholeBodyDynamicsThread: new calibration for FT r_foot_ft_sensor is -83.3633 -37.9535 17.0794 -2.09678 2.23292 0.0964969
10332       2546.005470             wholeBodyDynamicsThread::waitCalibrationDone() returning: calibration finished with success
13320       2997.128995             wholeBodyDynamicsTree: double support calibration for allrequested
13321       2997.129070             wholeBodyDynamicsThread::calibrateOffsetOnDoubleSupport called with code all
13322       2997.129107             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 0 is 87.0239 -52.8015 -68.7374 -1.80903 -2.28887 -0.284209
13323       2997.129138             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 1 is 30.5302 -32.0236 30.0272 -0.422248 -1.52667 0.288364
13324       2997.129167             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 2 is -71.0156 98.2011 -27.4324 -2.50149 0.413867 -0.478831
13325       2997.129196             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 3 is 46.6538 7.97054 9.06645 -0.0601097 -0.683076 -0.181356
13326       2997.129225             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 4 is -16.2101 -28.9667 7.52897 2.6964 0.434997 0.385001
13327       2997.129253             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 5 is -83.3633 -37.9535 17.0794 -2.09678 2.23292 0.0964969
13328       2997.129283             wholeBodyDynamicsThread::calibrateOffset all called successfully, starting calibration.
13365       3003.172884             wholeBodyDynamicsThread: new calibration for FT l_arm_ft_sensor is 86.6025 -52.5636 -69.24 -1.79334 -2.27758 -0.288863
13366       3003.172963             wholeBodyDynamicsThread: new calibration for FT r_arm_ft_sensor is 30.7161 -32.8977 28.2383 -0.406433 -1.53027 0.275779
13367       3003.172999             wholeBodyDynamicsThread: new calibration for FT l_leg_ft_sensor is -80.8481 94.6941 -25.7083 -2.5429 -1.84164 0.279946
13368       3003.173029             wholeBodyDynamicsThread: new calibration for FT l_foot_ft_sensor is 40.4045 6.46067 13.0176 0.568884 -0.527438 0.528973
13369       3003.173061             wholeBodyDynamicsThread: new calibration for FT r_leg_ft_sensor is -7.58666 -30.7909 2.0324 3.27865 2.97141 1.10992
13370       3003.173090             wholeBodyDynamicsThread: new calibration for FT r_foot_ft_sensor is -78.1765 -39.7729 12.5249 -1.02669 2.26358 0.666582
13371       3003.173118             wholeBodyDynamicsThread::waitCalibrationDone() returning: calibration finished with success
13437       3008.228485             wholeBodyDynamicsTree: double support calibration for allrequested
13438       3008.228562             wholeBodyDynamicsThread::calibrateOffsetOnDoubleSupport called with code all
13439       3008.228597             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 0 is 86.6025 -52.5636 -69.24 -1.79334 -2.27758 -0.288863
13440       3008.228628             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 1 is 30.7161 -32.8977 28.2383 -0.406433 -1.53027 0.275779
13441       3008.228658             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 2 is -80.8481 94.6941 -25.7083 -2.5429 -1.84164 0.279946
13442       3008.228687             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 3 is 40.4045 6.46067 13.0176 0.568884 -0.527438 0.528973
13443       3008.228715             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 4 is -7.58666 -30.7909 2.0324 3.27865 2.97141 1.10992
13444       3008.228745             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 5 is -78.1765 -39.7729 12.5249 -1.02669 2.26358 0.666582
13445       3008.228774             wholeBodyDynamicsThread::calibrateOffset all called successfully, starting calibration.
13482       3014.262644             wholeBodyDynamicsThread: new calibration for FT l_arm_ft_sensor is 86.6202 -52.5222 -69.2299 -1.79474 -2.27605 -0.288873
13483       3014.262720             wholeBodyDynamicsThread: new calibration for FT r_arm_ft_sensor is 30.761 -32.9513 28.2682 -0.405163 -1.52891 0.275554
13484       3014.262756             wholeBodyDynamicsThread: new calibration for FT l_leg_ft_sensor is -78.0645 96.2916 -25.2536 -2.53886 -1.14209 0.0213569
13485       3014.262786             wholeBodyDynamicsThread: new calibration for FT l_foot_ft_sensor is 42.1025 7.16972 13.5185 0.611534 -0.4733 0.282432
13486       3014.262817             wholeBodyDynamicsThread: new calibration for FT r_leg_ft_sensor is -9.45111 -30.7172 1.47189 3.38058 2.26882 0.996199
13487       3014.262845             wholeBodyDynamicsThread: new calibration for FT r_foot_ft_sensor is -80.0318 -39.7732 12.0817 -0.98113 2.29598 0.542562
13488       3014.262873             wholeBodyDynamicsThread::waitCalibrationDone() returning: calibration finished with success
15669       3220.539393             wholeBodyDynamicsTree: double support calibration for allrequested
15670       3220.539446             wholeBodyDynamicsThread::calibrateOffsetOnDoubleSupport called with code all
15671       3220.539469             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 0 is 86.6202 -52.5222 -69.2299 -1.79474 -2.27605 -0.288873
15672       3220.539490             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 1 is 30.761 -32.9513 28.2682 -0.405163 -1.52891 0.275554
15673       3220.539511             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 2 is -78.0645 96.2916 -25.2536 -2.53886 -1.14209 0.0213569
15674       3220.539530             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 3 is 42.1025 7.16972 13.5185 0.611534 -0.4733 0.282432
15675       3220.539550             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 4 is -9.45111 -30.7172 1.47189 3.38058 2.26882 0.996199
15676       3220.539570             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 5 is -80.0318 -39.7732 12.0817 -0.98113 2.29598 0.542562
15677       3220.539591             wholeBodyDynamicsThread::calibrateOffset all called successfully, starting calibration.
15716       3226.580366             wholeBodyDynamicsThread: new calibration for FT l_arm_ft_sensor is 86.1498 -52.781 -69.2734 -1.79148 -2.25162 -0.271546
15717       3226.580415             wholeBodyDynamicsThread: new calibration for FT r_arm_ft_sensor is 30.5523 -32.7766 27.5696 -0.414261 -1.525 0.268141
15718       3226.580439             wholeBodyDynamicsThread: new calibration for FT l_leg_ft_sensor is -87.6822 89.4537 -28.3197 -2.52689 -3.91893 0.609836
15719       3226.580461             wholeBodyDynamicsThread: new calibration for FT l_foot_ft_sensor is 36.5612 2.93867 12.0023 0.254358 -0.831326 0.857185
15720       3226.580482             wholeBodyDynamicsThread: new calibration for FT r_leg_ft_sensor is -3.41007 -33.4828 2.79187 2.83381 4.44724 1.64784
15721       3226.580501             wholeBodyDynamicsThread: new calibration for FT r_foot_ft_sensor is -73.4684 -42.675 14.3607 -1.6697 2.03471 1.24827
15722       3226.580520             wholeBodyDynamicsThread::waitCalibrationDone() returning: calibration finished with success
16229       3297.592604             iDynTree WARNING: base_residual_f.force.Norm() is 0.00392634 instead of zero.
16230       3297.592669             iDynTree WARNING: base_residual_f.torque.Norm() is 0.00710948 instead of zero.
16231       3297.663275             iDynTree WARNING: base_residual_f.force.Norm() is 0.00102881 instead of zero.
16232       3297.663324             iDynTree WARNING: base_residual_f.torque.Norm() is 0.000904643 instead of zero.
16233       3297.673379             iDynTree WARNING: base_residual_f.force.Norm() is 0.335852 instead of zero.
16234       3297.673426             iDynTree WARNING: base_residual_f.torque.Norm() is 0.139972 instead of zero.
16235       3297.683483             iDynTree WARNING: base_residual_f.force.Norm() is 0.251739 instead of zero.
16236       3297.683530             iDynTree WARNING: base_residual_f.torque.Norm() is 0.157853 instead of zero.
16237       3297.693544             iDynTree WARNING: base_residual_f.force.Norm() is 0.389373 instead of zero.
16238       3297.693587             iDynTree WARNING: base_residual_f.torque.Norm() is 0.123279 instead of zero.
16239       3297.703628             iDynTree WARNING: base_residual_f.force.Norm() is 0.214808 instead of zero.
16240       3297.703674             iDynTree WARNING: base_residual_f.torque.Norm() is 0.383052 instead of zero.
16309       3298.036199             iDynTree WARNING: base_residual_f.force.Norm() is 3.41654e-05 instead of zero.
16310       3298.036220             iDynTree WARNING: base_residual_f.torque.Norm() is 1.97148e-05 instead of zero.
16329       3298.046269             iDynTree WARNING: base_residual_f.force.Norm() is 2.7715e-05 instead of zero.
16330       3298.046291             iDynTree WARNING: base_residual_f.torque.Norm() is 2.38356e-05 instead of zero.
16345       3298.056305             iDynTree WARNING: base_residual_f.force.Norm() is 1.52394e-05 instead of zero.
16346       3298.056327             iDynTree WARNING: base_residual_f.torque.Norm() is 1.89207e-05 instead of zero.
16367       3298.066363             iDynTree WARNING: base_residual_f.force.Norm() is 2.18482e-05 instead of zero.
16369       3298.066417             iDynTree WARNING: base_residual_f.torque.Norm() is 1.34609e-05 instead of zero.
17928       3434.528421             wholeBodyDynamicsTree: double support calibration for allrequested
17929       3434.528484             wholeBodyDynamicsThread::calibrateOffsetOnDoubleSupport called with code all
17930       3434.528517             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 0 is 86.1498 -52.781 -69.2734 -1.79148 -2.25162 -0.271546
17931       3434.528548             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 1 is 30.5523 -32.7766 27.5696 -0.414261 -1.525 0.268141
17932       3434.528579             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 2 is -87.6822 89.4537 -28.3197 -2.52689 -3.91893 0.609836
17933       3434.528608             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 3 is 36.5612 2.93867 12.0023 0.254358 -0.831326 0.857185
17934       3434.528638             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 4 is -3.41007 -33.4828 2.79187 2.83381 4.44724 1.64784
17935       3434.528667             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 5 is -73.4684 -42.675 14.3607 -1.6697 2.03471 1.24827
17936       3434.528699             wholeBodyDynamicsThread::calibrateOffset all called successfully, starting calibration.
17974       3440.573279             wholeBodyDynamicsThread: new calibration for FT l_arm_ft_sensor is 86.5122 -52.3465 -69.9124 -1.79196 -2.24987 -0.268286
17975       3440.573358             wholeBodyDynamicsThread: new calibration for FT r_arm_ft_sensor is 30.3697 -32.7637 26.7658 -0.4205 -1.51706 0.276901
17976       3440.573392             wholeBodyDynamicsThread: new calibration for FT l_leg_ft_sensor is -74.5813 95.5928 -27.5478 -2.83115 -0.544785 -0.106335
17977       3440.573422             wholeBodyDynamicsThread: new calibration for FT l_foot_ft_sensor is 44.8372 7.0451 12.2406 -0.466043 -0.759768 0.216391
17978       3440.573451             wholeBodyDynamicsThread: new calibration for FT r_leg_ft_sensor is -12.0734 -29.7526 1.11338 3.19766 1.65892 0.608347
17979       3440.573480             wholeBodyDynamicsThread: new calibration for FT r_foot_ft_sensor is -81.3243 -39.3712 13.8634 -1.088 2.63308 0.13879
17980       3440.573509             wholeBodyDynamicsThread::waitCalibrationDone() returning: calibration finished with success
19558       3659.766465             wholeBodyDynamicsTree: double support calibration for allrequested
19559       3659.766531             wholeBodyDynamicsThread::calibrateOffsetOnDoubleSupport called with code all
19560       3659.766565             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 0 is 86.5122 -52.3465 -69.9124 -1.79196 -2.24987 -0.268286
19561       3659.766596             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 1 is 30.3697 -32.7637 26.7658 -0.4205 -1.51706 0.276901
19562       3659.766626             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 2 is -74.5813 95.5928 -27.5478 -2.83115 -0.544785 -0.106335
19563       3659.766658             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 3 is 44.8372 7.0451 12.2406 -0.466043 -0.759768 0.216391
19564       3659.766688             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 4 is -12.0734 -29.7526 1.11338 3.19766 1.65892 0.608347
19565       3659.766736             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 5 is -81.3243 -39.3712 13.8634 -1.088 2.63308 0.13879
19566       3659.766775             wholeBodyDynamicsThread::calibrateOffset all called successfully, starting calibration.
19603       3665.808946             wholeBodyDynamicsThread: new calibration for FT l_arm_ft_sensor is 86.3934 -51.9672 -69.8976 -1.79591 -2.2448 -0.278782
19604       3665.809021             wholeBodyDynamicsThread: new calibration for FT r_arm_ft_sensor is 30.3667 -32.4302 25.8671 -0.431135 -1.50948 0.268802
19605       3665.809063             wholeBodyDynamicsThread: new calibration for FT l_leg_ft_sensor is -78.4822 91.8407 -24.1967 -2.65119 -1.06919 0.0853157
19606       3665.809094             wholeBodyDynamicsThread: new calibration for FT l_foot_ft_sensor is 40.62 4.31087 15.2289 -0.799882 0.0226877 0.45789
19607       3665.809125             wholeBodyDynamicsThread: new calibration for FT r_leg_ft_sensor is -8.27432 -30.2145 -3.45959 3.1902 2.19802 0.934973
19608       3665.809154             wholeBodyDynamicsThread: new calibration for FT r_foot_ft_sensor is -79.6656 -42.8609 8.71358 -0.836884 1.9707 0.523471
19609       3665.809181             wholeBodyDynamicsThread::waitCalibrationDone() returning: calibration finished with success
20927       3844.258273             wholeBodyDynamicsTree: double support calibration for allrequested
20928       3844.258344             wholeBodyDynamicsThread::calibrateOffsetOnDoubleSupport called with code all
20929       3844.258380             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 0 is 86.3934 -51.9672 -69.8976 -1.79591 -2.2448 -0.278782
20930       3844.258421             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 1 is 30.3667 -32.4302 25.8671 -0.431135 -1.50948 0.268802
20931       3844.258451             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 2 is -78.4822 91.8407 -24.1967 -2.65119 -1.06919 0.0853157
20932       3844.258480             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 3 is 40.62 4.31087 15.2289 -0.799882 0.0226877 0.45789
20933       3844.258509             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 4 is -8.27432 -30.2145 -3.45959 3.1902 2.19802 0.934973
20934       3844.258539             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 5 is -79.6656 -42.8609 8.71358 -0.836884 1.9707 0.523471
20935       3844.258568             wholeBodyDynamicsThread::calibrateOffset all called successfully, starting calibration.
20973       3850.301974             wholeBodyDynamicsThread: new calibration for FT l_arm_ft_sensor is 86.5354 -52.1563 -70.0679 -1.7789 -2.2245 -0.277068
20974       3850.302038             wholeBodyDynamicsThread: new calibration for FT r_arm_ft_sensor is 30.5504 -32.6978 25.4789 -0.420908 -1.51285 0.266787
20975       3850.302074             wholeBodyDynamicsThread: new calibration for FT l_leg_ft_sensor is -72.3257 88.0663 -22.7391 -1.01722 0.610151 -0.094568
20976       3850.302104             wholeBodyDynamicsThread: new calibration for FT l_foot_ft_sensor is 45.332 0.752627 16.696 0.192856 0.0340841 0.127107
20977       3850.302150             wholeBodyDynamicsThread: new calibration for FT r_leg_ft_sensor is -12.7459 -21.7865 -5.45735 2.08991 0.613114 0.193389
20978       3850.302179             wholeBodyDynamicsThread: new calibration for FT r_foot_ft_sensor is -83.7756 -36.6376 7.30583 -0.940685 2.18813 -0.0775038
20979       3850.302207             wholeBodyDynamicsThread::waitCalibrationDone() returning: calibration finished with success
22889       3974.886886             wholeBodyDynamicsTree: double support calibration for allrequested
22890       3974.886960             wholeBodyDynamicsThread::calibrateOffsetOnDoubleSupport called with code all
22891       3974.886996             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 0 is 86.5354 -52.1563 -70.0679 -1.7789 -2.2245 -0.277068
22892       3974.887027             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 1 is 30.5504 -32.6978 25.4789 -0.420908 -1.51285 0.266787
22893       3974.887058             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 2 is -72.3257 88.0663 -22.7391 -1.01722 0.610151 -0.094568
22894       3974.887088             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 3 is 45.332 0.752627 16.696 0.192856 0.0340841 0.127107
22895       3974.887126             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 4 is -12.7459 -21.7865 -5.45735 2.08991 0.613114 0.193389
22896       3974.887159             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 5 is -83.7756 -36.6376 7.30583 -0.940685 2.18813 -0.0775038
22897       3974.887191             wholeBodyDynamicsThread::calibrateOffset all called successfully, starting calibration.
22935       3980.928920             wholeBodyDynamicsThread: new calibration for FT l_arm_ft_sensor is 86.6923 -51.9469 -70.4155 -1.78291 -2.22071 -0.276727
22936       3980.928996             wholeBodyDynamicsThread: new calibration for FT r_arm_ft_sensor is 30.6366 -32.7035 25.2572 -0.428141 -1.50838 0.269714
22937       3980.929029             wholeBodyDynamicsThread: new calibration for FT l_leg_ft_sensor is -70.8587 92.3727 -23.3701 -2.0679 1.42498 -0.040315
22938       3980.929059             wholeBodyDynamicsThread: new calibration for FT l_foot_ft_sensor is 44.8351 3.02422 16.4965 -0.261118 0.941304 0.366488
22939       3980.929090             wholeBodyDynamicsThread: new calibration for FT r_leg_ft_sensor is -12.8575 -27.5934 -4.34597 2.93998 0.0409951 -0.0420076
22940       3980.929117             wholeBodyDynamicsThread: new calibration for FT r_foot_ft_sensor is -83.7588 -37.2006 8.17247 -1.00996 1.84077 -0.399986
22941       3980.929144             wholeBodyDynamicsThread::waitCalibrationDone() returning: calibration finished with success
23692       4063.252002             wholeBodyDynamicsTree: double support calibration for allrequested
23693       4063.252053             wholeBodyDynamicsThread::calibrateOffsetOnDoubleSupport called with code all
23694       4063.252078             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 0 is 86.6923 -51.9469 -70.4155 -1.78291 -2.22071 -0.276727
23695       4063.252099             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 1 is 30.6366 -32.7035 25.2572 -0.428141 -1.50838 0.269714
23696       4063.252120             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 2 is -70.8587 92.3727 -23.3701 -2.0679 1.42498 -0.040315
23697       4063.252140             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 3 is 44.8351 3.02422 16.4965 -0.261118 0.941304 0.366488
23698       4063.252160             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 4 is -12.8575 -27.5934 -4.34597 2.93998 0.0409951 -0.0420076
23699       4063.252179             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 5 is -83.7588 -37.2006 8.17247 -1.00996 1.84077 -0.399986
23700       4063.252199             wholeBodyDynamicsThread::calibrateOffset all called successfully, starting calibration.
23738       4069.293001             wholeBodyDynamicsThread: new calibration for FT l_arm_ft_sensor is 86.8084 -52.0022 -70.2978 -1.77696 -2.2071 -0.271679
23739       4069.293051             wholeBodyDynamicsThread: new calibration for FT r_arm_ft_sensor is 31.9814 -32.642 24.7182 -0.430511 -1.51539 0.244115
23740       4069.293076             wholeBodyDynamicsThread: new calibration for FT l_leg_ft_sensor is -66.0457 98.2916 -26.777 -3.13949 1.85782 -0.860239
23741       4069.293097             wholeBodyDynamicsThread: new calibration for FT l_foot_ft_sensor is 48.2287 8.65654 13.2892 -0.635649 -0.0344737 -0.425573
23742       4069.293117             wholeBodyDynamicsThread: new calibration for FT r_leg_ft_sensor is -18.4307 -30.2658 -2.54269 3.57425 -0.462239 0.151807
23743       4069.293136             wholeBodyDynamicsThread: new calibration for FT r_foot_ft_sensor is -87.1166 -38.6553 10.9283 -1.40209 2.87186 -0.365333
23744       4069.293154             wholeBodyDynamicsThread::waitCalibrationDone() returning: calibration finished with success
24919       4134.380551             wholeBodyDynamicsTree: double support calibration for allrequested
24920       4134.380619             wholeBodyDynamicsThread::calibrateOffsetOnDoubleSupport called with code all
24921       4134.380651             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 0 is 86.8084 -52.0022 -70.2978 -1.77696 -2.2071 -0.271679
24922       4134.380683             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 1 is 31.9814 -32.642 24.7182 -0.430511 -1.51539 0.244115
24923       4134.380713             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 2 is -66.0457 98.2916 -26.777 -3.13949 1.85782 -0.860239
24924       4134.380742             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 3 is 48.2287 8.65654 13.2892 -0.635649 -0.0344737 -0.425573
24925       4134.380771             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 4 is -18.4307 -30.2658 -2.54269 3.57425 -0.462239 0.151807
24926       4134.380799             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 5 is -87.1166 -38.6553 10.9283 -1.40209 2.87186 -0.365333
24927       4134.380828             wholeBodyDynamicsThread::calibrateOffset all called successfully, starting calibration.
24964       4140.414654             wholeBodyDynamicsThread: new calibration for FT l_arm_ft_sensor is 86.8363 -52.0506 -70.6599 -1.77918 -2.21488 -0.27404
24965       4140.414723             wholeBodyDynamicsThread: new calibration for FT r_arm_ft_sensor is 30.6592 -32.2656 24.3032 -0.426614 -1.51218 0.279148
24966       4140.414757             wholeBodyDynamicsThread: new calibration for FT l_leg_ft_sensor is -85.5611 91.2601 -30.5421 -2.82352 -2.51499 0.585102
24967       4140.414788             wholeBodyDynamicsThread: new calibration for FT l_foot_ft_sensor is 36.0248 4.73059 10.5324 -0.804638 0.228641 0.950039
24968       4140.414816             wholeBodyDynamicsThread: new calibration for FT r_leg_ft_sensor is -1.78132 -35.4378 0.303305 2.52928 3.84025 1.19163
24969       4140.414847             wholeBodyDynamicsThread: new calibration for FT r_foot_ft_sensor is -74.9921 -39.4928 13.4877 -1.78234 2.09541 0.851447
24970       4140.414876             wholeBodyDynamicsThread::waitCalibrationDone() returning: calibration finished with success
25822       4228.315573             wholeBodyDynamicsTree: double support calibration for allrequested
25823       4228.315640             wholeBodyDynamicsThread::calibrateOffsetOnDoubleSupport called with code all
25824       4228.315674             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 0 is 86.8363 -52.0506 -70.6599 -1.77918 -2.21488 -0.27404
25825       4228.315706             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 1 is 30.6592 -32.2656 24.3032 -0.426614 -1.51218 0.279148
25826       4228.315735             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 2 is -85.5611 91.2601 -30.5421 -2.82352 -2.51499 0.585102
25827       4228.315764             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 3 is 36.0248 4.73059 10.5324 -0.804638 0.228641 0.950039
25828       4228.315792             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 4 is -1.78132 -35.4378 0.303305 2.52928 3.84025 1.19163
25829       4228.315822             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 5 is -74.9921 -39.4928 13.4877 -1.78234 2.09541 0.851447
25830       4228.315851             wholeBodyDynamicsThread::calibrateOffset all called successfully, starting calibration.
25867       4234.351070             wholeBodyDynamicsThread: new calibration for FT l_arm_ft_sensor is 86.2245 -52.143 -70.2862 -1.77826 -2.20531 -0.262596
25868       4234.351126             wholeBodyDynamicsThread: new calibration for FT r_arm_ft_sensor is 31.3552 -32.9729 23.9962 -0.430733 -1.50667 0.255835
25869       4234.361131             wholeBodyDynamicsThread: new calibration for FT l_leg_ft_sensor is -73.014 96.4218 -27.0901 -2.64882 1.00059 -0.333447
25870       4234.361190             wholeBodyDynamicsThread: new calibration for FT l_foot_ft_sensor is 42.8204 6.03817 13.7261 0.527915 1.31449 0.142523
25871       4234.361223             wholeBodyDynamicsThread: new calibration for FT r_leg_ft_sensor is -11.5704 -34.5324 -4.25469 3.20647 0.597972 0.508916
25872       4234.361255             wholeBodyDynamicsThread: new calibration for FT r_foot_ft_sensor is -84.024 -40.2746 10.1406 -1.85953 1.94022 0.163846
25873       4234.361353             wholeBodyDynamicsThread::waitCalibrationDone() returning: calibration finished with success
26187       4280.883518             wholeBodyDynamicsTree: double support calibration for allrequested
26188       4280.883586             wholeBodyDynamicsThread::calibrateOffsetOnDoubleSupport called with code all
26189       4280.883618             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 0 is 86.2245 -52.143 -70.2862 -1.77826 -2.20531 -0.262596
26190       4280.883649             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 1 is 31.3552 -32.9729 23.9962 -0.430733 -1.50667 0.255835
26191       4280.883679             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 2 is -73.014 96.4218 -27.0901 -2.64882 1.00059 -0.333447
26192       4280.883707             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 3 is 42.8204 6.03817 13.7261 0.527915 1.31449 0.142523
26193       4280.883737             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 4 is -11.5704 -34.5324 -4.25469 3.20647 0.597972 0.508916
26194       4280.883766             wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 5 is -84.024 -40.2746 10.1406 -1.85953 1.94022 0.163846
26195       4280.883795             wholeBodyDynamicsThread::calibrateOffset all called successfully, starting calibration.
26232       4286.933913             wholeBodyDynamicsThread: new calibration for FT l_arm_ft_sensor is 86.2292 -52.0923 -70.3984 -1.77807 -2.20487 -0.262698
26233       4286.933982             wholeBodyDynamicsThread: new calibration for FT r_arm_ft_sensor is 31.3144 -32.925 23.8588 -0.431037 -1.50778 0.25745
26234       4286.934015             wholeBodyDynamicsThread: new calibration for FT l_leg_ft_sensor is -74.0462 94.2887 -30.001 -2.8772 1.05466 -0.0401865
26235       4286.934046             wholeBodyDynamicsThread: new calibration for FT l_foot_ft_sensor is 41.4953 5.55383 11.2128 0.0994219 1.67664 0.489181
26236       4286.934075             wholeBodyDynamicsThread: new calibration for FT r_leg_ft_sensor is -10.6402 -34.0553 -1.8598 3.24487 0.577511 0.304635
26237       4286.934106             wholeBodyDynamicsThread: new calibration for FT r_foot_ft_sensor is -82.7832 -40.0498 12.4959 -1.82982 1.54662 -0.0111047
26238       4286.934148             wholeBodyDynamicsThread::waitCalibrationDone() returning: calibration finished with success
36166       5882.655102             iDynTree WARNING: base_residual_f.force.Norm() is 12.9997 instead of zero.
36167       5882.655184             iDynTree WARNING: base_residual_f.torque.Norm() is 19.8536 instead of zero.
36168       5882.665201             iDynTree WARNING: base_residual_f.force.Norm() is 109.825 instead of zero.
36169       5882.665266             iDynTree WARNING: base_residual_f.torque.Norm() is 98.9868 instead of zero.
36170       5882.675308             iDynTree WARNING: base_residual_f.force.Norm() is 413.086 instead of zero.
36171       5882.675379             iDynTree WARNING: base_residual_f.torque.Norm() is 403.714 instead of zero.
36172       5882.675413             iDynTree WARNING: base_residual_f.force.Norm() is 5.71277 instead of zero.
36173       5882.685376             iDynTree WARNING: base_residual_f.torque.Norm() is 8.35167 instead of zero.
36174       5882.685439             iDynTree WARNING: base_residual_f.force.Norm() is 271.148 instead of zero.
36175       5882.695444             iDynTree WARNING: base_residual_f.torque.Norm() is 363.706 instead of zero.
36176       5882.705514             iDynTree WARNING: base_residual_f.force.Norm() is 112.677 instead of zero.
36177       5882.705577             iDynTree WARNING: base_residual_f.torque.Norm() is 58.7495 instead of zero.
36193       5890.971596             iDynTree WARNING: base_residual_f.force.Norm() is 0.0391496 instead of zero.
36194       5890.971666             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0276686 instead of zero.
36195       5890.981657             iDynTree WARNING: base_residual_f.force.Norm() is 0.170822 instead of zero.
36196       5890.981711             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0559934 instead of zero.
36197       5890.991717             iDynTree WARNING: base_residual_f.force.Norm() is 0.171075 instead of zero.
36198       5890.991774             iDynTree WARNING: base_residual_f.torque.Norm() is 0.05581 instead of zero.
36199       5891.001777             iDynTree WARNING: base_residual_f.force.Norm() is 0.0957283 instead of zero.
36200       5891.001836             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0736852 instead of zero.
36201       5891.011837             iDynTree WARNING: base_residual_f.force.Norm() is 0.130231 instead of zero.
36202       5891.011894             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0539015 instead of zero.
36203       5891.021898             iDynTree WARNING: base_residual_f.force.Norm() is 0.181193 instead of zero.
36204       5891.021955             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0892851 instead of zero.
36205       5891.031958             iDynTree WARNING: base_residual_f.force.Norm() is 0.16909 instead of zero.
36206       5891.032017             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0761378 instead of zero.
36207       5891.042018             iDynTree WARNING: base_residual_f.force.Norm() is 0.180837 instead of zero.
36208       5891.042075             iDynTree WARNING: base_residual_f.torque.Norm() is 0.097911 instead of zero.
36209       5891.052078             iDynTree WARNING: base_residual_f.force.Norm() is 0.164062 instead of zero.
36210       5891.052134             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0883897 instead of zero.
36211       5891.062138             iDynTree WARNING: base_residual_f.force.Norm() is 0.149984 instead of zero.
36212       5891.062191             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0869665 instead of zero.
36213       5891.072196             iDynTree WARNING: base_residual_f.force.Norm() is 0.17162 instead of zero.
36214       5891.072251             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0854478 instead of zero.
36215       5891.082257             iDynTree WARNING: base_residual_f.force.Norm() is 0.139768 instead of zero.
36216       5891.082313             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0864192 instead of zero.
36217       5891.092317             iDynTree WARNING: base_residual_f.force.Norm() is 0.139768 instead of zero.
36218       5891.092375             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0864192 instead of zero.
36219       5891.102378             iDynTree WARNING: base_residual_f.force.Norm() is 0.149984 instead of zero.
36220       5891.102442             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0869665 instead of zero.
36327       5894.233174             iDynTree WARNING: base_residual_f.force.Norm() is 0.001107 instead of zero.
36328       5894.233209             iDynTree WARNING: base_residual_f.torque.Norm() is 0.00103807 instead of zero.
36389       5894.273402             iDynTree WARNING: base_residual_f.force.Norm() is 0.0475251 instead of zero.
36390       5894.273433             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0499222 instead of zero.
36742       5924.537574             iDynTree WARNING: base_residual_f.force.Norm() is 1.37635 instead of zero.
36743       5924.537646             iDynTree WARNING: base_residual_f.torque.Norm() is 0.826159 instead of zero.
37504       6045.978078             iDynTree WARNING: base_residual_f.force.Norm() is 0.433334 instead of zero.
37505       6045.978160             iDynTree WARNING: base_residual_f.torque.Norm() is 0.907358 instead of zero.
37773       6087.047497             iDynTree WARNING: base_residual_f.force.Norm() is 0.00975666 instead of zero.
37774       6087.047578             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0166785 instead of zero.
37894       6108.143539             iDynTree WARNING: base_residual_f.force.Norm() is 0.117061 instead of zero.
37895       6108.143617             iDynTree WARNING: base_residual_f.torque.Norm() is 0.2597 instead of zero.
37978       6121.606626             iDynTree WARNING: base_residual_f.force.Norm() is 0.568933 instead of zero.
37979       6121.606694             iDynTree WARNING: base_residual_f.torque.Norm() is 0.912255 instead of zero.
38243       6163.138983             iDynTree WARNING: base_residual_f.force.Norm() is 0.342918 instead of zero.
38244       6163.139049             iDynTree WARNING: base_residual_f.torque.Norm() is 0.445658 instead of zero.
38278       6166.180410             iDynTree WARNING: base_residual_f.force.Norm() is 0.44239 instead of zero.
38279       6166.180470             iDynTree WARNING: base_residual_f.torque.Norm() is 0.432987 instead of zero.
38280       6166.190473             iDynTree WARNING: base_residual_f.force.Norm() is 0.0197633 instead of zero.
38281       6166.190535             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0243138 instead of zero.
38398       6184.356109             iDynTree WARNING: base_residual_f.force.Norm() is 0.504682 instead of zero.
38399       6184.356192             iDynTree WARNING: base_residual_f.torque.Norm() is 0.865093 instead of zero.
38400       6184.366138             iDynTree WARNING: base_residual_f.force.Norm() is 0.114191 instead of zero.
38401       6184.366190             iDynTree WARNING: base_residual_f.torque.Norm() is 0.257995 instead of zero.
38439       6191.565944             iDynTree WARNING: base_residual_f.force.Norm() is 0.543292 instead of zero.
38440       6191.566018             iDynTree WARNING: base_residual_f.torque.Norm() is 0.855993 instead of zero.
38746       6241.505861             iDynTree WARNING: base_residual_f.force.Norm() is 0.19067 instead of zero.
38747       6241.505925             iDynTree WARNING: base_residual_f.torque.Norm() is 0.471598 instead of zero.
38974       6278.024831             iDynTree WARNING: base_residual_f.force.Norm() is 1.40134 instead of zero.
38975       6278.024907             iDynTree WARNING: base_residual_f.torque.Norm() is 0.881585 instead of zero.
39229       6316.708151             iDynTree WARNING: base_residual_f.force.Norm() is 1.12197 instead of zero.
39230       6316.708230             iDynTree WARNING: base_residual_f.torque.Norm() is 0.884689 instead of zero.
39233       6317.644409             iDynTree WARNING: base_residual_f.force.Norm() is 0.313968 instead of zero.
39234       6317.644481             iDynTree WARNING: base_residual_f.torque.Norm() is 0.425564 instead of zero.
39235       6317.654469             iDynTree WARNING: base_residual_f.force.Norm() is 0.0272122 instead of zero.
39236       6317.654526             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0222612 instead of zero.
39352       6335.233559             iDynTree WARNING: base_residual_f.force.Norm() is 0.0911351 instead of zero.
39353       6335.233629             iDynTree WARNING: base_residual_f.torque.Norm() is 0.108197 instead of zero.
39578       6370.306011             iDynTree WARNING: base_residual_f.force.Norm() is 1.19642 instead of zero.
39579       6370.306074             iDynTree WARNING: base_residual_f.torque.Norm() is 0.85118 instead of zero.
39612       6375.902428             iDynTree WARNING: base_residual_f.force.Norm() is 0.0150325 instead of zero.
39613       6375.902509             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0395509 instead of zero.
39704       6392.568624             iDynTree WARNING: base_residual_f.force.Norm() is 0.844907 instead of zero.
39705       6392.568658             iDynTree WARNING: base_residual_f.torque.Norm() is 0.938114 instead of zero.
39708       6392.578688             iDynTree WARNING: base_residual_f.force.Norm() is 0.0199558 instead of zero.
39709       6392.578721             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0335541 instead of zero.
39888       6419.665209             iDynTree WARNING: base_residual_f.force.Norm() is 0.53966 instead of zero.
39889       6419.665274             iDynTree WARNING: base_residual_f.torque.Norm() is 0.932859 instead of zero.
39890       6419.675277             iDynTree WARNING: base_residual_f.force.Norm() is 0.0114279 instead of zero.
39891       6419.675337             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0270835 instead of zero.
39922       6423.853730             iDynTree WARNING: base_residual_f.force.Norm() is 0.407169 instead of zero.
39923       6423.853805             iDynTree WARNING: base_residual_f.torque.Norm() is 0.99196 instead of zero.
39925       6423.863853             iDynTree WARNING: base_residual_f.force.Norm() is 0.0366297 instead of zero.
39927       6423.863915             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0393381 instead of zero.
39971       6430.370031             iDynTree WARNING: base_residual_f.force.Norm() is 0.682913 instead of zero.
39972       6430.370093             iDynTree WARNING: base_residual_f.torque.Norm() is 0.93447 instead of zero.
39975       6431.417070             iDynTree WARNING: base_residual_f.force.Norm() is 0.237222 instead of zero.
39976       6431.417146             iDynTree WARNING: base_residual_f.torque.Norm() is 0.857294 instead of zero.
39977       6431.427145             iDynTree WARNING: base_residual_f.force.Norm() is 0.0298531 instead of zero.
39978       6431.427205             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0457518 instead of zero.
39979       6431.457366             iDynTree WARNING: base_residual_f.force.Norm() is 0.0644642 instead of zero.
39980       6431.457431             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0458959 instead of zero.
39981       6431.467481             iDynTree WARNING: base_residual_f.force.Norm() is 0.0385141 instead of zero.
39982       6431.467542             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0300362 instead of zero.
40020       6437.318633             iDynTree WARNING: base_residual_f.force.Norm() is 0.0236212 instead of zero.
40021       6437.318709             iDynTree WARNING: base_residual_f.torque.Norm() is 0.023516 instead of zero.
40024       6438.718848             iDynTree WARNING: base_residual_f.force.Norm() is 1.16605 instead of zero.
40025       6438.718918             iDynTree WARNING: base_residual_f.torque.Norm() is 0.850165 instead of zero.
40035       6440.057855             iDynTree WARNING: base_residual_f.force.Norm() is 0.0270611 instead of zero.
40036       6440.057927             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0307419 instead of zero.
40037       6440.319652             iDynTree WARNING: base_residual_f.force.Norm() is 0.602171 instead of zero.
40038       6440.319722             iDynTree WARNING: base_residual_f.torque.Norm() is 0.900969 instead of zero.
40039       6440.329714             iDynTree WARNING: base_residual_f.force.Norm() is 0.0175102 instead of zero.
40040       6440.329771             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0303291 instead of zero.
40288       6477.270822             iDynTree WARNING: base_residual_f.force.Norm() is 0.0261313 instead of zero.
40289       6477.270896             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0367305 instead of zero.
40373       6492.595758             iDynTree WARNING: base_residual_f.force.Norm() is 0.228916 instead of zero.
40374       6492.595837             iDynTree WARNING: base_residual_f.torque.Norm() is 0.893167 instead of zero.
40376       6492.605828             iDynTree WARNING: base_residual_f.force.Norm() is 0.233907 instead of zero.
40377       6492.605892             iDynTree WARNING: base_residual_f.torque.Norm() is 0.216639 instead of zero.
40378       6492.615890             iDynTree WARNING: base_residual_f.force.Norm() is 0.152434 instead of zero.
40379       6492.615949             iDynTree WARNING: base_residual_f.torque.Norm() is 0.220305 instead of zero.
40491       6509.339761             iDynTree WARNING: base_residual_f.force.Norm() is 0.275501 instead of zero.
40492       6509.339827             iDynTree WARNING: base_residual_f.torque.Norm() is 0.963541 instead of zero.
40711       6544.106994             iDynTree WARNING: base_residual_f.force.Norm() is 0.726058 instead of zero.
40712       6544.107067             iDynTree WARNING: base_residual_f.torque.Norm() is 0.890376 instead of zero.
40713       6544.117064             iDynTree WARNING: base_residual_f.force.Norm() is 0.0287257 instead of zero.
40714       6544.117127             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0467599 instead of zero.
40758       6553.007488             iDynTree WARNING: base_residual_f.force.Norm() is 0.0573695 instead of zero.
40759       6553.007564             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0634518 instead of zero.
40837       6565.038261             iDynTree WARNING: base_residual_f.force.Norm() is 0.176131 instead of zero.
40838       6565.038331             iDynTree WARNING: base_residual_f.torque.Norm() is 0.460432 instead of zero.
40841       6565.169054             iDynTree WARNING: base_residual_f.force.Norm() is 0.123345 instead of zero.
40842       6565.169116             iDynTree WARNING: base_residual_f.torque.Norm() is 0.252269 instead of zero.
40846       6565.813169             iDynTree WARNING: base_residual_f.force.Norm() is 0.578653 instead of zero.
40847       6565.813247             iDynTree WARNING: base_residual_f.torque.Norm() is 0.847334 instead of zero.
40848       6565.823230             iDynTree WARNING: base_residual_f.force.Norm() is 0.0397192 instead of zero.
40849       6565.823284             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0871765 instead of zero.
40850       6565.833290             iDynTree WARNING: base_residual_f.force.Norm() is 0.0148435 instead of zero.
40851       6565.833345             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0139553 instead of zero.
40888       6571.168519             iDynTree WARNING: base_residual_f.force.Norm() is 0.496893 instead of zero.
40889       6571.168591             iDynTree WARNING: base_residual_f.torque.Norm() is 0.903259 instead of zero.
40890       6571.178580             iDynTree WARNING: base_residual_f.force.Norm() is 0.0112677 instead of zero.
40891       6571.178643             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0302189 instead of zero.
40933       6577.219468             iDynTree WARNING: base_residual_f.force.Norm() is 0.153143 instead of zero.
40934       6577.219526             iDynTree WARNING: base_residual_f.torque.Norm() is 0.419041 instead of zero.
40936       6577.511606             iDynTree WARNING: base_residual_f.force.Norm() is 0.286391 instead of zero.
40937       6577.511683             iDynTree WARNING: base_residual_f.torque.Norm() is 0.438042 instead of zero.
40970       6582.142912             iDynTree WARNING: base_residual_f.force.Norm() is 0.435893 instead of zero.
40971       6582.142988             iDynTree WARNING: base_residual_f.torque.Norm() is 0.927809 instead of zero.
40972       6582.152976             iDynTree WARNING: base_residual_f.force.Norm() is 0.0157448 instead of zero.
40973       6582.153037             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0238758 instead of zero.
40974       6582.193217             iDynTree WARNING: base_residual_f.force.Norm() is 0.401456 instead of zero.
40975       6582.193279             iDynTree WARNING: base_residual_f.torque.Norm() is 0.904459 instead of zero.
41155       6609.018160             iDynTree WARNING: base_residual_f.force.Norm() is 0.0238687 instead of zero.
41156       6609.018234             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0469855 instead of zero.
41201       6616.773045             iDynTree WARNING: base_residual_f.force.Norm() is 0.879392 instead of zero.
41202       6616.773115             iDynTree WARNING: base_residual_f.torque.Norm() is 0.829925 instead of zero.
41284       6631.655734             iDynTree WARNING: base_residual_f.force.Norm() is 0.483528 instead of zero.
41285       6631.655806             iDynTree WARNING: base_residual_f.torque.Norm() is 0.871138 instead of zero.
41286       6631.665804             iDynTree WARNING: base_residual_f.force.Norm() is 0.0195694 instead of zero.
41287       6631.665865             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0640856 instead of zero.
41290       6632.290304             iDynTree WARNING: base_residual_f.force.Norm() is 0.660561 instead of zero.
41291       6632.290377             iDynTree WARNING: base_residual_f.torque.Norm() is 0.865308 instead of zero.
41292       6632.300377             iDynTree WARNING: base_residual_f.force.Norm() is 0.190373 instead of zero.
41293       6632.300439             iDynTree WARNING: base_residual_f.torque.Norm() is 0.182437 instead of zero.
41356       6639.408498             iDynTree WARNING: base_residual_f.force.Norm() is 0.711712 instead of zero.
41357       6639.408576             iDynTree WARNING: base_residual_f.torque.Norm() is 0.923782 instead of zero.
41358       6639.418572             iDynTree WARNING: base_residual_f.force.Norm() is 0.0513833 instead of zero.
41359       6639.418633             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0510362 instead of zero.
41360       6639.640036             iDynTree WARNING: base_residual_f.force.Norm() is 0.634781 instead of zero.
41361       6639.640112             iDynTree WARNING: base_residual_f.torque.Norm() is 0.428344 instead of zero.
41362       6639.650103             iDynTree WARNING: base_residual_f.force.Norm() is 0.0537248 instead of zero.
41363       6639.650178             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0963246 instead of zero.
41364       6639.720569             iDynTree WARNING: base_residual_f.force.Norm() is 0.446161 instead of zero.
41365       6639.720636             iDynTree WARNING: base_residual_f.torque.Norm() is 0.429392 instead of zero.
41366       6639.730629             iDynTree WARNING: base_residual_f.force.Norm() is 0.0364122 instead of zero.
41367       6639.730683             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0467075 instead of zero.
41455       6655.426288             iDynTree WARNING: base_residual_f.force.Norm() is 0.131749 instead of zero.
41456       6655.426358             iDynTree WARNING: base_residual_f.torque.Norm() is 0.455125 instead of zero.
41478       6656.825390             iDynTree WARNING: base_residual_f.force.Norm() is 0.269567 instead of zero.
41479       6656.825448             iDynTree WARNING: base_residual_f.torque.Norm() is 0.542312 instead of zero.
41492       6660.410041             iDynTree WARNING: base_residual_f.force.Norm() is 0.0157962 instead of zero.
41493       6660.410112             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0669232 instead of zero.
41540       6668.273273             iDynTree WARNING: base_residual_f.force.Norm() is 0.318425 instead of zero.
41541       6668.273339             iDynTree WARNING: base_residual_f.torque.Norm() is 0.451902 instead of zero.
41647       6683.892065             iDynTree WARNING: base_residual_f.force.Norm() is 0.0149835 instead of zero.
41648       6683.892145             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0287927 instead of zero.
41649       6683.952461             iDynTree WARNING: base_residual_f.force.Norm() is 0.0505884 instead of zero.
41650       6683.952530             iDynTree WARNING: base_residual_f.torque.Norm() is 0.062657 instead of zero.
41651       6684.244291             iDynTree WARNING: base_residual_f.force.Norm() is 0.0135219 instead of zero.
41652       6684.244360             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0246715 instead of zero.
41653       6684.254350             iDynTree WARNING: base_residual_f.force.Norm() is 0.026871 instead of zero.
41654       6684.254404             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0262097 instead of zero.
41696       6691.320981             iDynTree WARNING: base_residual_f.force.Norm() is 0.262156 instead of zero.
41697       6691.331040             iDynTree WARNING: base_residual_f.torque.Norm() is 0.523833 instead of zero.
41731       6695.720667             iDynTree WARNING: base_residual_f.force.Norm() is 1.3418 instead of zero.
41732       6695.720743             iDynTree WARNING: base_residual_f.torque.Norm() is 0.837301 instead of zero.
41769       6701.519970             iDynTree WARNING: base_residual_f.force.Norm() is 0.261553 instead of zero.
41770       6701.520053             iDynTree WARNING: base_residual_f.torque.Norm() is 0.168682 instead of zero.
41817       6709.744940             iDynTree WARNING: base_residual_f.force.Norm() is 0.264279 instead of zero.
41818       6709.745011             iDynTree WARNING: base_residual_f.torque.Norm() is 0.205962 instead of zero.
41819       6709.986683             iDynTree WARNING: base_residual_f.force.Norm() is 0.486639 instead of zero.
41820       6709.986760             iDynTree WARNING: base_residual_f.torque.Norm() is 0.414694 instead of zero.
41821       6709.996741             iDynTree WARNING: base_residual_f.force.Norm() is 0.0498723 instead of zero.
41822       6709.996789             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0588646 instead of zero.
41891       6721.092711             iDynTree WARNING: base_residual_f.force.Norm() is 0.358845 instead of zero.
41892       6721.092787             iDynTree WARNING: base_residual_f.torque.Norm() is 0.446627 instead of zero.
41900       6721.425062             iDynTree WARNING: base_residual_f.force.Norm() is 0.379228 instead of zero.
41901       6721.425138             iDynTree WARNING: base_residual_f.torque.Norm() is 0.896081 instead of zero.
41902       6721.435164             iDynTree WARNING: base_residual_f.force.Norm() is 0.0195438 instead of zero.
41903       6721.435224             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0264506 instead of zero.
41904       6721.505709             iDynTree WARNING: base_residual_f.force.Norm() is 0.154643 instead of zero.
41905       6721.505775             iDynTree WARNING: base_residual_f.torque.Norm() is 0.422457 instead of zero.
41906       6721.515780             iDynTree WARNING: base_residual_f.force.Norm() is 0.0094636 instead of zero.
41907       6721.515853             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0334675 instead of zero.
41947       6728.684624             iDynTree WARNING: base_residual_f.force.Norm() is 1.29171 instead of zero.
41948       6728.684691             iDynTree WARNING: base_residual_f.torque.Norm() is 0.868681 instead of zero.
42017       6738.397811             iDynTree WARNING: base_residual_f.force.Norm() is 0.191526 instead of zero.
42018       6738.397890             iDynTree WARNING: base_residual_f.torque.Norm() is 0.90535 instead of zero.
42022       6739.162869             iDynTree WARNING: base_residual_f.force.Norm() is 0.731797 instead of zero.
42023       6739.162925             iDynTree WARNING: base_residual_f.torque.Norm() is 0.936821 instead of zero.
42024       6739.172929             iDynTree WARNING: base_residual_f.force.Norm() is 0.058874 instead of zero.
42025       6739.172984             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0505117 instead of zero.
42063       6743.844111             iDynTree WARNING: base_residual_f.force.Norm() is 0.421006 instead of zero.
42064       6743.844181             iDynTree WARNING: base_residual_f.torque.Norm() is 0.945338 instead of zero.
42065       6743.854200             iDynTree WARNING: base_residual_f.force.Norm() is 0.0111673 instead of zero.
42066       6743.854259             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0255786 instead of zero.
42072       6746.350417             iDynTree WARNING: base_residual_f.force.Norm() is 0.390474 instead of zero.
42073       6746.350525             iDynTree WARNING: base_residual_f.torque.Norm() is 0.867128 instead of zero.
42107       6748.394174             iDynTree WARNING: base_residual_f.force.Norm() is 0.394609 instead of zero.
42108       6748.394263             iDynTree WARNING: base_residual_f.torque.Norm() is 0.976321 instead of zero.
42378       6794.691426             iDynTree WARNING: base_residual_f.force.Norm() is 0.238129 instead of zero.
42379       6794.691500             iDynTree WARNING: base_residual_f.torque.Norm() is 0.43158 instead of zero.
42381       6794.691566             iDynTree WARNING: base_residual_f.force.Norm() is 0.0205551 instead of zero.
42383       6794.701554             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0238413 instead of zero.
42414       6797.651343             iDynTree WARNING: base_residual_f.force.Norm() is 0.420716 instead of zero.
42415       6797.651412             iDynTree WARNING: base_residual_f.torque.Norm() is 0.915833 instead of zero.
42416       6797.661403             iDynTree WARNING: base_residual_f.force.Norm() is 0.07056 instead of zero.
42417       6797.661454             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0832204 instead of zero.
42425       6799.583705             iDynTree WARNING: base_residual_f.force.Norm() is 0.760385 instead of zero.
42426       6799.583782             iDynTree WARNING: base_residual_f.torque.Norm() is 0.908631 instead of zero.
42460       6802.794951             iDynTree WARNING: base_residual_f.force.Norm() is 0.165544 instead of zero.
42461       6802.795018             iDynTree WARNING: base_residual_f.torque.Norm() is 0.901728 instead of zero.
42462       6802.895583             iDynTree WARNING: base_residual_f.force.Norm() is 0.949486 instead of zero.
42463       6802.895654             iDynTree WARNING: base_residual_f.torque.Norm() is 0.865041 instead of zero.
42578       6822.147047             iDynTree WARNING: base_residual_f.force.Norm() is 0.8339 instead of zero.
42579       6822.147128             iDynTree WARNING: base_residual_f.torque.Norm() is 0.911767 instead of zero.
42613       6825.177953             iDynTree WARNING: base_residual_f.force.Norm() is 1.11454 instead of zero.
42614       6825.178022             iDynTree WARNING: base_residual_f.torque.Norm() is 2.1387 instead of zero.
42617       6825.188105             iDynTree WARNING: base_residual_f.force.Norm() is 0.0332948 instead of zero.
42618       6825.188136             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0490515 instead of zero.
42662       6833.232911             iDynTree WARNING: base_residual_f.force.Norm() is 0.933062 instead of zero.
42663       6833.232982             iDynTree WARNING: base_residual_f.torque.Norm() is 0.876443 instead of zero.
42782       6853.901914             iDynTree WARNING: base_residual_f.force.Norm() is 0.100192 instead of zero.
42783       6853.901986             iDynTree WARNING: base_residual_f.torque.Norm() is 0.133795 instead of zero.
42891       6870.302980             iDynTree WARNING: base_residual_f.force.Norm() is 0.0362843 instead of zero.
42892       6870.303054             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0522176 instead of zero.
42895       6870.614925             iDynTree WARNING: base_residual_f.force.Norm() is 0.666322 instead of zero.
42896       6870.614995             iDynTree WARNING: base_residual_f.torque.Norm() is 0.837757 instead of zero.
43006       6886.180634             iDynTree WARNING: base_residual_f.force.Norm() is 0.343134 instead of zero.
43007       6886.180690             iDynTree WARNING: base_residual_f.torque.Norm() is 0.928296 instead of zero.
43059       6896.893996             iDynTree WARNING: base_residual_f.force.Norm() is 0.529993 instead of zero.
43060       6896.894064             iDynTree WARNING: base_residual_f.torque.Norm() is 0.409658 instead of zero.
43132       6908.112423             iDynTree WARNING: base_residual_f.force.Norm() is 0.120011 instead of zero.
43133       6908.112519             iDynTree WARNING: base_residual_f.torque.Norm() is 0.235602 instead of zero.
43203       6918.200246             iDynTree WARNING: base_residual_f.force.Norm() is 0.903729 instead of zero.
43204       6918.200277             iDynTree WARNING: base_residual_f.torque.Norm() is 0.864987 instead of zero.
43206       6918.200302             iDynTree WARNING: base_residual_f.force.Norm() is 0.0204033 instead of zero.
43207       6918.200312             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0479318 instead of zero.
43253       6926.638432             iDynTree WARNING: base_residual_f.force.Norm() is 0.647696 instead of zero.
43254       6926.638483             iDynTree WARNING: base_residual_f.torque.Norm() is 0.924634 instead of zero.
43259       6926.940447             iDynTree WARNING: base_residual_f.force.Norm() is 0.467071 instead of zero.
43260       6926.940500             iDynTree WARNING: base_residual_f.torque.Norm() is 1.0125 instead of zero.
43315       6933.052251             iDynTree WARNING: base_residual_f.force.Norm() is 0.335489 instead of zero.
43316       6933.052306             iDynTree WARNING: base_residual_f.torque.Norm() is 0.932393 instead of zero.
43370       6944.891727             iDynTree WARNING: base_residual_f.force.Norm() is 0.323514 instead of zero.
43371       6944.891777             iDynTree WARNING: base_residual_f.torque.Norm() is 0.451818 instead of zero.
43394       6945.062912             iDynTree WARNING: base_residual_f.force.Norm() is 0.933277 instead of zero.
43395       6945.062955             iDynTree WARNING: base_residual_f.torque.Norm() is 0.94516 instead of zero.
43396       6945.072991             iDynTree WARNING: base_residual_f.force.Norm() is 0.0196166 instead of zero.
43397       6945.073035             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0387373 instead of zero.
43402       6945.203818             iDynTree WARNING: base_residual_f.force.Norm() is 0.00637693 instead of zero.
43403       6945.203866             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0257663 instead of zero.
43404       6945.274286             iDynTree WARNING: base_residual_f.force.Norm() is 0.0486465 instead of zero.
43405       6945.274332             iDynTree WARNING: base_residual_f.torque.Norm() is 0.0856977 instead of zero.
43406       6945.707158             iDynTree WARNING: base_residual_f.force.Norm() is 0.191939 instead of zero.
43407       6945.707213             iDynTree WARNING: base_residual_f.torque.Norm() is 0.211349 instead of zero.
43598       6977.745278             iDynTree WARNING: base_residual_f.force.Norm() is 0.996362 instead of zero.
43599       6977.745336             iDynTree WARNING: base_residual_f.torque.Norm() is 0.884212 instead of zero.
43639       6984.280892             iDynTree WARNING: base_residual_f.force.Norm() is 0.394126 instead of zero.
43640       6984.280946             iDynTree WARNING: base_residual_f.torque.Norm() is 0.443143 instead of zero.
43641       6984.290957             iDynTree WARNING: base_residual_f.force.Norm() is 0.0773633 instead of zero.
43642       6984.291008             iDynTree WARNING: base_residual_f.torque.Norm() is 0.056814 instead of zero.
43747       6999.112361             iDynTree WARNING: base_residual_f.force.Norm() is 1.0647 instead of zero.
43748       6999.112405             iDynTree WARNING: base_residual_f.torque.Norm() is 0.846706 instead of zero.
43749       6999.142583             iDynTree WARNING: base_residual_f.force.Norm() is 0.380378 instead of zero.
43750       6999.142628             iDynTree WARNING: base_residual_f.torque.Norm() is 0.935381 instead of zero.
43761       7001.589404             iDynTree WARNING: base_residual_f.force.Norm() is 0.999908 instead of zero.
43762       7001.589459             iDynTree WARNING: base_residual_f.torque.Norm() is 0.786293 instead of zero.
43791       7005.112922             iDynTree WARNING: base_residual_f.force.Norm() is 0.626621 instead of zero.
43792       7005.112948             iDynTree WARNING: base_residual_f.torque.Norm() is 0.972957 instead of zero.
traversaro commented 9 years ago 
96647       14573.726812                wholeBodyDynamicsThread::calibrateOffsetOnDoubleSupport called with code all
96648       14573.726847                wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 0 is 88.2863 -50.366 -73.8974 -1.75059 -2.14232 -0.318741
96649       14573.726878                wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 1 is 29.4178 -31.6457 16.1286 -0.414776 -1.4949 0.300277
96650       14573.726906                wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 2 is -66.4405 90.6395 -34.1493 -2.65656 3.55524 -0.459693
96651       14573.726934                wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 3 is 50.3886 7.86793 8.30326 -0.710595 0.488621 -0.0300305
96652       14573.726963                wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 4 is -16.4742 -21.7289 -12.2146 2.86362 -2.14509 -1.04518
96653       14573.726991                wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 5 is -89.7327 -32.7603 12.5575 -1.16617 2.25775 -1.38008
96654       14573.727019                wholeBodyDynamicsThread::calibrateOffset all called successfully, starting calibration.
96691       14579.769476                wholeBodyDynamicsThread: new calibration for FT l_arm_ft_sensor is 88.1561 -50.8234 -74.0591 -1.75623 -2.14354 -0.316833
96692       14579.769552                wholeBodyDynamicsThread: new calibration for FT r_arm_ft_sensor is 29.522 -31.3741 15.9098 -0.410898 -1.50436 0.286432
96693       14579.769587                wholeBodyDynamicsThread: new calibration for FT l_leg_ft_sensor is -69.7649 88.9746 -32.1717 -2.64402 2.04259 -0.171214
96694       14579.769616                wholeBodyDynamicsThread: new calibration for FT l_foot_ft_sensor is 48.4618 6.63787 10.8762 -0.592095 0.0226885 0.218741
96695       14579.769644                wholeBodyDynamicsThread: new calibration for FT r_leg_ft_sensor is -11.9311 -23.8294 -14.3976 2.95628 -0.651918 -0.664938
96696       14579.769672                wholeBodyDynamicsThread: new calibration for FT r_foot_ft_sensor is -87.5081 -34.9048 10.3362 -1.08757 2.61911 -1.06188
96697       14579.769700                wholeBodyDynamicsThread::waitCalibrationDone() returning: calibration finished with success
97975       14751.292269                wholeBodyDynamicsTree: double support calibration for allrequested
97976       14751.292356                wholeBodyDynamicsThread::calibrateOffsetOnDoubleSupport called with code all
97977       14751.292389                wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 0 is 88.1561 -50.8234 -74.0591 -1.75623 -2.14354 -0.316833
97978       14751.292420                wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 1 is 29.522 -31.3741 15.9098 -0.410898 -1.50436 0.286432
97979       14751.292452                wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 2 is -69.7649 88.9746 -32.1717 -2.64402 2.04259 -0.171214
97980       14751.292481                wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 3 is 48.4618 6.63787 10.8762 -0.592095 0.0226885 0.218741
97981       14751.292525                wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 4 is -11.9311 -23.8294 -14.3976 2.95628 -0.651918 -0.664938
97982       14751.292554                wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 5 is -87.5081 -34.9048 10.3362 -1.08757 2.61911 -1.06188
97983       14751.292583                wholeBodyDynamicsThread::calibrateOffset all called successfully, starting calibration.
98020       14757.333646                wholeBodyDynamicsThread: new calibration for FT l_arm_ft_sensor is 87.9276 -50.5468 -74.1263 -1.7554 -2.15693 -0.312316
98021       14757.333725                wholeBodyDynamicsThread: new calibration for FT r_arm_ft_sensor is 29.5668 -31.6658 16.1857 -0.402014 -1.51447 0.273345
98022       14757.333760                wholeBodyDynamicsThread: new calibration for FT l_leg_ft_sensor is -71.2937 91.0444 -32.1873 -2.76949 2.0004 -0.564645
98023       14757.333790                wholeBodyDynamicsThread: new calibration for FT l_foot_ft_sensor is 46.4645 8.03456 10.9253 -0.600317 0.804351 -0.110612
98024       14757.333827                wholeBodyDynamicsThread: new calibration for FT r_leg_ft_sensor is -9.71795 -26.0739 -14.2932 3.00568 -0.565145 -0.039529
98025       14757.333856                wholeBodyDynamicsThread: new calibration for FT r_foot_ft_sensor is -86.3304 -37.1381 10.6872 -1.08752 2.15941 -0.386666
98026       14757.333887                wholeBodyDynamicsThread::waitCalibrationDone() returning: calibration finished with success
98986       14868.487111                wholeBodyDynamicsTree: double support calibration for allrequested
98987       14868.487164                wholeBodyDynamicsThread::calibrateOffsetOnDoubleSupport called with code all
98988       14868.487190                wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 0 is 87.9276 -50.5468 -74.1263 -1.7554 -2.15693 -0.312316
98989       14868.487212                wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 1 is 29.5668 -31.6658 16.1857 -0.402014 -1.51447 0.273345
98990       14868.487232                wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 2 is -71.2937 91.0444 -32.1873 -2.76949 2.0004 -0.564645
98991       14868.487252                wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 3 is 46.4645 8.03456 10.9253 -0.600317 0.804351 -0.110612
98992       14868.487271                wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 4 is -9.71795 -26.0739 -14.2932 3.00568 -0.565145 -0.039529
98993       14868.487291                wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 5 is -86.3304 -37.1381 10.6872 -1.08752 2.15941 -0.386666
98994       14868.487309                wholeBodyDynamicsThread::calibrateOffset all called successfully, starting calibration.
99033       14874.531039                wholeBodyDynamicsThread: new calibration for FT l_arm_ft_sensor is 88.2458 -50.2138 -74.1943 -1.73579 -2.14641 -0.329966
99034       14874.531095                wholeBodyDynamicsThread: new calibration for FT r_arm_ft_sensor is 29.4627 -31.696 16.0493 -0.403712 -1.51121 0.277653
99035       14874.531120                wholeBodyDynamicsThread: new calibration for FT l_leg_ft_sensor is -81.7899 80.5272 -33.6154 -1.90047 -0.811387 0.079839
99036       14874.531140                wholeBodyDynamicsThread: new calibration for FT l_foot_ft_sensor is 40.1424 4.22265 9.35356 -0.445501 0.325063 0.371119
99037       14874.531158                wholeBodyDynamicsThread: new calibration for FT r_leg_ft_sensor is -0.757106 -24.6885 -11.9229 1.97247 2.0937 0.743379
99038       14874.531177                wholeBodyDynamicsThread: new calibration for FT r_foot_ft_sensor is -80.4704 -35.5712 11.8723 -0.877179 2.19084 0.487995
99039       14874.531195                wholeBodyDynamicsThread::waitCalibrationDone() returning: calibration finished with success
134937      20746.160729                yarp: Removing output from /wholeBodyDynamicsTree/contacts:o to /iCubGui/forces
134944      20746.432699                yarp: Removing output from /wholeBodyDynamicsTree/base:o to /iCubGui/base:i
134952      20747.650998                iDynTree WARNING: base_residual_f.force.Norm() is 0.0366408 instead of zero.
134953      20747.651064                iDynTree WARNING: base_residual_f.torque.Norm() is 0.037222 instead of zero.
134990      20751.265721                 Trying to shut down
134991      20751.265752                Module closing
134992      20751.265779                wholeBodyDynamicsTree: closing wholeBodyDynamicsThread
134993      20751.275700        INFO        Closing output torques ports 
134995      20751.275805                yarp: Removing output from /wholeBodyDynamicsTree/left_leg/Torques:o to /icub/joint_vsens/left_leg:i
134996      20751.315947                yarp: Removing output from /wholeBodyDynamicsTree/torso/Torques:o to /icub/joint_vsens/torso:i
135036      20751.477182                yarp: Removing output from /wholeBodyDynamicsTree/left_arm/Torques:o to /icub/joint_vsens/left_arm:i
135041      20751.587941                yarp: Removing output from /wholeBodyDynamicsTree/right_leg/Torques:o to /icub/joint_vsens/right_leg:i
135045      20751.648390                yarp: Removing output from /wholeBodyDynamicsTree/right_arm/Torques:o to /icub/joint_vsens/right_arm:i
135048      20751.718874                Closing contacts port
135050      20751.759150                Closing end effector wrenches port
135051      20751.869974                Closing iCubGui base port
135053      20751.910248                wholeBodyDynamicsTree: closing wholeBodyStateLocal interface
135054      20751.910281        INFO        yarpWholeBodyStatesLocal::close() : closing estimator thread
135055      20751.920309        INFO        [yarpWholeBodyDynamicsEstimator]:
135056      20751.920388                Expected period 10.000000 ms.
135057      20751.920400                Real period: 10.1+/-0.0 ms.
135058      20751.920409                Real duration of 'run' method: 1.7+/-0.3 ms.
135059      20751.920417        INFO        yarpWholeBodyStatesLocal::close() : closing sensor interface
135060      20751.920427                yarp: Removing output from /wholeBodyDynamicsTree/torso/rpc:o to /icub/torso/rpc:i
135061      20751.930363                yarp: Removing output from /wholeBodyDynamicsTree/torso/command:o to /icub/torso/command:i
135067      20752.202280                yarp: Removing input from /icub/torso/state:o to /wholeBodyDynamicsTree/torso/state:i
135077      20752.484136                yarp: Removing input from /icub/torso/stateExt:o to /wholeBodyDynamicsTree/torso/stateExt:i
135078      20752.514346                yarp: Removing output from /wholeBodyDynamicsTree/left_arm/rpc:o to /icub/left_arm/rpc:i
135079      20752.534462                yarp: Removing output from /wholeBodyDynamicsTree/left_arm/command:o to /icub/left_arm/command:i
135080      20752.806734                yarp: Removing input from /icub/left_arm/state:o to /wholeBodyDynamicsTree/left_arm/state:i
135081      20753.098900                yarp: Removing input from /icub/left_arm/stateExt:o to /wholeBodyDynamicsTree/left_arm/stateExt:i
135082      20753.119046                yarp: Removing output from /wholeBodyDynamicsTree/right_arm/rpc:o to /icub/right_arm/rpc:i
135083      20753.129123                yarp: Removing output from /wholeBodyDynamicsTree/right_arm/command:o to /icub/right_arm/command:i
135084      20753.391062                yarp: Removing input from /icub/right_arm/state:o to /wholeBodyDynamicsTree/right_arm/state:i
135085      20753.662958                yarp: Removing input from /icub/right_arm/stateExt:o to /wholeBodyDynamicsTree/right_arm/stateExt:i
135098      20754.812864                yarp: Removing output from /wholeBodyDynamicsTree/left_leg/rpc:o to /icub/left_leg/rpc:i
135102      20754.823011                yarp: Removing output from /wholeBodyDynamicsTree/left_leg/command:o to /icub/left_leg/command:i
135108      20755.084753                yarp: Removing input from /icub/left_leg/state:o to /wholeBodyDynamicsTree/left_leg/state:i
135112      20755.346808                yarp: Removing input from /icub/left_leg/stateExt:o to /wholeBodyDynamicsTree/left_leg/stateExt:i
135113      20755.366827                yarp: Removing output from /wholeBodyDynamicsTree/right_leg/rpc:o to /icub/right_leg/rpc:i
135114      20755.386959                yarp: Removing output from /wholeBodyDynamicsTree/right_leg/command:o to /icub/right_leg/command:i
135118      20755.638869                yarp: Removing input from /icub/right_leg/state:o to /wholeBodyDynamicsTree/right_leg/state:i
135119      20755.910958                yarp: Removing input from /icub/right_leg/stateExt:o to /wholeBodyDynamicsTree/right_leg/stateExt:i
135120      20756.182959                yarp: Removing input from /icub/inertial to /wholeBodyDynamicsTree/imu/imu_frame:i
135121      20756.444753                yarp: Removing input from /icub/left_arm/analog:o to /wholeBodyDynamicsTree/ftSens/l_arm_ft_sensor:i
135122      20756.716812                yarp: Removing input from /icub/right_arm/analog:o to /wholeBodyDynamicsTree/ftSens/r_arm_ft_sensor:i
135123      20756.988728                yarp: Removing input from /icub/left_leg/analog:o to /wholeBodyDynamicsTree/ftSens/l_leg_ft_sensor:i
135124      20757.260831                yarp: Removing input from /icub/left_foot/analog:o to /wholeBodyDynamicsTree/ftSens/l_foot_ft_sensor:i
135125      20757.603555                yarp: Removing input from /icub/right_leg/analog:o to /wholeBodyDynamicsTree/ftSens/r_leg_ft_sensor:i
135126      20757.885399                yarp: Removing input from /icub/right_foot/analog:o to /wholeBodyDynamicsTree/ftSens/r_foot_ft_sensor:i
135127      20757.905538        INFO        yarpWholeBodyStatesLocal::close() : closing skin_contacts_port
135128      20757.925657                yarpWholeBodyStatesLocal::close() : deleting sensor interface
135129      20757.925689                yarpWholeBodyStatesLocal::close() : deleting estimator thread
135130      20757.925700        INFO        yarpWholeBodyStatesLocal::close() : closing estimator thread
135131      20757.925711        INFO        yarpWholeBodyStatesLocal::close() : closing sensor interface
135132      20757.925744        INFO        yarpWholeBodyStatesLocal::close() : closing skin_contacts_port
135133      20757.925759                yarpWholeBodyStatesLocal::close() : deleting sensor interface
135134      20757.925773                yarpWholeBodyStatesLocal::close() : deleting estimator thread
135135      20757.925787                wholeBodyDynamicsTree: closing RPC port interface
135136      20757.935712        INFO        :
135137      20757.935748                Expected period 10 ms.
135138      20757.935763                Real period: 10.1+/-0.2 ms.
135139      20757.935776                Real duration of 'run' method: 0.4+/-0.5 ms.
135140      20757.935790                Next time you could set a lower period to improve the wholeBodyDynamics performance.
DanielePucci commented 9 years ago

+1 very well

traversaro commented 9 years ago

More example of this bug: https://gist.github.com/traversaro/b745fc4e8a82db581f01 .

traversaro commented 9 years ago

Just found a way of replicating this: just exert two forces with different application points on a single link. Working on a fix.

iron76 commented 9 years ago

:+1:

traversaro commented 9 years ago

Bug fixed in https://github.com/robotology-playground/idyntree/commit/b6985e807fc074b4763b7b60eb0a0a37ec4a50b8 . A slight mismatch remain in the base residual force, but I guess is something related to some kind of one-step offset between the value readed from the robot. However it is a relatively minor problem, addresed in issue https://github.com/robotology/codyco-modules/issues/122 .