Error in test contact

In test contact, the numerical errors are below the threshold, but for some initialization. Looking at it, it seems that they are above the noise. I start digging into it. To reproduce the error, use the following initialization of x and u at line 274 of test_contacts.py

x[:3] = 0
x[3:7] = [1,0,0,0]
x[7:] = 0
u[:] = 0

I believe this is coming from the state cost, when there is a free flyer.

changed milestone to %Towards python version 1.0

assigned to @proyan

mentioned in merge request !126 (merged)

See !126 (merged) for a patch, please merge it.

I located the error in StatePinocchio.Jdiff.

I'm quite sure that it is just the problem of a rotation matrix. I'm fixing it and adding a unit-test to check the SE3 case.

mentioned in issue #125 (closed)

Weird. I try to test the jdiff with finite differencing, and it works. Then I try to test the CostModelState.Rx with finite differencing, and it also works.

However, ActionModelNumdiff is doing the same thing, but it isn't working.

test_jdiff.py

Ok, I can reproduce the error if I take x_ref=state.zero() in the above script.

The problem comes from doing finite differences when the difference between the quaternions is pi. By definition of pinocchio, diff(q1,q2) returns values between pi and -pi. As a result, fd becomes unstable when diff(q1,q2) is exactly, or close to pi, since the increments could be positive or negative.

Attached script plots the errors around this value. plot_jdiff.py

Your patch is already taking care of this instability by making sure that the freeflyer quaternions are always [0 0 0 1].

A more robust approach might be to use state.rand(), and check that the difference is not pi for free-flyers. However, that seems to be too much of work for the unit-test. Perhaps that might be a low-priority task for the cpp case.

I would suggest not taking any action, and closing this issue.

This is bad, as num-diff is typically used in practice when we are to lazy to compute the derivatives. I think it is necessary, at least, assert that this case is not happening. Could you implement an assert, and then open an issue to advertise this assert?

Where would you put the assert? The error is specific for finite differencing of the state. I don't see this happening anywhere other than the costmodelstate, for which we already have a jdiff implemented.

In the numdiff class, maybe? That might be quite ugly code.

mentioned in merge request !146 (merged)

Please @proyan could you finalize this one?

mentioned in merge request !148 (merged)

Fixed by !148 (merged)

closed

mentioned in commit 975e94d5