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

getJointJacobian() error #2144

Closed Haoran-Zhao closed 7 months ago

Haoran-Zhao commented 7 months ago

Bug description

Hello, I built a Pinocchio model with panda.urdf, and try to get the jacobian of last joint.

pinocchio::computeJointJacobians(model_, data, joints);
pinocchio::getJointJacobian(model_, data, model_.getJointId(ee_joint_name_), pinocchio::LOCAL, data.J);

The ee_joint_name_ is panda_hand_tcp_joint I checked that the model can find the joint id, which is 8. and the joints are [ 0.832311 -0.825931 -0.906136 -2.05356 -0.238398 1.73503 0]

for some reason, I got this error from gdb /usr/include/boost/variant/detail/forced_return.hpp:38: T boost::detail::variant::forced_return() [with T = int]: Assertionfalse' failed.`

and this is the full gdb trace

#0  __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:50
#1  0x00007ffff79f1859 in __GI_abort () at abort.c:79
#2  0x00007ffff79f1729 in __assert_fail_base (fmt=0x7ffff7b87588 "%s%s%s:%u: %s%sAssertion `%s' failed.\n%n", assertion=0x555555744d46 "false", file=0x55555574d180 "/usr/include/boost/variant/detail/forced_return.hpp", line=38, function=<optimized out>) at assert.c:92
#3  0x00007ffff7a02fd6 in __GI___assert_fail (assertion=0x555555744d46 "false", file=0x55555574d180 "/usr/include/boost/variant/detail/forced_return.hpp", line=38, function=0x5555557516c0 "T boost::detail::variant::forced_return() [with T = int]") at assert.c:101
#4  0x00005555555b7659 in boost::detail::variant::forced_return<int> () at /usr/include/boost/variant/detail/forced_return.hpp:38
#5  0x00005555555b1cbf in boost::detail::variant::visitation_impl<mpl_::int_<30>, boost::detail::variant::visitation_impl_step<boost::mpl::l_iter<boost::mpl::l_end>, boost::mpl::l_iter<boost::mpl::l_end> >, boost::detail::variant::invoke_visitor<pinocchio::JointNvVisitor const, false>, void const*, boost::variant<pinocchio::JointModelRevoluteTpl<double, 0, 0>, pinocchio::JointModelRevoluteTpl<double, 0, 1>, pinocchio::JointModelRevoluteTpl<double, 0, 2>, pinocchio::JointModelMimic<pinocchio::JointModelRevoluteTpl<double, 0, 0> >, pinocchio::JointModelMimic<pinocchio::JointModelRevoluteTpl<double, 0, 1> >, pinocchio::JointModelMimic<pinocchio::JointModelRevoluteTpl<double, 0, 2> >, pinocchio::JointModelFreeFlyerTpl<double, 0>, pinocchio::JointModelPlanarTpl<double, 0>, pinocchio::JointModelRevoluteUnalignedTpl<double, 0>, pinocchio::JointModelSphericalTpl<double, 0>, pinocchio::JointModelSphericalZYXTpl<double, 0>, pinocchio::JointModelPrismaticTpl<double, 0, 0>, pinocchio::JointModelPrismaticTpl<double, 0, 1>, pinocchio::JointModelPrismaticTpl<double, 0, 2>, pinocchio::JointModelPrismaticUnalignedTpl<double, 0>, pinocchio::JointModelTranslationTpl<double, 0>, pinocchio::JointModelRevoluteUnboundedTpl<double, 0, 0>, pinocchio::JointModelRevoluteUnboundedTpl<double, 0, 1>, pinocchio::JointModelRevoluteUnboundedTpl<double, 0, 2>, pinocchio::JointModelRevoluteUnboundedUnalignedTpl<double, 0>, boost::recursive_wrapper<pinocchio::JointModelCompositeTpl<double, 0, pinocchio::JointCollectionDefaultTpl> > >::has_fallback_type_> () at /usr/include/boost/variant/detail/visitation_impl.hpp:185
#6  0x0000555555596a11 in _ZN5boost6detail7variant15visitation_implIN4mpl_4int_ILi0EEENS1_20visitation_impl_stepINS_3mpl6l_iterINS7_6l_itemINS3_5long_ILl21EEEN9pinocchio21JointModelRevoluteTplIdLi0ELi0EEENS9_INSA_ILl20EEENSD_IdLi0ELi1EEENS9_INSA_ILl19EEENSD_IdLi0ELi2EEENS9_INSA_ILl18EEENSC_15JointModelMimicISE_EENS9_INSA_ILl17EEENSK_ISG_EENS9_INSA_ILl16EEENSK_ISI_EENS9_INSA_ILl15EEENSC_22JointModelFreeFlyerTplIdLi0EEENS9_INSA_ILl14EEENSC_19JointModelPlanarTplIdLi0EEENS9_INSA_ILl13EEENSC_30JointModelRevoluteUnalignedTplIdLi0EEENS9_INSA_ILl12EEENSC_22JointModelSphericalTplIdLi0EEENS9_INSA_ILl11EEENSC_25JointModelSphericalZYXTplIdLi0EEENS9_INSA_ILl10EEENSC_22JointModelPrismaticTplIdLi0ELi0EEENS9_INSA_ILl9EEENS16_IdLi0ELi1EEENS9_INSA_ILl8EEENS16_IdLi0ELi2EEENS9_INSA_ILl7EEENSC_31JointModelPrismaticUnalignedTplIdLi0EEENS9_INSA_ILl6EEENSC_24JointModelTranslationTplIdLi0EEENS9_INSA_ILl5EEENSC_30JointModelRevoluteUnboundedTplIdLi0ELi0EEENS9_INSA_ILl4EEENS1J_IdLi0ELi1EEENS9_INSA_ILl3EEENS1J_IdLi0ELi2EEENS9_INSA_ILl2EEENSC_39JointModelRevoluteUnboundedUnalignedTplIdLi0EEENS9_INSA_ILl1EEENS_17recursive_wrapperINSC_22JointModelCompositeTplIdLi0ENSC_25JointCollectionDefaultTplEEEEENS7_5l_endEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEENS8_IS1Y_EEEENS1_14invoke_visitorIKNSC_14JointNvVisitorELb0EEEPKvNS_7variantISE_JSG_SI_SL_SN_SP_SS_SV_SY_S11_S14_S17_S19_S1B_S1E_S1H_S1K_S1M_S1O_S1R_S1X_EE18has_fallback_type_EEENT1_11result_typeEiiRS2W_T2_NS3_5bool_ILb0EEET3_PT_PT0_ (
    no_backup_flag=..., storage=0x5555558d4ff8, visitor=..., logical_which=608, internal_which=608) at /usr/include/boost/variant/detail/visitation_impl.hpp:254
