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.93k stars 396 forks source link

Segmentation fault of pinocchio::urdf::buildModel #2046

Closed Czworldy closed 1 year ago

Czworldy commented 1 year ago

Hi guys, When i try to build a robot model from a mpc library but i get Segmentation fault, and i use gdb to analyse the corefile i got these infomation. That's very strange because this function getPinocchioInterfaceFromUrdfModel works very well in other example, the only thing i have done is that i link a another library which also depends on pinocchio. Do you guys have any idea about this? If you need more infomation, please let me know, i'll reply as fast as possible.

#0  __GI___libc_free (mem=0xe1) at malloc.c:3102
#1  0x00005647d78a839e in Eigen::internal::handmade_aligned_free (ptr=<optimized out>) at /usr/include/eigen3/Eigen/src/Core/util/Memory.h:761
#2  Eigen::internal::aligned_free (ptr=<optimized out>) at /usr/include/eigen3/Eigen/src/Core/util/Memory.h:179
#3  Eigen::aligned_allocator<pinocchio::SE3Tpl<double, 0> >::deallocate (this=0x5647d9522278, p=<optimized out>) at /usr/include/eigen3/Eigen/src/Core/util/Memory.h:763
#4  std::allocator_traits<Eigen::aligned_allocator<pinocchio::SE3Tpl<double, 0> > >::deallocate (__a=..., __n=<optimized out>, __p=<optimized out>) at /usr/include/c++/9/bits/alloc_traits.h:332
#5  std::_Vector_base<pinocchio::SE3Tpl<double, 0>, Eigen::aligned_allocator<pinocchio::SE3Tpl<double, 0> > >::_M_deallocate (this=0x5647d9522278, __n=<optimized out>, __p=<optimized out>) at /usr/include/c++/9/bits/stl_vector.h:351
#6  std::_Vector_base<pinocchio::SE3Tpl<double, 0>, Eigen::aligned_allocator<pinocchio::SE3Tpl<double, 0> > >::~_Vector_base (this=0x5647d9522278, __in_chrg=<optimized out>) at /usr/include/c++/9/bits/stl_vector.h:332
#7  std::vector<pinocchio::SE3Tpl<double, 0>, Eigen::aligned_allocator<pinocchio::SE3Tpl<double, 0> > >::~vector (this=0x5647d9522278, __in_chrg=<optimized out>) at /usr/include/c++/9/bits/stl_vector.h:680
#8  pinocchio::container::aligned_vector<pinocchio::SE3Tpl<double, 0> >::~aligned_vector (this=0x5647d9522278, __in_chrg=<optimized out>) at /opt/openrobots/include/pinocchio/container/aligned-vector.hpp:26
#9  pinocchio::JointModelCompositeTpl<double, 0, pinocchio::JointCollectionDefaultTpl>::~JointModelCompositeTpl (this=0x5647d9522250, __in_chrg=<optimized out>) at /opt/openrobots/include/pinocchio/multibody/joint/joint-composite.hpp:149
#10 boost::checked_delete<pinocchio::JointModelCompositeTpl<double, 0, pinocchio::JointCollectionDefaultTpl> > (x=0x5647d9522250) at /usr/include/boost/core/checked_delete.hpp:36
#11 boost::recursive_wrapper<pinocchio::JointModelCompositeTpl<double, 0, pinocchio::JointCollectionDefaultTpl> >::~recursive_wrapper (this=<optimized out>, __in_chrg=<optimized out>) at /usr/include/boost/variant/recursive_wrapper.hpp:105
#12 boost::detail::variant::destroyer::internal_visit<boost::recursive_wrapper<pinocchio::JointModelCompositeTpl<double, 0, pinocchio::JointCollectionDefaultTpl> > > (this=<synthetic pointer>, operand=...) at /usr/include/boost/variant/variant.hpp:386
#13 boost::detail::variant::visitation_impl_invoke_impl<boost::detail::variant::destroyer, void*, boost::recursive_wrapper<pinocchio::JointModelCompositeTpl<double, 0, pinocchio::JointCollectionDefaultTpl> > > (visitor=<synthetic pointer>..., 
    storage=<optimized out>) at /usr/include/boost/variant/detail/visitation_impl.hpp:112
#14 boost::detail::variant::visitation_impl_invoke<boost::detail::variant::destroyer, void*, boost::recursive_wrapper<pinocchio::JointModelCompositeTpl<double, 0, pinocchio::JointCollectionDefaultTpl> >, 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_> (internal_which=<optimized out>, visitor=<synthetic pointer>..., t=0x0, storage=<optimized out>)
    at /usr/include/boost/variant/detail/visitation_impl.hpp:150
