Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Different results for CalcPointJacobian in Python vs C++ #101

Open
dyumanaditya opened this issue Jun 15, 2024 · 4 comments
Open

Different results for CalcPointJacobian in Python vs C++ #101

dyumanaditya opened this issue Jun 15, 2024 · 4 comments

Comments

@dyumanaditya
Copy link

dyumanaditya commented Jun 15, 2024 •
edited
Loading

Hello,

I'm trying to use the Python wrapper to compute the point jacobians for an A1 Quadruped represented in the URDF attached below.
a1_description.zip

I find that the results for CalcPointJacobian are different in Python vs in C++. Both codes are attached below

Python

import rbdl
import numpy as np


# Load the URDF model
rbdl_model = rbdl.loadModel("robots/a1_description/urdf/a1.urdf")

toe_id = rbdl_model.GetBodyId("toe0")

q = np.array([-0.00887838, 0.00140905, 0.257643, -0.00272031, 0.0182614, 0.00397527, -0.0288908, 0.690337, 1.2522, -0.0342197, 0.733392, 1.33806, 0.0103736, 0.689737, 1.23371, 0.0394928, 0.74035, 1.33224, 0.999822])

jac_block = np.zeros((3, 18))
rbdl.UpdateKinematics(rbdl_model, q, np.zeros(18), np.zeros(18))
rbdl.CalcPointJacobian(rbdl_model, q, toe_id, np.zeros(3), jac_block)

Output

[[ 0.          0.          0.          0.          0.          0.
   0.          0.23391215 -0.10655274  0.          0.          0.
   0.          0.          0.          0.          0.          0.        ]
 [ 0.23623525  0.00043465 -0.00488917  0.          0.          0.
   0.          0.          0.          0.          0.          0.
   0.07700806  0.01504024 -0.16918218  0.          0.          0.        ]
 [ 0.          0.          0.          0.          0.          0.
   0.          0.          0.          0.          0.          0.
   0.          0.          0.          0.          0.          0.        ]]

C++

#include <rbdl/rbdl.h>
#include <rbdl/addons/urdfreader/urdfreader.h>
#include <rbdl/rbdl_utils.h>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <iostream>


int main()
{
    RigidBodyDynamics::Model *model_;
    RigidBodyDynamics::Addons::URDFReadFromString(robot_description_string.c_str(), model_, true);

    // Calculate the Point jacobian of the toe of a quadruped robot
    Eigen::VectorXd q(19);
    q << -0.00887838, 0.00140905, 0.257643, -0.00272031, 0.0182614, 0.00397527, -0.0288908, 0.690337, 1.2522, -0.0342197, 0.733392, 1.33806, 0.0103736, 0.689737, 1.23371, 0.0394928, 0.74035, 1.33224, 0.999822;

    int toe_id = model_->GetBodyId("toe0".c_str());

    Eigen::MatrixXd jac_block(3, 18);
    jac_block.setZero();
    RigidBodyDynamics::CalcPointJacobian(*model_, q, toe_id, Eigen::Vector3d::Zero(), jac_block);
    std::cout << "Jacobian block: " << jac_block << std::endl;
}

Output

1.00000000 0.00000000 0.00000000 0.00301041 -0.236935 -0.134685 0 0 0 0 0 0 0.00129516 0.23123 -0.109695 0 0 0
0.00000000 1.00000000 0.00000000 0.230626 -0.00290677 0.196295 0 0 0 0 0 0 0.230364 0.00173085 6.93308e-06 0 0 0
0.00000000 0.00000000 1.00000000 0.131881 -0.188815 0.00382161 0 0 0 0 0 0 0.0849135 0.00840567 -0.167233 0 0 0

As you can see the results differ and there are ones in the c++ output.

Can anyone tell me what I might be doing wrong?
@martinfelis
Copy link
Collaborator

Can you check whether the correct body IDs are being used?

Both Python and C++ samples are incomplete. Both do not define body_id_list and the and the C++ sample does not define i.

@dyumanaditya
Copy link
Author

Hi @martinfelis Sorry about that, I made a mistake while copying them, I have updated the code in the original question -- it should have been for toe_id. I tried printing both values and found that in python toe_id = 2147483653 but in c++ toe_id = 2147483652. I want the python code to match the c++ output so I manually put toe_id = 2147483652 in the Python script and found that the output is:

[[ 0.          0.          0.          0.          0.          0.
   0.          0.          0.          0.          0.          0.
   0.          0.          0.         -0.00022034  0.          0.        ]
 [ 0.          0.          0.          0.          0.          0.
   0.          0.          0.         -0.0809997   0.          0.
   0.          0.          0.          0.          0.          0.        ]
 [ 0.          0.          0.          0.          0.          0.
   0.          0.          0.          0.          0.          0.
   0.          0.          0.          0.          0.          0.        ]]

Which is still different from the c++ output. Do you have any insights about why this might be happening and how I can fix it?

@martinfelis
Copy link
Collaborator

I am surprised that the IDs of the bodies differ and would first investigate this.

The output in your previous post: you get that by forcing the IDs to be the same or do you pass in the values you obtained via GetBodyId()?

What is the error with IDs from GetBodyId()?

Do you have errors when computing it for different bodies?

Do both Python C++ use the same RBDL version?

@dyumanaditya
Copy link
Author

dyumanaditya commented Jun 16, 2024 •
edited
Loading

Yes it's odd that the body IDs differ.

  1. The output in my first post was obtained with the value that was returned from GetBodyId(). The output in my second post was forcing the python code to use the same Body ID as the c++ code.
  2. I never got any errors for different bodies while computing the body IDs, just that they differ from python to c++
  3. Both python and c++ are using rbdl v3.13

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
2 participants
@martinfelis @dyumanaditya