Skip to content

Commit 68f21ab

Browse files
juanjqojuanjqo
authored
[DQ_SerialManipulator and subclasses] Add {get,set}_parameters to interact with DH parameters (#69)
* [DQ_Kinematics.h Added the DQ_ParameterDH enum class. * [DQ_SerialManipulatorDH.{h, .cpp}] Added methods to get and set the DH parameters. * [DQ_SerialManipulatorMDH.h] Updated the description of the contributions in the header * [DQ_SerialManipulatorDH.h] Updated the description of the contributions in the header * [DQ_SerialManipulatorMDH.cpp] Updated the description of the contributions in the header * Updated the copyright year * [DQ_Kinematics.h, DQ_SerialManipulator.{h,cpp}] Moved the DQ_ParameterDH enum class from DQ_Kinematics to DQ_SerialManipulator * [DQ_Kinematics] Removed all modifications in this class * [DQ_Kinematics.pp] Removed all modifications in this class * [DQ_ParameterDH.h] Added a new enum class for the DH parameters * [DQ_SerialManipulator.{h,cpp}] Removed the nested enum class related to the DH parameters. This class is now a standalone class. * [DQ_SerialManipulator{DH,MDH}.h] Added the DQ_ParameterDH class in the header. * [DQ_SerialManipulatorDH,MDH.cpp] Added the default case in the get_parameter and get_parameters methods to check if this fixes the compilation on Ubuntu. * [DQ_ParameterDH.h] Modified the class to support string and char constructors, as discussed in #69. * [DQ_ParameterDH.h] Updated the documentation of the class.
1 parent 83f5eb7 commit 68f21ab

File tree

7 files changed

+340
-19
lines changed

7 files changed

+340
-19
lines changed

‎include/dqrobotics/robot_modeling/DQ_ParameterDH.h‎

Lines changed: 101 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,101 @@
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+
#include <unordered_map>
25+
#include <iostream>
26+
#pragma once
27+
28+
namespace DQ_robotics
29+
{
30+
class DQ_ParameterDH
31+
{
32+
public:
33+
enum PARAMETER{
34+
THETA,
35+
D,
36+
A,
37+
ALPHA
38+
};
39+
private:
40+
PARAMETER parameter_;
41+
const std::unordered_map<std::string, PARAMETER>
42+
map_ = {{"THETA", THETA},
43+
{"D" , D},
44+
{"A" , A},
45+
{"ALPHA", ALPHA},
46+
};
47+
48+
/**
49+
* @brief _get_parameter sets the parameter member using a string as argument.
50+
* @param parameter The desired parameter to be set. Example: "THETA", "D", "A", or "ALPHA".
51+
*/
52+
void _set_parameter(const std::string& parameter)
53+
{
54+
try {
55+
parameter_ = map_.at(parameter);
56+
} catch (...) {
57+
throw std::runtime_error("The parameter \""+ parameter+ "\" is not supported. Use THETA, D, A, or ALPHA");
58+
}
59+
}
60+
public:
61+
/**
62+
* @brief DQ_ParameterDH Default constructor method.
63+
*/
64+
DQ_ParameterDH() = default;
65+
66+
/**
67+
* @brief DQ_ParameterDH Constructor method
68+
* @param parameter The desired DH parameter. Example: THETA, D, A, or ALPHA.
69+
*/
70+
DQ_ParameterDH(const PARAMETER& parameter): parameter_{parameter}{};
71+
72+
// This definition enables switch cases and comparisons.
73+
constexpr operator PARAMETER() const { return parameter_; }
74+
75+
/**
76+
* @brief DQ_ParameterDH Constructor method that allows string parameters.
77+
* This is done to keep the language compatibility between
78+
* Matlab and Python/C++, as discussed in
79+
* https://github.com/dqrobotics/cpp/pull/69
80+
* @param parameter The desired DH parameter. Example: "THETA", "D", "A", or "ALPHA".
81+
*/
82+
DQ_ParameterDH(const std::string& parameter){
83+
_set_parameter(parameter);
84+
}
85+
86+
87+
/**
88+
* @brief DQ_ParameterDH Constructor method that allows char parameters.
89+
* This is done to keep the language compatibility between
90+
* Matlab and Python/C++, as discussed in
91+
* https://github.com/dqrobotics/cpp/pull/69
92+
* @param parameter_c The desired DH parameter. Example: "THETA", "D", "A", or "ALPHA".
93+
*/
94+
DQ_ParameterDH(const char* parameter_c){
95+
_set_parameter(parameter_c);
96+
}
97+
98+
};
99+
}
100+
101+

‎include/dqrobotics/robot_modeling/DQ_SerialManipulator.h‎

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -18,9 +18,8 @@ This file is part of DQ Robotics.
1818
along with DQ Robotics. If not, see <http://www.gnu.org/licenses/>.
1919
2020
Contributors:
21-
- Murilo M. Marinho ([email protected])
22-
- Mateus Rodrigues Martins ([email protected])
23-
- Juan Jose Quiroz Omana ([email protected])
21+
1. Murilo M. Marinho ([email protected])
22+
2. Mateus Rodrigues Martins ([email protected])
2423
*/
2524

2625
#include <dqrobotics/robot_modeling/DQ_Kinematics.h>

‎include/dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h‎

Lines changed: 15 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
#pragma once
22
/**
3-
(C) Copyright 2020-2022 DQ Robotics Developers
3+
(C) Copyright 2011-2025 DQ Robotics Developers
44
55
This file is part of DQ Robotics.
66
@@ -18,12 +18,16 @@ This file is part of DQ Robotics.
1818
along with DQ Robotics. If not, see <http://www.gnu.org/licenses/>.
1919
2020
Contributors:
21-
- Murilo M. Marinho ([email protected])
22-
- Juan Jose Quiroz Omana ([email protected])
21+
1. Murilo M. Marinho ([email protected])
22+
- Responsible for the original implementation.
23+
24+
2. Juan Jose Quiroz Omana ([email protected])
25+
- Added methods to get and set the DH parameters.
2326
*/
2427

2528

2629
#include <dqrobotics/robot_modeling/DQ_SerialManipulator.h>
30+
#include <dqrobotics/robot_modeling/DQ_ParameterDH.h>
2731

2832
namespace DQ_robotics
2933
{
@@ -36,6 +40,14 @@ class DQ_SerialManipulatorDH: public DQ_SerialManipulator
3640
DQ _get_w(const int& ith) const;
3741
DQ _dh2dq(const double& q, const int& ith) const;
3842
public:
43+
VectorXd get_parameters(const DQ_ParameterDH& parameter_type) const;
44+
double get_parameter(const DQ_ParameterDH& parameter_type,
45+
const int& to_ith_link) const;
46+
void set_parameters(const DQ_ParameterDH& parameter_type,
47+
const VectorXd& vector_parameters);
48+
void set_parameter(const DQ_ParameterDH& parameter_type,
49+
const int& to_ith_link,
50+
const double& parameter);
3951

4052
// Deprecated on 22.04, will be removed on the next release.
4153
enum [[deprecated("Use ? instead.")]] JOINT_TYPES{ JOINT_ROTATIONAL=0, JOINT_PRISMATIC };

‎include/dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h‎

Lines changed: 15 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
#pragma once
22
/**
3-
(C) Copyright 2022 DQ Robotics Developers
3+
(C) Copyright 2011-2025 DQ Robotics Developers
44
55
This file is part of DQ Robotics.
66
@@ -18,11 +18,15 @@ This file is part of DQ Robotics.
1818
along with DQ Robotics. If not, see <http://www.gnu.org/licenses/>.
1919
2020
Contributors:
21-
- Murilo M. Marinho ([email protected])
22-
- Juan Jose Quiroz Omana - [email protected]
21+
1. Murilo M. Marinho ([email protected])
22+
- Responsible for the original implementation.
23+
24+
2. Juan Jose Quiroz Omana ([email protected])
25+
- Added methods to get and set the DH parameters.
2326
*/
2427

2528
#include <dqrobotics/robot_modeling/DQ_SerialManipulator.h>
29+
#include <dqrobotics/robot_modeling/DQ_ParameterDH.h>
2630

2731
namespace DQ_robotics
2832
{
@@ -35,6 +39,14 @@ class DQ_SerialManipulatorMDH: public DQ_SerialManipulator
3539
DQ _get_w(const int& ith) const;
3640
DQ _mdh2dq(const double& q, const int& ith) const;
3741
public:
42+
VectorXd get_parameters(const DQ_ParameterDH& parameter_type) const;
43+
double get_parameter(const DQ_ParameterDH& parameter_type,
44+
const int& to_ith_link) const;
45+
void set_parameters(const DQ_ParameterDH& parameter_type,
46+
const VectorXd& vector_parameters);
47+
void set_parameter(const DQ_ParameterDH& parameter_type,
48+
const int& to_ith_link,
49+
const double& parameter);
3850

3951
// Deprecated on 22.04, will be removed on the next release.
4052
enum [[deprecated("Use ? instead.")]] JOINT_TYPES{ JOINT_ROTATIONAL=0, JOINT_PRISMATIC };

‎src/robot_modeling/DQ_SerialManipulator.cpp‎

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
/**
2-
(C) Copyright 2011-2020 DQ Robotics Developers
2+
(C) Copyright 2011-2025 DQ Robotics Developers
33
44
This file is part of DQ Robotics.
55
@@ -17,9 +17,8 @@ This file is part of DQ Robotics.
1717
along with DQ Robotics. If not, see <http://www.gnu.org/licenses/>.
1818
1919
Contributors:
20-
- Murilo M. Marinho ([email protected])
21-
- Mateus Rodrigues Martins ([email protected])
22-
- Juan Jose Quiroz Omana ([email protected])
20+
1. Murilo M. Marinho ([email protected])
21+
2. Mateus Rodrigues Martins ([email protected])
2322
*/
2423

2524
#include <dqrobotics/robot_modeling/DQ_SerialManipulator.h>

‎src/robot_modeling/DQ_SerialManipulatorDH.cpp‎

Lines changed: 102 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
/**
2-
(C) Copyright 2020-2022 DQ Robotics Developers
2+
(C) Copyright 2011-2025 DQ Robotics Developers
33
44
This file is part of DQ Robotics.
55
@@ -17,8 +17,11 @@ This file is part of DQ Robotics.
1717
along with DQ Robotics. If not, see <http://www.gnu.org/licenses/>.
1818
1919
Contributors:
20-
- Murilo M. Marinho ([email protected])
21-
- Juan Jose Quiroz Omana ([email protected])
20+
1. Murilo M. Marinho ([email protected])
21+
- Responsible for the original implementation.
22+
23+
2. Juan Jose Quiroz Omana ([email protected])
24+
- Added methods to get and set the DH parameters.
2225
*/
2326

2427
#include <dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h>
@@ -99,6 +102,102 @@ DQ DQ_SerialManipulatorDH::_dh2dq(const double &q, const int &ith) const
99102
);
100103
}
101104

105+
/**
106+
* @brief DQ_SerialManipulatorDH::get_parameters returns a vector containing the DH parameters.
107+
* @param parameter_type Parameter type, which which corresponds to THETA, D, A, or ALPHA.
108+
* @return A vector containing the desired DH parameters.
109+
*/
110+
VectorXd DQ_SerialManipulatorDH::get_parameters(const DQ_ParameterDH &parameter_type) const
111+
{
112+
switch (parameter_type) {
113+
case DQ_ParameterDH::THETA:
114+
return dh_matrix_.row(0);
115+
case DQ_ParameterDH::D:
116+
return dh_matrix_.row(1);
117+
case DQ_ParameterDH::A:
118+
return dh_matrix_.row(2);
119+
case DQ_ParameterDH::ALPHA:
120+
return dh_matrix_.row(3);
121+
default:
122+
throw std::runtime_error("Wrong type of parameter");
123+
}
124+
}
125+
126+
/**
127+
* @brief DQ_SerialManipulatorDH::get_parameter returns the DH parameter of the ith joint.
128+
* @param parameter_type Parameter type, which which corresponds to THETA, D, A, or ALPHA.
129+
* @param to_ith_link The joint number.
130+
* @return The desired DH parameter.
131+
*/
132+
double DQ_SerialManipulatorDH::get_parameter(const DQ_ParameterDH &parameter_type,
133+
const int &to_ith_link) const
134+
{
135+
_check_to_ith_link(to_ith_link);
136+
switch (parameter_type) {
137+
case DQ_ParameterDH::THETA:
138+
return dh_matrix_(0, to_ith_link);
139+
case DQ_ParameterDH::D:
140+
return dh_matrix_(1, to_ith_link);
141+
case DQ_ParameterDH::A:
142+
return dh_matrix_(2, to_ith_link);
143+
case DQ_ParameterDH::ALPHA:
144+
return dh_matrix_(3, to_ith_link);
145+
default:
146+
throw std::runtime_error("Wrong type of parameter");
147+
}
148+
}
149+
150+
/**
151+
* @brief DQ_SerialManipulatorDH::set_parameters sets the DH parameters.
152+
* @param parameter_type Parameter type, which which corresponds to THETA, D, A, or ALPHA.
153+
* @param vector_parameters A vector containing the new parameters.
154+
*/
155+
void DQ_SerialManipulatorDH::set_parameters(const DQ_ParameterDH &parameter_type,
156+
const VectorXd &vector_parameters)
157+
{
158+
_check_q_vec(vector_parameters);
159+
switch (parameter_type) {
160+
case DQ_ParameterDH::THETA:
161+
dh_matrix_.row(0) = vector_parameters;
162+
break;
163+
case DQ_ParameterDH::D:
164+
dh_matrix_.row(1) = vector_parameters;
165+
break;
166+
case DQ_ParameterDH::A:
167+
dh_matrix_.row(2) = vector_parameters;
168+
break;
169+
case DQ_ParameterDH::ALPHA:
170+
dh_matrix_.row(3) = vector_parameters;
171+
break;
172+
}
173+
}
174+
175+
/**
176+
* @brief DQ_SerialManipulatorDH::set_parameter sets the DH parameter of the ith joint.
177+
* @param parameter_type Parameter type, which which corresponds to THETA, D, A, or ALPHA.
178+
* @param to_ith_link The joint number.
179+
* @param parameter The new parameter.
180+
*/
181+
void DQ_SerialManipulatorDH::set_parameter(const DQ_ParameterDH &parameter_type,
182+
const int &to_ith_link, const double &parameter)
183+
{
184+
_check_to_ith_link(to_ith_link);
185+
switch (parameter_type) {
186+
case DQ_ParameterDH::THETA:
187+
dh_matrix_(0, to_ith_link) = parameter;
188+
break;
189+
case DQ_ParameterDH::D:
190+
dh_matrix_(1, to_ith_link) = parameter;
191+
break;
192+
case DQ_ParameterDH::A:
193+
dh_matrix_(2, to_ith_link) = parameter;
194+
break;
195+
case DQ_ParameterDH::ALPHA:
196+
dh_matrix_(3, to_ith_link) = parameter;
197+
break;
198+
}
199+
}
200+
102201

103202
/**
104203
* @brief This protected method computes the dual quaternion related with the time derivative of the

0 commit comments

Comments
 (0)