Thiết kế website giá rẻ

Question

I am trying to use Pinocchio library for robot (Franka Emika Panda in my case) dynamics computation and simulation in ROS 2 Foxy environment. And I have a problem while loading model using pinocchio::urdf::buildModel.

I installed Pinocchio from the source following the procedure described here.

And I also installed Pinocchio ROS package.

sudo apt install ros-foxy-pinocchio

After that, I included pinocchio library in the CMakeLists.txt and package.xml of my customized ROS package as below.

CMakeLists.txt

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(msg_interface_franka_emika_panda REQUIRED)
find_package(pinocchio REQUIRED)

set(PINOCCHIO_MODEL_DIR "/home/<user-name>/git-repos/pinocchio/models")
add_definitions('-DPINOCCHIO_MODEL_DIR="${PINOCCHIO_MODEL_DIR}"')

# Find source files and Include header files to create executable file of node
file(GLOB_RECURSE SOURCES
  ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp
)

# controller node
add_executable(${PROJECT_NAME} ${SOURCES}
)

ament_target_dependencies(${PROJECT_NAME}
  msg_interface_franka_emika_panda
  sensor_msgs
  rclcpp
)

target_include_directories(${PROJECT_NAME}
PUBLIC
  ${CMAKE_CURRENT_SOURCE_DIR}/include/controller
  ${CMAKE_CURRENT_SOURCE_DIR}/include
  $<INSTALL_INTERFACE:include>
)

target_link_libraries(${PROJECT_NAME}
  pinocchio::pinocchio
)

package.xml

  <depend>rclcpp</depend>
  <depend>sensor_msgs</depend>
  <depend>geometry_msgs</depend>
  <depend>msg_interface_franka_emika_panda</depend>
  <depend>pinocchio</depend>

After that, I wrote main.cpp as below, but there’s an error when I tried to load and build URDF file of the robot.

main.cpp

#include "my_panda_controller.hpp"

#include <filesystem>
#include <fstream>
#include <iostream>

#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
// #include "pinocchio/parsers/mjcf.hpp"
#include "pinocchio/parsers/urdf.hpp"

using namespace std;

int main(int argc, char ** argv)
{
  const std::string urdf_filename =
    PINOCCHIO_MODEL_DIR +
    std::string("/example-robot-data/robots/panda_description/urdf/panda.urdf");
  cout << "urdf_filename: " << urdf_filename << endl;

  /* Check if URDF file exists and can be opened */
  std::ifstream urdf_file(urdf_filename);
  if (!urdf_file.is_open())
  {
    cerr << "Error: Unable to open URDF file: " << urdf_filename << endl;
    return -1;
  }
  else
  {
    cout << "URDF file is opened." << endl;
  }

  pinocchio::Model model;
  pinocchio::urdf::buildModel(urdf_filename, model);  // TODO : debug here
  cout << "Model name: " << model.name << endl;

  cout << "Dim of generalized coordinates: " << model.nq << endl;
  cout << "Dim of generalized velocities: " << model.nv << endl;

  pinocchio::Data data(model);

  rclcpp::init(argc, argv);
  auto node = std::make_shared<MyPandaController>();
  rclcpp::spin(node);

  rclcpp::shutdown();
  return 0;
}

When I build my package, there’s a warning message as below,

/usr/bin/ld: warning: libconsole_bridge.so.0.4, needed by /usr/lib/x86_64-linux-gnu/liburdfdom_world.so, may conflict with libconsole_bridge.so.1.0

and when I ros2 launch my package, the process is terminated showing the following messages.

[INFO] [my_franka_emika_panda_controller-1]: process started with pid [7238]
[my_franka_emika_panda_controller-1] urdf_filename: /home/<user-name>/git-repos/pinocchio/models/example-robot-data/robots/panda_description/urdf/panda.urdf
[my_franka_emika_panda_controller-1] URDF file is opened.
[ERROR] [my_franka_emika_panda_controller-1]: process has died [pid 7238, exit code -11, cmd '/home/<user-name>/robot_ws/install/my_franka_emika_panda_controller/lib/my_franka_emika_panda_controller/my_franka_emika_panda_controller --ros-args -r __node:=my_franka_emika_panda_controller --params-file /home/<user-name>/robot_ws/install/my_franka_emika_panda_controller/share/my_franka_emika_panda_controller/param/panda_param.yaml'].

It seems that the error occurred while the process tried to load model information from the URDF file (when I commented the buildModel line, there’s no error), I don’t have an idea what is the exact issue and how to fix it.

I referred to the issue about adding pinocchio in cmake and I think I did it right. When I build model with simple example described in overview-simple.cpp, there’s no error.

The issue about Linker conflicts and usage clarification seems to be related with the warning message during build of my package, but I am not sure this is the cause of the problem in loading model.

At first, I thought there might be some problem in URDF file (file itself, or the path), so I tried to build the model with MJCF file.

So, I included the header for mjcf parser, and build the package, but there are bunch of error during build.

#include "pinocchio/parsers/mjcf.hpp"

The parser for MJCF file is updated since the version 2.7.0, but the Pinocchio package version for ROS Foxy is 2.6.17. So, I wonder if I can not user the parser for MJCF file in ROS 2 Foxy environment.

To summarize my questions,

I want to use Pinocchio library in ROS 2 Foxy environment, but there’s an error in loading URDF model of Panda robot. I think I added the library to my ROS package, and set the URDF file and path correctly, but there’s a problem in loading model.
If I can, I want to load model using MJCF file rather than URDF file. But it seems that MJCF file can be parsed after the version 2.7.0, and it looks that the pinocchio ROS package for Foxy is 2.6.17. So, I wonder if there’s a way that I can use MJCF file in Foxy environment.

Thiết kế website giá rẻ

Danh mục

How to use Pinocchio (C++) in ROS 2 Foxy?