Closed someilay closed 6 months ago
I have an urdf that contains two independent identical manipulators. I noticed that getJointJacobian works weirdly for independent robots
getJointJacobian
C++ code:
C++
#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.
getJac
It's quite big, so for convenience, I attach it as a link
Code compiled by the following cmake file:
cmake
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)
Ubuntu 22.04.3 LTS
2.6.20
You should initialize your matrices jac1 and jac2 with zeros. Best, Justin
jac1
jac2
Bug description
I have an urdf that contains two independent identical manipulators. I noticed that
getJointJacobian
works weirdly for independent robotsReproduction steps
C++
code:The output is the following:
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:System
Ubuntu 22.04.3 LTS
2.6.20