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

Using pinocchio within a ROS package #1722

Closed BolunDai0216 closed 2 years ago

BolunDai0216 commented 2 years ago

Hi,

I am trying to use pinocchio within a ROS package. When running catkin_make --only-pkg-with-deps package_name I get the following error:

Base path: /home/bolun/catkin_ws
Source space: /home/bolun/catkin_ws/src
Build space: /home/bolun/catkin_ws/build
Devel space: /home/bolun/catkin_ws/devel
Install space: /home/bolun/catkin_ws/install
Whitelisted packages: franka_arm_ros
####
#### Running command: "cmake /home/bolun/catkin_ws/src -DCATKIN_DEVEL_PREFIX=/home/bolun/catkin_ws/devel -DCMAKE_INSTALL_PREFIX=/home/bolun/catkin_ws/install -DCATKIN_WHITELIST_PACKAGES=franka_arm_ros -G Unix Makefiles" in "/home/bolun/catkin_ws/build"
####
-- Using CATKIN_DEVEL_PREFIX: /home/bolun/catkin_ws/devel
-- Using CMAKE_PREFIX_PATH: /opt/openrobots;/opt/ros/noetic;/opt/openrobots;/opt/openrobots
-- This workspace overlays: /opt/ros/noetic
-- Found PythonInterp: /usr/bin/python3 (found suitable version "3.8.10", minimum required is "3") 
-- Using PYTHON_EXECUTABLE: /usr/bin/python3
-- Using Debian Python package layout
-- Using empy: /usr/lib/python3/dist-packages/em.py
-- Using CATKIN_ENABLE_TESTING: ON
-- Call enable_testing()
-- Using CATKIN_TEST_RESULTS_DIR: /home/bolun/catkin_ws/build/test_results
-- Forcing gtest/gmock from source, though one was otherwise available.
-- Found gtest sources under '/usr/src/googletest': gtests will be built
-- Found gmock sources under '/usr/src/googletest': gmock will be built
-- Found PythonInterp: /usr/bin/python3 (found version "3.8.10") 
-- Using Python nosetests: /usr/bin/nosetests3
-- catkin 0.8.10
-- BUILD_SHARED_LIBS is on
-- BUILD_SHARED_LIBS is on
-- Using CATKIN_WHITELIST_PACKAGES: franka_arm_ros
-- ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-- ~~  traversing 1 packages in topological order:
-- ~~  - franka_arm_ros
-- ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-- +++ processing catkin package: 'franka_arm_ros'
-- ==> add_subdirectory(FrankaArmROS/franka_arm_ros)
-- Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
-- pinocchio FOUND. pinocchio at /opt/openrobots/lib/libpinocchio.so
-- boost_filesystem FOUND. boost_filesystem at /usr/lib/x86_64-linux-gnu/libboost_filesystem.so
-- boost_serialization FOUND. boost_serialization at /usr/lib/x86_64-linux-gnu/libboost_serialization.so
-- boost_system FOUND. boost_system at /usr/lib/x86_64-linux-gnu/libboost_system.so
-- Found Boost: /usr/include (found version "1.71.0") found components: filesystem serialization system 
-- hpp-fcl FOUND. hpp-fcl at /opt/openrobots/lib/libhpp-fcl.so
-- Found Boost: /usr/include (found version "1.71.0") found components: chrono serialization 
-- Default C++ standard: 201402
-- C++ standard sufficient: Minimal required 11, currently defined: 14
-- C++ standard sufficient: Minimal required 11, currently defined: 14
-- Configuring done
CMake Warning at FrankaArmROS/franka_arm_ros/CMakeLists.txt:148 (add_library):
  Cannot generate a safe runtime search path for target franka_arm_ros_lib
  because files in some directories may conflict with libraries in implicit
  directories:

    runtime library [liboctomap.so.1.9] in /usr/lib/x86_64-linux-gnu may be hidden by files in:
      /opt/ros/noetic/lib
    runtime library [liboctomath.so.1.9] in /usr/lib/x86_64-linux-gnu may be hidden by files in:
      /opt/ros/noetic/lib

  Some of these libraries may not be found correctly.

