search

robotology / whole-body-estimators

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

Add some debug prints to debug why WBD has a startup delay #105

Closed S-Dafarra closed 3 years ago

S-Dafarra commented 3 years ago

As per https://github.com/robotology/icub-tech-support/issues/999#issuecomment-706312574

S-Dafarra commented 3 years ago

cc @traversaro @prashanthr05

S-Dafarra commented 3 years ago

I have added some debug prints in https://github.com/S-Dafarra/whole-body-estimators/tree/feature/startupDebugPrints. I will test it on the robot before opening a PR

prashanthr05 commented 3 years ago

Better to add a prefix WholeBodyDynamicsDevice similar to other existing debug prints. This will help to easily track the outputs in the long yarprobotinterface logs on the robot.

S-Dafarra commented 3 years ago

yDebug should do that automatically, taking the name of the method in which it is called (that's why I did not use any helper method).

prashanthr05 commented 3 years ago

Understood.

S-Dafarra commented 3 years ago

Indeed, I was wrong :grin: Maybe on icub30 we had an old version of yarp and a tag for finding the message easily was definitely missing

S-Dafarra commented 3 years ago

Here a "normal" run

    1703.886865 DEBUG       wholeBodyDynamics Statistics: Opening estimator.
    1703.907108 DEBUG       wholeBodyDynamics Statistics: Estimator opened in  0.0196292 s.
    1703.907223 DEBUG       wholeBodyDynamics Statistics: Loading settings from configuration files.
    1703.907451 DEBUG       wholeBodyDynamics: using secondary calibration matrix for sensor  l_leg_ft_sensor
    1703.917230 DEBUG       wholeBodyDynamics: using secondary calibration matrix for sensor  r_leg_ft_sensor
    1703.917343 DEBUG       wholeBodyDynamics Statistics: Settings loaded in  0.00346279 s.
    1703.917399 DEBUG       wholeBodyDynamics Statistics: Opening RPC port.
    1703.927473 DEBUG       wholeBodyDynamics Statistics: RPC port opened in  0.0141134 s.
    1703.927532 DEBUG       wholeBodyDynamics Statistics: Opening settings port.
    1703.937619 DEBUG       wholeBodyDynamics Statistics: Settings port opened in  0.0142069 s.
    1703.937680 DEBUG       wholeBodyDynamics Statistics: Opening remapper control board.
    1703.947730 DEBUG       wholeBodyDynamics Statistics: Remapper control board opened in  0.00902534 s.
    1703.947795 DEBUG       wholeBodyDynamics Statistics: Opening remapper virtual sensors.
    1703.957824 DEBUG       wholeBodyDynamics Statistics: Remapper virtual sensors opened in  0.00988364 s.
    1703.957887 DEBUG       wholeBodyDynamics Statistics: Opening contact frames.
    1703.958027 DEBUG       wholeBodyDynamics Statistics: Contact frames opened in  0.000769377 s.
    1703.958068 DEBUG       wholeBodyDynamics Statistics: Opening skin contact list ports.
    1703.988153 DEBUG       wholeBodyDynamics Statistics: Skin contact list opened in  0.0283587 s.
    1703.988214 DEBUG       wholeBodyDynamics Statistics: Opening external wrenches ports.
    1704.099252 DEBUG       wholeBodyDynamics Statistics: External port wrenches opened in  0.104179 s.
    1704.099367 DEBUG       wholeBodyDynamics Statistics: Opening multiple analog sensors remapper.
    1704.119622 DEBUG       wholeBodyDynamics Statistics: Multiple analog sensors remapper opened in  0.0278461 s.
    1704.119685 DEBUG       wholeBodyDynamics Statistics: Opening filtered FT ports.
    1704.210629 DEBUG       wholeBodyDynamics Statistics: Filtered FT ports opened in  0.0925698 s.
    1744.505921 DEBUG       wholeBodyDynamics Statistics: Attaching all control board.
    1744.505961 DEBUG       wholeBodyDynamics Statistics: Attaching all control board took  0.00104523 s.
    1744.506005 DEBUG       wholeBodyDynamics Statistics: Attaching all virtual analog sensors.
    1744.506047 DEBUG       wholeBodyDynamics Statistics: Attaching all virtual analog sensors took  0.000403404 s.
    1744.506126 DEBUG       wholeBodyDynamics Statistics: Attaching all FTs.
    1744.515817 DEBUG       wholeBodyDynamicsDevice :: number of ft sensors found in both ft + mas 10 where analog are  6  and mas are  4
    1744.515878 DEBUG       wholeBodyDynamics Statistics: Attaching all Fts took  0.000277758 s.
    1744.515923 DEBUG       wholeBodyDynamics Statistics: Attaching all IMUs.
    1744.515965 DEBUG       wholeBodyDynamics Statistics: Attaching all IMUs took  8.55923e-05 s.
    1744.516007 DEBUG       wholeBodyDynamics Statistics: Calibrating offsets.
    1744.516047 DEBUG       wholeBodyDynamics Statistics: Calibrating took  0.000117064 s.
    1744.516086 DEBUG       wholeBodyDynamics Statistics: Starting

I noticed that there might be some time between the configuration phase and the attach all phase.

traversaro commented 3 years ago

I noticed that there might be some time between the configuration phase and the attach all phase.

I guess that that time is the robot that is calibrating the encoders, if instead it is much longer then that it is fishy.

S-Dafarra commented 3 years ago

I noticed that there might be some time between the configuration phase and the attach all phase.

I guess that that time is the robot that is calibrating the encoders, if instead it is much longer then that it is fishy.

Yes, the attachAll seems to be called after the hands finish calibrating. Related comment https://github.com/robotology/icub-tech-support/issues/999#issuecomment-801147788

S-Dafarra commented 3 years ago

I opened #106 with the modifications. After that, we can close this and keep discussing on https://github.com/robotology/icub-tech-support/issues/999