Question about frame-velocity derivatives for LOCAL_WORLD_ALIGNED

[larsrpe]

I just had a look into the unit test of Frames-Derivatives: https://github.com/stack-of-tasks/pinocchio/blob/master/unittest/frames-derivatives.cpp . I noted that in the finite difference test for partial_vel_local_world_aligned_partial_q the test adds an extra element when computing the finite difference:

 // dvel/dq
  Eigen::VectorXd q_plus(q), v_eps(Eigen::VectorXd::Zero(model.nv));
  forwardKinematics(model,data_ref,q,v);
  updateFramePlacements(model,data_ref);

  for(int k = 0; k < model.nv; ++k)
  {
    v_eps[k] += alpha;
    q_plus = integrate(model,q,v_eps);
    forwardKinematics(model,data_plus,q_plus,v);
    updateFramePlacements(model,data_plus);
   
   // ------------ the snippet below -------------- //
    Motion v_plus_local_world_aligned = getFrameVelocity(model,data_plus,frameId,LOCAL_WORLD_ALIGNED);
    SE3::Vector3 trans = data_plus.oMf[frameId].translation() - data_ref.oMf[frameId].translation();
    v_plus_local_world_aligned.linear() -= v_plus_local_world_aligned.angular().cross(trans);
  // ------------ the snippet above -------------- //

    partial_dq_fd.col(k) = (getFrameVelocity(model,data_plus,frameId,WORLD) - v0).toVector()/alpha;
    partial_dq_fd_local_world_aligned.col(k) = (v_plus_local_world_aligned - v0_local_world_aligned).toVector()/alpha;
    partial_dq_fd_local.col(k) = (getFrameVelocity(model,data_plus,frameId,LOCAL) - v0_local).toVector()/alpha;
    v_eps[k] -= alpha;
  }

Instead of computing $\Delta v = v^+ -v$ , it uses $\Delta v = v^+ -\omega^+ \times (r^+ -r) -v$.

I am sure the existing implementation is correct, and that there probably is som combination of Spatial and Lie-algebra here which I quite don't understand.

However I don't see howe the above implementation aligns with the definition of a partial derivative.

[stephane-caron]

In case of doubt, always write down motion vectors in full, i.e. from which frame, to which frame, expressed in which frame. Let's denote by $0$ the world frame, $l$ the local frame and $a$ the world-aligned local frame:

• ${}_a V_{0l}$ is the velocity of the transform from $l$ to $0$, expressed in frame $a$ (see e.g. this notebook for a refresher)
• partial_dq_local_world_aligned is $\frac{\partial {}_a V_{0l}}{\partial q}$, this test computes it by finite differences, coordinate by coordinate
• consider coordinate $k$: we start by integrating $q^+_k = q \oplus \alpha e_k$, then we get what you wrote $v^+$ which is in full ${}_a V_{al}(q^+_k)$
• now note that both $a$ and $l$ may move during integration of $\alpha e_k$, thus v_plus_local_world_aligned is initially some ${}_{a^+} V_{a^+ l^+}$: it is a velocity in the new frame $a^+$, therefore we cannot add it to the velocity ${}_a V_{al}$ in the old frame $a$
• we can express the motion vector ${}_a V_{a^+ l^+}$ from ${}_{a^+} V_{a^+ l^+}$ by applying the Varignon formula to the linear term $v_a = v_{a^+} - \omega \times (r_{a^+} - r_a)$ (in general we would apply the adjoint matrix $\mathrm{Ad}_{T_{aa^+}}$, but here Varignon works as well since $a$ and $a^+$ have the same orientation)
• once in the same frame we can calculate the finite-diff. approximation $\frac{1}{\alpha} ({}_a V_{a^+ l^+} - {}_a V_{al})$

Hoping this helps!

[larsrpe]

Thank you for the explanation. I will look into the notebook, and try to catch up on the Vector Algebra!

[larsrpe]

Why does this not also apply for frame=LOCAL. Isn't that frame also moving?