diff --git a/include/dqrobotics/robot_modeling/DQ_JointType.h b/include/dqrobotics/robot_modeling/DQ_JointType.h new file mode 100644 index 0000000..102ad38 --- /dev/null +++ b/include/dqrobotics/robot_modeling/DQ_JointType.h @@ -0,0 +1,125 @@ +/** +(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. +*/ + + +#pragma once +#include + +namespace DQ_robotics +{ +class DQ_JointType +{ +public: + enum JOINT_TYPE{ + REVOLUTE = 0, + PRISMATIC, + SPHERICAL, + CYLINDRICAL, + PLANAR, + SIX_DOF, + HELICAL + }; + // This definition enables switch cases and comparisons. + constexpr operator JOINT_TYPE() const { return joint_type_; } +private: + JOINT_TYPE joint_type_; + +public: + /** + * @brief DQ_JointType Default constructor method. + * This class is based on Table 1 of Silva, Quiroz-Omaña, and Adorno (2022). + * Dynamics of Mobile Manipulators Using Dual Quaternion Algebra. + */ + DQ_JointType() = default; + + /** + * @brief DQ_JointType Constructor method. + * This class is based on Table 1 of Silva, Quiroz-Omaña, and Adorno (2022). + * Dynamics of Mobile Manipulators Using Dual Quaternion Algebra. + * @param joint_type The joint type. Example: REVOLUTE, PRISMATIC, + * SPHERICAL, CYLINDRICAL, PLANAR, SIX_DOF, or HELICAL. + */ + DQ_JointType(const JOINT_TYPE& joint_type): joint_type_{joint_type}{}; + + /** + * @brief DQ_JointType Constructor method that allows integer arguments. + * This class is based on Table 1 of Silva, Quiroz-Omaña, and Adorno (2022). + * Dynamics of Mobile Manipulators Using Dual Quaternion Algebra + * @param joint_type The joint type. + */ + DQ_JointType(const int& joint_type){ + switch (joint_type) { + case 0: + joint_type_ = REVOLUTE; + break; + case 1: + joint_type_ = PRISMATIC; + break; + case 2: + joint_type_ = SPHERICAL; + break; + case 3: + joint_type_ = CYLINDRICAL; + break; + case 4: + joint_type_ = PLANAR; + break; + case 5: + joint_type_ = SIX_DOF; + break; + case 6: + joint_type_ = HELICAL; + break; + default: + throw std::runtime_error("Invalid joint type"); + } + } + + /** + * @brief to_string() converts the DQ_JointType to string. + * @return A string that corresponds to the joint type. + */ + std::string to_string() const { + switch (joint_type_) { + + case REVOLUTE: + return std::string("REVOLUTE"); + case PRISMATIC: + return std::string("PRISMATIC"); + case SPHERICAL: + return std::string("SPHERICAL"); + case CYLINDRICAL: + return std::string("CYLINDRICAL"); + case PLANAR: + return std::string("PLANAR"); + case SIX_DOF: + return std::string("SIX_DOF"); + case HELICAL: + return std::string("HELICAL"); + default: + throw std::runtime_error("Invalid joint type"); + } + } +}; + +} diff --git a/include/dqrobotics/robot_modeling/DQ_SerialManipulator.h b/include/dqrobotics/robot_modeling/DQ_SerialManipulator.h index 0f577ea..651a815 100644 --- a/include/dqrobotics/robot_modeling/DQ_SerialManipulator.h +++ b/include/dqrobotics/robot_modeling/DQ_SerialManipulator.h @@ -1,6 +1,6 @@ #pragma once /** -(C) Copyright 2011-2022 DQ Robotics Developers +(C) Copyright 2011-2025 DQ Robotics Developers This file is part of DQ Robotics. @@ -20,9 +20,15 @@ This file is part of DQ Robotics. Contributors: 1. Murilo M. Marinho (murilomarinho@ieee.org) 2. Mateus Rodrigues Martins (martinsrmateus@gmail.com) + +3. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) + - Added the joint_types member, and the following methods: + _check_joint_types(), and {set,get}_joint_{type, types}. */ #include +#include +#include namespace DQ_robotics { @@ -31,8 +37,9 @@ class DQ_SerialManipulator: public DQ_Kinematics { protected: DQ curr_effector_; - + std::vector joint_types_; DQ_SerialManipulator(const int& dofs); + void _check_joint_types() const; public: DQ get_effector() const; DQ set_effector(const DQ& new_effector); @@ -46,6 +53,11 @@ class DQ_SerialManipulator: public DQ_Kinematics VectorXd get_upper_q_dot_limit() const; void set_upper_q_dot_limit(const VectorXd &upper_q_dot_limit); + DQ_JointType get_joint_type(const int& ith_joint) const; + std::vector get_joint_types() const; + void set_joint_type(const DQ_JointType& joint_type, const int& ith_joint); + void set_joint_types(const std::vector& joint_types); + void set_joint_types(const VectorXd& joint_types); //Virtual virtual MatrixXd raw_pose_jacobian(const VectorXd& q_vec) const; @@ -56,6 +68,7 @@ class DQ_SerialManipulator: public DQ_Kinematics virtual MatrixXd raw_pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const = 0; virtual MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const = 0; virtual DQ raw_fkm(const VectorXd& q_vec, const int& to_ith_link) const = 0; + virtual std::vector get_supported_joint_types() const = 0; //Overrides from DQ_Kinematics virtual DQ fkm(const VectorXd& q_vec) const override; //Override from DQ_Kinematics diff --git a/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h b/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h index 09964cf..56e01d4 100644 --- a/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h +++ b/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h @@ -23,6 +23,7 @@ This file is part of DQ Robotics. 2. Juan Jose Quiroz Omana (juanjqogm@gmail.com) - Added methods to get and set the DH parameters. + - Added the get_supported_joint_types() method. */ @@ -49,13 +50,16 @@ class DQ_SerialManipulatorDH: public DQ_SerialManipulator const int& to_ith_link, const double& parameter); + std::vector get_supported_joint_types()const override; + // Deprecated on 22.04, will be removed on the next release. - enum [[deprecated("Use ? instead.")]] JOINT_TYPES{ JOINT_ROTATIONAL=0, JOINT_PRISMATIC }; - [[deprecated("Use ? instead.")]] VectorXd get_thetas() const; - [[deprecated("Use ? instead.")]] VectorXd get_ds() const; - [[deprecated("Use ? instead.")]] VectorXd get_as() const; - [[deprecated("Use ? instead.")]] VectorXd get_alphas() const; - [[deprecated("Use ? instead.")]] VectorXd get_types() const; + enum [[deprecated("Use DQ_JointType instead.")]] JOINT_TYPES{ JOINT_ROTATIONAL=0, JOINT_PRISMATIC }; + + [[deprecated("Use get_parameters(DQ_ParameterDH::THETA) instead.")]] VectorXd get_thetas() const; + [[deprecated("Use get_parameters(DQ_ParameterDH::D) instead.")]] VectorXd get_ds() const; + [[deprecated("Use get_parameters(DQ_ParameterDH::A) instead.")]] VectorXd get_as() const; + [[deprecated("Use get_parameters(DQ_ParameterDH::ALPHA) instead.")]] VectorXd get_alphas() const; + [[deprecated("Use get_joint_types() instead.")]] VectorXd get_types() const; DQ_SerialManipulatorDH()=delete; DQ_SerialManipulatorDH(const MatrixXd& dh_matrix); diff --git a/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDenso.h b/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDenso.h index 7df807f..d083f51 100644 --- a/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDenso.h +++ b/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDenso.h @@ -1,6 +1,6 @@ #pragma once /** -(C) Copyright 2021-2022 DQ Robotics Developers +(C) Copyright 2011-2025 DQ Robotics Developers This file is part of DQ Robotics. @@ -17,8 +17,13 @@ This file is part of DQ Robotics. You should have received a copy of the GNU Lesser General Public License along with DQ Robotics. If not, see . + Contributors: -- Murilo M. Marinho (murilomarinho@ieee.org) +1. Murilo M. Marinho (murilomarinho@ieee.org) + - Responsible for the original implementation. + +2. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) + - Added the get_supported_joint_types() method. */ #include @@ -33,6 +38,7 @@ class DQ_SerialManipulatorDenso: public DQ_SerialManipulator DQ _denso2dh(const double& q, const int& ith) const; public: + std::vector get_supported_joint_types() const override; // Deprecated on 22.04, will be removed on the next release. [[deprecated("Use ? instead.")]] VectorXd get_as() const; diff --git a/include/dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h b/include/dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h index 3795ecd..d48f77d 100644 --- a/include/dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h +++ b/include/dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h @@ -23,6 +23,7 @@ This file is part of DQ Robotics. 2. Juan Jose Quiroz Omana (juanjqogm@gmail.com) - Added methods to get and set the DH parameters. + - Added the get_supported_joint_types() method. */ #include @@ -48,13 +49,16 @@ class DQ_SerialManipulatorMDH: public DQ_SerialManipulator const int& to_ith_link, const double& parameter); + std::vector get_supported_joint_types()const override; + // Deprecated on 22.04, will be removed on the next release. - enum [[deprecated("Use ? instead.")]] JOINT_TYPES{ JOINT_ROTATIONAL=0, JOINT_PRISMATIC }; - [[deprecated("Use ? instead.")]] VectorXd get_thetas() const; - [[deprecated("Use ? instead.")]] VectorXd get_ds() const; - [[deprecated("Use ? instead.")]] VectorXd get_as() const; - [[deprecated("Use ? instead.")]] VectorXd get_alphas() const; - [[deprecated("Use ? instead.")]] VectorXd get_types() const; + enum [[deprecated("Use DQ_JointType instead.")]] JOINT_TYPES{ JOINT_ROTATIONAL=0, JOINT_PRISMATIC }; + + [[deprecated("Use get_parameters(DQ_ParameterDH::THETA) instead.")]] VectorXd get_thetas() const; + [[deprecated("Use get_parameters(DQ_ParameterDH::D) instead.")]] VectorXd get_ds() const; + [[deprecated("Use get_parameters(DQ_ParameterDH::A) instead.")]] VectorXd get_as() const; + [[deprecated("Use get_parameters(DQ_ParameterDH::ALPHA) instead.")]] VectorXd get_alphas() const; + [[deprecated("Use get_joint_types() instead.")]] VectorXd get_types() const; DQ_SerialManipulatorMDH()=delete; DQ_SerialManipulatorMDH(const MatrixXd& mdh_matrix); diff --git a/src/robot_modeling/DQ_SerialManipulator.cpp b/src/robot_modeling/DQ_SerialManipulator.cpp index cc2192a..10520bc 100644 --- a/src/robot_modeling/DQ_SerialManipulator.cpp +++ b/src/robot_modeling/DQ_SerialManipulator.cpp @@ -18,7 +18,12 @@ This file is part of DQ Robotics. Contributors: 1. Murilo M. Marinho (murilomarinho@ieee.org) + 2. Mateus Rodrigues Martins (martinsrmateus@gmail.com) + +3. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) + - Added the joint_types member, and the following methods: + _check_joint_types(), and {set,get}_joint_{type, types}. */ #include @@ -40,6 +45,47 @@ DQ_SerialManipulator::DQ_SerialManipulator(const int &dim_configuration_space): dim_configuration_space_ = dim_configuration_space; } +/** + * @brief DQ_SerialManipulator::_check_joint_types throws an exception if the joint types are + * different from the supported joints. + */ +void DQ_SerialManipulator::_check_joint_types() const +{ + std::vector types = get_joint_types(); + std::vector supported_types = get_supported_joint_types(); + std::string msg = "Unsupported joint types. Use valid joint types: "; + std::string msg_type; + std::string ps; + size_t k = supported_types.size(); + size_t n = types.size(); + for (size_t i=0;i DQ_SerialManipulator::get_joint_types() const +{ + return joint_types_; +} + +/** + * @brief DQ_SerialManipulator::set_joint_type sets the joint type of the ith joint + * @param joint_type The joint_type. + * @param ith_joint The index to a joint. + */ +void DQ_SerialManipulator::set_joint_type(const DQ_JointType &joint_type, const int &ith_joint) +{ + _check_to_ith_link(ith_joint); + joint_types_.at(ith_joint) = joint_type; + _check_joint_types(); +} + +/** + * @brief DQ_SerialManipulator::set_joint_types sets the joint types. + * @param joint_types A vector containing the joint types. + */ +void DQ_SerialManipulator::set_joint_types(const std::vector &joint_types) +{ + joint_types_ = joint_types; + _check_joint_types(); +} + +/** + * @brief DQ_SerialManipulator::set_joint_types sets the joint types. + * @param joint_types A vector containing the joint types. + */ +void DQ_SerialManipulator::set_joint_types(const VectorXd &joint_types) +{ + for (int i=0;i @@ -54,6 +55,7 @@ DQ_SerialManipulatorDH::DQ_SerialManipulatorDH(const MatrixXd& dh_matrix): throw(std::range_error("Bad DQ_SerialManipulatorDH(dh_matrix) call: dh_matrix should be 5xn")); } dh_matrix_ = dh_matrix; + set_joint_types(dh_matrix.row(4)); } /** @@ -198,6 +200,15 @@ void DQ_SerialManipulatorDH::set_parameter(const DQ_ParameterDH ¶meter_type, } } +/** + * @brief DQ_SerialManipulatorDH::get_supported_joint_types gets the supported joint types. + * @return A vector containing the supported joint types. + */ +std::vector DQ_SerialManipulatorDH::get_supported_joint_types() const +{ + return {DQ_JointType::REVOLUTE, DQ_JointType::PRISMATIC}; +} + /** * @brief This protected method computes the dual quaternion related with the time derivative of the diff --git a/src/robot_modeling/DQ_SerialManipulatorDenso.cpp b/src/robot_modeling/DQ_SerialManipulatorDenso.cpp index 71eee93..f49741e 100644 --- a/src/robot_modeling/DQ_SerialManipulatorDenso.cpp +++ b/src/robot_modeling/DQ_SerialManipulatorDenso.cpp @@ -1,3 +1,29 @@ +/** +(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. Murilo M. Marinho (murilomarinho@ieee.org) + - Responsible for the original implementation. + +2. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) + - Added the get_supported_joint_types() method. +*/ + #include namespace DQ_robotics { @@ -10,6 +36,7 @@ DQ_SerialManipulatorDenso::DQ_SerialManipulatorDenso(const MatrixXd& denso_matri throw(std::range_error("Bad DQ_SerialManipulatorDenso(MatrixXd) call: denso_matrix should be 6xn")); } denso_matrix_ = denso_matrix; + set_joint_types(std::vector(get_dim_configuration_space(), DQ_JointType::REVOLUTE)); } DQ DQ_SerialManipulatorDenso::_denso2dh(const double &q, const int &ith) const @@ -29,6 +56,15 @@ DQ DQ_SerialManipulatorDenso::_denso2dh(const double &q, const int &ith) const return z_rot*q_t*q_alpha*q_beta; } +/** + * @brief DQ_SerialManipulatorDenso::get_supported_joint_types gets the supported joint types. + * @return A vector containing the supported joint types. + */ +std::vector DQ_SerialManipulatorDenso::get_supported_joint_types() const +{ + return {DQ_JointType::REVOLUTE}; +} + VectorXd DQ_SerialManipulatorDenso::get_as() const { return denso_matrix_.row(0); diff --git a/src/robot_modeling/DQ_SerialManipulatorMDH.cpp b/src/robot_modeling/DQ_SerialManipulatorMDH.cpp index 3196355..a2b6ddd 100644 --- a/src/robot_modeling/DQ_SerialManipulatorMDH.cpp +++ b/src/robot_modeling/DQ_SerialManipulatorMDH.cpp @@ -20,8 +20,9 @@ This file is part of DQ Robotics. 1. Murilo M. Marinho (murilomarinho@ieee.org) - Responsible for the original implementation. -2. Juan Jose Quiroz Omana (juanjqogm@gmail.com) +2. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) - Added methods to get and set the DH parameters. + - Added the get_supported_joint_types() method. */ #include @@ -57,6 +58,7 @@ DQ_SerialManipulatorMDH::DQ_SerialManipulatorMDH(const MatrixXd& mdh_matrix): throw(std::range_error("Bad DQ_SerialManipulatorDH(mdh_matrix) call: mdh_matrix should be 5xn")); } mdh_matrix_ = mdh_matrix; + set_joint_types(mdh_matrix.row(4)); } /** @@ -209,6 +211,16 @@ void DQ_SerialManipulatorMDH::set_parameter(const DQ_ParameterDH ¶meter_type } } +/** + * @brief DQ_SerialManipulatorMDH::get_supported_joint_types gets the supported joint types. + * @return A vector containing the supported joint types. + */ +std::vector DQ_SerialManipulatorMDH::get_supported_joint_types() const +{ + return {DQ_JointType::REVOLUTE, DQ_JointType::PRISMATIC}; +} + + /** * @brief This protected method computes the dual quaternion related with the time derivative of the * unit dual quaternion pose using the MDH convention.