`getJointJacobian` works weirdly for independent robots

[someilay]

Bug description

I have an urdf that contains two independent identical manipulators. I noticed that getJointJacobian works weirdly for independent robots

Reproduction steps

C++ code:

#include <iostream>
#include <Eigen/Dense>
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include <pinocchio/algorithm/kinematics-derivatives.hpp>
#include <pinocchio/parsers/urdf.hpp>

namespace pin = pinocchio;

using std::cout;
using VecXd = Eigen::VectorXd;
using MatXd = Eigen::MatrixXd;

void prettyPrint(MatXd &mat) {
    int rows = (int) mat.rows();
    int cols = (int) mat.cols();
    for (int j = 0; j < rows; ++j) {
        for (int k = 0; k < cols; ++k) {
            if (mat(j, k) >= 0)
                printf(" %.4f ", mat(j, k));
            else
                printf("%.4f ", mat(j, k));
        }
        printf("\n");
    }
}

MatXd getJac(pin::Model &m, pin::Data &d, VecXd &q, int jIdx1, int jIdx2, bool printInside = true) {
    pin::computeJointJacobians(m, d, q);
    MatXd jac1(6, m.nv);
    MatXd jac2(6, m.nv);
    MatXd res(6, m.nv);
    pin::getJointJacobian(m, d, (pin::JointIndex) jIdx1, pin::LOCAL_WORLD_ALIGNED, jac1);
    pin::getJointJacobian(m, d, (pin::JointIndex) jIdx2, pin::LOCAL_WORLD_ALIGNED, jac2);
    res = jac1 - jac2;
    if (printInside) {
        printf("Inside:\n");
        prettyPrint(res);
    }
    return res;
}

int main() {
    pin::Model m;
    pin::urdf::buildModel("urdf_manipulators.urdf", m);
    pin::Data d = pin::Data(m);

    int jIdx1 = (int) m.getJointId("1_gripper-body_5-joint");
    int jIdx2 = (int) m.getJointId("2_gripper-body_5-joint");
    printf("jIdx1: %d, jIdx2: %d\n", jIdx1, jIdx2);
    VecXd q(m.nv);
    q << -0.0005, 0.0012, 0.0002, -0.0002, -0.0059, 0.0031, -0.0005, 0.0012, 0.0002, -0.0002, -0.0059, 0.0031;

    getJac(m, d, q, jIdx1, jIdx2);
    MatXd jac = getJac(m, d, q, jIdx1, jIdx2, false);
    printf("Outside:\n");
    prettyPrint(jac);

    return 0;
}

The output is the following:

jIdx1: 6, jIdx2: 12
Inside:
-0.0000  0.4937  0.0001  0.0000 -0.0015 -0.0000  0.0000 -0.4937 -0.0001 -0.0000  0.0015  0.0000 
 0.0006  0.0002 -0.2982 -0.3000 -0.0001  0.0000 -0.0006 -0.0002  0.2982  0.3000  0.0001  0.0000 
 0.0000 -0.0004  0.0000 -0.0001  0.3000 -0.0000  0.0000  0.0004 -0.0000  0.0001 -0.3000  0.0000 
 0.0000  0.0004  1.0000  0.0008 -0.0001 -0.0001  0.0000 -0.0004 -1.0000 -0.0008  0.0001  0.0001 
 0.0000  0.7071  0.0003 -0.0002  1.0000  1.0000  0.0000 -0.7071 -0.0003  0.0002 -1.0000 -1.0000 
 1.0000  0.7071 -0.0008  1.0000  0.0002  0.0002 -1.0000 -0.7071  0.0008 -1.0000 -0.0002 -0.0002 
Outside:
-0.0000  0.0000  0.0000  0.0000  0.0000  0.0000  0.0001 -0.9874 -0.0002 -0.0001  0.0030  0.0000 
 0.0006  0.0000  0.0000  0.0000  0.0000  0.0000 -0.0012 -0.0004  0.5965  0.6000  0.0001  0.0000 
 0.0000  0.0000  0.0000  0.0000  0.0000  0.0000  0.0000  0.0008 -0.0000  0.0001 -0.6000  0.0000 
 0.0000  0.0000  0.0000  0.0000  0.0000  0.0000  0.0000 -0.0007 -2.0000 -0.0017  0.0003  0.0003 
 0.0000  0.0000  0.0000  0.0000  0.0000  0.0000  0.0000 -1.4142 -0.0007  0.0004 -2.0000 -2.0000 
 0.0000  0.0000  0.0000  0.0000  0.0000  0.0000 -2.0000 -1.4142  0.0017 -2.0000 -0.0004 -0.0004 

As we can see the result of the second call of getJac differs from the first one. However, if the result of the first call is saved, the code will run fine.

URDF file

It's quite big, so for convenience, I attach it as a link

Additional context

Code compiled by the following cmake file:

cmake_minimum_required(VERSION 3.27)
project(cpp_scripts)

set(CMAKE_CXX_STANDARD 17)

if (NOT PINOCCHIO_DIR)
    set(PINOCCHIO_DIR /opt/openrobots)
endif()

# Check pkg config
find_package(PkgConfig REQUIRED)

# Update prefix
set(CMAKE_PREFIX_PATH ${PINOCCHIO_DIR})

# Connect pinocchio
find_package(pinocchio 2.6.20 REQUIRED)
link_directories(${PINOCCHIO_DIR}/lib)

# Include Eigen headers
pkg_search_module(Eigen3 REQUIRED eigen3 eigen3>=3.4 eigen<3.5)
link_directories(${Eigen3_INCLUDE_DIRS})

add_executable(cpp_scripts main.cpp)
target_link_libraries(cpp_scripts pinocchio::pinocchio)

System

• OS: Ubuntu 22.04.3 LTS
• Pinocchio version: 2.6.20

[jcarpent]

You should initialize your matrices jac1 and jac2 with zeros. Best, Justin