diff --git a/include/dqrobotics/robot_modeling/DQ_ParameterDH.h b/include/dqrobotics/robot_modeling/DQ_ParameterDH.h new file mode 100644 index 0000000..696a650 --- /dev/null +++ b/include/dqrobotics/robot_modeling/DQ_ParameterDH.h @@ -0,0 +1,101 @@ +/** +(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: +1. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) + - Responsible for the original implementation. +*/ + +#include +#include +#pragma once + +namespace DQ_robotics +{ +class DQ_ParameterDH +{ +public: + enum PARAMETER{ + THETA, + D, + A, + ALPHA + }; +private: + PARAMETER parameter_; + const std::unordered_map + map_ = {{"THETA", THETA}, + {"D" , D}, + {"A" , A}, + {"ALPHA", ALPHA}, + }; + + /** + * @brief _get_parameter sets the parameter member using a string as argument. + * @param parameter The desired parameter to be set. Example: "THETA", "D", "A", or "ALPHA". + */ + void _set_parameter(const std::string& parameter) + { + try { + parameter_ = map_.at(parameter); + } catch (...) { + throw std::runtime_error("The parameter \""+ parameter+ "\" is not supported. Use THETA, D, A, or ALPHA"); + } + } +public: + /** + * @brief DQ_ParameterDH Default constructor method. + */ + DQ_ParameterDH() = default; + + /** + * @brief DQ_ParameterDH Constructor method + * @param parameter The desired DH parameter. Example: THETA, D, A, or ALPHA. + */ + DQ_ParameterDH(const PARAMETER& parameter): parameter_{parameter}{}; + + // This definition enables switch cases and comparisons. + constexpr operator PARAMETER() const { return parameter_; } + + /** + * @brief DQ_ParameterDH Constructor method that allows string parameters. + * This is done to keep the language compatibility between + * Matlab and Python/C++, as discussed in + * https://github.com/dqrobotics/cpp/pull/69 + * @param parameter The desired DH parameter. Example: "THETA", "D", "A", or "ALPHA". + */ + DQ_ParameterDH(const std::string& parameter){ + _set_parameter(parameter); + } + + + /** + * @brief DQ_ParameterDH Constructor method that allows char parameters. + * This is done to keep the language compatibility between + * Matlab and Python/C++, as discussed in + * https://github.com/dqrobotics/cpp/pull/69 + * @param parameter_c The desired DH parameter. Example: "THETA", "D", "A", or "ALPHA". + */ + DQ_ParameterDH(const char* parameter_c){ + _set_parameter(parameter_c); + } + +}; +} + + diff --git a/include/dqrobotics/robot_modeling/DQ_SerialManipulator.h b/include/dqrobotics/robot_modeling/DQ_SerialManipulator.h index a4c609b..0f577ea 100644 --- a/include/dqrobotics/robot_modeling/DQ_SerialManipulator.h +++ b/include/dqrobotics/robot_modeling/DQ_SerialManipulator.h @@ -18,9 +18,8 @@ This file is part of DQ Robotics. along with DQ Robotics. If not, see . Contributors: -- Murilo M. Marinho (murilomarinho@ieee.org) -- Mateus Rodrigues Martins (martinsrmateus@gmail.com) -- Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) +1. Murilo M. Marinho (murilomarinho@ieee.org) +2. Mateus Rodrigues Martins (martinsrmateus@gmail.com) */ #include diff --git a/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h b/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h index b0bbf7d..09964cf 100644 --- a/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h +++ b/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h @@ -1,6 +1,6 @@ #pragma once /** -(C) Copyright 2020-2022 DQ Robotics Developers +(C) Copyright 2011-2025 DQ Robotics Developers This file is part of DQ Robotics. @@ -18,12 +18,16 @@ This file is part of DQ Robotics. along with DQ Robotics. If not, see . Contributors: -- Murilo M. Marinho (murilomarinho@ieee.org) -- Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) +1. Murilo M. Marinho (murilomarinho@ieee.org) + - Responsible for the original implementation. + +2. Juan Jose Quiroz Omana (juanjqogm@gmail.com) + - Added methods to get and set the DH parameters. */ #include +#include namespace DQ_robotics { @@ -36,6 +40,14 @@ class DQ_SerialManipulatorDH: public DQ_SerialManipulator DQ _get_w(const int& ith) const; DQ _dh2dq(const double& q, const int& ith) const; public: + VectorXd get_parameters(const DQ_ParameterDH& parameter_type) const; + double get_parameter(const DQ_ParameterDH& parameter_type, + const int& to_ith_link) const; + void set_parameters(const DQ_ParameterDH& parameter_type, + const VectorXd& vector_parameters); + void set_parameter(const DQ_ParameterDH& parameter_type, + const int& to_ith_link, + const double& parameter); // Deprecated on 22.04, will be removed on the next release. enum [[deprecated("Use ? instead.")]] JOINT_TYPES{ JOINT_ROTATIONAL=0, JOINT_PRISMATIC }; diff --git a/include/dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h b/include/dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h index 03e6874..3795ecd 100644 --- a/include/dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h +++ b/include/dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h @@ -1,6 +1,6 @@ #pragma once /** -(C) Copyright 2022 DQ Robotics Developers +(C) Copyright 2011-2025 DQ Robotics Developers This file is part of DQ Robotics. @@ -18,11 +18,15 @@ This file is part of DQ Robotics. along with DQ Robotics. If not, see . Contributors: -- Murilo M. Marinho (murilomarinho@ieee.org) -- Juan Jose Quiroz Omana - juanjqo@g.ecc.u-tokyo.ac.jp +1. Murilo M. Marinho (murilomarinho@ieee.org) + - Responsible for the original implementation. + +2. Juan Jose Quiroz Omana (juanjqogm@gmail.com) + - Added methods to get and set the DH parameters. */ #include +#include namespace DQ_robotics { @@ -35,6 +39,14 @@ class DQ_SerialManipulatorMDH: public DQ_SerialManipulator DQ _get_w(const int& ith) const; DQ _mdh2dq(const double& q, const int& ith) const; public: + VectorXd get_parameters(const DQ_ParameterDH& parameter_type) const; + double get_parameter(const DQ_ParameterDH& parameter_type, + const int& to_ith_link) const; + void set_parameters(const DQ_ParameterDH& parameter_type, + const VectorXd& vector_parameters); + void set_parameter(const DQ_ParameterDH& parameter_type, + const int& to_ith_link, + const double& parameter); // Deprecated on 22.04, will be removed on the next release. enum [[deprecated("Use ? instead.")]] JOINT_TYPES{ JOINT_ROTATIONAL=0, JOINT_PRISMATIC }; diff --git a/src/robot_modeling/DQ_SerialManipulator.cpp b/src/robot_modeling/DQ_SerialManipulator.cpp index 1d923af..cc2192a 100644 --- a/src/robot_modeling/DQ_SerialManipulator.cpp +++ b/src/robot_modeling/DQ_SerialManipulator.cpp @@ -1,5 +1,5 @@ /** -(C) Copyright 2011-2020 DQ Robotics Developers +(C) Copyright 2011-2025 DQ Robotics Developers This file is part of DQ Robotics. @@ -17,9 +17,8 @@ This file is part of DQ Robotics. along with DQ Robotics. If not, see . Contributors: -- Murilo M. Marinho (murilomarinho@ieee.org) -- Mateus Rodrigues Martins (martinsrmateus@gmail.com) -- Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) +1. Murilo M. Marinho (murilomarinho@ieee.org) +2. Mateus Rodrigues Martins (martinsrmateus@gmail.com) */ #include diff --git a/src/robot_modeling/DQ_SerialManipulatorDH.cpp b/src/robot_modeling/DQ_SerialManipulatorDH.cpp index c40f0d9..6020509 100644 --- a/src/robot_modeling/DQ_SerialManipulatorDH.cpp +++ b/src/robot_modeling/DQ_SerialManipulatorDH.cpp @@ -1,5 +1,5 @@ /** -(C) Copyright 2020-2022 DQ Robotics Developers +(C) Copyright 2011-2025 DQ Robotics Developers This file is part of DQ Robotics. @@ -17,8 +17,11 @@ This file is part of DQ Robotics. along with DQ Robotics. If not, see . Contributors: -- Murilo M. Marinho (murilomarinho@ieee.org) -- Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) +1. Murilo M. Marinho (murilomarinho@ieee.org) + - Responsible for the original implementation. + +2. Juan Jose Quiroz Omana (juanjqogm@gmail.com) + - Added methods to get and set the DH parameters. */ #include @@ -99,6 +102,102 @@ DQ DQ_SerialManipulatorDH::_dh2dq(const double &q, const int &ith) const ); } +/** + * @brief DQ_SerialManipulatorDH::get_parameters returns a vector containing the DH parameters. + * @param parameter_type Parameter type, which which corresponds to THETA, D, A, or ALPHA. + * @return A vector containing the desired DH parameters. + */ +VectorXd DQ_SerialManipulatorDH::get_parameters(const DQ_ParameterDH ¶meter_type) const +{ + switch (parameter_type) { + case DQ_ParameterDH::THETA: + return dh_matrix_.row(0); + case DQ_ParameterDH::D: + return dh_matrix_.row(1); + case DQ_ParameterDH::A: + return dh_matrix_.row(2); + case DQ_ParameterDH::ALPHA: + return dh_matrix_.row(3); + default: + throw std::runtime_error("Wrong type of parameter"); + } +} + +/** + * @brief DQ_SerialManipulatorDH::get_parameter returns the DH parameter of the ith joint. + * @param parameter_type Parameter type, which which corresponds to THETA, D, A, or ALPHA. + * @param to_ith_link The joint number. + * @return The desired DH parameter. + */ +double DQ_SerialManipulatorDH::get_parameter(const DQ_ParameterDH ¶meter_type, + const int &to_ith_link) const +{ + _check_to_ith_link(to_ith_link); + switch (parameter_type) { + case DQ_ParameterDH::THETA: + return dh_matrix_(0, to_ith_link); + case DQ_ParameterDH::D: + return dh_matrix_(1, to_ith_link); + case DQ_ParameterDH::A: + return dh_matrix_(2, to_ith_link); + case DQ_ParameterDH::ALPHA: + return dh_matrix_(3, to_ith_link); + default: + throw std::runtime_error("Wrong type of parameter"); + } +} + +/** + * @brief DQ_SerialManipulatorDH::set_parameters sets the DH parameters. + * @param parameter_type Parameter type, which which corresponds to THETA, D, A, or ALPHA. + * @param vector_parameters A vector containing the new parameters. + */ +void DQ_SerialManipulatorDH::set_parameters(const DQ_ParameterDH ¶meter_type, + const VectorXd &vector_parameters) +{ + _check_q_vec(vector_parameters); + switch (parameter_type) { + case DQ_ParameterDH::THETA: + dh_matrix_.row(0) = vector_parameters; + break; + case DQ_ParameterDH::D: + dh_matrix_.row(1) = vector_parameters; + break; + case DQ_ParameterDH::A: + dh_matrix_.row(2) = vector_parameters; + break; + case DQ_ParameterDH::ALPHA: + dh_matrix_.row(3) = vector_parameters; + break; + } +} + +/** + * @brief DQ_SerialManipulatorDH::set_parameter sets the DH parameter of the ith joint. + * @param parameter_type Parameter type, which which corresponds to THETA, D, A, or ALPHA. + * @param to_ith_link The joint number. + * @param parameter The new parameter. + */ +void DQ_SerialManipulatorDH::set_parameter(const DQ_ParameterDH ¶meter_type, + const int &to_ith_link, const double ¶meter) +{ + _check_to_ith_link(to_ith_link); + switch (parameter_type) { + case DQ_ParameterDH::THETA: + dh_matrix_(0, to_ith_link) = parameter; + break; + case DQ_ParameterDH::D: + dh_matrix_(1, to_ith_link) = parameter; + break; + case DQ_ParameterDH::A: + dh_matrix_(2, to_ith_link) = parameter; + break; + case DQ_ParameterDH::ALPHA: + dh_matrix_(3, to_ith_link) = parameter; + break; + } +} + /** * @brief This protected method computes the dual quaternion related with the time derivative of the diff --git a/src/robot_modeling/DQ_SerialManipulatorMDH.cpp b/src/robot_modeling/DQ_SerialManipulatorMDH.cpp index d7f8305..3196355 100644 --- a/src/robot_modeling/DQ_SerialManipulatorMDH.cpp +++ b/src/robot_modeling/DQ_SerialManipulatorMDH.cpp @@ -1,5 +1,5 @@ /** -(C) Copyright 2022 DQ Robotics Developers +(C) Copyright 2011-2025 DQ Robotics Developers This file is part of DQ Robotics. @@ -17,8 +17,11 @@ This file is part of DQ Robotics. along with DQ Robotics. If not, see . Contributors: -- Murilo M. Marinho (murilomarinho@ieee.org) -- Juan Jose Quiroz Omana - juanjqo@g.ecc.u-tokyo.ac.jp +1. Murilo M. Marinho (murilomarinho@ieee.org) + - Responsible for the original implementation. + +2. Juan Jose Quiroz Omana (juanjqogm@gmail.com) + - Added methods to get and set the DH parameters. */ #include @@ -110,6 +113,102 @@ DQ DQ_SerialManipulatorMDH::_mdh2dq(const double &q, const int &ith) const ); } +/** + * @brief DQ_SerialManipulatorMDH::get_parameters returns a vector containing the MDH parameters. + * @param parameter_type Parameter type, which which corresponds to THETA, D, A, or ALPHA. + * @return A vector containing the desired MDH parameters. + */ +VectorXd DQ_SerialManipulatorMDH::get_parameters(const DQ_ParameterDH ¶meter_type) const +{ + switch (parameter_type) { + case DQ_ParameterDH::THETA: + return mdh_matrix_.row(0); + case DQ_ParameterDH::D: + return mdh_matrix_.row(1); + case DQ_ParameterDH::A: + return mdh_matrix_.row(2); + case DQ_ParameterDH::ALPHA: + return mdh_matrix_.row(3); + default: + throw std::runtime_error("Wrong type of parameter"); + } +} + +/** + * @brief DQ_SerialManipulatorMDH::get_parameter returns the MDH parameter of the ith joint. + * @param parameter_type Parameter type, which which corresponds to THETA, D, A, or ALPHA. + * @param to_ith_link The joint number. + * @return The desired MDH parameter. + */ +double DQ_SerialManipulatorMDH::get_parameter(const DQ_ParameterDH ¶meter_type, + const int &to_ith_link) const +{ + _check_to_ith_link(to_ith_link); + switch (parameter_type) { + case DQ_ParameterDH::THETA: + return mdh_matrix_(0, to_ith_link); + case DQ_ParameterDH::D: + return mdh_matrix_(1, to_ith_link); + case DQ_ParameterDH::A: + return mdh_matrix_(2, to_ith_link); + case DQ_ParameterDH::ALPHA: + return mdh_matrix_(3, to_ith_link); + default: + throw std::runtime_error("Wrong type of parameter"); + } +} + +/** + * @brief DQ_SerialManipulatorMDH::set_parameters sets the MDH parameters. + * @param parameter_type Parameter type, which which corresponds to THETA, D, A, or ALPHA. + * @param vector_parameters A vector containing the new parameters. + */ +void DQ_SerialManipulatorMDH::set_parameters(const DQ_ParameterDH ¶meter_type, + const VectorXd &vector_parameters) +{ + _check_q_vec(vector_parameters); + switch (parameter_type) { + case DQ_ParameterDH::THETA: + mdh_matrix_.row(0) = vector_parameters; + break; + case DQ_ParameterDH::D: + mdh_matrix_.row(1) = vector_parameters; + break; + case DQ_ParameterDH::A: + mdh_matrix_.row(2) = vector_parameters; + break; + case DQ_ParameterDH::ALPHA: + mdh_matrix_.row(3) = vector_parameters; + break; + } +} + +/** + * @brief DQ_SerialManipulatorMDH::set_parameter sets the MDH parameter of the ith joint. + * @param parameter_type Parameter type, which which corresponds to THETA, D, A, or ALPHA. + * @param to_ith_link The joint number. + * @param parameter The new parameter. + */ +void DQ_SerialManipulatorMDH::set_parameter(const DQ_ParameterDH ¶meter_type, + const int &to_ith_link, const double ¶meter) +{ + _check_to_ith_link(to_ith_link); + switch (parameter_type) { + case DQ_ParameterDH::THETA: + mdh_matrix_(0, to_ith_link) = parameter; + break; + case DQ_ParameterDH::D: + mdh_matrix_(1, to_ith_link) = parameter; + break; + case DQ_ParameterDH::A: + mdh_matrix_(2, to_ith_link) = parameter; + break; + case DQ_ParameterDH::ALPHA: + mdh_matrix_(3, to_ith_link) = parameter; + break; + } +} + /** * @brief This protected method computes the dual quaternion related with the time derivative of the * unit dual quaternion pose using the MDH convention.