crba is not working in c++, but works in python about pinocchio HOT 7 CLOSED

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.

from pinocchio.

@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.

from pinocchio.

@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

from pinocchio.

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 ?

from pinocchio.

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

from pinocchio.

@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.

from pinocchio.

@jorisv, now it makes senses. Thank you

from pinocchio.