Skip to content

Hotfix (#75) - #76

Merged
mmmarinho merged 2 commits into
dqrobotics:masterfrom
juanjqo:hotfix
Nov 4, 2026
Merged

mmmarinho merged 2 commits into
dqrobotics:masterfrom
juanjqo:hotfix

Conversation

@juanjqo

@juanjqo juanjqo commented Nov 4, 2026

Copy link
Copy Markdown
Member

Hi @mmmarinho, this PR fixes #75.

Modifications:

  • raw_pose_jacobian_by_chain() now follows pose_jacobian_by_chain() from MATLAB. The Jacobian has one column per joint up to ith, as in MATLAB.
  • The matrix is built from the actual number of columns of each block, which removes the out-of-bounds read.
  • Added raw_pose_jacobian_by_chain(q, to_ith_chain) for complete chains. pose_jacobian(q) uses it, in the same way that MATLAB's pose_jacobian(q) calls pose_jacobian_by_chain(q).
11:58:35: Starting /home/juanjqo/Documents/dqrobotics_tests/cpp/serial_whole_body/build/terminal/serial_whole_body...
pose_jacobian(q, 0) (8x1) =
  0.1478
       0
       0
  0.4777
-0.07953
0.006102
-0.01973
  0.0246

pose_jacobian(q, 1) (8x2) =
   0.1848     0.1848
 -0.01767    0.01767
  -0.3531     0.3531
   0.3014     0.3014
 -0.04946   -0.05092
  0.06643   -0.05116
 -0.01539  -0.009506
  0.01619    0.04535

pose_jacobian(q, 2) (8x3) =
   0.2323     0.1086     0.2323
   0.1231    0.04836    -0.1231
 -0.01235      0.482    0.01235
   0.4252   -0.05931     0.4252
   -0.143    0.01725    -0.1329
 0.009697  0.0003862   0.009487
 -0.01651    0.00286    -0.0186
  0.07486    0.05514    0.07588

pose_jacobian(q, 7) (8x8) =
  -0.03276     0.01493      0.2094      0.1621     0.07541      0.2513    -0.03276      0.1237
    0.2928    0.002449      0.2831    -0.03832      0.2954    -0.03695     -0.2928  -3.362e-18
    0.4016    -0.05444      0.3536      -0.024      0.3959    -0.04006     -0.4016   8.584e-19
  -0.04355     -0.4968    -0.03106      0.4708     0.01825      0.4288    -0.04355      0.4845
 -0.003975     0.01944   -0.003866      -0.168      -0.022     -0.3092      0.0185     -0.1613
   -0.1181     0.02179     -0.1033     0.00559     -0.1131     0.02838      0.1153   -0.000986
   0.08769     0.01964      0.0871    0.005716     0.08967     0.06803    -0.08754     -0.0197
   0.01725   -0.001461     0.02451     0.05861    -0.02454        0.19     0.01838     0.04119

Kind regards,

Juancho

…obian returning a wrong size matrix for intermediate links

- raw_pose_jacobian_by_chain() now follows pose_jacobian_by_chain() from MATLAB. The Jacobian has one column per joint up to the requested link.
- Fixed the out-of-bounds read when copying the Jacobian of a partial chain.
- Added raw_pose_jacobian_by_chain(q, to_ith_chain) for the complete chains, used by pose_jacobian(q).
@juanjqo juanjqo changed the title Hotfix to fix #75 Hotfix (#75) Nov 4, 2026
@mmmarinho

mmmarinho commented Nov 4, 2026 •

Copy link
Copy Markdown
Member

Hi @juanjqo, thanks for the careful fix. I built this branch and checked it against the repro in #75, and it looks good to merge.

What I verified.

  1. pose_jacobian(q, ith) now returns an 8x(ith+1) matrix for every ith, and the values agree with the MATLAB output posted in [LANGUAGE INCOMPATIBILITY] DQ Serial WholeBody::pose jacobian behaves differently from the MATLAB version #75 for every column I compared. I checked ith equal to 0, 1, 2, 5, 6, 7, 8, 10 and 13 for two Pandas in series and for a holonomic base plus a Panda.
  2. pose_jacobian(q) agrees with central finite differences of fkm(q) up to numerical precision, and the read out of bounds in the old column loop is gone.
  3. The index translation is faithful, because MATLAB passes the chain loop index i to obj.fkm(q,i), which counts joints from 1, and get_chain_and_link_from_index(i-1) reproduces exactly that pose. Likewise get_reference_frame() in the case i == 0 reproduces obj.fkm(q,0), which equals reference_frame * raw_fkm(q,0) in MATLAB, since raw_fkm(q,0) is an empty product. The fallback to the full chain for DQ_HolonomicBase also matches MATLAB, where DQ_HolonomicBase.fkm(q,~) ignores the link index, and it avoids the throw that C++ raises for fkm(q, to_ith_link != 2).

Two notes

  1. One gap with MATLAB remains around the effector. The C++ fkm(q, dim-1) appends curr_effector_, while MATLAB fkm(q, ith) never applies the effector, so the multiplier of the partial chain diverges from MATLAB when it lands on the last link of an intermediate chain and set_effector() was used.
  2. This PR also pins down what might be a bug in MATLAB, but I don't have the bandwidth to investigate this further. In pose_jacobian_by_chain the partial branch uses hamiplus8(obj.fkm(q,i)), which counts joints, while the branch for complete chains uses hamiplus8(obj.fkm_by_chain(q,i)), which multiplies the first i chains and therefore gives the base of chain i. Finite differences of fkm(q, ith) confirm that the second one is the correct multiplier. After this merge pose_jacobian(q, ith) matches MATLAB, but pose_jacobian(q) and pose_jacobian(q, n-1) are no longer equal. I will open a separate issue with a See below the minimal finite difference repro so the MATLAB side is fixed first and we mirror the change here afterwards.

This is the code in case someone wants to check it further.

#include <dqrobotics/DQ.h>
#include <dqrobotics/robot_modeling/DQ_SerialWholeBody.h>
#include <dqrobotics/robot_modeling/DQ_HolonomicBase.h>
#include <dqrobotics/robots/FrankaEmikaPandaRobot.h>
#include <iostream>
#include <cmath>

using namespace DQ_robotics;

static double fd_check(DQ_SerialWholeBody& robot, const VectorXd& q, int ith, int ncols_expected)
{
    MatrixXd J;
    if (ith < 0) J = robot.pose_jacobian(q);
    else         J = robot.pose_jacobian(q, ith);
    std::cout << "ith=" << ith << " -> " << J.rows() << "x" << J.cols();
    if (J.cols() != ncols_expected) { std::cout << "  WRONG SIZE (expected " << ncols_expected << ")\n"; return 1e9; }

    const double h = 1e-6;
    double max_err = 0.0;
    VectorXd qp = q, qm = q;
    for (int j = 0; j < J.cols(); j++)
    {
        qp = q; qm = q;
        qp(j) += h; qm(j) -= h;
        VectorXd vec_p, vec_m;
        if (ith < 0) vec_p = robot.fkm(qp).vec8(); else vec_p = robot.fkm(qp, ith).vec8();
        if (ith < 0) vec_m = robot.fkm(qm).vec8(); else vec_m = robot.fkm(qm, ith).vec8();
        VectorXd fd = (vec_p - vec_m) / (2 * h);
        max_err = std::max(max_err, (fd - J.col(j)).lpNorm<Eigen::Infinity>());
    }
    std::cout << "  max|J_fd - J| = " << max_err << (max_err < 1e-5 ? "  OK" : "  MISMATCH") << "\n";
    return max_err;
}

int main()
{
    {
        std::cout << "=== two Pandas ===\n";
        DQ_SerialWholeBody robot(std::make_shared<DQ_SerialManipulatorMDH>(FrankaEmikaPandaRobot::kinematics()));
        robot.add(std::make_shared<DQ_SerialManipulatorMDH>(FrankaEmikaPandaRobot::kinematics()));
        const int n = robot.get_dim_configuration_space();
        VectorXd q(n);
        for (int i = 0; i < n; i++) q(i) = 0.1 * (i + 1) - 0.7;
        double worst = 0;
        for (int ith : {0,1,2,5,6,7,8,10,13}) worst = std::max(worst, fd_check(robot, q, ith, ith + 1));
        worst = std::max(worst, fd_check(robot, q, -1, n));
        std::cout << "worst err (2 pandas): " << worst << "\n";
    }
    {
        std::cout << "=== holonomic base + Panda ===\n";
        DQ_SerialWholeBody robot(std::make_shared<DQ_HolonomicBase>());
        robot.add(std::make_shared<DQ_SerialManipulatorMDH>(FrankaEmikaPandaRobot::kinematics()));
        const int n = robot.get_dim_configuration_space(); // 3 + 7
        VectorXd q(n);
        for (int i = 0; i < n; i++) q(i) = 0.1 * (i + 1) - 0.7;
        double worst = 0;
        for (int ith : {2,3,4,9}) worst = std::max(worst, fd_check(robot, q, ith, ith + 1));
        worst = std::max(worst, fd_check(robot, q, -1, n));
        std::cout << "worst err (base+panda): " << worst << "\n";
    }
    return 0;
}

Output

=== two Pandas ===
ith=0 -> 8x1  max|J_fd - J| = 0.00986385  MISMATCH
ith=1 -> 8x2  max|J_fd - J| = 0.00729175  MISMATCH
ith=2 -> 8x3  max|J_fd - J| = 0.00995404  MISMATCH
ith=5 -> 8x6  max|J_fd - J| = 0.00804811  MISMATCH
ith=6 -> 8x7  max|J_fd - J| = 0.0102614  MISMATCH
ith=7 -> 8x8  max|J_fd - J| = 0.528007  MISMATCH
ith=8 -> 8x9  max|J_fd - J| = 0.685762  MISMATCH
ith=10 -> 8x11  max|J_fd - J| = 0.695775  MISMATCH
ith=13 -> 8x14  max|J_fd - J| = 0.681906  MISMATCH
ith=-1 -> 8x14  max|J_fd - J| = 1.563e-10  OK
worst err (2 pandas): 0.695775
=== holonomic base + Panda ===
ith=2 -> 8x3  max|J_fd - J| = 0.146237  MISMATCH
ith=3 -> 8x4  max|J_fd - J| = 4.24694e-11  OK
ith=4 -> 8x5  max|J_fd - J| = 2.92643e-11  OK
ith=9 -> 8x10  max|J_fd - J| = 1.34305e-10  OK
ith=-1 -> 8x10  max|J_fd - J| = 1.34305e-10  OK
worst err (base+panda): 0.146237

@mmmarinho
mmmarinho merged commit 5801c6f into dqrobotics:master Nov 4, 2026
3 of 6 checks passed
@juanjqo

juanjqo commented Nov 4, 2026

Copy link
Copy Markdown
Member Author

Thanks, @mmmarinho.

I'll investigate the potential bug in the MATLAB version.

Kind regards,

Juancho

@bvadorno

Copy link
Copy Markdown
Member

Thanks, @mmmarinho!

One gap with MATLAB remains around the effector. The C++ fkm(q, dim-1) appends curr_effector_, while MATLAB fkm(q, ith) never applies the effector, so the multiplier of the partial chain diverges from MATLAB when it lands on the last link of an intermediate chain and set_effector() was used.

This should be very easy to fix.

This PR also pins down what might be a bug in MATLAB, but I don't have the bandwidth to investigate this further. In pose_jacobian_by_chain the partial branch uses hamiplus8(obj.fkm(q,i)), which counts joints, while the branch for complete chains uses hamiplus8(obj.fkm_by_chain(q,i)), which multiplies the first i chains and therefore gives the base of chain i. Finite differences of fkm(q, ith) confirm that the second one is the correct multiplier. After this merge pose_jacobian(q, ith) matches MATLAB, but pose_jacobian(q) and pose_jacobian(q, n-1) are no longer equal. I will open a separate issue with a See below the minimal finite difference repro so the MATLAB side is fixed first and we mirror the change here afterwards.

This will indeed require a bit more investigation.

@juanjqo, after you submit a pull request on MATLAB, please assign me as the reviewer. Please also ensure that the existing examples run without issues after the modifications.

Kind regards,
Bruno

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

[LANGUAGE INCOMPATIBILITY] DQ Serial WholeBody::pose jacobian behaves differently from the MATLAB version

3 participants