search

humanoid-path-planner / hpp-rbprm

"Implementation of RB-PRM planner using hpp."
5 stars 7 forks source link

quaternion not normalized in kinodynamic test #85

Closed nim65s closed 3 years ago

nim65s commented 3 years ago

Hi,

While testing #84, I got the following error:

Running 15 test cases...
start nav_bauzil test case, this may take a couple of minutes ...
Solve complete, start optimization. This may take few minutes ...
(1/10)  (2/10)  (3/10)  (4/10)  (5/10)  (6/10)  (7/10)  (8/10)  (9/10)  (10/10)  
start nav_bauzil_oriented test case, this may take a couple of minutes ...
Solve complete, start optimization. This may take few minutes ...
(1/10)  (2/10)  (3/10)  (4/10)  (5/10)  (6/10)  (7/10)  (8/10)  (9/10)  (10/10)  
start nav_bauzil_oriented_kino test case, this may take a couple of minutes ...
Solve complete, start optimization. This may take few minutes ...
(1/10)  (2/10)  (3/10)  (4/10)  (5/10)  (6/10)  (7/10)  (8/10)  (9/10)  (10/10)  
start nav_bauzil_hyq test case, this may take a couple of minutes ...
kinodynamic: /opt/openrobots/include/pinocchio/multibody/liegroup/special-orthogonal.hpp:382: static void pinocchio::SpecialOrthogonalOperationTpl<3, _Scalar, _Options>::difference_impl(const Eigen::MatrixBase<OtherDerived>&, const Eigen::MatrixBase<OtherDerived>&, const Eigen::MatrixBase<ScaleDerived>&) [with ConfigL_t = Eigen::Block<const Eigen::Block<Eigen::Ref<const Eigen::Matrix<double, -1, 1> >, 7, 1, false>, 4, 1, false>; ConfigR_t = Eigen::Block<const Eigen::Block<Eigen::Ref<const Eigen::Matrix<double, -1, 1> >, 7, 1, false>, 4, 1, false>; Tangent_t = Eigen::Matrix<double, 3, 1>; _Scalar = double; int _Options = 0]: Assertion `quaternion::isNormalized(quat1,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))' failed.
unknown location(0): fatal error: in "rbrrt_kinodynamic/nav_bauzil_hyq": signal: SIGABRT (application abort requested)
…/hpp-rbprm/tests/test-kinodynamic.cc(1108): last checkpoint
start nav_bauzil_oriented_hyq test case, this may take a couple of minutes ...
kinodynamic: /opt/openrobots/include/pinocchio/multibody/liegroup/special-orthogonal.hpp:382: static void pinocchio::SpecialOrthogonalOperationTpl<3, _Scalar, _Options>::difference_impl(const Eigen::MatrixBase<OtherDerived>&, const Eigen::MatrixBase<OtherDerived>&, const Eigen::MatrixBase<ScaleDerived>&) [with ConfigL_t = Eigen::Block<const Eigen::Block<Eigen::Ref<const Eigen::Matrix<double, -1, 1> >, 7, 1, false>, 4, 1, false>; ConfigR_t = Eigen::Block<const Eigen::Block<Eigen::Ref<const Eigen::Matrix<double, -1, 1> >, 7, 1, false>, 4, 1, false>; Tangent_t = Eigen::Matrix<double, 3, 1>; _Scalar = double; int _Options = 0]: Assertion `quaternion::isNormalized(quat1,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))' failed.
unknown location(0): fatal error: in "rbrrt_kinodynamic/nav_bauzil_oriented_hyq": signal: SIGABRT (application abort requested)
…/hpp-rbprm/tests/test-kinodynamic.cc(1187): last checkpoint

*** 2 failures are detected in the test module "test - kinodynamic"

0% tests passed, 1 tests failed out of 1

Total Test time (real) =  71.19 sec

The following tests FAILED:
     4 - kinodynamic (Failed)
Errors while running CTest

@pFernbach : does this ring a bell ? I don't think we have anything new in pinocchio about this, but I think we got something similar while testing Ewen motions on Pyrene…

jmirabel commented 3 years ago

Are you able to run this in gdb and get the value of quat1 ?

nim65s commented 3 years ago

Sure, that's 0 0 -0.7071 0.7071. Then its norm is 0.9999904099540154. I guess this comes from https://github.com/humanoid-path-planner/hpp-rbprm/blob/dd65faf3c9d70f29deaa7596724a5849612ce899/tests/test-kinodynamic.cc#L1104