diff --git a/CHANGELOG.md b/CHANGELOG.md index 5ea46a918..8c4ce120f 100644 --- a/CHANGELOG.md +++ b/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) diff --git a/VERSION b/VERSION index 7dc1035aa..6fddc82bb 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -7.3.9 +7.3.10 diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt index a87fd1bcb..8489fc1db 100644 --- a/demos/CMakeLists.txt +++ b/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 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index eb02462a5..356825da1 100644 --- a/doxygen/doxygen.conf +++ b/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 diff --git a/protocol/clproto_cpp/CMakeLists.txt b/protocol/clproto_cpp/CMakeLists.txt index 16ef7c472..e386a6962 100644 --- a/protocol/clproto_cpp/CMakeLists.txt +++ b/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) diff --git a/python/setup.py b/python/setup.py index 2f38acf7f..8cafe31f3 100644 --- a/python/setup.py +++ b/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'] diff --git a/python/source/robot_model/bind_exceptions.cpp b/python/source/robot_model/bind_exceptions.cpp index 114e34dcd..4d9a01d4b 100644 --- a/python/source/robot_model/bind_exceptions.cpp +++ b/python/source/robot_model/bind_exceptions.cpp @@ -3,9 +3,11 @@ #include #include #include +#include void bind_exceptions(py::module_& m) { py::register_exception(m, "FrameNotFoundError", PyExc_RuntimeError); py::register_exception(m, "InvalidJointStateSizeError", PyExc_RuntimeError); py::register_exception(m, "InverseKinematicsNotConvergingErrors", PyExc_RuntimeError); + py::register_exception(m, "CollisionGeometryError", PyExc_RuntimeError); } diff --git a/python/source/robot_model/bind_model.cpp b/python/source/robot_model/bind_model.cpp index 74e379517..eaab80a3b 100644 --- a/python/source/robot_model/bind_model.cpp +++ b/python/source/robot_model/bind_model.cpp @@ -30,7 +30,24 @@ void model(py::module_& m) { py::class_ c(m, "Model"); - c.def(py::init(), "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 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(); + 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(), "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(), "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(&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(&Model::compute_jacobian), "Compute the Jacobian from a given joint state at the frame given in parameter.", "joint_positions"_a, "frame"_a = std::string("")); diff --git a/python/test/model/meshes/ur5e/collision/base.stl b/python/test/model/meshes/ur5e/collision/base.stl new file mode 100644 index 000000000..c58810466 Binary files /dev/null and b/python/test/model/meshes/ur5e/collision/base.stl differ diff --git a/python/test/model/meshes/ur5e/collision/forearm.stl b/python/test/model/meshes/ur5e/collision/forearm.stl new file mode 100644 index 000000000..3a02f8b0e Binary files /dev/null and b/python/test/model/meshes/ur5e/collision/forearm.stl differ diff --git a/python/test/model/meshes/ur5e/collision/shoulder.stl b/python/test/model/meshes/ur5e/collision/shoulder.stl new file mode 100644 index 000000000..ba79079f6 Binary files /dev/null and b/python/test/model/meshes/ur5e/collision/shoulder.stl differ diff --git a/python/test/model/meshes/ur5e/collision/upperarm.stl b/python/test/model/meshes/ur5e/collision/upperarm.stl new file mode 100644 index 000000000..02f6edfdd Binary files /dev/null and b/python/test/model/meshes/ur5e/collision/upperarm.stl differ diff --git a/python/test/model/meshes/ur5e/collision/wrist1.stl b/python/test/model/meshes/ur5e/collision/wrist1.stl new file mode 100644 index 000000000..694700f7d Binary files /dev/null and b/python/test/model/meshes/ur5e/collision/wrist1.stl differ diff --git a/python/test/model/meshes/ur5e/collision/wrist2.stl b/python/test/model/meshes/ur5e/collision/wrist2.stl new file mode 100644 index 000000000..667d60e30 Binary files /dev/null and b/python/test/model/meshes/ur5e/collision/wrist2.stl differ diff --git a/python/test/model/meshes/ur5e/collision/wrist3.stl b/python/test/model/meshes/ur5e/collision/wrist3.stl new file mode 100644 index 000000000..d6bdb411e Binary files /dev/null and b/python/test/model/meshes/ur5e/collision/wrist3.stl differ diff --git a/python/test/model/test_collisions.py b/python/test/model/test_collisions.py new file mode 100644 index 000000000..132ec5541 --- /dev/null +++ b/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() diff --git a/python/test/model/ur5e.urdf b/python/test/model/ur5e.urdf new file mode 100644 index 000000000..35377d74f --- /dev/null +++ b/python/test/model/ur5e.urdf @@ -0,0 +1,296 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/source/CMakeLists.txt b/source/CMakeLists.txt index 4385b4287..ec3acb614 100644 --- a/source/CMakeLists.txt +++ b/source/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.15) -project(control_libraries VERSION 7.3.9) +project(control_libraries VERSION 7.3.10) # Build options option(BUILD_TESTING "Build all tests." OFF) diff --git a/source/robot_model/include/robot_model/Model.hpp b/source/robot_model/include/robot_model/Model.hpp index 098c621f4..af3276e2f 100644 --- a/source/robot_model/include/robot_model/Model.hpp +++ b/source/robot_model/include/robot_model/Model.hpp @@ -2,11 +2,13 @@ #include #include +#include #include #include #include #include #include +#include #include #include #include @@ -67,18 +69,28 @@ class Model { std::vector frames_; ///< name of the frames pinocchio::Model robot_model_; ///< the robot model with pinocchio pinocchio::Data robot_data_; ///< the robot data with pinocchio + std::optional> meshloader_callback_; ///< callback function to resolve package paths + pinocchio::GeometryModel geom_model_; ///< the robot geometry model with pinocchio + pinocchio::GeometryData geom_data_; ///< the robot geometry data with pinocchio OsqpEigen::Solver solver_; ///< osqp solver for the quadratic programming based inverse kinematics Eigen::SparseMatrix hessian_; ///< hessian matrix for the quadratic programming based inverse kinematics Eigen::VectorXd gradient_; ///< gradient vector for the quadratic programming based inverse kinematics Eigen::SparseMatrix constraint_matrix_; ///< constraint matrix for the quadratic programming based inverse kinematics Eigen::VectorXd lower_bound_constraints_; ///< lower bound matrix for the quadratic programming based inverse kinematics Eigen::VectorXd upper_bound_constraints_; ///< upper bound matrix for the quadratic programming based inverse kinematics + bool load_collision_geometries_ = false; ///< flag to load collision geometries + // @format:on /** * @brief Initialize the pinocchio model from the URDF */ void init_model(); + /** + * @brief Initialize the pinocchio geometry model from the URDF and the package paths + */ + void init_geom_model(std::string urdf); + /** * @brief initialize the constraints for the QP solver */ @@ -98,6 +110,13 @@ class Model { */ unsigned int get_frame_id(const std::string& frame); + /** + * @brief Find all the package paths in the URDF and replaces them with the absolute path using meshloader_callback_ + * @param urdf string containing the URDF description of the robot + * @return vector of the package paths + */ + std::vector resolve_package_paths_in_urdf(std::string& urdf) const; + /** * @brief Compute the Jacobian from given joint positions at the frame in parameter * @param joint_positions containing the joint positions of the robot @@ -185,12 +204,37 @@ class Model { const state_representation::JointPositions& joint_positions, const std::vector& frames); + /** + * @brief Generates a list of collision pairs to exclude based on the kinematic tree of the model + * @return the list of collision pairs to exclude + */ + std::vector generate_joint_exclusion_list(); + public: /** - * @brief Constructor with robot name and path to URDF file + * @brief Construct with robot name and path to URDF file + * @details If the URDF contains references to collision geometry meshes, they will not be loaded into memory. + * To enable collision detection, use the alternate constructor. + * @param robot_name the name to associate with the model + * @param urdf_path the path to the URDF file */ explicit Model(const std::string& robot_name, const std::string& urdf_path); + /** + * @brief Construct a robot model with collision geometries from a URDF file + * @details If the URDF contains references to collision geometry meshes, they will be loaded into memory. + * Subsequently, the check_collision() method can be used to check for self-collisions in the robot model. + * If geometry meshes are referenced with a relative package path using the `package://` prefix, then + * the optional meshloader_callback function should be defined to return an absolute path to a package + * given the package name. + * @param robot_name the name to associate with the model + * @param urdf_path the path to the URDF file + * @param meshloader_callback optional callback to resolve the absolute package path from a package name + */ + explicit Model(const std::string& robot_name, + const std::string& urdf_path, + const std::optional>& meshloader_callback); + /** * @brief Copy constructor * @param model the model to copy @@ -220,6 +264,26 @@ class Model { */ static bool create_urdf_from_string(const std::string& urdf_string, const std::string& desired_path); + /** + * @brief Check if the links of the robot are in collision + * @param joint_positions containing the joint positions of the robot + * @throws robot_model::exceptions::CollisionGeometryException if collision geometry is not initialized + * @return true if the robot is in collision, false otherwise + */ + bool check_collision(const state_representation::JointPositions& joint_positions); + + /** + * @brief Getter of the number of collision pairs in the model + * @return the number of collision pairs + */ + unsigned int get_number_of_collision_pairs(); + + /** + * @brief Check if geometry model is initialized + * @return true if the geometry model is initialized, false otherwise + */ + bool is_geometry_model_initialized(); + /** * @brief Getter of the robot name * @return the robot name @@ -517,7 +581,6 @@ inline Model& Model::operator=(const Model& model) { return *this; } - inline const std::string& Model::get_robot_name() const { return this->robot_name_->get_value(); } diff --git a/source/robot_model/include/robot_model/exceptions/CollisionGeometryException.hpp b/source/robot_model/include/robot_model/exceptions/CollisionGeometryException.hpp new file mode 100644 index 000000000..a52122ab9 --- /dev/null +++ b/source/robot_model/include/robot_model/exceptions/CollisionGeometryException.hpp @@ -0,0 +1,11 @@ +#include +#include + +namespace robot_model::exceptions { +class CollisionGeometryException : public std::runtime_error { + +public: + explicit CollisionGeometryException(const std::string& error_message) + : runtime_error("Collision geometry error: " + error_message) {} +}; +} // namespace robot_model::exceptions diff --git a/source/robot_model/src/Model.cpp b/source/robot_model/src/Model.cpp index 1cd4b34ed..0f6f798bd 100644 --- a/source/robot_model/src/Model.cpp +++ b/source/robot_model/src/Model.cpp @@ -1,21 +1,39 @@ -#include +#include +#include #include #include #include "robot_model/Model.hpp" #include "robot_model/exceptions/FrameNotFoundException.hpp" #include "robot_model/exceptions/InverseKinematicsNotConvergingException.hpp" #include "robot_model/exceptions/InvalidJointStateSizeException.hpp" +#include "robot_model/exceptions/CollisionGeometryException.hpp" namespace robot_model { +Model::Model(const std::string& robot_name, + const std::string& urdf_path, + const std::optional>& meshloader_callback + ): + robot_name_(std::make_shared>("robot_name", robot_name)), + urdf_path_(std::make_shared>("urdf_path", urdf_path)), + meshloader_callback_(meshloader_callback), + load_collision_geometries_(true) + { + this->init_model(); +} + Model::Model(const std::string& robot_name, const std::string& urdf_path) : robot_name_(std::make_shared>("robot_name", robot_name)), - urdf_path_(std::make_shared>("urdf_path", urdf_path)) { + urdf_path_(std::make_shared>("urdf_path", urdf_path)) + { this->init_model(); } Model::Model(const Model& model) : robot_name_(model.robot_name_), - urdf_path_(model.urdf_path_) { + urdf_path_(model.urdf_path_), + meshloader_callback_(model.meshloader_callback_), + load_collision_geometries_(model.load_collision_geometries_) + { this->init_model(); } @@ -29,9 +47,60 @@ bool Model::create_urdf_from_string(const std::string& urdf_string, const std::s return false; } +std::vector Model::resolve_package_paths_in_urdf(std::string& urdf) const { + std::set package_names; + std::regex package_path_pattern(R"(filename=\"([^\"]+)\")"); + std::regex package_name_pattern(R"(package://([^/]+)/)"); + + std::smatch matches_file; + std::smatch matches_package_name; + auto start = urdf.cbegin(); + auto end = urdf.cend(); + + // Extract package paths + while (std::regex_search(start, end, matches_file, package_path_pattern)) { + std::string path = matches_file[1]; + if (std::regex_search(path, matches_package_name, package_name_pattern)) { + package_names.insert(matches_package_name[1]); + } + start = matches_file[0].second; + } + + std::vector package_paths; + for (const auto& package_name : package_names) { + if (meshloader_callback_) { + auto package_path = (*this->meshloader_callback_)(package_name); + auto target = "package://" + package_name + "/"; + auto replacement = package_path; + size_t start_position = 0; + while ((start_position = urdf.find(target, start_position)) != std::string::npos) { + // Replace the target with the replacement string + urdf.replace(start_position, target.length(), replacement); + // Move past the last replacement + start_position += replacement.length(); + } + package_paths.push_back(package_path); + } + } + return package_paths; +} + void Model::init_model() { - pinocchio::urdf::buildModel(this->get_urdf_path(), this->robot_model_); + std::ifstream file_stream(this->get_urdf_path()); + if (!file_stream.is_open()) { + throw std::runtime_error("Unable to open file: " + this->get_urdf_path()); + } + std::stringstream buffer; + buffer << file_stream.rdbuf(); + auto urdf = buffer.str(); + + pinocchio::urdf::buildModelFromXML(urdf, this->robot_model_); this->robot_data_ = pinocchio::Data(this->robot_model_); + + if (this->load_collision_geometries_) { + this->init_geom_model(urdf); + } + // get the frames std::vector frames; for (auto& f : this->robot_model_.frames) { @@ -42,6 +111,79 @@ void Model::init_model() { this->init_qp_solver(); } +void Model::init_geom_model(std::string urdf) { + try { + auto package_paths = this->resolve_package_paths_in_urdf(urdf); + pinocchio::urdf::buildGeom( + this->robot_model_, std::istringstream(urdf), pinocchio::COLLISION, this->geom_model_, package_paths); + this->geom_model_.addAllCollisionPairs(); + + std::vector excluded_pairs = this->generate_joint_exclusion_list(); + + // remove collision pairs for linked joints (i.e. parent-child joints) + for (const auto& pair : excluded_pairs) { + this->geom_model_.removeCollisionPair(pair); + } + + this->geom_data_ = pinocchio::GeometryData(this->geom_model_); + } catch (const std::exception& ex) { + throw robot_model::exceptions::CollisionGeometryException( + "Failed to initialize geometry model for " + this->get_robot_name() + ": " + ex.what()); + } +} + +std::vector Model::generate_joint_exclusion_list() { + std::vector excluded_pairs; + // Iterate through all joints, except the universe joint (0), which has no parent + for (pinocchio::JointIndex joint_id = 1u; joint_id < static_cast(this->robot_model_.njoints); + ++joint_id) { + // Find the parent joint of the current joint + pinocchio::JointIndex parent_id = this->robot_model_.parents[joint_id]; + + // TODO: Replace this logic with actual geometry index lookup + auto getGeometryIndexForJoint = [](pinocchio::JointIndex joint_id) -> int { + return static_cast(joint_id); + }; + + int geometryIndex1 = getGeometryIndexForJoint(joint_id); + int geometryIndex2 = getGeometryIndexForJoint(parent_id); + + // Check if the geometry indices are not equal + if (geometryIndex1 != geometryIndex2) { + excluded_pairs.push_back(pinocchio::CollisionPair(geometryIndex2, geometryIndex1)); + } + } + return excluded_pairs; +} + +unsigned int Model::get_number_of_collision_pairs() { + return this->geom_model_.collisionPairs.size(); +} + +bool Model::is_geometry_model_initialized() { + return !this->geom_model_.collisionPairs.empty(); +} + +bool Model::check_collision(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::computeCollisions( + this->robot_model_, this->robot_data_, this->geom_model_, this->geom_data_, configuration, true); + + for (size_t pair_index = 0; pair_index < this->geom_model_.collisionPairs.size(); ++pair_index) { + const auto& collision_result = this->geom_data_.collisionResults[pair_index]; + if (collision_result.isCollision()) { + return true; + } + } + return false; +} + bool Model::init_qp_solver() { // clear the solver this->solver_.data()->clearHessianMatrix(); @@ -111,7 +253,7 @@ std::vector Model::get_frame_ids(const std::vector& f } else { // throw error if specified frame does not exist if (!this->robot_model_.existFrame(frame)) { - throw (exceptions::FrameNotFoundException(frame)); + throw exceptions::FrameNotFoundException(frame); } frame_ids.push_back(this->robot_model_.getFrameId(frame)); } @@ -126,7 +268,7 @@ unsigned int Model::get_frame_id(const std::string& frame) { state_representation::Jacobian Model::compute_jacobian(const state_representation::JointPositions& joint_positions, unsigned int frame_id) { if (joint_positions.get_size() != this->get_number_of_joints()) { - throw (exceptions::InvalidJointStateSizeException(joint_positions.get_size(), this->get_number_of_joints())); + throw exceptions::InvalidJointStateSizeException(joint_positions.get_size(), this->get_number_of_joints()); } // compute the Jacobian from the joint state pinocchio::Data::Matrix6x J(6, this->get_number_of_joints()); @@ -155,10 +297,10 @@ Eigen::MatrixXd Model::compute_jacobian_time_derivative(const state_representati const state_representation::JointVelocities& joint_velocities, unsigned int frame_id) { if (joint_positions.get_size() != this->get_number_of_joints()) { - throw (exceptions::InvalidJointStateSizeException(joint_positions.get_size(), this->get_number_of_joints())); + throw exceptions::InvalidJointStateSizeException(joint_positions.get_size(), this->get_number_of_joints()); } if (joint_velocities.get_size() != this->get_number_of_joints()) { - throw (exceptions::InvalidJointStateSizeException(joint_velocities.get_size(), this->get_number_of_joints())); + throw exceptions::InvalidJointStateSizeException(joint_velocities.get_size(), this->get_number_of_joints()); } // compute the Jacobian from the joint state pinocchio::Data::Matrix6x dJ = Eigen::MatrixXd::Zero(6, this->get_number_of_joints()); @@ -228,13 +370,13 @@ state_representation::CartesianPose Model::forward_kinematics(const state_repres std::vector Model::forward_kinematics(const state_representation::JointPositions& joint_positions, const std::vector& frame_ids) { if (joint_positions.get_size() != this->get_number_of_joints()) { - throw (exceptions::InvalidJointStateSizeException(joint_positions.get_size(), this->get_number_of_joints())); + throw exceptions::InvalidJointStateSizeException(joint_positions.get_size(), this->get_number_of_joints()); } std::vector pose_vector; pinocchio::forwardKinematics(this->robot_model_, this->robot_data_, joint_positions.data()); for (unsigned int id : frame_ids) { if (id >= static_cast(this->robot_model_.nframes)) { - throw (exceptions::FrameNotFoundException(std::to_string(id))); + throw exceptions::FrameNotFoundException(std::to_string(id)); } pinocchio::updateFramePlacement(this->robot_model_, this->robot_data_, id); pinocchio::SE3 pose = this->robot_data_.oMf[id]; @@ -311,7 +453,7 @@ Model::inverse_kinematics(const state_representation::CartesianPose& cartesian_p const std::string& frame) { std::string actual_frame = frame.empty() ? this->robot_model_.frames.back().name : frame; if (!this->robot_model_.existFrame(actual_frame)) { - throw (exceptions::FrameNotFoundException(actual_frame)); + throw exceptions::FrameNotFoundException(actual_frame); } // 1 second for the Newton-Raphson method const std::chrono::nanoseconds dt(static_cast(1e9)); @@ -378,14 +520,14 @@ void Model::check_inverse_velocity_arguments(const std::vector& frames) { if (cartesian_twists.size() != frames.size()) { - throw (std::invalid_argument("The number of provided twists and frames does not match")); + throw std::invalid_argument("The number of provided twists and frames does not match"); } if (joint_positions.get_size() != this->get_number_of_joints()) { - throw (exceptions::InvalidJointStateSizeException(joint_positions.get_size(), this->get_number_of_joints())); + throw exceptions::InvalidJointStateSizeException(joint_positions.get_size(), this->get_number_of_joints()); } for (auto& frame : frames) { if (!this->robot_model_.existFrame(frame)) { - throw (exceptions::FrameNotFoundException(frame)); + throw exceptions::FrameNotFoundException(frame); } } } @@ -412,25 +554,25 @@ Model::inverse_velocity(const std::vector& dX.tail(6) = cartesian_twists.back().data(); jacobian.bottomRows(6) = this->compute_jacobian(joint_positions, frames.back()).data(); - if (dls_lambda == 0.0){ + if (dls_lambda == 0.0) { return state_representation::JointVelocities(joint_positions.get_name(), - joint_positions.get_names(), - jacobian.colPivHouseholderQr().solve(dX)); + joint_positions.get_names(), + jacobian.colPivHouseholderQr().solve(dX)); } // add damped least square term - if (jacobian.rows() > jacobian.cols()){ + if (jacobian.rows() > jacobian.cols()) { Eigen::MatrixXd j_prime = jacobian.transpose() * jacobian + dls_lambda * dls_lambda * Eigen::MatrixXd::Identity(jacobian.cols(), jacobian.cols()); return state_representation::JointVelocities(joint_positions.get_name(), - joint_positions.get_names(), - j_prime.colPivHouseholderQr().solve(jacobian.transpose() * dX)); + joint_positions.get_names(), + j_prime.colPivHouseholderQr().solve(jacobian.transpose() * dX)); } else { Eigen::MatrixXd j_prime = jacobian * jacobian.transpose() + dls_lambda * dls_lambda * Eigen::MatrixXd::Identity(jacobian.rows(), jacobian.rows()); return state_representation::JointVelocities(joint_positions.get_name(), - joint_positions.get_names(), - jacobian.transpose() * j_prime.colPivHouseholderQr().solve(dX)); + joint_positions.get_names(), + jacobian.transpose() * j_prime.colPivHouseholderQr().solve(dX)); } } diff --git a/source/robot_model/test/fixtures/meshes/ur5e/collision/base.stl b/source/robot_model/test/fixtures/meshes/ur5e/collision/base.stl new file mode 100644 index 000000000..c58810466 Binary files /dev/null and b/source/robot_model/test/fixtures/meshes/ur5e/collision/base.stl differ diff --git a/source/robot_model/test/fixtures/meshes/ur5e/collision/forearm.stl b/source/robot_model/test/fixtures/meshes/ur5e/collision/forearm.stl new file mode 100644 index 000000000..3a02f8b0e Binary files /dev/null and b/source/robot_model/test/fixtures/meshes/ur5e/collision/forearm.stl differ diff --git a/source/robot_model/test/fixtures/meshes/ur5e/collision/shoulder.stl b/source/robot_model/test/fixtures/meshes/ur5e/collision/shoulder.stl new file mode 100644 index 000000000..ba79079f6 Binary files /dev/null and b/source/robot_model/test/fixtures/meshes/ur5e/collision/shoulder.stl differ diff --git a/source/robot_model/test/fixtures/meshes/ur5e/collision/upperarm.stl b/source/robot_model/test/fixtures/meshes/ur5e/collision/upperarm.stl new file mode 100644 index 000000000..02f6edfdd Binary files /dev/null and b/source/robot_model/test/fixtures/meshes/ur5e/collision/upperarm.stl differ diff --git a/source/robot_model/test/fixtures/meshes/ur5e/collision/wrist1.stl b/source/robot_model/test/fixtures/meshes/ur5e/collision/wrist1.stl new file mode 100644 index 000000000..694700f7d Binary files /dev/null and b/source/robot_model/test/fixtures/meshes/ur5e/collision/wrist1.stl differ diff --git a/source/robot_model/test/fixtures/meshes/ur5e/collision/wrist2.stl b/source/robot_model/test/fixtures/meshes/ur5e/collision/wrist2.stl new file mode 100644 index 000000000..667d60e30 Binary files /dev/null and b/source/robot_model/test/fixtures/meshes/ur5e/collision/wrist2.stl differ diff --git a/source/robot_model/test/fixtures/meshes/ur5e/collision/wrist3.stl b/source/robot_model/test/fixtures/meshes/ur5e/collision/wrist3.stl new file mode 100644 index 000000000..d6bdb411e Binary files /dev/null and b/source/robot_model/test/fixtures/meshes/ur5e/collision/wrist3.stl differ diff --git a/source/robot_model/test/fixtures/ur5e.urdf b/source/robot_model/test/fixtures/ur5e.urdf index d93eac46d..35377d74f 100644 --- a/source/robot_model/test/fixtures/ur5e.urdf +++ b/source/robot_model/test/fixtures/ur5e.urdf @@ -1,9 +1,9 @@ - + - + - - - robot_interface/universal_robots/UniversalRobotsInterface - 172.16.0.2 - 1.0 - - - - -6.28318530718 - 6.28318530718 - - - -3.15 - 3.15 - - - - - 0.0 - - - - - -6.28318530718 - 6.28318530718 - - - -3.15 - 3.15 - - - - - -1.57 - - - - - -3.14159265359 - 3.14159265359 - - - -3.15 - 3.15 - - - - - 0.0 - - - - - -6.28318530718 - 6.28318530718 - - - -3.2 - 3.2 - - - - - -1.57 - - - - - -6.28318530718 - 6.28318530718 - - - -3.2 - 3.2 - - - - - 0.0 - - - - - -6.28318530718 - 6.28318530718 - - - -3.2 - 3.2 - - - - - 0.0 - - - - - - - - - - - - - - + + - - - @@ -196,9 +87,6 @@ - - - @@ -218,9 +106,6 @@ - - - @@ -240,9 +125,6 @@ - - - @@ -262,9 +144,6 @@ - - - @@ -284,9 +163,6 @@ - - - @@ -306,9 +182,6 @@ - - - @@ -322,9 +195,15 @@ + + + + + + - + @@ -390,7 +275,7 @@ the UR controller. --> - + @@ -408,4 +293,4 @@ - + \ No newline at end of file diff --git a/source/robot_model/test/tests/test_collisions.cpp b/source/robot_model/test/tests/test_collisions.cpp new file mode 100644 index 000000000..a309269c3 --- /dev/null +++ b/source/robot_model/test/tests/test_collisions.cpp @@ -0,0 +1,120 @@ +#include "robot_model/Model.hpp" +#include "robot_model/exceptions/CollisionGeometryException.hpp" + +#include +#include +#include + +using namespace robot_model; + +class RobotModelCollisionTesting : public testing::Test { +protected: + void SetUp() override { + // create a callback function to get that takes as input a package name and returns the path to the package + auto package_paths = [](const std::string& package_name) -> std::string { + if (package_name == "ur_description") { + return std::string(TEST_FIXTURES); + } else { + return ""; + } + }; + + ur5e_without_geometries = std::make_unique("ur5e", std::string(TEST_FIXTURES) + "ur5e.urdf"); + ur5e_with_geometries = std::make_unique("ur5e", std::string(TEST_FIXTURES) + "ur5e.urdf", package_paths); + }; + + std::unique_ptr ur5e_with_geometries; + std::unique_ptr ur5e_without_geometries; + std::vector test_non_coliding_configs; + std::vector test_coliding_configs; + + // create custom test fixture for robot model without collision + void set_test_non_coliding_configurations() { + // Random test configuration 1: + state_representation::JointPositions config1(ur5e_with_geometries->get_robot_name(), 6); + config1.set_positions(std::vector{0.0, -1.63, 1.45, 0.38, 0.0, 0.0}); + test_non_coliding_configs.push_back(config1); + + // Random test configuration 2: + state_representation::JointPositions config2(ur5e_with_geometries->get_robot_name(), 6); + config2.set_positions(std::vector{0.0, -1.88, 1.45, 0.38, -4.4, -3.14}); + test_non_coliding_configs.push_back(config2); + + // Random test configuration 3: + state_representation::JointPositions config3(ur5e_with_geometries->get_robot_name(), 6); + config3.set_positions(std::vector{1.26, -1.26, 0.82, 0.38, -4.4, 3.14}); + test_non_coliding_configs.push_back(config3); + }; + + // create custom test fixture for robot model with collision geometries + void set_test_coliding_configurations() { + // Random test configuration 1: + state_representation::JointPositions config1(ur5e_with_geometries->get_robot_name(), 6); + config1.set_positions(std::vector{1.26, -1.76, 2.89, 0.38, -4.4, -6.16}); + test_coliding_configs.push_back(config1); + + // Random test configuration 2: + state_representation::JointPositions config2(ur5e_with_geometries->get_robot_name(), 6); + config2.set_positions(std::vector{1.26, -1.76, 2.89, 0.38, -1.38, -1.16}); + test_coliding_configs.push_back(config2); + + // Random test configuration 3: + state_representation::JointPositions config3(ur5e_with_geometries->get_robot_name(), 6); + config3.set_positions(std::vector{1.26, -1.76, -3.08, 0.75, -1.38, -6.16}); + test_coliding_configs.push_back(config3); + }; +}; + +// Test calling check_collision() with an uninitialized geometry model +TEST_F(RobotModelCollisionTesting, CheckCollisionWithoutGeometries) { + // Random test configuration: + state_representation::JointPositions config(ur5e_without_geometries->get_robot_name(), 6); + config.set_positions(std::vector{0.0, -1.63, 1.45, 0.38, 0.0, 0.0}); + EXPECT_THROW(ur5e_without_geometries->check_collision(config), exceptions::CollisionGeometryException) + << "Expected exception for model without geometries."; +} + +// Test that get_number_of_collision_pairs() returns 0 for a model without collision geometries loaded +TEST_F(RobotModelCollisionTesting, NumberOfCollisionPairsWithoutGeometries) { + unsigned int num_pairs = ur5e_without_geometries->get_number_of_collision_pairs(); + EXPECT_EQ(num_pairs, 0) << "Expected zero collision pairs for model without geometries."; +} + +// Test that get_number_of_collision_pairs() returns a non-zero value for a model with collision geometries loaded +TEST_F(RobotModelCollisionTesting, NumberOfCollisionPairsWithGeometries) { + // Assuming your model initialization actually loads collision geometries if available + unsigned num_pairs = ur5e_with_geometries->get_number_of_collision_pairs(); + EXPECT_EQ(num_pairs, 15) << "Expected 15 collision pairs for ur5e with geometries."; +} + +// Test that is_geometry_model_initialized() returns true for a model with collision geometries loaded +TEST_F(RobotModelCollisionTesting, GeomModelInitializedWithGeometries) { + bool is_initialized = ur5e_with_geometries->is_geometry_model_initialized(); + EXPECT_TRUE(is_initialized) << "Expected geometry model to be initialized for model with geometries."; +} + +// Test that is_geometry_model_initialized() returns false for a model without collision geometries loaded +TEST_F(RobotModelCollisionTesting, GeomModelInitializedWithoutGeometries) { + bool is_initialized = ur5e_without_geometries->is_geometry_model_initialized(); + EXPECT_FALSE(is_initialized) << "Expected geometry model to not be initialized for model without geometries."; +} + +// Test that check_collision correctly identifies a collision-free state +TEST_F(RobotModelCollisionTesting, NoCollisionDetected) { + // iterate over test configurations and check for collision + set_test_non_coliding_configurations(); + for (auto& config : test_non_coliding_configs) { + bool is_colliding = ur5e_with_geometries->check_collision(config); + EXPECT_FALSE(is_colliding) << "Expected no collision for configuration " << config; + } +} + +// Test that check_collision correctly identifies a colliding state +TEST_F(RobotModelCollisionTesting, CollisionDetected) { + // iterate over test configurations and check for collision + set_test_coliding_configurations(); + for (auto& config : test_coliding_configs) { + bool is_colliding = ur5e_with_geometries->check_collision(config); + EXPECT_TRUE(is_colliding) << "Expected collision for configuration " << config; + } +}