Skip to content

Commit b970d90

Browse files
juanjqojuanjqo
authored
Add DQ_JointType class, setters, and getters in DQ_SerialManipulator. (#70)
* [DQ_JointType] Added a new class to represent the joint types. * [DQ_JointType.h] Updated the class to support switch and boolean comparisons * [DQ_SerialManipulator.{h,cpp}] Added the _check_joint_types(), and {set,get}_joint_{type, types} methods. * [DQ_SerialManipulatorDenso] Added the get_supported_joint_types() method * [DQ_SerialManipulatorDH.{h,cpp}] Added the get_supported_joint_types() method * [DQ_SerialManipulatorMDH.{h,cpp}] Added the get_supported_joint_types() method * [DQ_JointType] Added a new constructor with integer arguments * [DQ_SerialManipulator.h,cpp] Added a overloaded method for set_joint_types to accept eigen vectors. * [DQ_SerialManipulatorDH.cpp] Updated the constructor to set the joint types * [DQ_SerialManipulatorMDH.cpp] Updated the constructor to set the joint types * [DQ_SerialManipulator.cpp] Fixed typo in _check_joint_types() * [DQ_SerialManipulatorDenso.cpp] Updated the constructor to set the joint types * [DQ_JoinType.h] Fixed typo in the documentation * [DQ_SerialManipulatorDenso.h] Added missing space in the method definition * [DQ_JointType.h] Updated the documentation of the constructors including the paper in which the class is based on. * [DQ_SerialManipulator.h] include the vector header to check if it solves the compilation on Ubuntu. * [DQ_JointType.h, DQ_SerialManipulatorc.pp] Renamed the ToString method to to_string in the class DQ_JointType and updated the class DQ_SerialManipulator to comply with this modification.
1 parent e40b852 commit b970d90

9 files changed

+328
-18
lines changed

‎include/dqrobotics/robot_modeling/DQ_JointType.h‎

Lines changed: 125 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,125 @@
1+
/**
2+
(C) Copyright 2011-2025 DQ Robotics Developers
3+
4+
This file is part of DQ Robotics.
5+
6+
DQ Robotics is free software: you can redistribute it and/or modify
7+
it under the terms of the GNU Lesser General Public License as published by
8+
the Free Software Foundation, either version 3 of the License, or
9+
(at your option) any later version.
10+
11+
DQ Robotics is distributed in the hope that it will be useful,
12+
but WITHOUT ANY WARRANTY; without even the implied warranty of
13+
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14+
GNU Lesser General Public License for more details.
15+
16+
You should have received a copy of the GNU Lesser General Public License
17+
along with DQ Robotics. If not, see <http://www.gnu.org/licenses/>.
18+
19+
Contributors:
20+
1. Juan Jose Quiroz Omana ([email protected])
21+
- Responsible for the original implementation.
22+
*/
23+
24+
25+
#pragma once
26+
#include <string>
27+
28+
namespace DQ_robotics
29+
{
30+
class DQ_JointType
31+
{
32+
public:
33+
enum JOINT_TYPE{
34+
REVOLUTE = 0,
35+
PRISMATIC,
36+
SPHERICAL,
37+
CYLINDRICAL,
38+
PLANAR,
39+
SIX_DOF,
40+
HELICAL
41+
};
42+
// This definition enables switch cases and comparisons.
43+
constexpr operator JOINT_TYPE() const { return joint_type_; }
44+
private:
45+
JOINT_TYPE joint_type_;
46+
47+
public:
48+
/**
49+
* @brief DQ_JointType Default constructor method.
50+
* This class is based on Table 1 of Silva, Quiroz-Omaña, and Adorno (2022).
51+
* Dynamics of Mobile Manipulators Using Dual Quaternion Algebra.
52+
*/
53+
DQ_JointType() = default;
54+
55+
/**
56+
* @brief DQ_JointType Constructor method.
57+
* This class is based on Table 1 of Silva, Quiroz-Omaña, and Adorno (2022).
58+
* Dynamics of Mobile Manipulators Using Dual Quaternion Algebra.
59+
* @param joint_type The joint type. Example: REVOLUTE, PRISMATIC,
60+
* SPHERICAL, CYLINDRICAL, PLANAR, SIX_DOF, or HELICAL.
61+
*/
62+
DQ_JointType(const JOINT_TYPE& joint_type): joint_type_{joint_type}{};
63+
64+
/**
65+
* @brief DQ_JointType Constructor method that allows integer arguments.
66+
* This class is based on Table 1 of Silva, Quiroz-Omaña, and Adorno (2022).
67+
* Dynamics of Mobile Manipulators Using Dual Quaternion Algebra
68+
* @param joint_type The joint type.
69+
*/
70+
DQ_JointType(const int& joint_type){
71+
switch (joint_type) {
72+
case 0:
73+
joint_type_ = REVOLUTE;
74+
break;
75+
case 1:
76+
joint_type_ = PRISMATIC;
77+
break;
78+
case 2:
79+
joint_type_ = SPHERICAL;
80+
break;
81+
case 3:
82+
joint_type_ = CYLINDRICAL;
83+
break;
84+
case 4:
85+
joint_type_ = PLANAR;
86+
break;
87+
case 5:
88+
joint_type_ = SIX_DOF;
89+
break;
90+
case 6:
91+
joint_type_ = HELICAL;
92+
break;
93+
default:
94+
throw std::runtime_error("Invalid joint type");
95+
}
96+
}
97+
98+
/**
99+
* @brief to_string() converts the DQ_JointType to string.
100+
* @return A string that corresponds to the joint type.
101+
*/
102+
std::string to_string() const {
103+
switch (joint_type_) {
104+
105+
case REVOLUTE:
106+
return std::string("REVOLUTE");
107+
case PRISMATIC:
108+
return std::string("PRISMATIC");
109+
case SPHERICAL:
110+
return std::string("SPHERICAL");
111+
case CYLINDRICAL:
112+
return std::string("CYLINDRICAL");
113+
case PLANAR:
114+
return std::string("PLANAR");
115+
case SIX_DOF:
116+
return std::string("SIX_DOF");
117+
case HELICAL:
118+
return std::string("HELICAL");
119+
default:
120+
throw std::runtime_error("Invalid joint type");
121+
}
122+
}
123+
};
124+
125+
}

‎include/dqrobotics/robot_modeling/DQ_SerialManipulator.h‎

Lines changed: 15 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
#pragma once
22
/**
3-
(C) Copyright 2011-2022 DQ Robotics Developers
3+
(C) Copyright 2011-2025 DQ Robotics Developers
44
55
This file is part of DQ Robotics.
66
@@ -20,9 +20,15 @@ This file is part of DQ Robotics.
2020
Contributors:
2121
1. Murilo M. Marinho ([email protected])
2222
2. Mateus Rodrigues Martins ([email protected])
23+
24+
3. Juan Jose Quiroz Omana ([email protected])
25+
- Added the joint_types member, and the following methods:
26+
_check_joint_types(), and {set,get}_joint_{type, types}.
2327
*/
2428

2529
#include <dqrobotics/robot_modeling/DQ_Kinematics.h>
30+
#include <dqrobotics/robot_modeling/DQ_JointType.h>
31+
#include <vector>
2632

2733
namespace DQ_robotics
2834
{
@@ -31,8 +37,9 @@ class DQ_SerialManipulator: public DQ_Kinematics
3137
{
3238
protected:
3339
DQ curr_effector_;
34-
40+
std::vector<DQ_JointType> joint_types_;
3541
DQ_SerialManipulator(const int& dofs);
42+
void _check_joint_types() const;
3643
public:
3744
DQ get_effector() const;
3845
DQ set_effector(const DQ& new_effector);
@@ -46,6 +53,11 @@ class DQ_SerialManipulator: public DQ_Kinematics
4653
VectorXd get_upper_q_dot_limit() const;
4754
void set_upper_q_dot_limit(const VectorXd &upper_q_dot_limit);
4855

56+
DQ_JointType get_joint_type(const int& ith_joint) const;
57+
std::vector<DQ_JointType> get_joint_types() const;
58+
void set_joint_type(const DQ_JointType& joint_type, const int& ith_joint);
59+
void set_joint_types(const std::vector<DQ_JointType>& joint_types);
60+
void set_joint_types(const VectorXd& joint_types);
4961

5062
//Virtual
5163
virtual MatrixXd raw_pose_jacobian(const VectorXd& q_vec) const;
@@ -56,6 +68,7 @@ class DQ_SerialManipulator: public DQ_Kinematics
5668
virtual MatrixXd raw_pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const = 0;
5769
virtual MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const = 0;
5870
virtual DQ raw_fkm(const VectorXd& q_vec, const int& to_ith_link) const = 0;
71+
virtual std::vector<DQ_JointType> get_supported_joint_types() const = 0;
5972

6073
//Overrides from DQ_Kinematics
6174
virtual DQ fkm(const VectorXd& q_vec) const override; //Override from DQ_Kinematics

‎include/dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h‎

Lines changed: 10 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@ This file is part of DQ Robotics.
2323
2424
2. Juan Jose Quiroz Omana ([email protected])
2525
- Added methods to get and set the DH parameters.
26+
- Added the get_supported_joint_types() method.
2627
*/
2728

2829

@@ -49,13 +50,16 @@ class DQ_SerialManipulatorDH: public DQ_SerialManipulator
4950
const int& to_ith_link,
5051
const double& parameter);
5152

53+
std::vector<DQ_JointType> get_supported_joint_types()const override;
54+
5255
// Deprecated on 22.04, will be removed on the next release.
53-
enum [[deprecated("Use ? instead.")]] JOINT_TYPES{ JOINT_ROTATIONAL=0, JOINT_PRISMATIC };
54-
[[deprecated("Use ? instead.")]] VectorXd get_thetas() const;
55-
[[deprecated("Use ? instead.")]] VectorXd get_ds() const;
56-
[[deprecated("Use ? instead.")]] VectorXd get_as() const;
57-
[[deprecated("Use ? instead.")]] VectorXd get_alphas() const;
58-
[[deprecated("Use ? instead.")]] VectorXd get_types() const;
56+
enum [[deprecated("Use DQ_JointType instead.")]] JOINT_TYPES{ JOINT_ROTATIONAL=0, JOINT_PRISMATIC };
57+
58+
[[deprecated("Use get_parameters(DQ_ParameterDH::THETA) instead.")]] VectorXd get_thetas() const;
59+
[[deprecated("Use get_parameters(DQ_ParameterDH::D) instead.")]] VectorXd get_ds() const;
60+
[[deprecated("Use get_parameters(DQ_ParameterDH::A) instead.")]] VectorXd get_as() const;
61+
[[deprecated("Use get_parameters(DQ_ParameterDH::ALPHA) instead.")]] VectorXd get_alphas() const;
62+
[[deprecated("Use get_joint_types() instead.")]] VectorXd get_types() const;
5963

6064
DQ_SerialManipulatorDH()=delete;
6165
DQ_SerialManipulatorDH(const MatrixXd& dh_matrix);

‎include/dqrobotics/robot_modeling/DQ_SerialManipulatorDenso.h‎

Lines changed: 8 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
#pragma once
22
/**
3-
(C) Copyright 2021-2022 DQ Robotics Developers
3+
(C) Copyright 2011-2025 DQ Robotics Developers
44
55
This file is part of DQ Robotics.
66
@@ -17,8 +17,13 @@ This file is part of DQ Robotics.
1717
You should have received a copy of the GNU Lesser General Public License
1818
along with DQ Robotics. If not, see <http://www.gnu.org/licenses/>.
1919
20+
2021
Contributors:
21-
- Murilo M. Marinho ([email protected])
22+
1. Murilo M. Marinho ([email protected])
23+
- Responsible for the original implementation.
24+
25+
2. Juan Jose Quiroz Omana ([email protected])
26+
- Added the get_supported_joint_types() method.
2227
*/
2328

2429
#include <dqrobotics/robot_modeling/DQ_SerialManipulator.h>
@@ -33,6 +38,7 @@ class DQ_SerialManipulatorDenso: public DQ_SerialManipulator
3338

3439
DQ _denso2dh(const double& q, const int& ith) const;
3540
public:
41+
std::vector<DQ_JointType> get_supported_joint_types() const override;
3642

3743
// Deprecated on 22.04, will be removed on the next release.
3844
[[deprecated("Use ? instead.")]] VectorXd get_as() const;

‎include/dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h‎

Lines changed: 10 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@ This file is part of DQ Robotics.
2323
2424
2. Juan Jose Quiroz Omana ([email protected])
2525
- Added methods to get and set the DH parameters.
26+
- Added the get_supported_joint_types() method.
2627
*/
2728

2829
#include <dqrobotics/robot_modeling/DQ_SerialManipulator.h>
@@ -48,13 +49,16 @@ class DQ_SerialManipulatorMDH: public DQ_SerialManipulator
4849
const int& to_ith_link,
4950
const double& parameter);
5051

52+
std::vector<DQ_JointType> get_supported_joint_types()const override;
53+
5154
// Deprecated on 22.04, will be removed on the next release.
52-
enum [[deprecated("Use ? instead.")]] JOINT_TYPES{ JOINT_ROTATIONAL=0, JOINT_PRISMATIC };
53-
[[deprecated("Use ? instead.")]] VectorXd get_thetas() const;
54-
[[deprecated("Use ? instead.")]] VectorXd get_ds() const;
55-
[[deprecated("Use ? instead.")]] VectorXd get_as() const;
56-
[[deprecated("Use ? instead.")]] VectorXd get_alphas() const;
57-
[[deprecated("Use ? instead.")]] VectorXd get_types() const;
55+
enum [[deprecated("Use DQ_JointType instead.")]] JOINT_TYPES{ JOINT_ROTATIONAL=0, JOINT_PRISMATIC };
56+
57+
[[deprecated("Use get_parameters(DQ_ParameterDH::THETA) instead.")]] VectorXd get_thetas() const;
58+
[[deprecated("Use get_parameters(DQ_ParameterDH::D) instead.")]] VectorXd get_ds() const;
59+
[[deprecated("Use get_parameters(DQ_ParameterDH::A) instead.")]] VectorXd get_as() const;
60+
[[deprecated("Use get_parameters(DQ_ParameterDH::ALPHA) instead.")]] VectorXd get_alphas() const;
61+
[[deprecated("Use get_joint_types() instead.")]] VectorXd get_types() const;
5862

5963
DQ_SerialManipulatorMDH()=delete;
6064
DQ_SerialManipulatorMDH(const MatrixXd& mdh_matrix);

‎src/robot_modeling/DQ_SerialManipulator.cpp‎

Lines changed: 99 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,12 @@ This file is part of DQ Robotics.
1818
1919
Contributors:
2020
1. Murilo M. Marinho ([email protected])
21+
2122
2. Mateus Rodrigues Martins ([email protected])
23+
24+
3. Juan Jose Quiroz Omana ([email protected])
25+
- Added the joint_types member, and the following methods:
26+
_check_joint_types(), and {set,get}_joint_{type, types}.
2227
*/
2328

2429
#include <dqrobotics/robot_modeling/DQ_SerialManipulator.h>
@@ -40,6 +45,47 @@ DQ_SerialManipulator::DQ_SerialManipulator(const int &dim_configuration_space):
4045
dim_configuration_space_ = dim_configuration_space;
4146
}
4247

