Gravity Force Removal of FT Readings

[diogofbsilva]

Hello everyone!

I am trying to remove the gravitational component of the readings of the FT sensor in the arm. In order to do that I tried two different ways:

The first way was by initializing a force vector in the 0th frame as a gravity force vector. As in the 0th frame, the vertical axis is the x-axis points upwards, the gravity force vector along this axis would be equal to -g*(sum of all links mass). Then, a force in the FT sensor frame is computed using the second example of instantiation of the module iDynInv available at https://robotology.github.io/robotology-documentation/doc/html/group__iDynInv.html.

The second way was by using the method available at https://robotology.github.io/robotology-documentation/doc/html/idyn_one_chain_tutorial.html, where a force vector is initialized with all zeros in the end effector and also lin_acc[2] = 9.81. Of course I complemented this methods by obtaining the force in the sensor link, as it is made in the method shown above:

iCubArmDyn *arm = new iCubArmDyn("left")
armWSensorSolver = new iDynSensorArm(arm,STATIC,VERBOSE);
armWSensorSolver->computeFromSensorNewtonEuler(F,M);
Matrix F = armWSensorSolver->getForces();
Matrix Mu = armWSensorSolver->getMoments();

I used a simulator in gazebo where I can change the joint angles of the iCub, such as the shoulder pitch. The robot is not doing any activity, just standing. The goal was that the force obtained by the methods shown above would match the force obtained by the FT plugin but that did not happen.

What can I be doing wrong? Does anyone have suggestions of what may be making this conversion of forces not possible?

Thank you!

[traversaro]

The dynamic model provided in iDyn has not been updated in ages (>10 years), so it is highly possible that is different from the one used by the simulation model in Gazebo, but if the difference is really big, it may be possible there is a bug in iDyn or similar.

Anyhow or computing the FT measure induced by gravity, I strongly suggested to use instead iDynTree. The computeExpectedFTSensorsMeasurements method of the iDynTree::ExtWrenchesAndJointTorquesEstimator class is doing exactly what you need, and an example of its use (in MATLAB, but the C++ API is quite similar) can be found at https://github.com/robotology/idyntree/blob/bf15223cb6793c5a54bf367bc996243f9185f0a3/examples/matlab/SixAxisFTOffsetEstimation/SixAxisFTOffsetEstimation.m .

The main advantage of using iDynTree is that if you are simulating a given simulation model in Gazebo, you can ensure that exactly the same URDF loaded in Gazebo is loaded also in iDynTree by setting appropriately the YARP_ROBOT_NAME env variable to the one documented in https://github.com/robotology/icub-models (for example, iCubGazeboV2_7) and loading the model in iDynTree as (following the docs in https://github.com/robotology/icub-models?tab=readme-ov-file#use-the-models-from-c-using-yarp, see for example https://github.com/robotology/idyntree-yarp-tools/blob/v0.0.9/src/modules/yarprobotstatepublisher/src/robotstatepublisher.cpp):

std::string modelAbsolutePath = yarp::os::ResourceFinder::getResourceFinderSingleton().findFileByName("model.urdf");
iDynTree::KinDynComputations kinDynComp;
kinDynComp.loadRobotModel(modelAbsolutePath);

If you share more info (the simulation model you are using, the rest of your C++ code) I will be happy to provide more inputs.

[diogofbsilva]

Hello @traversaro !!

Thank you so much for your quick and helpful answer, I already tried to implement it loading my robot model, however I have a doubt regarding which link to choose, just like what is made in https://github.com/robotology/idyntree/blob/bf15223cb6793c5a54bf367bc996243f9185f0a3/examples/matlab/SixAxisFTOffsetEstimation/SixAxisFTOffsetEstimation.m#L43, where the sole is chosen.
In my simulation, iCub is standing just like in the following image:

If it helps, I can say as well that the model of the robot that I am using is derived from the iCubGazeboV2_5_visuomanip, being the only modifications the addition of the arm FT plugins and skin plugins.

