From 47ff4f4765af005f30773ca9845f2246061c16c7 Mon Sep 17 00:00:00 2001 From: Dominic Reber Date: Tue, 9 Apr 2024 12:13:34 +0200 Subject: [PATCH] wip --- python/source/robot_model/bind_model.cpp | 4 +- source/robot_model/CMakeLists.txt | 4 +- .../robot_model/include/robot_model/Model.hpp | 34 ++-- source/robot_model/src/Model.cpp | 134 +++++++------- .../test/tests/test_collisions.cpp | 174 ------------------ 5 files changed, 88 insertions(+), 262 deletions(-) delete mode 100644 source/robot_model/test/tests/test_collisions.cpp diff --git a/python/source/robot_model/bind_model.cpp b/python/source/robot_model/bind_model.cpp index 26f866cd0..0d582a5ab 100644 --- a/python/source/robot_model/bind_model.cpp +++ b/python/source/robot_model/bind_model.cpp @@ -62,8 +62,8 @@ void model(py::module_& m) { // 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("compute_minimum_collision_distances", py::overload_cast(&Model::compute_minimum_collision_distances), "Compute the minimum distances between the robot links.", "joint_positions"_a); +// 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("compute_minimum_collision_distances", py::overload_cast(&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( diff --git a/source/robot_model/CMakeLists.txt b/source/robot_model/CMakeLists.txt index 4b6ecd95a..8c7ec1411 100644 --- a/source/robot_model/CMakeLists.txt +++ b/source/robot_model/CMakeLists.txt @@ -7,7 +7,7 @@ find_package(pinocchio ${PINOCCHIO_VERSION} REQUIRED) find_package(hpp-fcl ${HPP_FCL_VERSION} REQUIRED) find_package(OsqpEigen ${OSQP_EIGEN_VERSION} REQUIRED) -add_definitions(-DPINOCCHIO_WITH_HPP_FCL) +# add_definitions(-DPINOCCHIO_WITH_HPP_FCL) set(CORE_SOURCES src/Model.cpp @@ -29,7 +29,7 @@ target_link_libraries(${LIBRARY_NAME} ${hpp-fcl_LIBRARIES} OsqpEigen::OsqpEigen ) -target_compile_definitions(${LIBRARY_NAME} PUBLIC -DPINOCCHIO_WITH_HPP_FCL) +# target_compile_definitions(${LIBRARY_NAME} PUBLIC -DPINOCCHIO_WITH_HPP_FCL) # install the target and create export-set install(TARGETS ${LIBRARY_NAME} diff --git a/source/robot_model/include/robot_model/Model.hpp b/source/robot_model/include/robot_model/Model.hpp index 7bbdeeb9f..ed5b077a7 100644 --- a/source/robot_model/include/robot_model/Model.hpp +++ b/source/robot_model/include/robot_model/Model.hpp @@ -89,7 +89,7 @@ class Model { /** * @brief Initialize the pinocchio geometry model from the URDF and the package paths */ - void init_geom_model(std::string urdf); + // void init_geom_model(std::string urdf); /** * @brief initialize the constraints for the QP solver @@ -264,22 +264,22 @@ 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 - * @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 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 + // * @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 diff --git a/source/robot_model/src/Model.cpp b/source/robot_model/src/Model.cpp index 75d12d8bb..0b2f154a0 100644 --- a/source/robot_model/src/Model.cpp +++ b/source/robot_model/src/Model.cpp @@ -97,9 +97,9 @@ void Model::init_model() { 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); - } + // if (this->load_collision_geometries_) { + // this->init_geom_model(urdf); + // } // get the frames std::vector frames; @@ -111,26 +111,26 @@ 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(); +// 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(); +// 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); - } +// // 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()); - } -} +// 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; @@ -164,53 +164,53 @@ 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; -} - -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::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; +// } + +// 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 diff --git a/source/robot_model/test/tests/test_collisions.cpp b/source/robot_model/test/tests/test_collisions.cpp deleted file mode 100644 index 0edb5eedd..000000000 --- a/source/robot_model/test/tests/test_collisions.cpp +++ /dev/null @@ -1,174 +0,0 @@ -#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; - } -} - -// 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::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."; - } -}