Check for pulling force

[hammoudbilal]

Hi @andreadelprete, for the euler simulator, whenever we compute a contact force that turns out to exert a pull, we set it manually to zero to avoid a pulling force, this is implemented in the link below https://github.com/andreadelprete/consim/blob/master/src/contact.cpp#L48

however, computing the force the way we do currently in con_sim, it seems to me that we have no way of setting this pull force to zero, most likely this explains the different bounce back during the ball drop between both simulators. The easiest solution I can think of is to set f_projection during checking the friction cone to zero in case this occurs. and use dv_projection = Minv(tau - h + J.T f_projection) however, I cannot think of a solution for the case when there is no cone violation.

we could use this method to integrate all the time, but we lose the advantage of the closed form second order solution for the positions.

how do you think I should proceed with this ?

[hammoudbilal]

this is indeed the case, setting the contact force to zero in case of an average pulling force fixes the bouncing issue in the ball drop test. https://github.com/andreadelprete/consim/blob/bhammoud_cpp_exp_integrator/src/simulator.cpp#L362

friction cone is implemented, i need to write an example before I do a pull request

[andreadelprete]

The easiest solution I can think of is to set f_projection during checking the friction cone to zero in case this occurs. and use dv_projection = Minv(tau - h + J.T f_projection) however, I cannot think of a solution for the case when there is no cone violation.

we could use this method to integrate all the time, but we lose the advantage of the closed form second order solution for the positions.

It's true that by using this approach we lose the closed-form solution for integrating the robot configuration, which is a pity. However, I don't know how much we gain by using the double integral rather than either i) neglecting the contribution of accelerations when integrating positions, or ii) compute an approximate double integral based on the assumption that contact forces are constant at their average value (computed with the integral). The second option would be something like this:

int_x = compute_integral(...);
f_avg = D*int_x/dt + K*p0;
f_proj = project_inside_cone(f_avg);
dv_mean = dv_bar + Minv * J.T * f_proj
v_mean = v + 0.5*dt*dv_mean
v += dt*dv_mean
q = se3.integrate(model, q, v_mean*dt)

I expect this method to give results that are very similar to the ones we get with the double integral, but this should be checked.

[hammoudbilal]

for now I have it implemented this way. However this reduces the time step we have to use to keep the simulation perfectly matching with euler simulation. I still have some code cleaning to do, I will do a pull request tomorrow.

For the projection, what I do is compute the angle using atan2(fy_avg, fx_ang) then project mu*fz on the x&y direction whenever it violates the cone, is this the proper way of implementing it ? or is there a more formal way of how people usually do it ?

[andreadelprete]

for now I have it implemented this way. However this reduces the time step we have to use to keep the simulation perfectly matching with euler simulation.

Yes, this is the only advantage of using a small time step even for a linear system: more accurate contact detection. The only workaround I can see would be to try and detect the exact time of contact switch and then use adaptive time steps. However, I don't think that's what we wanna do right now.

For the projection, what I do is compute the angle using atan2(fy_avg, fx_ang) then project mu*fz on the x&y direction whenever it violates the cone, is this the proper way of implementing it ? or is there a more formal way of how people usually do it ?

It looks fine to me!