Analytic Jacobian of the End-Effector Kinematic State with respect to the Joint State.

[kklankers]

Dear Pinocchio team,

first, thank you very much for developing this awesome library!

I am running into continues problems regarding an analytic differentiation. Before going into details, I created a minimal example of it, which you can find on GitHub: minimal example. Further, I am using a Franka URDF model, where I added another Link with a fixed joint. To my understanding, this should have no influence?

I want to accomplish the following: I have a function that computes the complete EE state (pose, velocity, acceleration) provided the complete joint state (position, velocity, acceleration).

Eigen::Matrix<double, 19, 1> compute_cartesian_state(
    const pinocchio::Model& model, pinocchio::FrameIndex frame_id,
    const Eigen::VectorXd& q, const Eigen::VectorXd& dq, const Eigen::VectorXd& ddq)
{
    pinocchio::Data data = pinocchio::Data(model);

    pinocchio::forwardKinematics(model, data, q, dq, ddq);
    pinocchio::updateFramePlacements(model, data);

    const pinocchio::SE3& frame_pose = data.oMf[frame_id];
    const auto& position = frame_pose.translation();
    const Eigen::Quaternion<double> quaternion(frame_pose.rotation());

    const pinocchio::Motion velocity = pinocchio::getFrameVelocity(model, data, frame_id, pinocchio::LOCAL_WORLD_ALIGNED);
    const pinocchio::Motion acceleration = pinocchio::getFrameAcceleration(model, data, frame_id, pinocchio::LOCAL_WORLD_ALIGNED);

    Eigen::Matrix<double, 19, 1> cartesian_state;
    cartesian_state << position,
                       quaternion.x(), quaternion.y(), quaternion.z(), quaternion.w(),
                       velocity.linear(),
                       velocity.angular(),
                       acceleration.linear(),
                       acceleration.angular();
    return cartesian_state;
}

Now, I want to compute the analytic Jacobian of this function. For debugging and comparison, I implemented a numerical differentiation function.

Eigen::MatrixXd compute_ee_state_jacobian_numerical(
    const pinocchio::Model& model, pinocchio::FrameIndex frame_id,
    const Eigen::VectorXd& q, const Eigen::VectorXd& dq, const Eigen::VectorXd& ddq,
    double eps = 1e-7)
{
    const int nv = q.size();
    Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(19, 3 * nv);
    Eigen::Matrix<double, 19, 1> ee_state_base = compute_cartesian_state(model, frame_id, q, dq, ddq);

    // Perturb q
    for (int i = 0; i < nv; ++i) {
        Eigen::VectorXd q_plus = q;
        q_plus(i) += eps;
        jacobian.col(i) = (compute_cartesian_state(model, frame_id, q_plus, dq, ddq) - ee_state_base) / eps;
    }
    // Perturb dq
    for (int i = 0; i < nv; ++i) {
        Eigen::VectorXd dq_plus = dq;
        dq_plus(i) += eps;
        jacobian.col(nv + i) = (compute_cartesian_state(model, frame_id, q, dq_plus, ddq) - ee_state_base) / eps;
    }
    // Perturb ddq
    for (int i = 0; i < nv; ++i) {
        Eigen::VectorXd ddq_plus = ddq;
        ddq_plus(i) += eps;
        jacobian.col(2 * nv + i) = (compute_cartesian_state(model, frame_id, q, dq, ddq_plus) - ee_state_base) / eps;
    }

    return jacobian;
}

I am implemented the analytic Jacobian function like this:

