search

robotology / whole-body-estimators

YARP devices that implement estimators for humanoid robots.
26 stars 12 forks source link

Slight mismatch in base force torque computed by dynamicRNEA when reading skin contacts. #22

Open traversaro opened 9 years ago

traversaro commented 9 years ago

If the external forces are properlty estimated from sensor readings, the residual coming from the computation of the dynamic step of the RNEA (including the estimated external forces) should be zero. Sometimes a warning appears saying that this residual is not zero, but this mismatch appear to be within the noise band of the estimation. My impression is that there is some sort of one-step mismatch between some of the estimated quantities and some of the quantities readed by the robot. Interestingly this happens only when readings skin contacts from the skinManager Example:

[ERROR]wholeBodyDynamics: external forces computation and torque estimation failed 
iDynTree WARNING: base_residual_f.force.Norm() is 0.000465194 instead of zero.
iDynTree WARNING: base_residual_f.torque.Norm() is 0.0156275 instead of zero.
[ERROR]wholeBodyDynamics: external forces computation and torque estimation failed 
iDynTree WARNING: base_residual_f.force.Norm() is 0.000770842 instead of zero.
iDynTree WARNING: base_residual_f.torque.Norm() is 0.0263195 instead of zero.
[ERROR]wholeBodyDynamics: external forces computation and torque estimation failed 
iDynTree WARNING: base_residual_f.force.Norm() is 0.000743258 instead of zero.
iDynTree WARNING: base_residual_f.torque.Norm() is 0.0253776 instead of zero.
[ERROR]wholeBodyDynamics: external forces computation and torque estimation failed 
iDynTree WARNING: base_residual_f.force.Norm() is 0.00135301 instead of zero.
iDynTree WARNING: base_residual_f.torque.Norm() is 0.0524522 instead of zero.
[ERROR]wholeBodyDynamics: external forces computation and torque estimation failed 
iDynTree WARNING: base_residual_f.force.Norm() is 0.00120079 instead of zero.
iDynTree WARNING: base_residual_f.torque.Norm() is 0.0465509 instead of zero.
[ERROR]wholeBodyDynamics: external forces computation and torque estimation failed 
iDynTree WARNING: base_residual_f.force.Norm() is 0.00108016 instead of zero.
iDynTree WARNING: base_residual_f.torque.Norm() is 0.0446214 instead of zero.
[ERROR]wholeBodyDynamics: external forces computation and torque estimation failed 
iDynTree WARNING: base_residual_f.force.Norm() is 0.00108747 instead of zero.
iDynTree WARNING: base_residual_f.torque.Norm() is 0.0449235 instead of zero.
[ERROR]wholeBodyDynamics: external forces computation and torque estimation failed 
iDynTree WARNING: base_residual_f.force.Norm() is 0.00138708 instead of zero.
iDynTree WARNING: base_residual_f.torque.Norm() is 0.049056 instead of zero.
[ERROR]wholeBodyDynamics: external forces computation and torque estimation failed 
iDynTree WARNING: base_residual_f.force.Norm() is 0.00129282 instead of zero.
iDynTree WARNING: base_residual_f.torque.Norm() is 0.0457224 instead of zero.
[ERROR]wholeBodyDynamics: external forces computation and torque estimation failed 
iDynTree WARNING: base_residual_f.force.Norm() is 0.00083809 instead of zero.
iDynTree WARNING: base_residual_f.torque.Norm() is 0.0326779 instead of zero.
[ERROR]wholeBodyDynamics: external forces computation and torque estimation failed 
iDynTree WARNING: base_residual_f.force.Norm() is 0.000859733 instead of zero.
iDynTree WARNING: base_residual_f.torque.Norm() is 0.0335218 instead of zero.
[ERROR]wholeBodyDynamics: external forces computation and torque estimation failed 
iDynTree WARNING: base_residual_f.force.Norm() is 0.000729497 instead of zero.
iDynTree WARNING: base_residual_f.torque.Norm() is 0.0284438 instead of zero.
[ERROR]wholeBodyDynamics: external forces computation and torque estimation failed 
iDynTree WARNING: base_residual_f.force.Norm() is 0.000818487 instead of zero.
iDynTree WARNING: base_residual_f.torque.Norm() is 0.0319136 instead of zero.
[ERROR]wholeBodyDynamics: external forces computation and torque estimation failed 
iDynTree WARNING: base_residual_f.force.Norm() is 0.000195079 instead of zero.
iDynTree WARNING: base_residual_f.torque.Norm() is 0.0117391 instead of zero.
[ERROR]wholeBodyDynamics: external forces computation and torque estimation failed 
iDynTree WARNING: base_residual_f.force.Norm() is 0.000221889 instead of zero.
iDynTree WARNING: base_residual_f.torque.Norm() is 0.0133524 instead of zero.
[ERROR]wholeBodyDynamics: external forces computation and torque estimation failed 
iDynTree WARNING: base_residual_f.force.Norm() is 0.000423716 instead of zero.
iDynTree WARNING: base_residual_f.torque.Norm() is 0.0142523 instead of zero.
[ERROR]wholeBodyDynamics: external forces computation and torque estimation failed 
iDynTree WARNING: base_residual_f.force.Norm() is 0.000459187 instead of zero.
iDynTree WARNING: base_residual_f.torque.Norm() is 0.0154455 instead of zero.
[ERROR]wholeBodyDynamics: external forces computation and torque estimation failed 
yarp: Datagram packet size set to 65000
yarp: Removing input from /skinManager/skin_events:o to /wholeBodyDynamicsTree/skin_contacts:i
(Nothing happens anymore)
traversaro commented 4 years ago

I now know what happened here, and probably this will also happen even with the modifications proposed by @HosameldinMohamed in https://github.com/robotology/whole-body-estimators/issues/17 . @HosameldinMohamed remember me to explain you.