search

stack-of-tasks / pinocchio

A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
http://stack-of-tasks.github.io/pinocchio/
BSD 2-Clause "Simplified" License
1.78k stars 375 forks source link

Compute Coriolis Matrix Error #1422

Closed EpicDuckPotato closed 3 years ago

EpicDuckPotato commented 3 years ago

Hi, I'm trying to compute the coriolis matrix for a floating-base system, and I get the following error:

/opt/openrobots/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]: AssertionisUnitary(R.transpose()*R) && "R is not a Unitary matrix"' failed.`

The configuration consists of position, orientation (quaternion), and two revolute joint angles. I've confirmed that the configuration vector is valid:

0 2.23517e-08 0 -0.707107 0 0 0.707107 0 0

I've been able to compute the mass matrix and gravity just fine, and I've also been able to use rnea. I'm not sure why I'm having issues with the coriolis function.

Thanks, Anoop

jcarpent commented 3 years ago

You should normalize your configuration vector:

pinocchio::normalize(model,q)
jcarpent commented 3 years ago

This also means that you are working in devel mode.