Computation of Jacobian of $Ag(q) v$ with respect to $q$, when $n_q > n_v$

[HiroIshida]

Closed due to my basic mathematical mistake
$h = Ag(q) v$ is a frequently appearing equation in literature on trajectory optimization using centroidal dynamics, where $h$ is the centroidal angular momentum, Ag is CMM, $q$ and $v$ are generalized position and velocity. (e.g. Eq. (7c) in Dai+2014, Eq. (5) in Kuindersma+2016)

To enforce this relation as equality constraints in trajectory optimization, we need to compute the Jacobian $J = \frac{\partial A_g(q) v}{\partial q}$. If $n_q = n_v$, we can compute the Jacobian using a pinocchio-provided function dccrba which computes the $dA_g(q)/dt$ matrix. Namely $dA_g(q)/dt = \frac{\partial A_g(q)}{\partial q} \cdot \frac{dq}{dt} = J$.
However, this is not possible if the pinocchio representation of the floating body's orientation uses a quaternion. In this case, the representation of $q$ has a redundant dimension and $n_q > n_v$.
If anybody has worked on the computation of the Jacobian in such a case, I'd be grateful if you could give me some ideas.

reference

• Kuindersma, Scott, et al. "Optimization-based locomotion planning, estimation, and control design for the atlas humanoid robot." Autonomous robots 40 (2016): 429-455.
• Dai, Hongkai, Andrés Valenzuela, and Russ Tedrake. "Whole-body motion planning with centroidal dynamics and full kinematics." 2014 IEEE-RAS International Conference on Humanoid Robots. IEEE, 2014.

[matheecs]

The notebook from JNRH'2023 may give some ideas:

You can set up the optimization variables to contain a configuration $q$, but in that case you must enforce the normalization constraint $q[4:7] == 1$. More efficiently, we represent the decision variable to be $dq \in R^{n_v}$ a small displacement from the robot reference configuration robot.q0. Then $q = robot.q0 \oplus dq$ becomes a function of the decision variable, and no constraint need to be additionally enforce. The nice aspect of Casadi is that you can define e.g var_dq to be your decision variable, then call var_q the integration of $dq$ from robot.q0, and use var_q as if it was you variable for the simple robot case. More about that in practice later.