crba is not working in c++, but works in python

[someilay]

Bug description

crba is not working in C++, but works in Python

Reproduction steps

C++ code:

#include <iostream>

#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/algorithm/contact-dynamics.hpp"


int main() {
    pinocchio::Model model;
    pinocchio::urdf::buildModel("./example.xml", model);
    pinocchio::Data data(model);

    auto q = Eigen::VectorXd::Zero(model.nq);
    std::cout << pinocchio::crba(model, data, q) << "\n";
    return 0;
}

Compiled by g++ -std=c++11 main.cpp -o main $(pkg-config --cflags --libs pinocchio)

Python code:

model = pin.buildModelFromUrdf('example.xml')
data = model.createData()
q = np.zeros(model.nq)
print(pin.crba(model, data, q))

Runned by python3.9 main.py

example.xml:

<robot name="manipulator">
  <link name="ground_frame">
    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
  </link>

  <joint name="joint_1" type="continuous">
    <origin xyz="0 0 0" rpy="0 0 0"/>-->
    <parent link="ground_frame"/>
    <child link="body_1"/>
    <axis xyz="0 0 1" />
  </joint>

  <joint name="joint_2" type="continuous">
    <origin xyz="0 0 1" rpy="0 0 0"/>-->
    <parent link="body_1"/>
    <child link="body_2"/>
    <axis xyz="1 0 0" />
  </joint>

  <link name="body_1">
    <inertial>
      <origin xyz="0 0 1" rpy="0 0 0"/>
      <mass value="1"/>
      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
    </inertial>
  </link>

  <link name="body_2">
    <inertial>
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <mass value="1"/>
      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
    </inertial>
  </link>
</robot>

Running C++ version leads to:

main: /opt/openrobots/lib/pkgconfig/../../include/pinocchio/spatial/symmetric3.hpp:418: pinocchio::Symmetric3Tpl<Scalar, Options> pinocchio::Symmetric3Tpl<Scalar, Options>::rotate(const Eigen::MatrixBase<OtherDerived>&) const [with D = Eigen::Matrix<double, 3, 3>; _Scalar = double; int _Options = 0]: Assertion `isUnitary(R.transpose()*R) && "R is not a Unitary matrix"' failed.
Aborted (core dumped)

Running Python version:

[[2. 0.]
 [0. 2.]]

Additional info:

Pinocchio installed according to this

System

• OS: Ubuntu 22.04.3 LTS
• Pinocchio version: 2.6.20

[nim65s]

@someilay : I can't test why your version is not working right now, but could you test your code with CMake instead of pkg-config ? We have an example here: https://github.com/stack-of-tasks/pinocchio-minimal

[nim65s]

Oh, I guess it is "working" in python because the module is compiled in release mode.

What happen if you add -O2 to your compilation line ?

[someilay]

@nim65s. Thank you for response. With minimal CMake this code starts to work). Can you explain why manual compiled code fails?

[jorisv]

Hello @someilay,

Thanks for the nice reproduction steps you have give. I have been able to reproduce your issue easily.

There is no issue with the compilation. When building with pkg-config you're not giving the -DNDEBUG flags and asserts are then activated.
It's a good thing, because the assert had detect that you give an unnormalized quaternion in the freeflyer.

If you change your code with the following code then the assert is not raised anymore:

#include <iostream>

#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/algorithm/contact-dynamics.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"


int main() {
    pinocchio::Model model;
    pinocchio::urdf::buildModel("./example.xml", model);
    pinocchio::Data data(model);

    auto q = pinocchio::neutral(model);
    std::cout << pinocchio::crba(model, data, q) << "\n";
    return 0;
}

Here, I just call pinocchio::neutral instead of Eigen::Zero to initialize q. This ensure an unit quaternion for the freeflyer joint.

I'm still available if you have anymore questions.

[someilay]

@jorisv. Thank you for clarifying the compilation process. However, I didn't get why such simple system needs quaternions. Moreover, suggested solution doesn't explain how to set manual configuration.

[jorisv]

@someilay,

I check again your example and I answered too fast.
You don't have freeflyier joint (by default buildModel add a Fixed joint as robot base), but you have continuous joints.

Here the description of this joint in the URDF documentation:

a continuous hinge joint that rotates around the axis and has no upper and lower limits.

So... it seem to be a revolute joint without limits. But, it's a little more complicated than that. This kind of joint can be used to model wheel. Issue is the value of this joint, since it doesn't have limits, can reach a really high value.

So instead of using joint angle to model the joint configuration we use the cos(angle) and sin(angle) value.
The continuous joint can then rotate indefinitely without reaching infinity.
It's why the 0 configuration you give at the beginning wasn't working.

If you want to manually set the configuration you can just wrote:

Eigen::VectorXd q(4);
q << std::cos(angle1), std::sin(angle1), std::cos(angle2), std::sin(angle2);

Also, if you finally don't need continuous joints, you can use revolute joints.
q will then be a size 2 vector that will contains angle1 and angle2.

[someilay]

@jorisv, now it makes senses. Thank you