Error when verifying rnea derivatives (with and w/o fext) by finite differencing

[proyan]

I made this small script (attached) to test the rnea-derivatives with external force from Pinocchio. Here I am verifying derivatives wrt q.

1) I first verify that the rnea-derivative without external force are indeed (dM/dq ddq + d (bias)/dq). 2) Then I compute the dJ/dq for the set of contacts, and verify that dJdt = dJdq v 3) Then I try to verify that rnea_derivative_without_fext - (dJdq)^T data.lambda_c == rnea_derivative_with_fext

on Point 3, I fail. I am unable to understand what the issue is here. Could you please explain why this discrepency is there?

ques.py.zip

[nmansard]

I checked on my side that the rnea derivative (with and without forces) matches the finite-diff of rnea. I also did the same with aba and checked that the derivatives are linked through M^-1. As we already checked the derivatives of the jacobians by finite diff, and that rnea with forces is rnea plus jacobian transpose time forces, everything should match.

You should check first that rnea with forces is consistent with your jacobian. Could you do that?

[proyan]

I checked that the jacobian is consistent with the rnea. It is.

rnea_with_ext_force =se3.rnea(robot.model, robot.data, q, v ,ddq_ref, force_vec) rnea_wo_ext_force =se3.rnea(robot.model, robot.data, q, v ,ddq_ref) assert(np.isclose(rnea_with_ext_force, rnea_wo_ext_force-np.matmul(Jc_ref.T, lambda_ref)).all())

[nmansard]

@proyan I close this issue, feel free to reopen if need be.