computeCoriolisMatrix error

[Neotriple]

Hi again,

When I use the computeCoriolisMatrix function in RNEA, I get the following error:

vision60: /opt/ros/melodic/include/pinocchio/spatial/symmetric3.hpp:419: pinocchio::Symmetric3Tpl<Scalar, Options> pinocchio::Symmetric3Tpl<Scalar, Options>::rotate(const Eigen::MatrixBase&) 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)

Based on the output, I assume this is because of an error in my input configuration vector q_, however, I am unsure. This is a bit strange since forwardKinematics works fine, but I'm not sure the internal workings, and forwardKinematics may not end up calling the same assertion.

For what it's worth, my q vector is as follows:

0 0 0.55 0.427179 -0.409477 -0.773786 0.226057 0 -0.392699 0.785398 0 0.392699 0.785398 0 0.392699 0.785398 0 0.392699 0.785398

I converted the quaternion (from above: starting with 0.427 to the number ending in 0.226), into an euler-angle rotation matrix and did R_T*R, and I got close to a unitary, but off because of some floating point error I guess. Maybe this is the cause of the problem?

Thanks once again for all the help!

[jcarpent]

For robotics application, it is important to stay close to the manifold on which your system evolves, to avoid numerical issues when solving optimization problems for instance.

That’s why, in Pinocchio we have introduced in the file algorithm/joint-configuration.hpp the function normalize which allows the user to normalize the configuration vector.

[Neotriple]

Thanks for this!