-- Generating done
-- Build files have been written to: /home/bolun/catkin_ws/build
####
#### Running command: "make -j24 -l24" in "/home/bolun/catkin_ws/build"
####
[ 50%] Building CXX object FrankaArmROS/franka_arm_ros/CMakeFiles/franka_arm_ros_lib.dir/src/pd_controller.cpp.o
In file included from /usr/include/boost/variant/variant.hpp:31,
                 from /usr/include/boost/variant.hpp:17,
                 from /opt/ros/noetic/include/pinocchio/multibody/joint/joint-collection.hpp:11,
                 from /opt/ros/noetic/include/pinocchio/multibody/joint/joint-generic.hpp:8,
                 from /opt/ros/noetic/include/pinocchio/multibody/model.hpp:17,
                 from /opt/ros/noetic/include/pinocchio/parsers/sample-models.hpp:9,
                 from /home/bolun/catkin_ws/src/FrankaArmROS/franka_arm_ros/include/franka_arm_ros/pd_controller.h:21,
                 from /home/bolun/catkin_ws/src/FrankaArmROS/franka_arm_ros/src/pd_controller.cpp:1:
/usr/include/boost/variant/detail/make_variant_list.hpp: In instantiation of ‘struct boost::detail::variant::make_variant_list<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> > >’:
/usr/include/boost/mpl/eval_if.hpp:38:31:   required from ‘struct boost::mpl::eval_if<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> > >::is_sequence_based_, pinocchio::JointModelRevoluteTpl<double, 0, 0>, boost::detail::variant::make_variant_list<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> > > >’
/usr/include/boost/variant/variant.hpp:1138:17:   required from ‘class 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> > >’
/opt/ros/noetic/include/pinocchio/multibody/joint/joint-generic.hpp:161:10:   required from ‘struct pinocchio::JointModelTpl<double>’
/opt/ros/noetic/include/pinocchio/container/aligned-vector.hpp:42:71:   required from ‘pinocchio::ModelTpl<Scalar, Options, JointCollectionTpl>::ModelTpl() [with _Scalar = double; int _Options = 0; JointCollectionTpl = pinocchio::JointCollectionDefaultTpl]’
/home/bolun/catkin_ws/src/FrankaArmROS/franka_arm_ros/include/franka_arm_ros/pd_controller.h:29:7:   required from ‘B* class_loader::impl::MetaObject<C, B>::create() const [with C = franka_arm_ros::PDController; B = controller_interface::ControllerBase]’
/opt/ros/noetic/include/class_loader/meta_object.hpp:196:7:   required from here
/usr/include/boost/variant/detail/make_variant_list.hpp:40:46: error: wrong number of template arguments (21, should be at least 0)
   40 |     typedef typename mpl::list< T... >::type type;
      |                                              ^~~~
In file included from /usr/include/boost/mpl/aux_/include_preprocessed.hpp:37,
                 from /usr/include/boost/mpl/list.hpp:46,
                 from /usr/include/boost/math/policies/policy.hpp:9,
                 from /usr/include/boost/math/policies/error_handling.hpp:21,
                 from /usr/include/boost/math/special_functions/round.hpp:14,
                 from /opt/ros/noetic/include/ros/time.h:58,
                 from /opt/ros/noetic/include/ros/forwards.h:42,
                 from /opt/ros/noetic/include/ros/node_handle.h:31,
                 from /opt/ros/noetic/include/controller_interface/controller_base.h:35,
                 from /opt/ros/noetic/include/controller_interface/multi_interface_controller.h:33,
                 from /home/bolun/catkin_ws/src/FrankaArmROS/franka_arm_ros/include/franka_arm_ros/pd_controller.h:7,
                 from /home/bolun/catkin_ws/src/FrankaArmROS/franka_arm_ros/src/pd_controller.cpp:1:
/usr/include/boost/mpl/aux_/preprocessed/gcc/list.hpp:22:8: note: provided for ‘template<class T0, class T1, class T2, class T3, class T4, class T5, class T6, class T7, class T8, class T9, class T10, class T11, class T12, class T13, class T14, class T15, class T16, class T17, class T18, class T19> struct boost::mpl::list’
   22 | struct list;
      |        ^~~~
In file included from /opt/ros/noetic/include/pinocchio/parsers/sample-models.hpp:9,
                 from /home/bolun/catkin_ws/src/FrankaArmROS/franka_arm_ros/include/franka_arm_ros/pd_controller.h:21,
                 from /home/bolun/catkin_ws/src/FrankaArmROS/franka_arm_ros/src/pd_controller.cpp:1:
