How to ensure the result satisfies manifold structure when calculating partial derivatives with CppAD?

[Zionshang]

When using CppAD to calculate the partial derivative of a variable with respect to q, how can I ensure the result satisfies the manifold structure? That is, the Jacobian matrix should have nv columns instead of nq.

For example, when I use CppAD to calculate the partial derivatives of a with respect to q in the aba function, the resulting Jacobian matrix has nv rows and nq columns.

However, the analytical derivatives of pinocchio data.ddq_dq result in nv columns.

I want to obtain the same result as pinocchio's analytical derivatives using CppAD.

In Pinocchio's example, only dtau_dv are shown, and there is no dtau_dq.

I'd appreciate it if you could provide some ideas!

[Zionshang]

I've noticed that the tangent space matrix might be of some help. The tangent space matrix $S\left( q \right) \in \mathbb{R} ^{n_q\times n_v}$ defines a linear map from the velocity space $\mathbb{R} ^{n_v}$ to the coordinate space $\mathbb{R} ^{n_q}$:

$$ \delta q=S\left( q \right) \cdot \delta v $$

Does Pinocchio provide some function for calculating the tangent matrices?

[ManifoldFR]

Hello @Zionshang !

There is a similar issue with CasADi where differentiating an expression by the configuration vector q results (for non-Euclidean configuration spaces, e.g. those with floating bases for quadrupeds, bipeds, quadrotors...) in a Jacobian that has the wrong dimensions.

What we usually do for CasADi and CppAD is write the expression using another, dependent expression $\tilde{q} = q_0 \oplus \mathrm{\delta q}$ where $\mathrm{\delta q} \in \mathbf{R}^{n_v}$ is gonna be the (Euclidean space) variable with respect to which we differentiate (and evaluate the derivative at $\delta\mathrm{q} = 0$). What we get then is

$$ \frac{\partial f(\tilde q)} {\partial \delta \mathrm{q} } \mid_{\delta q=0} = \frac{\partial f}{\partial \tilde{q} } \cdot \left(\frac{\partial (q_0\oplus\delta\mathrm{q}) } { \partial \delta \mathrm{q} } \Bigg|_{\delta q=0}\right) $$

The matrix between parentheses should be $S(q_0)$, but using the expression trick above you actually never need it when using CasADi or CppAD autodiff.

You can see examples of this in the CasADi examples and unit tests in the Pinocchio repo.

[Zionshang]

Completely solved my problem, thank you very much！