#15 _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_9destroyerEPvNS_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_typeEiiRS2S_T2_NS3_5bool_ILb0EEET3_PT_PT0_ (no_backup_flag=..., storage=<optimized out>, visitor=<synthetic pointer>..., logical_which=<optimized out>, internal_which=<optimized out>)
    at /usr/include/boost/variant/detail/visitation_impl.hpp:231
#16 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::destroyer, void*>
    (storage=<optimized out>, visitor=<synthetic pointer>..., logical_which=<optimized out>, internal_which=<optimized out>) at /usr/include/boost/variant/variant.hpp:2334
#17 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::destroyer> (
    visitor=<synthetic pointer>..., this=<optimized out>) at /usr/include/boost/variant/variant.hpp:2346
#18 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> > >::destroy_content (this=<optimized out>)
    at /usr/include/boost/variant/variant.hpp:1365
#19 0x00007f8a8b8baf86 in 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> > >::~variant (this=0x7fff12767380, 
    __in_chrg=<optimized out>) at /usr/include/boost/variant/variant.hpp:1370
#20 pinocchio::JointModelTpl<double, 0, pinocchio::JointCollectionDefaultTpl>::~JointModelTpl (this=0x7fff12767370, __in_chrg=<optimized out>) at /opt/openrobots/lib/pkgconfig/../../include/pinocchio/multibody/joint/joint-generic.hpp:161
--Type <RET> for more, q to quit, c to continue without paging--
#21 pinocchio::ModelTpl<double, 0, pinocchio::JointCollectionDefaultTpl>::addJoint (this=this@entry=0x7fff12767900, parent=<optimized out>, parent@entry=0, joint_model=..., joint_placement=..., joint_name="root_joint", max_effort=..., max_velocity=..., 
    min_config=..., max_config=..., joint_friction=..., joint_damping=...) at /opt/openrobots/lib/pkgconfig/../../include/pinocchio/multibody/model.hxx:83
#22 0x00007f8a8b8be52f in pinocchio::ModelTpl<double, 0, pinocchio::JointCollectionDefaultTpl>::addJoint (max_config=..., min_config=..., max_velocity=..., max_effort=..., joint_name="root_joint", joint_placement=..., joint_model=..., parent=0, 
    this=0x7fff12767900) at /usr/include/eigen3/Eigen/src/Core/util/Meta.h:300
#23 pinocchio::ModelTpl<double, 0, pinocchio::JointCollectionDefaultTpl>::addJoint (this=0x7fff12767900, parent=0, joint_model=..., joint_placement=..., joint_name="root_joint") at /opt/openrobots/lib/pkgconfig/../../include/pinocchio/multibody/model.hxx:171
#24 0x00007f8a8b8be9a0 in pinocchio::urdf::details::UrdfVisitorWithRootJoint<double, 0, pinocchio::JointCollectionDefaultTpl>::addRootJoint (this=0x7fff127678a0, Y=..., body_name="BASE") at /usr/include/eigen3/Eigen/src/Core/util/Meta.h:300
#25 0x00007f8a8a31dfaf in pinocchio::urdf::details::parseRootTree(urdf::ModelInterface const*, pinocchio::urdf::details::UrdfVisitorBaseTpl<double, 0>&) () from /opt/openrobots/lib/libpinocchio.so.2.6.17
#26 0x00007f8a8b8b3613 in pinocchio::urdf::buildModel<double, 0, pinocchio::JointCollectionDefaultTpl> (verbose=false, model=..., rootJoint=..., urdfTree=...) at /opt/openrobots/lib/pkgconfig/../../include/pinocchio/multibody/joint/joint-model-base.hpp:318

#27 ocs2::getPinocchioInterfaceFromUrdfModel (urdfTree=std::shared_ptr<class urdf::ModelInterface> (use count 2, weak count 0) = {...}, rootJoint=...) at /home/yjy/MPC_WBC_sim/ocs2_ws/src/ocs2/ocs2_pinocchio/ocs2_pinocchio_interface/src/urdf.cpp:103
nim65s commented 1 year ago

Hi @Czworldy,

We won't be able to help without a way to reproduce your issue. If you can provide a Dockerfile which consistently lead to this, that would be great.

jcarpent commented 1 year ago

At first glance, you have compiled the library without using the same compilation flags. In particular, if you active vectorization flags (with Eigen), Eigen uses a different malloc and free for memory management. This typically the error you obtained.

Czworldy commented 1 year ago

@nim65s @jcarpent thanks a lot, i will try compiled the library with the same compilation flags. btw, i found a library i used simply use find_package(pinocchio) to link pinocchio, let's call it A library,

