How to get contact forces?

[rohanpsingh]

Hi,

I am trying to investigate how to get the data for contact forces after generating the contact plan for a robot, but I am unable to find the appropriate method to do so. Can you please point me in the right direction? Thanks!

-Rohan

[stonneau]

Hi, in the current pipeline, you won't be able to obtain directly the contact forces. Indeed, in the planning phase, we are not really interested in the forces, only in verifying that we can obtain static equilibrium.

However they are computed to check static equilibrium using the robust-equilibrium-lib library. The library is called here: https://github.com/stonneau/hpp-rbprm/blob/master/src/stability/stability.cc#L238

But you would have to modify it to obtain the computed forces. https://github.com/andreadelprete/robust-equilibrium-lib/blob/master/include/robust-equilibrium-lib/static_equilibrium.hh

Make sure that you call the LP algorithm: STATIC_EQUILIBRIUM_ALGORITHM_LP

This lp computes a variable b that provides the contact force coefficients https://github.com/andreadelprete/robust-equilibrium-lib/blob/master/include/robust-equilibrium-lib/static_equilibrium.hh#L141

it is returned by this call; https://github.com/andreadelprete/robust-equilibrium-lib/blob/master/src/static_equilibrium.cpp#L212

I hope this helps !

[rohanpsingh]

Hi,

Thanks a lot for providing the information. I have been able to obtain the b_b0 vector using the information given above. However, I am having trouble interpreting the resulting vector.

The vector b_b0 returned by the 'm_solver->solve' call (https://github.com/andreadelprete/robust-equilibrium-lib/blob/master/src/static_equilibrium.cpp#L212) is a [(4n x 1) + 1] dimensional vector, where n is the number of contact limbs.

I see that the number of contact limbs is required to compute the rbprm::State of the config which is used to estimate the CoM of the robot. Ultimately, the CoM is required by the solver to obtain the contact forces.

So, can you please help me interpret the b_b0 vector? Does it include the torque coefficients as well? Also, are the contact forces defined at the contact points or at the base link?

Thanks again! -Rohan

[stonneau]

Hi, I refer to the page 8 of the journal paper presenting the planner to get the meaning of this https://stevetonneau.fr/files/publications/ijrr16/main.pdf

You will need the matrix V to retrieve the forces. I hope this helps

[rohanpsingh]

Hi, Thanks for your help. I found that for the HyQ robot, with the default settings, sometimes the value of normals in z-direction is negative for some limbs, at the q_init configuration. Also, in this configuration, the value of robustness calculated at https://github.com/andreadelprete/robust-equilibrium-lib/blob/master/src/static_equilibrium.cpp#L215 is negative.

Can you please tell why this could be happening?

Thanks -Rohan

[stonneau]

Hi, in general q_init is just a random configuration that I discard in my planning. If you want to initialize it properly with a configuration of your choice, you should also mention the contacts active here https://github.com/stonneau/hpp-rbprm-corba/blob/0e27d538c57929e4d01bf7cef3911f72629fa964/script/demos/darpa_hyq.py#L86

If you already do this, there may be a bug in the method setStartState