48+
/**
49+
* @brief DQ_SerialManipulator::_check_joint_types throws an exception if the joint types are
50+
* different from the supported joints.
51+
*/
52+
void DQ_SerialManipulator::_check_joint_types() const
53+
{
54+
std::vector<DQ_JointType> types = get_joint_types();
55+
std::vector<DQ_JointType> supported_types = get_supported_joint_types();
56+
std::string msg = "Unsupported joint types. Use valid joint types: ";
57+
std::string msg_type;
58+
std::string ps;
59+
size_t k = supported_types.size();
60+
size_t n = types.size();
61+
for (size_t i=0;i<k;i++)
62+
{
63+
msg_type = std::string("DQ_JointType::"+supported_types.at(i).to_string());
64+
if (i==k-1)
65+
ps = std::string(". ");
66+
else
67+
ps = std::string(", ");
68+
69+
msg += msg_type + ps;
70+
}
71+
72+
73+
for (size_t i=0;i<n;i++)
74+
{
75+
bool match = false;
76+
for (size_t j=0;j<k;j++)
77+
{
78+
if (types.at(i) == supported_types.at(j))
79+
{
80+
match = true;
81+
break;
82+
}
83+
}
84+
if (match == false)
85+
throw std::runtime_error(msg);
86+
}
87+
}
88+
4389