add_library(
  ${PROJECT_NAME} 
  SHARED
  ${${PROJECT_NAME}_SOURCES} 
  ${${PROJECT_NAME}_HEADERS}
)
target_link_libraries(
  ${PROJECT_NAME} 
  PUBLIC
  ${PINOCCHIO_LIBRARIES}
  PRIVATE
  ${OpenMP_CXX_FLAGS}
)
target_include_directories(
  ${PROJECT_NAME} 
  PUBLIC
  ${EIGEN3_INCLUDE_DIR}
  ${PINOCCHIO_INCLUDE_DIRS}
  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
)
target_compile_options(
  ${PROJECT_NAME} 
  PRIVATE
  ${OpenMP_CXX_FLAGS}
)

When it comes to another library it use PkgConfig to config pinocchio, let's call it B library

find_package(PkgConfig REQUIRED)
pkg_check_modules(pinocchio REQUIRED pinocchio)

add_library(${PROJECT_NAME} ......)
add_dependencies(${PROJECT_NAME}
  ${catkin_EXPORTED_TARGETS}
)
target_link_libraries(${PROJECT_NAME}
  ${Boost_LIBRARIES}
  ${catkin_LIBRARIES}
  ${pinocchio_LIBRARIES}
)
target_compile_options(${PROJECT_NAME} PUBLIC  
  ${pinocchio_CFLAGS_OTHER}
  -Wno-ignored-attributes
  -Wno-invalid-partial-specialization   
  -DPINOCCHIO_URDFDOM_TYPEDEF_SHARED_PTR
  -DPINOCCHIO_URDFDOM_USE_STD_SHARED_PTR)

This is just like @jcarpent said, A and B library have different compilation flags, but my project depends on these two library, Does this cause problem? Thanks a lot!

nim65s commented 1 year ago

Yes, this is an issue. Both libraries A and B should follow modern CMake practise, and use only target_link_libraries(${PROJECT_NAME} pinocchio::pinocchio) after a find_package(pinocchio REQUIRED), as shown in our https://github.com/stack-of-tasks/pinocchio-minimal/blob/master/CMakeLists.txt example.

Also, there should be no need to involve boost or eigen3, as pinocchio is already re-exporting those.

Czworldy commented 1 year ago

@nim65s So the lastest version of pinocchio do not need use pkgconfig to config it any more? I current use focal, 2.6.20 amd64 version. In fact, i have no idea what those strange compilation flags means, and i think the way B library used is widely used in project. Which way is better? Thank you.

nim65s commented 1 year ago

pinocchio export a CMake module, which will be found by find_package(pinocchio REQUIRED), and this is way more powerful than pkgconfig. So if you have a CMake project, you should use pinocchio with CMake, not with pkgconfig.

So, for project A without catkin:

find_package(pinocchio REQUIRED)
add_library(
  ${PROJECT_NAME} 
  SHARED
  ${${PROJECT_NAME}_SOURCES} 
  ${${PROJECT_NAME}_HEADERS}
)
target_link_libraries(
  ${PROJECT_NAME} 
  PUBLIC
  pinocchio::pinocchio
  PRIVATE
  ${OpenMP_CXX_FLAGS}
)
target_include_directories(
  ${PROJECT_NAME} 
  PUBLIC
  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
)
target_compile_options(
  ${PROJECT_NAME} 
  PRIVATE
  ${OpenMP_CXX_FLAGS}
)

And for project B with catkin:

find_package(pinocchio REQUIRED)

add_library(${PROJECT_NAME} ......)
add_dependencies(${PROJECT_NAME}
  ${catkin_EXPORTED_TARGETS}
)
target_link_libraries(${PROJECT_NAME}
  ${catkin_LIBRARIES}
  pinocchio::pinocchio
)
target_compile_options(${PROJECT_NAME} PUBLIC  
  -Wno-ignored-attributes
  -Wno-invalid-partial-specialization)

(an alternative to project B with catkin would be to declare pinocchio as catkin dependency and let catkin handle the CMake for you, but I'm less confident here)

Czworldy commented 1 year ago

@nim65s Thanks for your advice! i'll try it.

Czworldy commented 1 year ago

@nim65s Okay i found out that A library compiled with option -march=native, when remove it, then the seg fault disappear. But i am curious about how this flags influence Pinocchio Thanks your guys reply. :)

nim65s commented 1 year ago

This is not specific to pinocchio, but to your compiler, compiler version, and current CPU. Compiling some of your linked libraries with this flag and some without will very often lead to troubles, pinocchio or not.