Anchor point update during slipping about consim HOT 10 CLOSED

in method A for updating anchor points you mention that there is no need to solve the QP and instead it could be done by exploiting the geometry.
in my understanding, the minimum displacement of p0 is such that the contact force is exactly at the boundary of the cone, meaning
f = f_projection
then by simple algebra, p0_new = K^(-1)(f_projection - D * x_new)
is this what you meant ? or should I compute it in terms of the velocity of the contact point where p0 slides with p itself along the vector v of the contact points?
p0_new = p0 + dtv_avg (where v_avg is some average velocity across the interval)

from consim.

I think your first guess is the correct one: p0_new = K^(-1)(f_projection - D * x_new)

from consim.

Digging a bit deeper into the code, there is a bit more into the logic rather than only resetting the anchor point to the current contact position.

in case the tangent force violates the friction cone, then in the model used, the spring breaks, hence the anchor point is not effective/useful anymore, and the dynamic friction coefficient is used to update the contact force at the boundary of the cone as is done here. The anchor point gets reset to the contact position (on the line below) only in the case when the sliding velocity becomes minimal, this is the point where we might want to actually fix it. This point is still a bit vague to me how this interacts with the rest of the simulation.

I did a simple check to see the difference between using a dynamic friction coefficient (breaking the spring) versus updating the anchor point to compute the average force (method A in the paper). Here are some results

the net force applied on a 1 kg point mass including the weight is [4., 0., 10.] N in the world frame. with mu = 0.3 this should result in 1 m /s^2 acceleration in the x-direction since the friction cone ,

For the exponential simulator I update the anchor point according to the equation in your earlier comment. For the euler simulator I simply rely on dynamic friction coefficient (breaking the spring)

the Ball positions and tangent forces surprisingly match

in this preliminary result the position is a better indicator, since the actual force used to integrate in the exponential simulator is computed as the average of the integral (method A in the paper). While the resulting force computed by the contact model is still due to the dynamic friction coefficient at the end of the integration step.

with the current implementation I end up computing the contact force twice for the exponential simulator, once inside the contact model (separate from the simulator), and once inside the simulation step by computing f = D delta_x
I will clean up the implementation of the contact model tomorrow so I can asses this in a more objective way and log the average contact force in the exponential simulator. But it seems that sliding friction might not be an issue that will create alot of trouble.

from consim.

Hi @hammoudbilal

thanks for the detailed report.

The anchor point gets reset to the contact position (on the line below) only in the case when the sliding velocity becomes minimal, this is the point where we might want to actually fix it.

Agreed. This threshold of 0.01 on the sliding velocity is clearly arbitrary and it cannot work in general. I find it more reasonable to update the anchor point all the time, and based on the equations we discussed above.

This point is still a bit vague to me how this interacts with the rest of the simulation.

Yes, it's not clear to me either. However, I'd rather to clean this up by removing the friction_flag, always updating the anchor point, and relying on the usual friction cone inequality (tangent_force > static_friction_coeff_ * normal_force ) to check whether the contact point is sliding.

I did a simple check to see the difference between using a dynamic friction coefficient (breaking the spring) versus updating the anchor point to compute the average force (method A in the paper). Here are some results

It's reassuring that results match, but this test is quite limited. I'm pretty sure there would be cases where results wouldn't match, so I'd rather make the two approaches more similar (as mentioned above) so that they're easier to compare.

I will clean up the implementation of the contact model tomorrow so I can asses this in a more objective way and log the average contact force in the exponential simulator.

That seems a good idea to me!

from consim.

for the qp anchor point update I start by constructing normal and tangent matrices here

ExponentialSimulator::checkFrictionCone() here now only checks if there is a violation and enables cone_flag_

then ExponentialSimulator::computeSlipping() here fills the inequality matrix and vector, solves the qp, updates x_start and recomputes the contact forces.

The only issue I can see is that we need to compute \int_{0}{dt} exp^{\tau A} d\tau
I tried to find a workaround for this since we avoid to compute it throughout the whole implementation but still no success, maybe you could take a look ?

from consim.

I think there is no way around computing the whole matrix exponential (is that what you refer to?) if we wanna use a standard QP solver such as eiquadprog. Let's not worry too much about performance right now. First we need to get a sound simulation. Then we can improve performance.

from consim.

back to this, I think this is the last major issue that is still stuck,
There is no problem in computing the slipping using projection of the average contact forces (method A) in the script.
However, for (method B) optimizing the anchor point slipping velocity, an issue rises when trying to compute the integral of the exponential (computing the average force while slipping in eq. 31), any idea if there is a way to approximate this integral ?

from consim.

Regarding method A, yesterday I've fixed some bugs in the c++ code (I'll assume that from now on we are both working on branch master) and I've tested it using your script sliding_point_mass.py. It seems to work. However, given the results of my test on the importance of using the double integral, we should update method A to also use the double integral. The theory in the paper is already updated, so it's just a matter of implementation and it shouldn't be hard. In the meantime @olimexsmart is investigating the parallelization of the computations of first and double integral (that would basically halve computation time).

A little issue I haven't tackled yet is that when contacts are slipping we should also consider the velocity of the anchor points $\dot{p}_0$. Maybe we could just use the average velocity of the anchor point computed as $(p_0^+ - p_0) / dt$. This should be also implemented for the Euler simulator though.

Regarding method B, what issue rises when computing the integral of the exponential? How are you computing it? If you provide me with some code to reproduce the issue I can look into it.

from consim.

I added both methods,

• for method A, f_avg and f_avg2 are checked separately and projected accordingly, there seems to be an offset when I compare the x_start from euler simulator and predictedX0_ computed through the force projection. I believe this is due to the fact that I update the anchor point based on f_projected but the contact position & velocity is that of the beginning of the integration step.
  predictedX0_ = invK*(f_projected - D*x(0))
  It might make sense to use the average x across the integration interval or the final x at the end of the integration, do you agree ?

• for method B, computing the integral of the exponential, I check if A is invertible then I use the closed form solution as
  intEdtA_ = invA*(e^{dt*A}-I)
  else I use the trapezoid rule by dividing sub_dt to 10 steps (just a heuristic for now). it seems that A is invertible for both the point mass and the quadruped though. QP still gives zero as a result, there might be a bug in setting up the constraints, I will check it tomorrow

from consim.

Method A

For updating the anchor point we should use the new contact positions, after integrating q and v. This is also what's written in the paper in equation (27).

Method B

When A is invertible you can indeed use the closed form solution. When the quadruped in on 3 or more feet then A is not invertible though (the contact Jacobian is no longer full-row rank), so you cannot use it. I've found a fast way to compute the integrals of expm, but it's a bit convoluted, and we should implement it in expokit. In the meantime you can keep testing method B in simple cases where A is invertible (e.g., point mass, quadruped on 2 feet).

from consim.