#7  boost::variant<pinocchio::JointModelRevoluteTpl<double, 0, 0>, pinocchio::JointModelRevoluteTpl<double, 0, 1>, pinocchio::JointModelRevoluteTpl<double, 0, 2>, pinocchio::JointModelMimic<pinocchio::JointModelRevoluteTpl<double, 0, 0> >, pinocchio::JointModelMimic<pinocchio::JointModelRevoluteTpl<double, 0, 1> >, pinocchio::JointModelMimic<pinocchio::JointModelRevoluteTpl<double, 0, 2> >, pinocchio::JointModelFreeFlyerTpl<double, 0>, pinocchio::JointModelPlanarTpl<double, 0>, pinocchio::JointModelRevoluteUnalignedTpl<double, 0>, pinocchio::JointModelSphericalTpl<double, 0>, pinocchio::JointModelSphericalZYXTpl<double, 0>, pinocchio::JointModelPrismaticTpl<double, 0, 0>, pinocchio::JointModelPrismaticTpl<double, 0, 1>, pinocchio::JointModelPrismaticTpl<double, 0, 2>, pinocchio::JointModelPrismaticUnalignedTpl<double, 0>, pinocchio::JointModelTranslationTpl<double, 0>, pinocchio::JointModelRevoluteUnboundedTpl<double, 0, 0>, pinocchio::JointModelRevoluteUnboundedTpl<double, 0, 1>, pinocchio::JointModelRevoluteUnboundedTpl<double, 0, 2>, pinocchio::JointModelRevoluteUnboundedUnalignedTpl<double, 0>, boost::recursive_wrapper<pinocchio::JointModelCompositeTpl<double, 0, pinocchio::JointCollectionDefaultTpl> > >::internal_apply_visitor_impl<boost::detail::variant::invoke_visitor<pinocchio::JointNvVisitor const, false>, void const*> (storage=0x5555558d4ff8, visitor=..., logical_which=608, internal_which=608) at /usr/include/boost/variant/variant.hpp:2334
#8  boost::variant<pinocchio::JointModelRevoluteTpl<double, 0, 0>, pinocchio::JointModelRevoluteTpl<double, 0, 1>, pinocchio::JointModelRevoluteTpl<double, 0, 2>, pinocchio::JointModelMimic<pinocchio::JointModelRevoluteTpl<double, 0, 0> >, pinocchio::JointModelMimic<pinocchio::JointModelRevoluteTpl<double, 0, 1> >, pinocchio::JointModelMimic<pinocchio::JointModelRevoluteTpl<double, 0, 2> >, pinocchio::JointModelFreeFlyerTpl<double, 0>, pinocchio::JointModelPlanarTpl<double, 0>, pinocchio::JointModelRevoluteUnalignedTpl<double, 0>, pinocchio::JointModelSphericalTpl<double, 0>, pinocchio::JointModelSphericalZYXTpl<double, 0>, pinocchio::JointModelPrismaticTpl<double, 0, 0>, pinocchio::JointModelPrismaticTpl<double, 0, 1>, pinocchio::JointModelPrismaticTpl<double, 0, 2>, pinocchio::JointModelPrismaticUnalignedTpl<double, 0>, pinocchio::JointModelTranslationTpl<double, 0>, pinocchio::JointModelRevoluteUnboundedTpl<double, 0, 0>, pinocchio::JointModelRevoluteUnboundedTpl<double, 0, 1>, pinocchio::JointModelRevoluteUnboundedTpl<double, 0, 2>, pinocchio::JointModelRevoluteUnboundedUnalignedTpl<double, 0>, boost::recursive_wrapper<pinocchio::JointModelCompositeTpl<double, 0, pinocchio::JointCollectionDefaultTpl> > >::internal_apply_visitor<boost::detail::variant::invoke_visitor<pinocchio::JointNvVisitor const, false> > (visitor=..., this=0x5555558d4ff0) at /usr/include/boost/variant/variant.hpp:2355
#9  boost::variant<pinocchio::JointModelRevoluteTpl<double, 0, 0>, pinocchio::JointModelRevoluteTpl<double, 0, 1>, pinocchio::JointModelRevoluteTpl<double, 0, 2>, pinocchio::JointModelMimic<pinocchio::JointModelRevoluteTpl<double, 0, 0> >, pinocchio::JointModelMimic<pinocchio::JointModelRevoluteTpl<double, 0, 1> >, pinocchio::JointModelMimic<pinocchio::JointModelRevoluteTpl<double, 0, 2> >, pinocchio::JointModelFreeFlyerTpl<double, 0>, pinocchio::JointModelPlanarTpl<double, 0>, pinocchio::JointModelRevoluteUnalignedTpl<double, 0>, pinocchio::JointModelSphericalTpl<double, 0>, pinocchio::JointModelSphericalZYXTpl<double, 0>, pinocchio::JointModelPrismaticTpl<double, 0, 0>, pinocchio::JointModelPrismaticTpl<double, 0, 1>, pinocchio::JointModelPrismaticTpl<double, 0, 2>, pinocchio::JointModelPrismaticUnalignedTpl<double, 0>, pinocchio::JointModelTranslationTpl<double, 0>, pinocchio::JointModelRevoluteUnboundedTpl<double, 0, 0>, pinocchio::JointModelRevoluteUnboundedTpl<double, 0, 1>, pinocchio::JointModelRevoluteUnboundedTpl<double, 0, 2>, pinocchio::JointModelRevoluteUnboundedUnalignedTpl<double, 0>, boost::recursive_wrapper<pinocchio::JointModelCompositeTpl<double, 0, pinocchio::JointCollectionDefaultTpl> > >::apply_visitor<pinocchio::JointNvVisitor const>(pinocchio::JointNvVisitor const&) const & (this=0x5555558d4ff0, visitor=...) at /usr/include/boost/variant/variant.hpp:2401
#10 0x000055555558fb32 in boost::apply_visitor<pinocchio::JointNvVisitor, pinocchio::JointModelTpl<double, 0, pinocchio::JointCollectionDefaultTpl> const&> (visitor=..., visitable=...) at /usr/include/boost/variant/detail/apply_visitor_unary.hpp:68
#11 0x0000555555587ee4 in pinocchio::JointNvVisitor::run<double, 0, pinocchio::JointCollectionDefaultTpl> (jmodel=...) at /usr/local/include/pinocchio/multibody/joint/joint-basic-visitors.hxx:154
#12 0x000055555557eb6a in pinocchio::nv<double, 0, pinocchio::JointCollectionDefaultTpl> (jmodel=...) at /usr/local/include/pinocchio/multibody/joint/joint-basic-visitors.hxx:159
#13 0x0000555555684b07 in pinocchio::details::translateJointJacobian<double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::Matrix<double, 6, -1, 0, 6, -1>, Eigen::Matrix<double, 6, -1, 0, 6, -1> > (model=..., data=..., joint_id=8, rf=pinocchio::LOCAL, placement=..., Jin=..., Jout=...) at /usr/local/include/pinocchio/algorithm/jacobian.hxx:175
#14 0x000055555568086a in pinocchio::details::translateJointJacobian<double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::Matrix<double, 6, -1, 0, 6, -1>, Eigen::Matrix<double, 6, -1, 0, 6, -1> > (model=..., data=..., joint_id=8, rf=pinocchio::LOCAL, Jin=..., Jout=...) at /usr/local/include/pinocchio/algorithm/jacobian.hxx:229
#15 0x000055555567e66c in pinocchio::getJointJacobian<double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::Matrix<double, 6, -1, 0, 6, -1> > (model=..., data=..., jointId=8, rf=pinocchio::LOCAL, J=...) at /usr/local/include/pinocchio/algorithm/jacobian.hxx:245

Thanks for your help!

System

jorisv commented 7 months ago

Hello @Haoran-Zhao,

Can you provide a script to reproduce the issue ?

Haoran-Zhao commented 7 months ago

Thanks for your reply! It was in a gtest script, and I found if I added one test Fixture before, it had no problem, otherwise, same issue. it's probably the gtest issue. and I did not have this issue before. I will close the ticket for now.