Fix dynamics unittests involving finite difference function

[edantec]

This PR aims at solving issue https://github.com/Simple-Robotics/aligator/issues/127#issue-2131916127 about an error in the finite difference python function used in unittest. It also adds some missing unittest for MultibodyFreeFwdDynamics derivatives.

[ManifoldFR]

I'm working on this PR at the moment

[ManifoldFR]

The error in the Jx Jacobian of the multibody free fwd dynamics gives something like this (in log-scale, white means 0)

I set $\epsilon = 10^{-6}$ here. Not sure where the problem comes from.

I changed some things such as passing the $\epsilon$ finite-difference parameter to finite_diff

[ManifoldFR]

Setting

x0 = space.rand()
x0[:3] = 0.

Did increase the residual. I think there's some kind of high-frequency component that 1st-order finite differences just can't capture.

[ManifoldFR]

We'll be switching to centered finite differences then. I'll also be scaling $\epsilon$ in the finite differences by the norms of x and u.

I restored $\epsilon$ to 1e-6.

[ManifoldFR]

For further reference, here's what Crocoddyl does:

• selects the finite difference parameter $\epsilon = \sqrt{2\epsilon^\text{mach}}$ where $\epsilon^\text{mach}$ is the scalar-type machine precision, which should be 2.22045e-16 for doubles according to the C standard (hence $\epsilon = 2.1\times 10^{-8}$)
• tests for numerical tolerance $tol = \epsilon^{1/3}$ which is about 0.002762 for doubles (comments in the source code refer to this dead link, which I think is this file)
• does one-sided (1st order) forward finite differences with step $e_x = \epsilon \max(1, \|x \ominus 0_\mathcal{X} \|)$ for the state variable and $e_u = \epsilon\max(1,\|u\|)$ for the controls

@jorisv would you agree with the state of this PR? @edantec do you agree with my analysis of Crocoddyl's behaviour?

[edantec]

Looks good to me!

[jorisv]

@ManifoldFR We can also look at the 5 points finite difference made in roboptim

It's maybe more precise and the code estimate the truncation error and the rounding error.

[ManifoldFR]

@ManifoldFR We can also look at the 5 points finite difference made in roboptim

It's maybe more precise and the code estimate the truncation error and the rounding error.

That must be very accurate, wow