Implement the derivative of inverse dynamics

[gabrielenava]

Given a floating base system subject to a set of rigid constraints:

\begin{equation} M \dot{\nu} + h = B\tau + J_c^\top f \\ J_c \dot{\nu} + \dot{J}_c \nu = 0 \end{equation}

It would be nice to be able to compute the derivative of Eq. (1), namely:

\begin{equation} M \ddot{\nu} + \dot{M} \dot{\nu} + \dot{h} = B\dot{\tau} + \dot{J}_c^\top f + J_c^\top \dot{f}\\ 2\dot{J}_c \dot{\nu} + J_c \ddot{\nu} + \ddot{J}_c \nu = 0 \end{equation}

in order to design control laws that uses as control input the derivative of the joint torques, which is directly related to the motor current.

In order to use Eq. (2) for control purposes, the following calculations are required:

• inverse dynamics derivative: can be computed by differentiation of the Recursive Newton Euler algorithm.

• contact constraint bias acc derivative: can be obtained by properly differentiating the following passage of the RNEA algorithm:

  

• constact Jacobian derivative: namely, $\dot{J}_c$. Still to be defined.

How to proceed

1. Write down the algorithm's pseudocode;
2. Implement the contact constraint bias acc derivative;
3. Implement the full inverse dynamics derivative;
4. Discuss on how to implement $\dot{J}_c$;
5. Tests and debugging.

I will open all the issues when I start to address this activities.

[gabrielenava]

Related issue: https://github.com/robotology/idyntree/issues/85