Question about collision calculation time

[Linjackffy]

Hello！
I have noticed that the calculated time increases as the distance between objects decreases.
Is this expected behavior, or is there a mistake in my parameter settings.
Could someone help answer this question?
Below is the code I am using.

The UR model is download from https://github.com/stack-of-tasks/pinocchio/tree/master/models
And you need to replace ur5.srdf with this file:

<?xml version="1.0"?>
<!--

   UR-5 SRDF

   This file is an alternative way of specifying semantics on the robot
   structure.

   It also provides:
   - the set of interesting collision pairs

  -->
<robot name="ur5">
  <disable_collisions link1="base_link" link2="shoulder_link"/>
  <disable_collisions link1="base_link" link2="upper_arm_link"/>
  <disable_collisions link1="shoulder_link" link2="upper_arm_link"/>
  <disable_collisions link1="upper_arm_link" link2="forearm_link"/>
  <disable_collisions link1="forearm_link" link2="wrist_1_link"/>
  <disable_collisions link1="forearm_link" link2="wrist_2_link"/>
  <disable_collisions link1="forearm_link" link2="wrist_3_link"/>
  <disable_collisions link1="wrist_1_link" link2="wrist_2_link"/>
  <disable_collisions link1="wrist_1_link" link2="wrist_3_link"/>
  <disable_collisions link1="wrist_2_link" link2="wrist_3_link"/>
  <disable_collisions link1="ee_link" link2="base_link"/>
  <disable_collisions link1="ee_link" link2="shoulder_link"/>
  <disable_collisions link1="ee_link" link2="upper_arm_link"/>
  <disable_collisions link1="ee_link" link2="forearm_link"/>
  <disable_collisions link1="ee_link" link2="wrist_1_link"/>
  <disable_collisions link1="ee_link" link2="wrist_2_link"/>
  <disable_collisions link1="ee_link" link2="wrist_3_link"/>
</robot>

#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/srdf.hpp"

#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/geometry.hpp"
#include "pinocchio/collision/collision.hpp"

#include <iostream>

// PINOCCHIO_MODEL_DIR is defined by the CMake but you can define your own modeldirectory here.
#ifndef PINOCCHIO_MODEL_DIR
  #define PINOCCHIO_MODEL_DIR "path_to_the_model_dir"
#endif

int main(int /*argc*/, char ** /*argv*/)
{
  using namespace pinocchio;
  const std::string robots_model_path = PINOCCHIO_MODEL_DIR;

  // You should change here to set up your own URDF file
  const std::string urdf_filename =
    robots_model_path
    + std::string("/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf");
  // You should change here to set up your own SRDF file
  const std::string srdf_filename =
    robots_model_path + std::string("/example-robot-data/robots/ur_description/srdf/ur5.srdf");

  // Load the URDF model contained in urdf_filename
  Model model;
  pinocchio::urdf::buildModel(urdf_filename, model);

  // Build the data associated to the model
  Data data(model);

  // Load the geometries associated to model which are contained in the URDF file
  GeometryModel geom_model;
  pinocchio::urdf::buildGeom(
    model, urdf_filename, pinocchio::COLLISION, geom_model, robots_model_path);

  // Add all possible collision pairs and remove the ones collected in the SRDF file
  geom_model.addAllCollisionPairs();
  pinocchio::srdf::removeCollisionPairs(model, geom_model, srdf_filename);
  
  // Build the data associated to the geom_model
  GeometryData geom_data(geom_model); // contained the intermediate computations, like the placement
                                      // of all the geometries with respect to the world frame

  for (size_t k = 0; k < geom_model.collisionPairs.size(); ++k) {
    geom_data.collisionRequests[k].security_margin = 0.003;
    geom_data.collisionRequests[k].num_max_contacts = 1;
    geom_data.collisionRequests[k].enable_contact = false;
    geom_data.collisionRequests[k].enable_distance_lower_bound = false;
  }

  Eigen::VectorXd q(model.nq);
  q << 0,0,-135,180,90,0;
  q *= M_PI / 180;

  for (size_t i = 0; i < 100; i++) {
    q[2] -= 0.2 * M_PI / (100 - 1);
    std::chrono::time_point<std::chrono::high_resolution_clock> start = std::chrono::high_resolution_clock::now();
    bool ret = computeCollisions(model, data, geom_model, geom_data, q, true);
    auto stop = std::chrono::high_resolution_clock::now();
    double time = std::chrono::duration_cast<std::chrono::nanoseconds>(stop - start).count() / 1000.0; // [µs]
    std::size_t min_idx = computeDistances(geom_model, geom_data);
    const hpp::fcl::DistanceResult & dr = geom_data.distanceResults[geom_model.collisionPairs.size() - 1];
    std::cout << "min dis " << dr.min_distance << " calc time : " << time;

    if (ret) {
        auto k = geom_data.collisionPairIndex;
        const CollisionPair &cp = geom_model.collisionPairs[k];
        std::cout << " axis " << cp.first << " axis " << cp.second << " collision" << std::endl;
        const hpp::fcl::CollisionResult & cr = geom_data.collisionResults[k];

        // if (cr.numContacts() > 0) {
        //   for (auto &c : cr.getContacts()) {
        //     std::cout << "contact point " << c.pos.transpose() << std::endl;
        //   }
        // }
    } else {
        std::cout << std::endl;
    }
  }

  return 0;
}

