From 363b9fe101229b9b772d2b3d8d1c29cd990a6a2d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Fri, 16 May 2025 15:19:34 +0100 Subject: [PATCH 1/4] Added a new example to test the DQ_JointType class and the setter and getter methods related to the joint types in the DQ_SerialManipulator class. --- .../CMakeLists.txt | 12 +++ .../serial_manipulator_joint_types.cpp | 96 +++++++++++++++++++ 2 files changed, 108 insertions(+) create mode 100644 cmake/serial_manipulator_joint_types/CMakeLists.txt create mode 100644 cmake/serial_manipulator_joint_types/serial_manipulator_joint_types.cpp diff --git a/cmake/serial_manipulator_joint_types/CMakeLists.txt b/cmake/serial_manipulator_joint_types/CMakeLists.txt new file mode 100644 index 0000000..219567c --- /dev/null +++ b/cmake/serial_manipulator_joint_types/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.16) +include(${CMAKE_CURRENT_SOURCE_DIR}/../dqrobotics_dependencies.cmake) + +project(serial_manipulator_joint_types) + +add_executable(${PROJECT_NAME} + ${PROJECT_NAME}.cpp + ) + +TARGET_LINK_LIBRARIES(${PROJECT_NAME} + dqrobotics + ) diff --git a/cmake/serial_manipulator_joint_types/serial_manipulator_joint_types.cpp b/cmake/serial_manipulator_joint_types/serial_manipulator_joint_types.cpp new file mode 100644 index 0000000..52c0af8 --- /dev/null +++ b/cmake/serial_manipulator_joint_types/serial_manipulator_joint_types.cpp @@ -0,0 +1,96 @@ +/** +(C) Copyright 2011-2025 DQ Robotics Developers + +This file is part of DQ Robotics. + + DQ Robotics is free software: you can redistribute it and/or modify + it under the terms of the GNU Lesser General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + DQ Robotics is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public License + along with DQ Robotics. If not, see . + +Contributors: +- Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) +*/ + +#include +#include +#include +#include +#include + + +using namespace DQ_robotics; + +/** + * @brief perform_tests This function tests the following methods: set_joint_type, set_joint_types, + * get_joint_type, and get_joint_types. + * @param robot A serial manipulator robot. + * @param msg A message to be displayed. + * @param is_denso Flag used to indicate if the robot is an object of the DQ_SerialManipulatorDenso class. + */ +template +void perform_tests(const std::shared_ptr& robot, + const std::string& msg, + const bool& is_denso = false + ); + + +int main() +{ + DQ_JointType R = DQ_JointType::REVOLUTE; + + Matrix dh_matrix(5,7); + dh_matrix <<11, 12, 13, 14, 15, 16, 17, // theta + 21, 22, 23, 24, 25, 26, 27, // d + 31, 32, 33, 34, 35, 36, 37, // a + 41, 42, 43, 44, 45, 46, 47, // alpha + R, R, R, R, R, R, R; + + auto dh_robot = std::make_shared(dh_matrix); + auto mdh_robot = std::make_shared(dh_matrix); + auto denso_robot = std::make_shared(MatrixXd::Ones(6,6)); + + perform_tests(dh_robot, "DQ_SerialManipulatorDH"); + perform_tests(mdh_robot, "DQ_SerialManipulatorMDH"); + perform_tests(mdh_robot, "DQ_SerialManipulatorDenso", true); + + dh_robot->set_joint_type(DQ_JointType::HELICAL, 0); + + return 0; +} + +template +void perform_tests(const std::shared_ptr& robot, const std::string& msg, const bool& is_denso){ + + // Test get_joint_type + for (int i=0;iget_dim_configuration_space();i++) + assert(robot->get_joint_type(i) == DQ_JointType::REVOLUTE); + + // Test set_joint_type and get_joint_types + DQ_JointType target_joint_type = DQ_JointType::PRISMATIC; + if (is_denso) + target_joint_type = DQ_JointType::REVOLUTE; + for (int i=0;iget_dim_configuration_space();i++) + robot->set_joint_type(target_joint_type, i); + + DQ_JointType expected_joint_type = DQ_JointType::PRISMATIC; + if (is_denso) + expected_joint_type = DQ_JointType::REVOLUTE; + assert(robot->get_joint_types() == + std::vector(robot->get_dim_configuration_space(), expected_joint_type)); + + // Test set_joint_types + robot->set_joint_types( std::vector(robot->get_dim_configuration_space(), DQ_JointType::REVOLUTE)); + assert(robot->get_joint_types() == + std::vector(robot->get_dim_configuration_space(), DQ_JointType::REVOLUTE)); + + std::cout< Date: Fri, 16 May 2025 15:25:33 +0100 Subject: [PATCH 2/4] [serial_manipulator_joint_types.cpp] Removed line to test the exception. --- .../serial_manipulator_joint_types.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/cmake/serial_manipulator_joint_types/serial_manipulator_joint_types.cpp b/cmake/serial_manipulator_joint_types/serial_manipulator_joint_types.cpp index 52c0af8..62079a2 100644 --- a/cmake/serial_manipulator_joint_types/serial_manipulator_joint_types.cpp +++ b/cmake/serial_manipulator_joint_types/serial_manipulator_joint_types.cpp @@ -62,8 +62,6 @@ int main() perform_tests(mdh_robot, "DQ_SerialManipulatorMDH"); perform_tests(mdh_robot, "DQ_SerialManipulatorDenso", true); - dh_robot->set_joint_type(DQ_JointType::HELICAL, 0); - return 0; } From c3e4cdf2d0fe0f69e6c1fe6ae5c3b5bf9d79ef6a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Sat, 17 May 2025 23:06:46 +0100 Subject: [PATCH 3/4] [serial_manipulator_joint_types.cpp] Removed the flag in the perform_tests function, as suggested by Murilo. --- .../serial_manipulator_joint_types.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/cmake/serial_manipulator_joint_types/serial_manipulator_joint_types.cpp b/cmake/serial_manipulator_joint_types/serial_manipulator_joint_types.cpp index 62079a2..92b9bc8 100644 --- a/cmake/serial_manipulator_joint_types/serial_manipulator_joint_types.cpp +++ b/cmake/serial_manipulator_joint_types/serial_manipulator_joint_types.cpp @@ -34,12 +34,10 @@ using namespace DQ_robotics; * get_joint_type, and get_joint_types. * @param robot A serial manipulator robot. * @param msg A message to be displayed. - * @param is_denso Flag used to indicate if the robot is an object of the DQ_SerialManipulatorDenso class. */ template void perform_tests(const std::shared_ptr& robot, - const std::string& msg, - const bool& is_denso = false + const std::string& msg ); @@ -60,13 +58,13 @@ int main() perform_tests(dh_robot, "DQ_SerialManipulatorDH"); perform_tests(mdh_robot, "DQ_SerialManipulatorMDH"); - perform_tests(mdh_robot, "DQ_SerialManipulatorDenso", true); + perform_tests(mdh_robot, "DQ_SerialManipulatorDenso"); return 0; } template -void perform_tests(const std::shared_ptr& robot, const std::string& msg, const bool& is_denso){ +void perform_tests(const std::shared_ptr& robot, const std::string& msg){ // Test get_joint_type for (int i=0;iget_dim_configuration_space();i++) @@ -74,13 +72,14 @@ void perform_tests(const std::shared_ptr& robot, const std::string& msg, cons // Test set_joint_type and get_joint_types DQ_JointType target_joint_type = DQ_JointType::PRISMATIC; - if (is_denso) + auto denso = std::dynamic_pointer_cast(robot); + if (denso) target_joint_type = DQ_JointType::REVOLUTE; for (int i=0;iget_dim_configuration_space();i++) robot->set_joint_type(target_joint_type, i); DQ_JointType expected_joint_type = DQ_JointType::PRISMATIC; - if (is_denso) + if (denso) expected_joint_type = DQ_JointType::REVOLUTE; assert(robot->get_joint_types() == std::vector(robot->get_dim_configuration_space(), expected_joint_type)); From 8dcfc390feb5902b41aabd0ae0fa91b0b75b4497 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Mon, 19 May 2025 10:18:08 +0100 Subject: [PATCH 4/4] [serial_manipulator_joint_types.cpp] Modified the example to check the robot type in compilation time. --- .../serial_manipulator_joint_types.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/cmake/serial_manipulator_joint_types/serial_manipulator_joint_types.cpp b/cmake/serial_manipulator_joint_types/serial_manipulator_joint_types.cpp index 92b9bc8..5665c73 100644 --- a/cmake/serial_manipulator_joint_types/serial_manipulator_joint_types.cpp +++ b/cmake/serial_manipulator_joint_types/serial_manipulator_joint_types.cpp @@ -25,6 +25,7 @@ This file is part of DQ Robotics. #include #include #include +#include using namespace DQ_robotics; @@ -36,7 +37,7 @@ using namespace DQ_robotics; * @param msg A message to be displayed. */ template -void perform_tests(const std::shared_ptr& robot, +void perform_tests(const T& robot, const std::string& msg ); @@ -58,21 +59,21 @@ int main() perform_tests(dh_robot, "DQ_SerialManipulatorDH"); perform_tests(mdh_robot, "DQ_SerialManipulatorMDH"); - perform_tests(mdh_robot, "DQ_SerialManipulatorDenso"); + perform_tests(denso_robot, "DQ_SerialManipulatorDenso"); return 0; } template -void perform_tests(const std::shared_ptr& robot, const std::string& msg){ +void perform_tests(const T& robot, const std::string& msg){ // Test get_joint_type for (int i=0;iget_dim_configuration_space();i++) assert(robot->get_joint_type(i) == DQ_JointType::REVOLUTE); // Test set_joint_type and get_joint_types + constexpr bool denso = (std::is_same>::value); DQ_JointType target_joint_type = DQ_JointType::PRISMATIC; - auto denso = std::dynamic_pointer_cast(robot); if (denso) target_joint_type = DQ_JointType::REVOLUTE; for (int i=0;iget_dim_configuration_space();i++)