computecollisions() missing from ros humble installation

[Rank-N-Tensor]

Bug description

the function computecollisions() cannot be called when writing code in c++. I understand that this is only available when pinocchio is built with HPP-FCL support, which is how it should be since its installed via ROS.

/home/adi/hum_rob_ws/src/six_dof/src/IK_test.cpp: In function ‘int main(int, char**)’:
/home/adi/hum_rob_ws/src/six_dof/src/IK_test.cpp:96:5: error: ‘computeCollisions’ was not declared in this scope
   96 |     computeCollisions(model,data,collision_model,collision_data,q,false);

Expected behavior

Since It is installed via ROS(Humble) , it should come with HPP-FCL support and I should be able to call it.

Reproduction steps

Steps to reproduce the behavior:

1. Load the code attached below.
2. build with colcon
3. Scroll down to the build failed error
4. See error

Code

please see the code below

#include "pinocchio/fwd.hpp"
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/geometry.hpp"
#include "pinocchio/spatial/explog.hpp"

#include <cmath>
#include <iostream>
#include <string>

using namespace pinocchio;

double rad2deg(double radians) { return radians * (180.0 / M_PI); }

int main(int argc, char *argv[]) {

  // mesh_dir
  const std::string mesh_dir = "src/six_dof/meshes";

  // urdf file name
  const std::string urdf_file = "src/six_dof/urdf/6dof_from_hip.urdf";

  // loading urdf
  Model model;
  model = pinocchio::urdf::buildModel(urdf_file, model);
  GeometryModel visual_model;
  visual_model = pinocchio::urdf::buildGeom(model, urdf_file, VISUAL,
                                            visual_model, mesh_dir);
  GeometryModel collision_model;
  collision_model = pinocchio::urdf::buildGeom(model, urdf_file, COLLISION,
                                               collision_model, mesh_dir);

  std::cout << "model name: " << model.name << std::endl;

  // Geometry data
  Data data(model);
  GeometryData collision_data(collision_model);
  GeometryData visual_data(visual_model);

  // neutral_config
  Eigen::VectorXd q = pinocchio::neutral(model);

  const int JOINT_ID = 8; // 8 is right foot, 4 is left foot

  const Eigen::Matrix3d R_des{
      {1.0, 0.0, 0.0}, {0.0, 0.000796327, -1.0}, {0.0, 1.0, 0.000796327}};

  const pinocchio::SE3 oMdes(R_des, Eigen::Vector3d(0.0235, -0.04, -0.260));

  const double eps = 1e-3;
  const int IT_MAX = 20000;
  const double DT = 1e-1;
  const double damp = 1e-1;

  pinocchio::Data::Matrix6x J(6, model.nv);
  J.setZero();

  bool success = false;
  typedef Eigen::Matrix<double, 6, 1> Vector6d;
  Vector6d err;
  Eigen::VectorXd v(model.nv);

  // LOOP
  for (int i = 0;; i++) {
    pinocchio::forwardKinematics(model, data, q);
    const pinocchio::SE3 iMd = data.oMi[JOINT_ID].actInv(oMdes);
    err = pinocchio::log6(iMd).toVector(); // in joint frame
    if (err.norm() < eps) {
      success = true;
      break;
    }
    if (i >= IT_MAX) {
      success = false;
      break;
    }
    pinocchio::computeJointJacobian(model, data, q, JOINT_ID,
                                    J); // J in joint frame
    pinocchio::Data::Matrix6 Jlog;
    pinocchio::Jlog6(iMd.inverse(), Jlog);
    J = -Jlog * J;
    pinocchio::Data::Matrix6 JJt;
    JJt.noalias() = J * J.transpose();
    JJt.diagonal().array() += damp;
    v.noalias() = -J.transpose() * JJt.ldlt().solve(err);
    q = pinocchio::integrate(model, q, v * DT);

    // collision modeling
    updateGeometryPlacements(model,data,collision_model,collision_data,q);
    computeCollisions(model,data,collision_model,collision_data,q,false);

    if (!(i % 10))
      std::cout << i << ": error = " << err.transpose() << std::endl;
  }

  if (success) {
    std::cout << "Convergence achieved!" << std::endl;
  } else {
    std::cout << "\nWarning: the iterative algorithm has not reached "
                 "convergence to the desired precision"
              << std::endl;
  }

  std::cout << "\nresult: " << q.transpose() << std::endl;
  std::cout << "\nfinal error: " << err.transpose() << std::endl;

  // convert to deg
  for (int i = 0; i < q.size(); i++) {
    q[i] = rad2deg(q[i]);
  }

  std::cout << "\n result in deg is:" << q.transpose() << "\n";

}

