stack-of-tasks / pinocchio

A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
http://stack-of-tasks.github.io/pinocchio/
BSD 2-Clause "Simplified" License
1.81k stars 381 forks source link

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

Closed proyan closed 5 years ago

proyan commented 5 years ago

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 commented 5 years ago

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 commented 5 years ago

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 commented 5 years ago

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