4490
int DQ_SerialManipulator::get_dim_configuration_space() const
4591
{
@@ -97,6 +143,59 @@ void DQ_SerialManipulator::set_upper_q_dot_limit(const VectorXd &upper_q_dot_lim
97143
upper_q_dot_limit_ = upper_q_dot_limit;
98144
}
99145

146+
/**
147+
* @brief DQ_SerialManipulator::get_joint_type returns the joint type of the ith joint.
148+
* @param ith_joint The index to a joint.
149+
* @return The desired ith joint type.
150+
*/
151+
DQ_JointType DQ_SerialManipulator::get_joint_type(const int &ith_joint) const
152+
{
153+
_check_to_ith_link(ith_joint);
154+
return joint_types_.at(ith_joint);
155+
}
156+
157+
/**
158+
* @brief DQ_SerialManipulator::get_joint_types returns a vector containing the joint types.
159+
* @return The desired joint types.
160+
*/
161+
std::vector<DQ_JointType> DQ_SerialManipulator::get_joint_types() const
162+
{
163+
return joint_types_;
164+
}
165+
166+
/**
167+
* @brief DQ_SerialManipulator::set_joint_type sets the joint type of the ith joint
168+
* @param joint_type The joint_type.
169+
* @param ith_joint The index to a joint.
170+
*/
171+
void DQ_SerialManipulator::set_joint_type(const DQ_JointType &joint_type, const int &ith_joint)
172+
{
173+
_check_to_ith_link(ith_joint);
174+
joint_types_.at(ith_joint) = joint_type;
175+
_check_joint_types();
176+
}
177+
178+
/**
179+
* @brief DQ_SerialManipulator::set_joint_types sets the joint types.
180+
* @param joint_types A vector containing the joint types.
181+
*/
182+
void DQ_SerialManipulator::set_joint_types(const std::vector<DQ_JointType> &joint_types)
183+
{
184+
joint_types_ = joint_types;
185+
_check_joint_types();
186+
}
187+
188+
/**
189+
* @brief DQ_SerialManipulator::set_joint_types sets the joint types.
190+
* @param joint_types A vector containing the joint types.
191+
*/
192+
void DQ_SerialManipulator::set_joint_types(const VectorXd &joint_types)
193+
{
194+
for (int i=0;i<joint_types.size();i++)
195+
joint_types_.push_back(DQ_JointType(joint_types(i)));
196+
_check_joint_types();
197+
}
198+
100199
DQ DQ_SerialManipulator::raw_fkm(const VectorXd& q_vec) const
101200
{
102201
_check_q_vec(q_vec);

0 commit comments

Comments
 (0)