traversaro
commented
7 years ago Even if this may sound confusing, this is exactly how the estimators is supposed to work.
From the documentation of the wholebodydynamics yarp device (contained in the WholeBodyDynamicsDevice class) [1]:
defaultContactFrames : Vector of default contact frames. If no external force read from the skin is found on a given submodel, the defaultContactFrames list is scanned and the first frame found on the submodel is the one at which origin the unknown contact force is assumed to be.
This relatively complex logic was implemented to simplify the configuration of the device in "strange" situations, for example when an force-torque sensor is not working, and we still want to run the torque estimator but not using the measurements from a given force-torque sensor.
For example, let's assume that the r_foot_ft_sensor stops working: you can remove it from the estimation by avoiding to attach it to the wholebodydynamics device by commenting out this line [4].
This causes the submodels used for estimation to be reduced from 7 to 6 (the submodel below the foot force-torque sensor and the one between the two force-torque sensors of the leg are merged together).
If the default contact frames were specified in a sub-model specific way, then you would also need to modify the defaultContactFrames parameter. However, this is not necessary because the new default contact frame of right leg submodel will become the frame that comes before in the list, i.e. r_sole).
What you are asking know to the estimator is to:
- First calibrate the force-torque offsets, assuming that the external forces are just acting on the
r_upper_leg/l_upper_leglinks. - Then (using the f-t offset obtained during the calibration) you assume that, relatively to the leg submodel, the forces are acting on the
r_lower_legandl_lower_leglinks, because those are the link references in thedefaultContactFrameslist.
The proper way to solve this would be to make sure that the skin on the legs is correctly working to avoid hardcoding assumptions on the contact points in the configuration, but this is blocked by https://github.com/robotology/codyco-modules/issues/227 .
In the meanwhile, we should make sure that the defaultContactFrames contains frames belonging to r_upper_leg and l_upper_leg rather then r_lower_leg and l_lower_leg, as for the moment we do not have any relevant experiment in which the robot is interacting with the enviroment throught the *_lower_leg links.
For this reason, to fix this problem I would suggest to simply replace the instances of l_lower_leg and r_lower_leg in the defaultContactFrames with l_upper_leg and r_upper_leg, rather then keeping both of them, that may be confusing.
[1] http://wiki.icub.org/codyco/dox/html/classyarp_1_1dev_1_1WholeBodyDynamicsDevice.html [2] https://github.com/robotology/codyco-modules/blob/master/src/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp#L290 [3] https://github.com/robotology/codyco-modules/blob/master/src/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp#L1305 [4] https://github.com/robotology/codyco-modules/blob/master/src/devices/wholeBodyDynamics/app/wholebodydynamics-icub-eth-six-fts.xml#L62
On iCubGenova02, I tested the new wholeBodyDynamics command
calibStandingOnTwoLinks. At the beginning it was not working properly. There were no errors, but measured torque on knee joint was quite high (25 Nm), and this was not realistic because iCub was sitting on its chair. For making the calibration working properly, it was necessary to manually update filewholebodydynamics.xml(the one in local folder onicub-head), in particular I added to this line:the list of frames that I was using after calling
calibStandingOnTwoLinks,l_upper_leg_contactandr_upper_leg_contact.