/opt/ros/noetic/include/pinocchio/multibody/model.hpp: In instantiation of ‘pinocchio::ModelTpl<Scalar, Options, JointCollectionTpl>::ModelTpl() [with _Scalar = double; int _Options = 0; JointCollectionTpl = pinocchio::JointCollectionDefaultTpl]’:
/home/bolun/catkin_ws/src/FrankaArmROS/franka_arm_ros/include/franka_arm_ros/pd_controller.h:29:7:   required from ‘B* class_loader::impl::MetaObject<C, B>::create() const [with C = franka_arm_ros::PDController; B = controller_interface::ControllerBase]’
/opt/ros/noetic/include/class_loader/meta_object.hpp:196:7:   required from here
/opt/ros/noetic/include/pinocchio/multibody/model.hpp:184:41: note:   when instantiating default argument for call to ‘pinocchio::container::aligned_vector<T>::aligned_vector(pinocchio::container::aligned_vector<T>::size_type, const value_type&) [with T = pinocchio::JointModelTpl<double>; pinocchio::container::aligned_vector<T>::size_type = long unsigned int; pinocchio::container::aligned_vector<T>::value_type = pinocchio::JointModelTpl<double>]’
  184 |     , gravity(gravity981,Vector3::Zero())
      |                                         ^
make[2]: *** [FrankaArmROS/franka_arm_ros/CMakeFiles/franka_arm_ros_lib.dir/build.make:63: FrankaArmROS/franka_arm_ros/CMakeFiles/franka_arm_ros_lib.dir/src/pd_controller.cpp.o] Error 1
make[1]: *** [CMakeFiles/Makefile2:1348: FrankaArmROS/franka_arm_ros/CMakeFiles/franka_arm_ros_lib.dir/all] Error 2
make: *** [Makefile:141: all] Error 2
Invoking "make -j24 -l24" failed

Would it be possible to help me take a look at this? My understanding of the build system is limited so I really appreciate any help. Thanks in advance!

BolunDai0216 commented 2 years ago

My CMakeLists.txt file is:

cmake_minimum_required(VERSION 3.0.2)
project(franka_arm_ros)

set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)

find_package(catkin REQUIRED COMPONENTS
  controller_interface
  dynamic_reconfigure
  eigen_conversions
  franka_gripper
  franka_hw
  geometry_msgs
  hardware_interface
  joint_limits_interface
  pluginlib
  realtime_tools
  roscpp
  tf
  tf_conversions
  urdf
  visualization_msgs
)

find_package(Eigen3 REQUIRED)
find_package(pinocchio REQUIRED)
find_package(Franka 0.9.0 QUIET)
if(NOT Franka_FOUND)
  find_package(Franka 0.8.0 REQUIRED)
endif()

catkin_package(
 INCLUDE_DIRS include
 LIBRARIES franka_arm_ros
 CATKIN_DEPENDS controller_interface dynamic_reconfigure eigen_conversions franka_gripper franka_hw geometry_msgs hardware_interface joint_limits_interface pluginlib realtime_tools roscpp tf tf_conversions urdf visualization_msgs
 DEPENDS Franka
)

include_directories(
include
  ${catkin_INCLUDE_DIRS}
)

add_library(franka_arm_ros_lib src/pd_controller.cpp)
target_link_libraries(franka_arm_ros_lib pinocchio::pinocchio ${catkin_LIBRARIES})
jcarpent commented 2 years ago

It seems that the compile definitions of Pinocchio are not taken into account. @wxmerkt or @nim65s Do you know why?

wxmerkt commented 2 years ago

I put together an example on how to use Pinocchio in a ROS package here: https://github.com/wxmerkt/pinocchio_ros_example

Key is the following note from the README:

When including a ROS header, e.g. ros/ros.h, compilation errors may occur. This is due to different requirements for BOOST_MPL_LIMIT_LIST_SIZE between ROS and Pinocchio. To avoid this error, make sure to always include pinocchio/fwd.hpp before any other header.

jcarpent commented 2 years ago

Thanks for the quick answer @wxmerkt and the quick solution.

BolunDai0216 commented 2 years ago

I put together an example on how to use Pinocchio in a ROS package here: https://github.com/wxmerkt/pinocchio_ros_example

Key is the following note from the README:

When including a ROS header, e.g. ros/ros.h, compilation errors may occur. This is due to different requirements for BOOST_MPL_LIMIT_LIST_SIZE between ROS and Pinocchio. To avoid this error, make sure to always include pinocchio/fwd.hpp before any other header.

Thanks for the quick response! After moving the header files on top it successfully compiled!