feat: Integrate min distance

CHANGELOG.md

@@ -13,6 +13,7 @@ Release Versions:
 
 ## Upcoming changes (in development)
 
+- feat: integrate minimum distance calculation feature into robot model(#167)
 - ci: update workflows (#175)
 - feat: integrate collision detection feature into robot model (#163)
 - feat: add IO states to state representation (py) (#173)

VERSION

@@ -1 +1 @@
-7.3.11
+7.3.12

demos/CMakeLists.txt

@@ -15,7 +15,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-find_package(control_libraries 7.3.11 CONFIG REQUIRED)
+find_package(control_libraries 7.3.12 CONFIG REQUIRED)
 
 set(DEMOS_SCRIPTS
   task_space_control_loop

doxygen/doxygen.conf

@@ -38,7 +38,7 @@ PROJECT_NAME           = "Control Libraries"
 # could be handy for archiving the generated documentation or if some version
 # control system is used.
 
-PROJECT_NUMBER = 7.3.11
+PROJECT_NUMBER = 7.3.12
 
 # Using the PROJECT_BRIEF tag one can provide an optional one line description
 # for a project that appears at the top of each page and should give viewer a

protocol/clproto_cpp/CMakeLists.txt

@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 3.15)
 
-project(clproto VERSION 7.3.11)
+project(clproto VERSION 7.3.12)
 
 # Default to C99
 if(NOT CMAKE_C_STANDARD)

python/setup.py

@@ -11,7 +11,7 @@
 # names of the environment variables that define osqp and openrobots include directories
 osqp_path_var = 'OSQP_INCLUDE_DIR'
 
-__version__ = "7.3.11"
+__version__ = "7.3.12"
 __libraries__ = ['state_representation', 'clproto', 'controllers', 'dynamical_systems', 'robot_model']
 __include_dirs__ = ['include']
 

python/source/robot_model/bind_model.cpp

@@ -63,6 +63,7 @@ void model(py::module_& m) {
 
 
   c.def("check_collision", py::overload_cast<const JointPositions&>(&Model::check_collision), "Check if the robot is in collision at a given joint state.", "joint_positions"_a);
+  c.def("compute_minimum_collision_distances", py::overload_cast<const JointPositions&>(&Model::compute_minimum_collision_distances), "Compute the minimum distances between the robot links.", "joint_positions"_a);
   c.def("get_number_of_collision_pairs", &Model::get_number_of_collision_pairs, "Get the number of collision pairs in the model.");
   c.def("is_geometry_model_initialized", &Model::is_geometry_model_initialized, "Check if the geometry model is initialized.");
   c.def(

python/test/model/test_collisions.py

@@ -1,9 +1,11 @@
 import os
 import unittest
 
+import numpy as np
 from robot_model import Model
 from state_representation import JointPositions
 
+
 class RobotModelCollisionTesting(unittest.TestCase):
     ur5e_with_geometries = None
     ur5e_without_geometries = None
@@ -78,6 +80,28 @@ def test_collision_detected(self):
             is_colliding = self.ur5e_with_geometries.check_collision(config)
             self.assertTrue(is_colliding, "Expected collision for configuration")
 
+    def test_minimum_distance_computed_no_collision(self):
+        for config in self.test_non_colliding_configs:
+            distances = self.ur5e_with_geometries.compute_minimum_collision_distances(config)
+            self.assertEqual(distances.shape, (6, 6), "Distance matrix has incorrect shape.")
+
+            # Check that no element is equal to zero besides the diagonals
+            for i in range(distances.shape[0]):
+                for j in range(distances.shape[1]):
+                    if i != j and j != i+1 and i != j+1:  # Skip diagonal and adjacent elements
+                        self.assertGreaterEqual(distances[i, j], 0.01, "Found a distance at non-diagonal element indicating a collision.")
+
+    def test_minimum_distance_computed_collision(self):
+        for config in self.test_colliding_configs:
+            distances = self.ur5e_with_geometries.compute_minimum_collision_distances(config)
+            self.assertEqual(distances.shape, (6, 6), "Distance matrix has incorrect shape.")
+
+            # Initialize a variable to keep track of the minimum non-diagonal distance
+            minimum_distance = np.min(distances[np.triu_indices(n=6, k=2)])
+            # Expect the minimum non-diagonal distance to indicate a collision
+            self.assertLessEqual(minimum_distance, 0.01, "Did not find a minimum distance less than a threshold indicating a collision.")
+
+
 
 if __name__ == '__main__':
     unittest.main()

source/CMakeLists.txt

@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 3.15)
 
-project(control_libraries VERSION 7.3.11)
+project(control_libraries VERSION 7.3.12)
 
 # Build options
 option(BUILD_TESTING "Build all tests." OFF)

source/robot_model/include/robot_model/Model.hpp

@@ -264,6 +264,15 @@ class Model {
    */
   static bool create_urdf_from_string(const std::string& urdf_string, const std::string& desired_path);
 
+  /**
+   * @brief Compute the minimum distances between the robot links
+   * @details The distances are computed for each collision pair, resulting in a square matrix with
+   * the same size as the number of joints. The diagonal entries are always zero.
+   * @param joint_positions state_representation object containing the joint positions of the robot
+   * @return the matrix containing the minimum distance between the robot links
+   */
+  Eigen::MatrixXd compute_minimum_collision_distances(const state_representation::JointPositions& joint_positions); 
+
   /**
    * @brief Check if the links of the robot are in collision
    * @param joint_positions containing the joint positions of the robot

source/robot_model/src/Model.cpp

@@ -184,6 +184,34 @@ bool Model::check_collision(const state_representation::JointPositions& joint_po
   return false;
 }
 
+Eigen::MatrixXd Model::compute_minimum_collision_distances(const state_representation::JointPositions& joint_positions) {
+  if (!this->is_geometry_model_initialized()) {
+    throw robot_model::exceptions::CollisionGeometryException(
+        "Geometry model not loaded for " + this->get_robot_name());
+  }
+  Eigen::VectorXd configuration = joint_positions.get_positions();
+  pinocchio::computeDistances(
+      this->robot_model_, this->robot_data_, this->geom_model_, this->geom_data_, configuration);
+
+  // nb_joints is the number of joints in the robot model
+  unsigned int nb_joints = this->get_number_of_joints();
+
+  // create a square matrix to store the distances and initialize to zero
+  Eigen::MatrixXd distances = Eigen::MatrixXd::Zero(nb_joints, nb_joints);
+
+  // iterate over the collision pairs and extract the distances
+  unsigned int pair_index = 0;
+  for (unsigned int row_index = 0; row_index < nb_joints; ++row_index) {
+    for (unsigned int column_index = row_index + 1; column_index < nb_joints; ++column_index) {
+      distances(row_index, column_index) = this->geom_data_.distanceResults[pair_index].min_distance;
+      distances(column_index, row_index) = distances(row_index, column_index);
+      pair_index++;
+    }
+  }
+
+  return distances;
+}
+
 bool Model::init_qp_solver() {
   // clear the solver
   this->solver_.data()->clearHessianMatrix();

source/robot_model/test/tests/test_collisions.cpp

@@ -118,3 +118,57 @@ TEST_F(RobotModelCollisionTesting, CollisionDetected) {
     EXPECT_TRUE(is_colliding) << "Expected collision for configuration " << config;
   }
 }
+
+// Test that compute_minimum_collision_distances method identifies a collision-free state
+TEST_F(RobotModelCollisionTesting, MinimumDistanceComputedNoCollision) {
+  // iterate over test configurations and check for collision
+  set_test_non_coliding_configurations();
+
+  for (auto& config : test_non_coliding_configs) {
+    Eigen::MatrixXd distances = ur5e_with_geometries->compute_minimum_collision_distances(config);
+
+    // check that no element is equal to zero besided the diagonals
+    EXPECT_EQ(distances.rows(), 6) << "Distance matrix has incorrect number of rows.";
+    EXPECT_EQ(distances.cols(), 6) << "Distance matrix has incorrect number of columns.";
+
+    // Then check that no element is equal to zero besides the diagonals
+    for (int i = 0; i < distances.rows(); ++i) {
+      for (int j = 0; j < distances.cols(); ++j) {
+        if (i != j && j != i + 1 && i != j + 1) {// Skip diagonal elements & adjacent links
+          EXPECT_GE(distances(i, j), 0.01)
+              << "Found a distance at non-diagonal element [" << i << ", " << j << "], indicating a collision.";
+        }
+      }
+    }
+  }
+}
+
+TEST_F(RobotModelCollisionTesting, MinimumDistanceComputedCollision) {
+  // Iterate over test configurations expected to result in collisions
+  set_test_coliding_configurations();
+
+  for (auto& config : test_coliding_configs) {
+    Eigen::MatrixXd distances = ur5e_with_geometries->compute_minimum_collision_distances(config);
+
+    // Check the size of the distance matrix is 6x6
+    EXPECT_EQ(distances.rows(), 6) << "Distance matrix has incorrect number of rows.";
+    EXPECT_EQ(distances.cols(), 6) << "Distance matrix has incorrect number of columns.";
+
+    // Initialize a variable to keep track of the minimum non-diagonal distance
+    double minimum_distance = std::numeric_limits<double>::max();
+
+    // Iterate over the matrix to find the minimum non-diagonal distance
+    for (int i = 0; i < distances.rows(); ++i) {
+      for (int j = 0; j < distances.cols(); ++j) {
+        if (i != j && j != i + 1 && i != j + 1
+            && distances(i, j) < minimum_distance) {// Skip diagonal elements & adjacent links
+          minimum_distance = distances(i, j);
+        }
+      }
+    }
+
+    // Expect the minimum non-diagonal distance to be 0, indicating a collision
+    EXPECT_LE(minimum_distance, 0.01)
+        << "Did not find a minimum distance less than a threshold indicating a collision.";
+  }
+}