Eigen::MatrixXd compute_ee_state_jacobian_analytical(
    const pinocchio::Model& model, pinocchio::FrameIndex frame_id,
    const Eigen::VectorXd& q, const Eigen::VectorXd& dq, const Eigen::VectorXd& ddq)
{
    pinocchio::Data data = pinocchio::Data(model);
    const int nv = model.nv;
    Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(19, 3 * nv);

    pinocchio::computeForwardKinematicsDerivatives(model, data, q, dq, ddq);
    pinocchio::updateFramePlacements(model, data);
    Eigen::MatrixXd dv_dq(6, nv), dv_dv(6, nv), da_dq(6, nv), da_dv(6, nv), da_da(6, nv);
    pinocchio::getFrameAccelerationDerivatives(
        model, data, frame_id, pinocchio::LOCAL_WORLD_ALIGNED,
        dv_dq, dv_dv, da_dq, da_dv, da_da
    );

    const Eigen::MatrixXd& J = dv_dv; // This is the geometric Jacobian in world frame
    const pinocchio::Motion frame_vel = pinocchio::getFrameVelocity(model, data, frame_id, pinocchio::LOCAL_WORLD_ALIGNED);
    const Eigen::Vector3d& v = frame_vel.linear();
    const Eigen::Vector3d& w = frame_vel.angular();

    // The classical velocity derivative requires a correction term
    Eigen::MatrixXd dvc_dq = dv_dq.topRows(3) + pinocchio::skew(w) * J.topRows(3);
    const Eigen::Ref<const Eigen::MatrixXd> dw_dq = dv_dq.bottomRows(3);

    // The classical acceleration derivative requires a more complex correction term
    Eigen::MatrixXd d_wxv_dq = Eigen::MatrixXd::Zero(3, nv);
    for (int i = 0; i < nv; ++i) {
        Eigen::Vector3d dw_dq_i = dw_dq.col(i);
        d_wxv_dq.col(i) = pinocchio::skew(dw_dq_i) * v + pinocchio::skew(w) * dvc_dq.col(i);
    }
    Eigen::MatrixXd dac_dq = da_dq.topRows(3) - d_wxv_dq;

    // --- Assemble the full Jacobian ---
    // Partials with respect to q
    jacobian.block(0, 0, 3, nv) = J.topRows<3>();
    const pinocchio::SE3& ee_pose = data.oMf[frame_id];
    Eigen::Quaterniond q_current(ee_pose.rotation());
    Eigen::Matrix<double, 4, 3> E_quat;
    E_quat <<  q_current.w(),  q_current.z(), -q_current.y(),
              -q_current.z(),  q_current.w(),  q_current.x(),
               q_current.y(), -q_current.x(),  q_current.w(),
              -q_current.x(), -q_current.y(), -q_current.z();
    jacobian.block(3, 0, 4, nv) = 0.5 * E_quat * J.bottomRows<3>();
    jacobian.block(7, 0, 3, nv) = dvc_dq;
    jacobian.block(10, 0, 3, nv) = dw_dq;
    jacobian.block(13, 0, 3, nv) = dac_dq;
    jacobian.block(16, 0, 3, nv) = da_dq.bottomRows(3);

    // Partials with respect to dq
    jacobian.block(7, nv, 6, nv) = J;
    jacobian.block(13, nv, 6, nv) = da_dv;

    // Partials with respect to ddq
    jacobian.block(13, 2 * nv, 6, nv) = da_da;

    return jacobian;
}

When I compare the results of the numerical vs the analytical Jacobian, most of the parts are identical. The only error that persist is the associated with the "da_dq" term of the analytic Jacobian. Here is the print of the error (I set values smaller than 1e-6 to zero):

ERROR (Analytical - Numerical):
        0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0
        0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0
        0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0
        0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0
        0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0
        0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0
        0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0
        0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0
        0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0
        0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0
        0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0
        0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0
        0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0
 0.755665   1.62998  -1.14018   -1.3623 -0.226228   0.40849         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0
  1.31438  -1.25937  0.916614 0.0512549   1.32471   2.33937         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0
  -1.7761  0.733451 -0.700428  0.340086 -0.894987 -0.404963         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0
        0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0
        0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0
        0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0

----------------------------------------------------

Max absolute error: 2.33937

So far, my current understanding is:

The "getFrameAccelerationDerivatives" function should compute the derivatives wrt. to the "pinocchio::LOCAL_WORLD_ALIGNED" frame, which should be similar to the calculations in the numerical differentiation. For the terms "dv_dv, da_dv, da_da" i can confirm that this is the case, based on the comparison of the analytic and numerical results.

However, for the terms "dv_dq" and "da_dq" the terms are incorrect.

Now it gets very unclear for me, so please apologize if I misunderstand something. According to my research, "dv_dq" and "da_dq" are the spatial quantities, even if the "pinocchio::LOCAL_WORLD_ALIGNED" is set. Or maybe differently formulated, the calculate it from a perspective of a moving body frame, and therefore neglect effects of direct changes and frame rotation. Therefore, I need to correct the linear part of the "dv_dq" and "da_dq" terms compared to the numerical one. I need to add these effects back again.

For the "dv_dq" term, I corrected them via

Eigen::MatrixXd dvc_dq = dv_dq.topRows(3) + pinocchio::skew(w) \* J.topRows(3);

This correction seemed to be correct when comparing the results of the numerical and the analytical one.

But now I have a problem with the linear part of the "da_dq" term. According to my understanding, I have to apply the same correction, which is more complex since you need to account for centripetal and Coriolis effects. I used the following correction:

