Inconsistent Frame Jacobians for some URDFs

[larsrpe]

Hi, I have not pinned down the issue yet, but it seems like there are some issues with computing FrameJacobians for some URDFs.

In particular the following property does not hold: J_frame*v = vel_frame.

I am using a custom URDF, which might cause the problem. I have not managed to recreate the issue with any other URDFs.

I have made a test script, and have also uploaded everything you need to reproduce here(Code and URDF).
jacobian_example.zip

Thanks in advance for your help.

int main()
{
  const std::string urdf_file("../example.urdf");

  pinocchio::Model model;
  pinocchio::urdf::buildModel(urdf_file,JointModelFreeFlyer(),model);
  pinocchio::Data data(model), data_ref(model);
  
  std::cout << "Model has " << model.nq << " joints and " << model.nv << " degrees of freedom" << std::endl;
  
  VectorXd q = VectorXd::Zero(model.nq);
  Eigen::VectorXd q_lim = Eigen::VectorXd::Ones(model.nq);
  pinocchio::randomConfiguration(model,-q_lim,q_lim,q);
  VectorXd v = VectorXd::Random(model.nv);
  VectorXd a = VectorXd::Random(model.nv);
  
  const std::string FRAME_ID = "Body";

  const int FRAME_IDX = model.getFrameId(FRAME_ID);
 

  pinocchio:forwardKinematics(model,data,q,v,a);
  pinocchio::updateFramePlacements(model,data);
  pinocchio::computeJointJacobians(model,data);
  
  pinocchio::SE3 oMf = data.oMf[FRAME_IDX];
  Eigen::Vector3d vel_local_aligned = pinocchio::getFrameVelocity(model, data, FRAME_IDX, pinocchio::LOCAL_WORLD_ALIGNED).linear();
  Eigen::Vector3d vel_world = pinocchio::getFrameVelocity(model, data, FRAME_IDX, pinocchio::WORLD).linear();
  Eigen::Vector3d vel_local = pinocchio::getFrameVelocity(model, data, FRAME_IDX, pinocchio::LOCAL).linear();
  Eigen::Vector3d p = data.oMf[FRAME_IDX].translation();
  Eigen::Vector3d acc = pinocchio::getFrameAcceleration(model, data, FRAME_IDX, pinocchio::LOCAL_WORLD_ALIGNED).linear();
  Eigen::MatrixXd J_local(6,model.nv);
  Eigen::MatrixXd J_world(6,model.nv);
  Eigen::MatrixXd J_local_world_aligned(6,model.nv);
  pinocchio::getFrameJacobian(model,data,FRAME_IDX,pinocchio::LOCAL,J_local);
  pinocchio::getFrameJacobian(model,data,FRAME_IDX,pinocchio::WORLD,J_world);
  pinocchio::getFrameJacobian(model,data,FRAME_IDX,pinocchio::LOCAL_WORLD_ALIGNED,J_local_world_aligned);


  std::cout << "v_local_aligned: " << vel_local_aligned.transpose() << std::endl;
  std::cout << "J_local_world_aligned_v: " << (J_local_world_aligned*v).transpose() << std::endl;
  std::cout << "--------------------------------------------------------" << std::endl;
  std::cout << "v_world: " << vel_world.transpose() << std::endl;
  std::cout << "J_world_v: " << (J_world*v).transpose() << std::endl;
  std::cout << "--------------------------------------------------------" << std::endl;

  std::cout << "v_local: " << vel_local.transpose() << std::endl;
  std::cout << "J_local_v: " << (J_local*v).transpose() << std::endl;

  bool success = true;

    if(((J_local_world_aligned*v).segment(0,3) - vel_local_aligned).norm() > 1e-6)
    {
        std::cout << "Error in getFrameVelocity with LOCAL_WORLD_ALIGNED" << std::endl;
        success = false;
    }

    if(((J_world*v).segment(0,3) - vel_world).norm() > 1e-6)
    {
        std::cout << "Error in getFrameVelocity with WORLD" << std::endl;
        success = false;
    }

    if(((J_local*v).segment(0,3) - vel_local).norm() > 1e-6)
    {
        std::cout << "Error in getFrameVelocity with LOCAL" << std::endl;
        success = false;
    }

    if (success)
    {
        std::cout << "All tests passed" << std::endl;
    }
    else
    {
        std::cout << "Some tests failed" << std::endl;
    }
}

[jorisv]

Hello @larsrpe,

Thanks for the example.

I have launch you example in Debug mode with valgrind (valgrind --track-origins=yes ./jacobian_example).
valgrind show me some memory issues. Apparently, some values in J_local, J_world and J_local_world_aligned are not initialized.

If you replace

  Eigen::MatrixXd J_local(6, model.nv);
  Eigen::MatrixXd J_world(6, model.nv);
  Eigen::MatrixXd J_local_world_aligned(6, model.nv);

by

  Eigen::MatrixXd J_local = Eigen::MatrixXd::Zero(6, model.nv);
  Eigen::MatrixXd J_world = Eigen::MatrixXd::Zero(6, model.nv);
  Eigen::MatrixXd J_local_world_aligned = Eigen::MatrixXd::Zero(6, model.nv);

Your code will work properly.

getFrameJacobian Assume your matrix is zero initialized since it will only modify the columns that are on the frame kinematics chain.

It precised in the documentation :  /// \param[out] J A reference on the Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill J with zero elements, e.g. J.setZero().

You can also use another version of this function that return the matrix :

  Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options>
  getFrameJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                   DataTpl<Scalar,Options,JointCollectionTpl> & data,
                   const FrameIndex frame_id,
                   const ReferenceFrame reference_frame)

[larsrpe]

Thanks a lot for the help and the explanation. Have a great day!