I have also other doubt that is regarding the joints position, velocity and acceleration. Is it supposed to be only composed of zeros just like in https://github.com/robotology/idyntree/blob/bf15223cb6793c5a54bf367bc996243f9185f0a3/examples/matlab/SixAxisFTOffsetEstimation/SixAxisFTOffsetEstimation.m#L30?

Thank you for all the help!
Diogo Silva

[traversaro]

In my simulation, iCub is standing just like in the following image:

For which FT sensors you want to compute the so-called sensor offset? If it is for the FT sensors in the arm, any link in torso and/or legs would give you the same results.

I have also other doubt that is regarding the joints position, velocity and acceleration. Is it supposed to be only composed of zeros just like in https://github.com/robotology/idyntree/blob/bf15223cb6793c5a54bf367bc996243f9185f0a3/examples/matlab/SixAxisFTOffsetEstimation/SixAxisFTOffsetEstimation.m#L30?

If you set the velocity and acceleration different from zero, you can compute the FT sensor measurements that you would have if the robot was moving at exactly that velocity and acceleration, but for removing FT offsets the robot is tipically kept still (i.e. with zero velocit and acceleration) to reduce the source of errors.

[diogofbsilva]

Hello @traversaro !

I already implemented the necessary code to extract the gravitational component of the right arm FT sensor measurements however the results I am obtaining are not the best, having some differences:

[ERROR] Fgravity0 - -20.9092 [ERROR] Fgravity1 - 0.172481 [ERROR] Fgravity2 - 0.573743 [ERROR] Flido0 - -12.9263 [ERROR] Flido1 - -6.34489 [ERROR] Flido2 - 15.6787

being Flido the values read from the FT sensor and Fgravity the values obtained using this iDynTree methods.

Below I will leave you with the code I am using and if you could take a look to see what I am doing wrong it would really help:
Here are some initialization: https://github.com/diogofbsilva/forceConv/blob/e772f018fb8b46a8cafcab072e8416a0b0306030/forceConv.cpp#L145-L151

Here are the initialization of the joint position, velocity and acceleration arrays and also grav_idyn: https://github.com/diogofbsilva/forceConv/blob/e772f018fb8b46a8cafcab072e8416a0b0306030/forceConv.cpp#L269-L280

Finally here is the rest of the code: https://github.com/diogofbsilva/forceConv/blob/e772f018fb8b46a8cafcab072e8416a0b0306030/forceConv.cpp#L421-L480

Look that I am updating the joint position vector qj with data from the port /icubSim/right_arm/state:o.

Note: This repo is not where I am simulating

Is there something I am doing wrong? Thank you!

[traversaro]

In https://github.com/diogofbsilva/forceConv/blob/e772f018fb8b46a8cafcab072e8416a0b0306030/forceConv.cpp#L348 seems to me that you are trying to set the gravity for the link "torso", but there is no "torso" link in your URDF model? In general, I suggest to check the return values for the functiosn that you call, to understand if they were successful or not.

Furthermore, in https://github.com/diogofbsilva/forceConv/blob/e772f018fb8b46a8cafcab072e8416a0b0306030/forceConv.cpp#L423-L425 you seem to assume that the joint serialization used for the visuomanip model is the one that matches the right arm (at least for the initial joints) but that is not ensure in anyway. To read data from a real iCub robot, I suggest you to use the remotecontrolboardremapper and loadReducedModel to load a YARP interface and a iDynTree model that share the same serialization, see for example the code in https://github.com/vvv-school/tutorial_impedance-control/blob/master/Module.cpp#L110 .

[diogofbsilva]

Thank you so much for your quick answer @traversaro !

Regarding, the use of the torso link yes that is a typo however that part of the code is not being used at the moment as it for the left arm and I am just using the right arm (the code is a bit messy).

Regarding the joint serialization I will look into the remotecontrolboardremapper and loadReducedModel and try to use it iny my code.

Thanks once again!

[diogofbsilva]

Hello @traversaro !!
My issues were indeed related to the joint serialization that I was not making correctly. Taking into account your suggestion I was able to have the gravity forces to match more or less the readings from the FT.

Thank you so much!