Questions about computing of dAcc/dv and dAcc/dq

[MrCCaesar]

From paperAnalytical Derivatives of Rigid Body Dynamics Algorithms in Pinocchio's citation, derivative of Link Spatial Acceleration with respect to generalized joint velocity is

$$\frac{\partial a_i}{\partial \dot{q}_i}=\frac{\partial {}^{i}X_{\lambda(i)}}{\partial \dot{q}_{i}}a_{\lambda(i)}+{}^{i}X_{\lambda(i)}\frac{\partial a_{\lambda(i)}}{\partial \dot{q}_i} + \frac{\partial S_i}{\partial \dot{q}_i} \ddot{q}_i + \frac{\partial c_J}{\partial \dot{q}_i} + \frac{\partial V_i}{\partial \dot{q}_i} \times V_J + V_i \times \frac{\partial V_J}{\partial \dot{q}_i}$$

With assumptions of joint types in paper, I got this simplified as:

$$\frac{\partial a_i}{\partial \dot{q}_i} = S_i \times V_J$$

My question is this seems not consistent with code in pinocchio:

case LOCAL:
          // also computes dvec/dq
          if (parent > 0)
          {
            vtmp = oMlast.actInv(data.ov[parent]);
            motionSet::motionAction(vtmp, a_partial_da_cols, v_partial_dq_cols);
          }

          if (parent > 0)
            vtmp -= data.v[jointId];
          else
            vtmp = -data.v[jointId];

          motionSet::motionAction(vtmp, a_partial_da_cols, a_partial_dv_cols);
          motionSet::se3ActionInverse<ADDTO>(oMlast, dJcols, a_partial_dv_cols);
          break;

in JointAccelerationDerivativesBackwardStep Class's algo() method which I think do the following computation:

$$\frac{\partial a_i}{\partial \dot{q}_i} = -V_J \times S_i + {}^{O}X_{i} \dot{J}_i$$

Thanks for your help in advance.

[jcarpent]

In fact, it is consistent (as far as I remember). In the code of Pinocchio, we use quantities that are expressed in the WORLD coordinate systems for fast computations. Thus, we must add missing terms to get a complete formula in the LOCAL frame.

[MrCCaesar]

Sorry, I am still confused. For me, it seems that

motionSet::motionAction(vtmp, a_partial_da_cols, a_partial_dv_cols);

is doing

$$-{}^{i}V_J \times {}^{i}S_i$$

in the right frame. Could you please give a bit more details about this? Thanks.

[jcarpent]

I don’t understand your question

[MrCCaesar]

Sorry about this. I mean after transform

case LOCAL:
          motionSet::se3ActionInverse(oMlast, Jcols, a_partial_da_cols);
          break;

and

if (parent > 0)
          {
            vtmp = oMlast.actInv(data.ov[parent]);
            motionSet::motionAction(vtmp, a_partial_da_cols, v_partial_dq_cols);
          }

          if (parent > 0)
            vtmp -= data.v[jointId];
          else
            vtmp = -data.v[jointId];

looks lile

$$-{}^{i}V_J = vtmp. {}^{i}S_i = a\_partial\_da\_cols$$

Thus

motionSet::motionAction(vtmp, a_partial_da_cols, a_partial_dv_cols);

has done all the work for computing

$${}^{i}S_{i} \times {}^{i}V_{J}$$

So, why this still miss terms and what's missing? Thank you.