From b564e9950a1145255e956842625503701a95fe9f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Thu, 8 May 2025 16:35:33 +0100 Subject: [PATCH 1/3] Added a new example to test the setter and getter methods in the DQ_SerialManipulator class --- cmake/dh_parameters/CMakeLists.txt | 12 +++ cmake/dh_parameters/dh_parameters.cpp | 112 ++++++++++++++++++++++++++ 2 files changed, 124 insertions(+) create mode 100644 cmake/dh_parameters/CMakeLists.txt create mode 100644 cmake/dh_parameters/dh_parameters.cpp diff --git a/cmake/dh_parameters/CMakeLists.txt b/cmake/dh_parameters/CMakeLists.txt new file mode 100644 index 0000000..9f2c52b --- /dev/null +++ b/cmake/dh_parameters/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.16) +include(${CMAKE_CURRENT_SOURCE_DIR}/../dqrobotics_dependencies.cmake) + +project(dh_parameters) + +add_executable(${PROJECT_NAME} + ${PROJECT_NAME}.cpp + ) + +TARGET_LINK_LIBRARIES(${PROJECT_NAME} + dqrobotics + ) diff --git a/cmake/dh_parameters/dh_parameters.cpp b/cmake/dh_parameters/dh_parameters.cpp new file mode 100644 index 0000000..791e8bd --- /dev/null +++ b/cmake/dh_parameters/dh_parameters.cpp @@ -0,0 +1,112 @@ +/** +(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 +using namespace DQ_robotics; + +/** + * @brief perform_tests This function tests the following methods: get_parameter, get_parameters, + * set_parameter, and set_parameters in the DQ_SerialManipulatorDH and + * DQ_SerialManipulatorMDH classes. + * @param robot A smart pointer of a DQ_SerialManipulatorDH or DQ_SerialManipulatorMDH robot. + * @param dh_matrix The DH matrix used to create the smart pointer of the robot. + * @param msg The desired message to be displayed. + */ +template +void perform_tests(const std::shared_ptr& robot, const MatrixXd& dh_matrix, const std::string& msg); + + +int main() +{ + 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 + 0, 0, 0, 0, 0, 0, 0; + + auto dh_robot = std::make_shared(dh_matrix); + auto mdh_robot = std::make_shared(dh_matrix); + + perform_tests(dh_robot, dh_matrix, "DQ_SerialManipulatorDH"); + perform_tests(mdh_robot, dh_matrix, "DQ_SerialManipulatorMDH"); + + return 0; +} + + + +template +void perform_tests(const std::shared_ptr& robot, const MatrixXd& dh_matrix, const std::string& msg) +{ + for (int i=0;iget_dim_configuration_space();i++) + { + if (dh_matrix(0,i) != robot->get_parameter(DQ_SerialManipulator::DQ_ParameterDH::THETA, i)) + throw std::runtime_error(msg + " Error in get_parameter THETA"); + if (dh_matrix(1,i) != robot->get_parameter(DQ_SerialManipulator::DQ_ParameterDH::D, i)) + throw std::runtime_error(msg +" Error in get_parameter D"); + if (dh_matrix(2,i) != robot->get_parameter(DQ_SerialManipulator::DQ_ParameterDH::A, i)) + throw std::runtime_error(msg + " Error in get_parameter A"); + if (dh_matrix(3,i) != robot->get_parameter(DQ_SerialManipulator::DQ_ParameterDH::ALPHA, i)) + throw std::runtime_error(msg + " Error in get_parameter ALPHA"); + } + + if (dh_matrix.row(0).transpose() != robot->get_parameters(DQ_SerialManipulator::DQ_ParameterDH::THETA)) + throw std::runtime_error(msg + " Error in get_parameters THETA"); + if (dh_matrix.row(1).transpose() != robot->get_parameters(DQ_SerialManipulator::DQ_ParameterDH::D)) + throw std::runtime_error(msg + " Error in get_parameters D"); + if (dh_matrix.row(2).transpose() != robot->get_parameters(DQ_SerialManipulator::DQ_ParameterDH::A)) + throw std::runtime_error(msg + " Error in get_parameters A"); + if (dh_matrix.row(3).transpose() != robot->get_parameters(DQ_SerialManipulator::DQ_ParameterDH::ALPHA)) + throw std::runtime_error(msg + " Error in get_parameters ALPHA"); + + robot->set_parameters(DQ_SerialManipulator::DQ_ParameterDH::THETA, dh_matrix.row(0)/10); + robot->set_parameters(DQ_SerialManipulator::DQ_ParameterDH::D, dh_matrix.row(1)/10); + robot->set_parameters(DQ_SerialManipulator::DQ_ParameterDH::A, dh_matrix.row(2)/10); + robot->set_parameters(DQ_SerialManipulator::DQ_ParameterDH::ALPHA, dh_matrix.row(3)/10); + + for (int i=0;iget_dim_configuration_space();i++) + { + if (dh_matrix(0,i)/10 != robot->get_parameter(DQ_SerialManipulator::DQ_ParameterDH::THETA, i)) + throw std::runtime_error(msg + " Error in set_parameter THETA"); + if (dh_matrix(1,i)/10 != robot->get_parameter(DQ_SerialManipulator::DQ_ParameterDH::D, i)) + throw std::runtime_error(msg + " Error in set_parameter D"); + if (dh_matrix(2,i)/10 != robot->get_parameter(DQ_SerialManipulator::DQ_ParameterDH::A, i)) + throw std::runtime_error(msg + " Error in set_parameter A"); + if (dh_matrix(3,i)/10 != robot->get_parameter(DQ_SerialManipulator::DQ_ParameterDH::ALPHA, i)) + throw std::runtime_error(msg + "Error in set_parameter ALPHA"); + } + + if (dh_matrix.row(0).transpose()/10 != robot->get_parameters(DQ_SerialManipulator::DQ_ParameterDH::THETA)) + throw std::runtime_error(msg + " Error in get_parameters THETA"); + if (dh_matrix.row(1).transpose()/10 != robot->get_parameters(DQ_SerialManipulator::DQ_ParameterDH::D)) + throw std::runtime_error(msg + " Error in get_parameters D"); + if (dh_matrix.row(2).transpose()/10 != robot->get_parameters(DQ_SerialManipulator::DQ_ParameterDH::A)) + throw std::runtime_error(msg + " Error in get_parameters A"); + if (dh_matrix.row(3).transpose()/10 != robot->get_parameters(DQ_SerialManipulator::DQ_ParameterDH::ALPHA)) + throw std::runtime_error(msg + " Error in get_parameters ALPHA"); + + std::cout< Date: Thu, 8 May 2025 22:25:17 +0100 Subject: [PATCH 2/3] [dh_parameters.cpp] Updated the example using assert as suggested by Murilo. --- cmake/dh_parameters/dh_parameters.cpp | 65 +++++++++------------------ 1 file changed, 22 insertions(+), 43 deletions(-) diff --git a/cmake/dh_parameters/dh_parameters.cpp b/cmake/dh_parameters/dh_parameters.cpp index 791e8bd..2fe28af 100644 --- a/cmake/dh_parameters/dh_parameters.cpp +++ b/cmake/dh_parameters/dh_parameters.cpp @@ -61,52 +61,31 @@ int main() template void perform_tests(const std::shared_ptr& robot, const MatrixXd& dh_matrix, const std::string& msg) { + std::vector parameters = + {DQ_ParameterDH::THETA, DQ_ParameterDH::D, DQ_ParameterDH::A, DQ_ParameterDH::ALPHA}; + + // Test get_parameter for (int i=0;iget_dim_configuration_space();i++) - { - if (dh_matrix(0,i) != robot->get_parameter(DQ_SerialManipulator::DQ_ParameterDH::THETA, i)) - throw std::runtime_error(msg + " Error in get_parameter THETA"); - if (dh_matrix(1,i) != robot->get_parameter(DQ_SerialManipulator::DQ_ParameterDH::D, i)) - throw std::runtime_error(msg +" Error in get_parameter D"); - if (dh_matrix(2,i) != robot->get_parameter(DQ_SerialManipulator::DQ_ParameterDH::A, i)) - throw std::runtime_error(msg + " Error in get_parameter A"); - if (dh_matrix(3,i) != robot->get_parameter(DQ_SerialManipulator::DQ_ParameterDH::ALPHA, i)) - throw std::runtime_error(msg + " Error in get_parameter ALPHA"); - } - - if (dh_matrix.row(0).transpose() != robot->get_parameters(DQ_SerialManipulator::DQ_ParameterDH::THETA)) - throw std::runtime_error(msg + " Error in get_parameters THETA"); - if (dh_matrix.row(1).transpose() != robot->get_parameters(DQ_SerialManipulator::DQ_ParameterDH::D)) - throw std::runtime_error(msg + " Error in get_parameters D"); - if (dh_matrix.row(2).transpose() != robot->get_parameters(DQ_SerialManipulator::DQ_ParameterDH::A)) - throw std::runtime_error(msg + " Error in get_parameters A"); - if (dh_matrix.row(3).transpose() != robot->get_parameters(DQ_SerialManipulator::DQ_ParameterDH::ALPHA)) - throw std::runtime_error(msg + " Error in get_parameters ALPHA"); - - robot->set_parameters(DQ_SerialManipulator::DQ_ParameterDH::THETA, dh_matrix.row(0)/10); - robot->set_parameters(DQ_SerialManipulator::DQ_ParameterDH::D, dh_matrix.row(1)/10); - robot->set_parameters(DQ_SerialManipulator::DQ_ParameterDH::A, dh_matrix.row(2)/10); - robot->set_parameters(DQ_SerialManipulator::DQ_ParameterDH::ALPHA, dh_matrix.row(3)/10); + for(int j=0;j<=3;j++) + assert(dh_matrix(j,i) == robot->get_parameter(parameters.at(j), i)); + // Test get_parameters for (int i=0;iget_dim_configuration_space();i++) - { - if (dh_matrix(0,i)/10 != robot->get_parameter(DQ_SerialManipulator::DQ_ParameterDH::THETA, i)) - throw std::runtime_error(msg + " Error in set_parameter THETA"); - if (dh_matrix(1,i)/10 != robot->get_parameter(DQ_SerialManipulator::DQ_ParameterDH::D, i)) - throw std::runtime_error(msg + " Error in set_parameter D"); - if (dh_matrix(2,i)/10 != robot->get_parameter(DQ_SerialManipulator::DQ_ParameterDH::A, i)) - throw std::runtime_error(msg + " Error in set_parameter A"); - if (dh_matrix(3,i)/10 != robot->get_parameter(DQ_SerialManipulator::DQ_ParameterDH::ALPHA, i)) - throw std::runtime_error(msg + "Error in set_parameter ALPHA"); - } - - if (dh_matrix.row(0).transpose()/10 != robot->get_parameters(DQ_SerialManipulator::DQ_ParameterDH::THETA)) - throw std::runtime_error(msg + " Error in get_parameters THETA"); - if (dh_matrix.row(1).transpose()/10 != robot->get_parameters(DQ_SerialManipulator::DQ_ParameterDH::D)) - throw std::runtime_error(msg + " Error in get_parameters D"); - if (dh_matrix.row(2).transpose()/10 != robot->get_parameters(DQ_SerialManipulator::DQ_ParameterDH::A)) - throw std::runtime_error(msg + " Error in get_parameters A"); - if (dh_matrix.row(3).transpose()/10 != robot->get_parameters(DQ_SerialManipulator::DQ_ParameterDH::ALPHA)) - throw std::runtime_error(msg + " Error in get_parameters ALPHA"); + for(int j=0;j<=3;j++) + assert(dh_matrix.row(j).transpose() == robot->get_parameters(parameters.at(j))); + + // Set the new parameters + for(int j=0;j<=3;j++) + robot->set_parameters(parameters.at(j), dh_matrix.row(j)/10); + + // Test if the new parameters were set correctly using get_parameter + for (int i=0;iget_dim_configuration_space();i++) + for(int j=0;j<=3;j++) + assert(dh_matrix(j,i)/10 == robot->get_parameter(parameters.at(j), i)); + + // Test if the new parameters were set correctly using get_parameters + for(int j=0;j<=3;j++) + assert(dh_matrix.row(j).transpose()/10 == robot->get_parameters(parameters.at(j))); std::cout< Date: Mon, 12 May 2025 15:17:20 +0100 Subject: [PATCH 3/3] [dh_parameters.cpp] Updated the example to test the setter and getter methods using char/string-style. --- cmake/dh_parameters/dh_parameters.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/cmake/dh_parameters/dh_parameters.cpp b/cmake/dh_parameters/dh_parameters.cpp index 2fe28af..0be7a85 100644 --- a/cmake/dh_parameters/dh_parameters.cpp +++ b/cmake/dh_parameters/dh_parameters.cpp @@ -64,6 +64,12 @@ void perform_tests(const std::shared_ptr& robot, const MatrixXd& dh_matrix, c std::vector parameters = {DQ_ParameterDH::THETA, DQ_ParameterDH::D, DQ_ParameterDH::A, DQ_ParameterDH::ALPHA}; + std::vector string_parameters = + {"THETA", "D", "A", "ALPHA"}; + + std::vector char_parameters = + {"THETA", "D", "A", "ALPHA"}; + // Test get_parameter for (int i=0;iget_dim_configuration_space();i++) for(int j=0;j<=3;j++) @@ -76,16 +82,16 @@ void perform_tests(const std::shared_ptr& robot, const MatrixXd& dh_matrix, c // Set the new parameters for(int j=0;j<=3;j++) - robot->set_parameters(parameters.at(j), dh_matrix.row(j)/10); + robot->set_parameters(string_parameters.at(j), dh_matrix.row(j)/10); // Test if the new parameters were set correctly using get_parameter for (int i=0;iget_dim_configuration_space();i++) for(int j=0;j<=3;j++) - assert(dh_matrix(j,i)/10 == robot->get_parameter(parameters.at(j), i)); + assert(dh_matrix(j,i)/10 == robot->get_parameter(string_parameters.at(j), i)); // Test if the new parameters were set correctly using get_parameters for(int j=0;j<=3;j++) - assert(dh_matrix.row(j).transpose()/10 == robot->get_parameters(parameters.at(j))); + assert(dh_matrix.row(j).transpose()/10 == robot->get_parameters(char_parameters.at(j))); std::cout<