feat: integrate collision detection feature into robot model

CHANGELOG.md

@@ -13,6 +13,7 @@ Release Versions:
 
 ## Upcoming changes (in development)
 
+- feat: integrating collision detection feature into robot model (#163)
 - feat(build): handle installation and linking of dependencies for pinocchio collision support (#161)
 - build: update dockerfiles (#153)
 - build: copy python packages into /usr instead of ~ros2 to avoid permission issues (#155)

VERSION

@@ -1 +1 @@
-7.3.4
+7.3.5

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

python/source/robot_model/bind_model.cpp

@@ -30,7 +30,14 @@ 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, const std::vector<std::string>& geometry_package_paths) {
+       return new Model(robot_name, urdf_path, geometry_package_paths);
+    }), "Constructor with robot name, path to URDF file, and optionally geometry package paths.",
+       py::arg("robot_name"), 
+       py::arg("urdf_path"), 
+       py::arg("geometry_package_paths") = std::vector<std::string>()
+  );
+
   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 +51,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,77 @@
+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 = []
+
+    @classmethod
+    def setUpClass(cls):
+        package_paths = [os.path.join(os.path.dirname(os.path.realpath(__file__)), "ur5e")]
+        cls.ur5e_with_geometries = Model("ur5e", os.path.join(package_paths[0], "ur5e.urdf"), package_paths)
+        cls.ur5e_without_geometries = Model("ur5e", os.path.join(package_paths[0], "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()