feat: integrate collision detection feature into robot model (#163)

* feat: add headers

* feat: implement source code

* feat: add cpp tests

* feat: add meshes

* feat: add python bindings

* feat: test collisions in python

* fix: rename ur5e folder

* fix: Dockerfile cache ID

* 7.3.4 -> 7.3.5

* Update CHANGELOG

* Update source/robot_model/include/robot_model/Model.hpp

Co-authored-by: Dominic Reber <71256590+domire8@users.noreply.github.com>

* fix: apply review changes

* fix: move import from hpp to cpp

* feat: update tests

* fix: remove un-needed imports

* fix: urdf ros control blocks

* 7.3.6 -> 7.3.7

* Update CHANGELOG

* fix: update doc strings

* Update CHANGELOG.md

Co-authored-by: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com>

* fix: optional parameter

* fix: changelog

* fix: create two Model constructors

* fix: remove ambiguity in binding

* fix: adjust doc string

* fix: indentation

* fix: nitpicks

* 7.3.6 -> 7.3.7

* fix: apply nitpicks from code review

Co-authored-by: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com>

* fix: add flag for copy constructor

* feat: add default flag

* fix: suggestions from code review

Co-authored-by: Dominic Reber <71256590+domire8@users.noreply.github.com>

* fix: space between namings

Co-authored-by: Dominic Reber <71256590+domire8@users.noreply.github.com>

* 7.3.7 -> 7.3.8

* fix: remove space

Co-authored-by: Dominic Reber <71256590+domire8@users.noreply.github.com>

* 7.3.8 -> 7.3.10

---------

Co-authored-by: github-actions[bot] <github-actions[bot]@users.noreply.github.com>
Co-authored-by: Dominic Reber <71256590+domire8@users.noreply.github.com>
Co-authored-by: Dominic Reber <dominic@aica.tech>
Co-authored-by: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com>

CHANGELOG.md

@@ -13,6 +13,7 @@ Release Versions:
 
 ## Upcoming changes (in development)
 
+- feat: integrate collision detection feature into robot model (#163)
 - feat: add IO states to state representation (py) (#173)
 - ci: use caching from docker to run tests in CI (#429)
 - feat: add IO states to state representation (proto) (#172)

VERSION

@@ -1 +1 @@
-7.3.9
+7.3.10

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.9 CONFIG REQUIRED)
+find_package(control_libraries 7.3.10 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.9
+PROJECT_NUMBER = 7.3.10
 
 # 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.9)
+project(clproto VERSION 7.3.10)
 
 # 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.9"
+__version__ = "7.3.10"
 __libraries__ = ['state_representation', 'clproto', 'controllers', 'dynamical_systems', 'robot_model']
 __include_dirs__ = ['include']
 

python/source/robot_model/bind_exceptions.cpp

@@ -3,9 +3,11 @@
 #include <robot_model/exceptions/FrameNotFoundException.hpp>
 #include <robot_model/exceptions/InvalidJointStateSizeException.hpp>
 #include <robot_model/exceptions/InverseKinematicsNotConvergingException.hpp>
+#include <robot_model/exceptions/CollisionGeometryException.hpp>
 
 void bind_exceptions(py::module_& m) {
   py::register_exception<robot_model::exceptions::FrameNotFoundException>(m, "FrameNotFoundError", PyExc_RuntimeError);
   py::register_exception<robot_model::exceptions::InvalidJointStateSizeException>(m, "InvalidJointStateSizeError", PyExc_RuntimeError);
   py::register_exception<robot_model::exceptions::InverseKinematicsNotConvergingException>(m, "InverseKinematicsNotConvergingErrors", PyExc_RuntimeError);
+  py::register_exception<robot_model::exceptions::CollisionGeometryException>(m, "CollisionGeometryError", PyExc_RuntimeError);
 }

python/source/robot_model/bind_model.cpp

@@ -30,7 +30,24 @@ void model(py::module_& m) {
 
   py::class_<Model> c(m, "Model");
 
-  c.def(py::init<const std::string&, const std::string&>(), "Constructor with robot name and path to URDF file.", "robot_name"_a, "urdf_path"_a);
+  c.def(py::init([](const std::string& robot_name, const std::string& urdf_path, py::object meshloader_callback) {
+  std::function<std::string(const std::string&)> callback_cpp = nullptr;
+  if (!meshloader_callback.is_none()) {
+        callback_cpp = [meshloader_callback](const std::string& package_name) -> std::string {
+              auto result = meshloader_callback(package_name).template cast<std::string>();
+              return result;
+        };
+  }
+  return new Model(robot_name, urdf_path, callback_cpp);
+  }), "Constructor that creates a robot model instance with a name, URDF path, and an optional custom mesh loader callback. This constructor loads the Robot Geometries.", 
+  py::arg("robot_name"), py::arg("urdf_path"), py::arg("meshloader_callback"));
+
+
+  c.def(py::init<const std::string&, const std::string&>(), "Constructor that creates a robot model instance with a name and URDF path. This constructor doesn't loads the Robot Geometries.",
+        py::arg("robot_name"),
+        py::arg("urdf_path")
+  );
+
   c.def(py::init<const Model&>(), "Copy constructor from another Model", "model"_a);
 
   c.def("get_robot_name", &Model::get_robot_name, "Getter of the robot name.");
@@ -44,6 +61,10 @@ void model(py::module_& m) {
   c.def("set_gravity_vector", &Model::set_gravity_vector, "Setter of the gravity vector.", "gravity"_a);
 //  c.def("get_pinocchio_model", &Model::get_pinocchio_model, "Getter of the pinocchio model.");
 
+
+  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("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(
       "compute_jacobian", py::overload_cast<const JointPositions&, const std::string&>(&Model::compute_jacobian),
       "Compute the Jacobian from a given joint state at the frame given in parameter.", "joint_positions"_a, "frame"_a = std::string(""));

python/test/model/test_collisions.py

@@ -0,0 +1,83 @@
+import os
+import unittest
+
+from robot_model import Model
+from state_representation import JointPositions
+
+class RobotModelCollisionTesting(unittest.TestCase):
+    ur5e_with_geometries = None
+    ur5e_without_geometries = None
+    test_non_colliding_configs = []
+    test_colliding_configs = []
+
+    @staticmethod
+    def get_package_path_from_name(name):
+        if name == "ur_description":
+            return f'{os.path.dirname(os.path.realpath(__file__))}/'
+
+
+    @classmethod
+    def setUpClass(cls):
+        test_fixtures_path = os.path.join(os.path.dirname(os.path.realpath(__file__)))
+        cls.ur5e_with_geometries = Model("ur5e", os.path.join(test_fixtures_path, "ur5e.urdf"), meshloader_callback=cls.get_package_path_from_name)
+        cls.ur5e_without_geometries = Model("ur5e", os.path.join(test_fixtures_path, "ur5e.urdf"))
+        cls.set_test_non_colliding_configurations()
+        cls.set_test_colliding_configurations()
+
+    @classmethod
+    def set_test_non_colliding_configurations(cls):
+        config1 = JointPositions(cls.ur5e_with_geometries.get_robot_name(), 6)
+        config1.set_positions([0.0, -1.63, 1.45, 0.38, 0.0, 0.0])
+        cls.test_non_colliding_configs.append(config1)
+
+        config2 = JointPositions(cls.ur5e_with_geometries.get_robot_name(), 6)
+        config2.set_positions([0.0, -1.88, 1.45, 0.38, -4.4, -3.14])
+        cls.test_non_colliding_configs.append(config2)
+
+        config3 = JointPositions(cls.ur5e_with_geometries.get_robot_name(), 6)
+        config3.set_positions([1.26, -1.26, 0.82, 0.38, -4.4, 3.14])
+        cls.test_non_colliding_configs.append(config3)
+
+    @classmethod
+    def set_test_colliding_configurations(cls):
+        config1 = JointPositions(cls.ur5e_with_geometries.get_robot_name(), 6)
+        config1.set_positions([1.26, -1.76, 2.89, 0.38, -4.4, -6.16])
+        cls.test_colliding_configs.append(config1)
+
+        config2 = JointPositions(cls.ur5e_with_geometries.get_robot_name(), 6)
+        config2.set_positions([1.26, -1.76, 2.89, 0.38, -1.38, -1.16])
+        cls.test_colliding_configs.append(config2)
+
+        config3 = JointPositions(cls.ur5e_with_geometries.get_robot_name(), 6)
+        config3.set_positions([1.26, -1.76, -3.08, 0.75, -1.38, -6.16])
+        cls.test_colliding_configs.append(config3)
+
+    def test_number_of_collision_pairs_with_geometries(self):
+        num_pairs = self.ur5e_with_geometries.get_number_of_collision_pairs()
+        self.assertEqual(num_pairs, 15, "Expected 15 collision pairs for ur5e with geometries.")
+
+    def test_number_of_collision_pairs_without_geometries(self):
+        num_pairs = self.ur5e_without_geometries.get_number_of_collision_pairs()
+        self.assertEqual(num_pairs, 0, "Expected zero collision pairs for model without geometries.")
+
+    def test_geom_model_initialized_with_geometries(self):
+        is_initialized = self.ur5e_with_geometries.is_geometry_model_initialized()
+        self.assertTrue(is_initialized, "Expected geometry model to be initialized for model with geometries.")
+
+    def test_geom_model_initialized_without_geometries(self):
+        is_initialized = self.ur5e_without_geometries.is_geometry_model_initialized()
+        self.assertFalse(is_initialized, "Expected geometry model to not be initialized for model without geometries.")
+
+    def test_no_collision_detected(self):
+        for config in self.test_non_colliding_configs:
+            is_colliding = self.ur5e_with_geometries.check_collision(config)
+            self.assertFalse(is_colliding, "Expected no collision for configuration")
+
+    def test_collision_detected(self):
+        for config in self.test_colliding_configs:
+            is_colliding = self.ur5e_with_geometries.check_collision(config)
+            self.assertTrue(is_colliding, "Expected collision for configuration")
+
+
+if __name__ == '__main__':
+    unittest.main()