Here is the output:

min dis 0.0799548 calc time : 52.969
min dis 0.0777391 calc time : 14.056
min dis 0.0755248 calc time : 11.606
min dis 0.073312 calc time : 19.186
min dis 0.071101 calc time : 16.128
min dis 0.0688921 calc time : 14.519
min dis 0.0666855 calc time : 13.988
min dis 0.0644817 calc time : 16.037
min dis 0.0622809 calc time : 13.911
min dis 0.0600836 calc time : 16.362
min dis 0.0578904 calc time : 20.941
min dis 0.0557017 calc time : 13.269
min dis 0.0535182 calc time : 18.057
min dis 0.0513407 calc time : 29.612
min dis 0.0491702 calc time : 21.643
min dis 0.0470075 calc time : 18.708
min dis 0.044854 calc time : 16.54
min dis 0.0426585 calc time : 12.781
min dis 0.0404537 calc time : 45.242
min dis 0.0382486 calc time : 17.574
min dis 0.0360434 calc time : 32.419
min dis 0.0338381 calc time : 17.544
min dis 0.0316359 calc time : 13.488
min dis 0.02944 calc time : 25.817
min dis 0.0272518 calc time : 19.822
min dis 0.0250736 calc time : 232.99
min dis 0.0229084 calc time : 15.769
min dis 0.0207601 calc time : 50.033
min dis 0.0186346 calc time : 27.923
min dis 0.0165409 calc time : 30.177
min dis 0.0144908 calc time : 28.793
min dis 0.0124547 calc time : 41.515
min dis 0.0104179 calc time : 135.268
min dis 0.00838033 calc time : 163.621
min dis 0.00634214 calc time : 220.214
min dis 0.00430341 calc time : 408.062
min dis 0.00226422 calc time : 166.596 axis 2 axis 6 collision
min dis 0.00022464 calc time : 67.519 axis 2 axis 6 collision
min dis 0 calc time : 81.219 axis 2 axis 6 collision
min dis 0 calc time : 103.116 axis 2 axis 6 collision
min dis 0 calc time : 159.164 axis 2 axis 6 collision
min dis 0 calc time : 69.447 axis 2 axis 5 collision
min dis 0 calc time : 61.47 axis 2 axis 5 collision
min dis 0 calc time : 146.14 axis 2 axis 5 collision
min dis 0 calc time : 79.252 axis 2 axis 5 collision
min dis 0 calc time : 77.531 axis 2 axis 5 collision
min dis 0 calc time : 644.931 axis 2 axis 5 collision
min dis 0 calc time : 160.951 axis 2 axis 5 collision
min dis 0 calc time : 203.695 axis 2 axis 5 collision
min dis 0 calc time : 138.126 axis 2 axis 5 collision
min dis 0 calc time : 676.629 axis 2 axis 5 collision
min dis 0 calc time : 413.353 axis 2 axis 5 collision
min dis 0 calc time : 174.798 axis 2 axis 5 collision
min dis 0 calc time : 161.452 axis 2 axis 5 collision
min dis 0 calc time : 185.219 axis 2 axis 5 collision
min dis 0 calc time : 500.083 axis 2 axis 5 collision
min dis 0 calc time : 331.636 axis 2 axis 5 collision
min dis 0 calc time : 668.264 axis 2 axis 5 collision
min dis 0 calc time : 446.05 axis 2 axis 5 collision
min dis 0 calc time : 710.832 axis 2 axis 5 collision
min dis 0 calc time : 823.992 axis 1 axis 5 collision
min dis 0 calc time : 177.972 axis 1 axis 5 collision
min dis 0 calc time : 526.987 axis 1 axis 5 collision
min dis 0 calc time : 171.913 axis 1 axis 5 collision
min dis 0 calc time : 167.472 axis 1 axis 5 collision
min dis 0 calc time : 65.931 axis 1 axis 5 collision
min dis 0 calc time : 57.89 axis 1 axis 5 collision
min dis 0 calc time : 71.202 axis 1 axis 5 collision
min dis 0 calc time : 148.807 axis 1 axis 5 collision
min dis 0 calc time : 70.804 axis 1 axis 5 collision
min dis 0 calc time : 72.497 axis 1 axis 5 collision
min dis 0 calc time : 84.643 axis 1 axis 5 collision
min dis 0 calc time : 145.48 axis 1 axis 5 collision
min dis 0 calc time : 172.607 axis 1 axis 5 collision
min dis 0 calc time : 492.076 axis 1 axis 5 collision
min dis 0 calc time : 189.492 axis 1 axis 5 collision
min dis 0 calc time : 381.186 axis 1 axis 5 collision
min dis 0 calc time : 244.56 axis 1 axis 5 collision
min dis 0 calc time : 793.912 axis 1 axis 5 collision
min dis 0 calc time : 1253.15 axis 1 axis 5 collision
min dis 0 calc time : 188.201 axis 1 axis 3 collision
min dis 0 calc time : 431.315 axis 1 axis 3 collision
min dis 0 calc time : 188.977 axis 1 axis 3 collision
min dis 0 calc time : 175.32 axis 1 axis 3 collision
min dis 0 calc time : 761.087 axis 1 axis 3 collision
min dis 0 calc time : 188.233 axis 1 axis 3 collision
min dis 0 calc time : 344.052 axis 1 axis 3 collision
min dis 0 calc time : 322.679 axis 1 axis 3 collision
min dis 0 calc time : 301.51 axis 1 axis 3 collision
min dis 0 calc time : 392.077 axis 1 axis 3 collision
min dis 0 calc time : 97.263 axis 1 axis 3 collision
min dis 0 calc time : 1190.55 axis 1 axis 3 collision
min dis 0 calc time : 214.961 axis 1 axis 3 collision
min dis 0 calc time : 126.906 axis 1 axis 3 collision
min dis 0 calc time : 80.022 axis 1 axis 3 collision
min dis 0 calc time : 53.382 axis 1 axis 3 collision
min dis 0 calc time : 39.093 axis 1 axis 3 collision
min dis 0 calc time : 49.896 axis 1 axis 3 collision
min dis 0 calc time : 52.244 axis 1 axis 3 collision
min dis 0 calc time : 62.919 axis 1 axis 3 collision

[Everest-Zhao]

I have not used Pinocchio, but based on my understanding of FCL and Coal, this behavior is normal. If you are using a broad phase manager, it might be due to the use of Dynamic AABB bounding box structures in the broad phase. When objects are far apart, the broad phase can quickly filter out potential collisions using simple BV (Bounding Volume) calculations. However, when objects are close, the BV calculations may fail, causing the system to enter the narrow phase for further calculations, which are often more time-consuming.

If you are not using a broad phase manager but your model is mesh-based, the narrow phase will still use a BVH (Bounding Volume Hierarchy) model. Therefore, when the objects are not far apart, the BV tests are quick, but as the objects get closer, the computational load increases.

[Linjackffy]

Thank you for your detailed explanation, which has resolved my confusion.