∂a_c​​/∂q = ∂q/∂a_pin ​​− (∂ω/∂q ​× v_c​ + ω×∂v_c/∂q​​)

    Eigen::MatrixXd d_wxv_dq = Eigen::MatrixXd::Zero(3, nv);
    for (int i = 0; i < nv; ++i) {
        Eigen::Vector3d dw_dq_i = dw_dq.col(i);
        d_wxv_dq.col(i) = pinocchio::skew(dw_dq_i) * v + pinocchio::skew(w) * dvc_dq.col(i);
    }
    Eigen::MatrixXd dac_dq = da_dq.topRows(3) - d_wxv_dq;

Sadly, this term is still not correct. I spend quite some time to find the correct values and transformations for the da_dq terms, but I could not make them match the numerical differentiation terms.

Is there something important missing? I am quite sure I have a conceptual misunderstanding here. I would be deeply thankful for advises and feedback regarding this problem.

[kklankers]

Just in case anyone is interested in the future:

I found the working solution and updated my GitHub repository. I needed to "correct" the linear part of da_dq via:

Eigen::MatrixXd dac_dq = da_dq.topRows(3) + pinocchio::skew(dw) * J.topRows(3);

Can anybody explain why the two following "corrections" are needed in my case?

Eigen::MatrixXd dvc_dq = dv_dq.topRows(3) + pinocchio::skew(w) * J.topRows(3);
Eigen::MatrixXd dac_dq = da_dq.topRows(3) + pinocchio::skew(dw) * J.topRows(3);

The full function looks like:

Eigen::MatrixXd compute_ee_state_jacobian_analytical(
    const pinocchio::Model& model, pinocchio::FrameIndex frame_id,
    const Eigen::VectorXd& q, const Eigen::VectorXd& dq, const Eigen::VectorXd& ddq)
{
    pinocchio::Data data = pinocchio::Data(model);
    const int nv = model.nv;
    Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(19, 3 * nv);

    pinocchio::computeForwardKinematicsDerivatives(model, data, q, dq, ddq);
    pinocchio::updateFramePlacements(model, data);
    Eigen::MatrixXd dv_dq(6, nv), dv_dv(6, nv), da_dq(6, nv), da_dv(6, nv), da_da(6, nv);
    pinocchio::getFrameAccelerationDerivatives(
        model, data, frame_id, pinocchio::LOCAL_WORLD_ALIGNED,
        dv_dq, dv_dv, da_dq, da_dv, da_da
    );

    const Eigen::MatrixXd& J = dv_dv; // This is the geometric Jacobian in world frame
    const pinocchio::Motion frame_vel = pinocchio::getFrameVelocity(model, data, frame_id, pinocchio::LOCAL_WORLD_ALIGNED);
    const Eigen::Vector3d& v = frame_vel.linear();
    const Eigen::Vector3d& w = frame_vel.angular();

    const pinocchio::Motion frame_acc = pinocchio::getFrameAcceleration(model, data, frame_id, pinocchio::LOCAL_WORLD_ALIGNED);
    const Eigen::Vector3d& dv = frame_acc.linear();
    const Eigen::Vector3d& dw = frame_acc.angular();

    // The classical velocity derivative requires a correction term
    Eigen::MatrixXd dvc_dq = dv_dq.topRows(3) + pinocchio::skew(w) * J.topRows(3);
    Eigen::MatrixXd dac_dq = da_dq.topRows(3) + pinocchio::skew(dw) * J.topRows(3);

    // --- Assemble the full Jacobian ---
    // Partials with respect to q
    jacobian.block(0, 0, 3, nv) = J.topRows<3>();
    const pinocchio::SE3& ee_pose = data.oMf[frame_id];
    Eigen::Quaterniond q_current(ee_pose.rotation());
    Eigen::Matrix<double, 4, 3> E_quat;
    E_quat <<  q_current.w(),  q_current.z(), -q_current.y(),
              -q_current.z(),  q_current.w(),  q_current.x(),
               q_current.y(), -q_current.x(),  q_current.w(),
              -q_current.x(), -q_current.y(), -q_current.z();
    jacobian.block(3, 0, 4, nv) = 0.5 * E_quat * J.bottomRows<3>();
    jacobian.block(7, 0, 3, nv) = dvc_dq;
    jacobian.block(10, 0, 3, nv) = dv_dq.bottomRows(3);
    jacobian.block(13, 0, 3, nv) = dac_dq;
    jacobian.block(16, 0, 3, nv) = da_dq.bottomRows(3);

    // Partials with respect to dq
    jacobian.block(7, nv, 6, nv) = J;
    jacobian.block(13, nv, 6, nv) = da_dv;

    // Partials with respect to ddq
    jacobian.block(13, 2 * nv, 6, nv) = da_da;

    return jacobian;
}