Additional context

this is my CmakeLists.txt file

cmake_minimum_required(VERSION 3.8)
project(six_dof)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclpy REQUIRED)
find_package(pinocchio REQUIRED)
find_package(Eigen3 REQUIRED)

set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)

include_directories(${EIGEN3_INCLUDE_DIR})

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

add_executable(test_node src/IK_test.cpp)
ament_target_dependencies(test_node rclcpp pinocchio)

install(
  DIRECTORY
    urdf
    rviz
    launch
    meshes
  DESTINATION
    share/${PROJECT_NAME}/
)

install(TARGETS 
test_node
DESTINATION
lib/${PROJECT_NAME}
)

ament_package()

and my package.xml

 <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>
  <depend>rclpy</depend>
  <depend>pinocchio</depend>

Furthermore, I do have pinocchio installed via robotpkg. I am not sure if that would cause a conflict.

System

• OS: Ubuntu 22.04
• Pinocchio version: 2.6.2
• hpp-fcl version: 2.4.0

Please help me and thank you for building such a great package, it is really useful!

[Rank-N-Tensor]

i was able to fix it , by adding target_link_libraries(test_node pinocchio::pinocchio) to my CmakeLists.txt. This can be found in https://github.com/wxmerkt/pinocchio_ros_example.

that said, i would appreciate any ROS2 specific inputs from the authors as the example is for ROS1.

Thank you.

[ManifoldFR]

Have you tried using the namespaced target pinocchio::pinocchio in your call to ament_target_dependencies ?

The docs seem to indicate ament is supposed to take care of the includes and linking but it seems it didn't work here.

[Rank-N-Tensor]

hello @ManifoldFR, thank you for your response. Do you mean this ament_target_dependencies(test_node rclcpp pinocchio::pinocchio)?

If so , this is the output

  ament_target_dependencies() the passed package name 'pinocchio::pinocchio'
  was not found before
Call Stack (most recent call first):
  CMakeLists.txt:42 (ament_target_dependencies)

please advise.

[ManifoldFR]

Okay, after a quick look around it seems ament_target_dependencies was added to ROS2 before the advent of modern CMake namespaced targets (and using target_link_libraries). There's a discussion about deprecating it right here: https://github.com/ament/ament_cmake/issues/292 since many packages use these new targets now (including Pinocchio).

I think the docs you looked at (and that I found) are just old and don't reflect how things work nowadays. The tl;dr is that ament_target_dependencies is a vestigial abomination that you should ignore and use target_link_libraries for everything instead when you can - it will handle includes and linking just like the CMake docs promise :)

@nim65s would you agree with my analysis?

[Rank-N-Tensor]

Understood! thank you for this, I am glad i could learn something new today. Therefore, would you recommend something like this

add_executable(test_node src/IK_test.cpp)
ament_target_dependencies(test_node rclcpp)
target_link_libraries(test_node pinocchio::pinocchio)

? Or would you rather not use ament at all?

[ManifoldFR]

Understood! thank you for this, I am glad i could learn something new today. Therefore, would you recommend something like this

add_executable(test_node src/IK_test.cpp)
ament_target_dependencies(test_node rclcpp)
target_link_libraries(test_node pinocchio::pinocchio)

? Or would you rather not use ament at all?

It's better to use the modern CMake targets and target_link_libraries for everything. rlcpp does provide a modern CMake target so you should use it instead and banish that ament function to the shadow realm.

[ManifoldFR]

Closing as this is a problem with ROS2's maybe outdated docs and lack of deprecation