Line data Source code
1 : #include "Kinematics.hpp"
2 :
3 : #include <iostream>
4 :
5 : namespace HLR
6 : {
7 : namespace Kinematics
8 : {
9 : using IKSolution = std::pair<std::optional<Matrix<double, 5, 1>>,
10 : std::optional<Matrix<double, 5, 1>>>;
11 :
12 205701 : std::optional<Matrix<double, 3, 4>> Kinematics::forward(
13 : Matrix<double, 5, 1> angles)
14 : {
15 1234198 : for (std::size_t i = 0; i < angles.get_m(); ++i)
16 : {
17 1028499 : if (!std::isfinite(angles[i][0]))
18 : {
19 2 : return std::nullopt;
20 : }
21 : }
22 :
23 : // Set theta 3 always be horizontal.
24 205699 : angles[3][0] = 90 - angles[1][0] - angles[2][0];
25 :
26 205699 : if (!Kinematics::angles_within_limits(angles))
27 : {
28 11563 : return std::nullopt;
29 : }
30 :
31 : // Convert angles to radians.
32 194136 : angles *= (pi / 180);
33 :
34 : /*
35 : * keep names short because the formules below are unreadable with long
36 : * variable names.
37 : * p are the end positions of each link.
38 : * a are the thetas in radians
39 : * l are the joint lengths
40 : */
41 194136 : Matrix<double, 3, 4> p;
42 194136 : const auto& a = angles;
43 194136 : const auto& l = joint_lengths;
44 :
45 194136 : p[0][0] = 0.0;
46 194136 : p[1][0] = 0.0;
47 194136 : p[2][0] = 0.0;
48 :
49 194136 : p[0][1] = p[0][0] + (l[0][0] * std::sin(a[1][0]) * std::cos(a[0][0]));
50 194136 : p[1][1] = p[1][0] + (l[0][0] * std::cos(a[1][0]));
51 194136 : p[2][1] = p[2][0] + (l[0][0] * std::sin(a[1][0]) * std::sin(a[0][0]));
52 :
53 194136 : p[0][2] =
54 194136 : p[0][1] + (l[1][0] * std::sin(a[1][0] + a[2][0]) * std::cos(a[0][0]));
55 194136 : p[1][2] = p[1][1] + (l[1][0] * std::cos(a[1][0] + a[2][0]));
56 194136 : p[2][2] =
57 194136 : p[2][1] + (l[1][0] * std::sin(a[1][0] + a[2][0]) * std::sin(a[0][0]));
58 :
59 194136 : p[0][3] = p[0][2] + (l[2][0] * std::cos(a[0][0]));
60 194136 : p[1][3] = p[1][2];
61 194136 : p[2][3] = p[2][2] + (l[2][0] * std::sin(a[0][0]));
62 :
63 194136 : return p;
64 : }
65 :
66 10 : IKSolution Kinematics::jacobian_inverse(Matrix<double, 5, 1> current_angles,
67 : const Matrix<double, 3, 1>& goal)
68 : {
69 10 : IKSolution solutions;
70 :
71 : // Check if solution is in worksapce.
72 : static const double max_arm_length =
73 : joint_lengths[0][0] + joint_lengths[1][0] + joint_lengths[2][0];
74 10 : if (max_arm_length - goal.get_magnitude() <= 0)
75 : {
76 3 : return solutions;
77 : }
78 :
79 : // alias variables to the same names they have in the documentation.
80 7 : const auto& g = goal;
81 7 : auto new_angles = current_angles;
82 7 : double beta = 0.1;
83 :
84 7 : std::size_t i = 0;
85 7 : auto e_opt = forward(new_angles);
86 7 : auto e_opt_new = e_opt;
87 7 : if (!e_opt.has_value())
88 : {
89 1 : return solutions;
90 : }
91 6 : auto e = e_opt.value().template slice<0, 3, 3, 1>();
92 6 : auto e_new = e;
93 :
94 408666 : while ((g - e).get_magnitude() > precision && ++i <= max_iterations)
95 : {
96 204331 : const auto j_i_opt = compute_jacobian(current_angles).inverse();
97 204331 : if (!j_i_opt.has_value())
98 : {
99 1 : break;
100 : }
101 :
102 204330 : const auto& j_i = j_i_opt.value();
103 204330 : const auto d_e = beta * (g - e);
104 204330 : const auto d_angle = j_i * d_e;
105 204330 : new_angles = current_angles + d_angle.template slice<0, 0, 5, 1>();
106 204330 : new_angles[3][0] = (pi / 2.0) - new_angles[1][0] - new_angles[2][0];
107 204330 : e_opt_new = forward(new_angles);
108 :
109 215479 : if (!e_opt_new.has_value())
110 : {
111 11149 : beta /= 2.0;
112 11149 : continue;
113 : }
114 :
115 193181 : e_new = e_opt_new.value().template slice<0, 3, 3, 1>();
116 :
117 193181 : if ((e_new - e + d_e).get_magnitude() > deviation)
118 : {
119 31478 : beta /= 2.0;
120 : }
121 : else
122 : {
123 161703 : beta *= 1.2;
124 161703 : e = e_new;
125 161703 : current_angles = new_angles;
126 : }
127 : }
128 :
129 6 : if ((g - e).get_magnitude() <= precision)
130 : {
131 3 : solutions.first = new_angles;
132 : }
133 :
134 6 : return solutions;
135 : }
136 :
137 455 : IKSolution Kinematics::analytical_inverse(const Matrix<double, 3, 1>& goal)
138 : {
139 455 : IKSolution solutions;
140 :
141 : // Check if solution is in worksapce.
142 : static const double max_arm_length =
143 : joint_lengths[0][0] + joint_lengths[1][0] + joint_lengths[2][0];
144 455 : if (max_arm_length - goal.get_magnitude() <= 0)
145 : {
146 4 : return solutions;
147 : }
148 :
149 451 : solutions.first = Matrix<double, 5, 1>();
150 451 : solutions.second = Matrix<double, 5, 1>();
151 :
152 451 : const auto& l = joint_lengths;
153 :
154 451 : auto& s_1 = solutions.first.value();
155 451 : auto& s_2 = solutions.second.value();
156 :
157 451 : double theta_0 = std::atan2(goal[2][0], goal[0][0]);
158 451 : double x = std::hypot(goal[2][0], goal[0][0]) - l[2][0];
159 451 : double y = goal[1][0];
160 :
161 : double cos_theta_2 =
162 451 : ((x * x) + (y * y) - (l[0][0] * l[0][0]) - (l[1][0] * l[1][0]))
163 451 : / (2 * l[0][0] * l[1][0]);
164 :
165 451 : s_1[2][0] =
166 451 : std::atan2(std::sqrt(1 - cos_theta_2 * cos_theta_2), cos_theta_2);
167 451 : s_2[2][0] =
168 451 : std::atan2(-std::sqrt(1 - cos_theta_2 * cos_theta_2), cos_theta_2);
169 :
170 902 : s_1[1][0] = std::atan2(x, y)
171 902 : - std::atan2(l[1][0] * std::sin(s_1[2][0]),
172 902 : l[0][0] + l[1][0] * std::cos(s_1[2][0]));
173 902 : s_2[1][0] = std::atan2(x, y)
174 902 : - std::atan2(l[1][0] * std::sin(s_2[2][0]),
175 902 : l[0][0] + l[1][0] * std::cos(s_2[2][0]));
176 :
177 451 : s_1[3][0] = (pi / 2.0) - s_1[1][0] - s_1[2][0];
178 451 : s_2[3][0] = (pi / 2.0) - s_2[1][0] - s_2[2][0];
179 :
180 451 : s_1[4][0] = 0;
181 451 : s_2[4][0] = 0;
182 :
183 451 : s_1[0][0] = theta_0;
184 451 : s_2[0][0] = theta_0;
185 :
186 451 : s_1 *= (180.0 / pi);
187 451 : s_2 *= (180.0 / pi);
188 :
189 451 : const auto fk1 = forward(s_1);
190 451 : const auto fk2 = forward(s_2);
191 :
192 902 : if (!fk1.has_value()
193 451 : || (fk1.value().template slice<0, 3, 3, 1>() - goal).get_magnitude()
194 : > precision)
195 : {
196 4 : solutions.first = std::nullopt;
197 : }
198 :
199 902 : if (!fk2.has_value()
200 451 : || (fk2.value().template slice<0, 3, 3, 1>() - goal).get_magnitude()
201 : > precision)
202 : {
203 406 : solutions.second = std::nullopt;
204 : }
205 :
206 451 : return solutions;
207 : }
208 :
209 204331 : Matrix<double, 3, 3> Kinematics::compute_jacobian(
210 : const Matrix<double, 5, 1>& current_angles)
211 : {
212 : // alias current angles to a and joint lengths l.
213 204331 : auto a = current_angles * (pi / 180);
214 204331 : const auto& l = joint_lengths;
215 204331 : Matrix<double, 3, 3> jacobian;
216 :
217 204331 : jacobian[0][0] =
218 204331 : (l[0][0] * std::sin(a[1][0]) + l[1][0] * std::sin(a[1][0] + a[2][0]))
219 204331 : * -std::sin(a[0][0]);
220 204331 : jacobian[0][1] =
221 204331 : (l[0][0] * std::cos(a[1][0]) + l[1][0] * std::cos(a[1][0] + a[2][0]))
222 204331 : * std::cos(a[0][0]);
223 204331 : jacobian[0][2] = l[1][0] * std::cos(a[1][0] + a[2][0]) * std::cos(a[0][0]);
224 :
225 204331 : jacobian[1][0] = 0;
226 204331 : jacobian[1][1] =
227 204331 : -l[0][0] * std::sin(a[1][0]) - l[1][0] * std::sin(a[1][0] + a[2][0]);
228 204331 : jacobian[1][2] = -l[1][0] * std::sin(a[1][0] + a[2][0]);
229 :
230 204331 : jacobian[2][0] =
231 204331 : (l[0][0] * std::sin(a[1][0]) + l[1][0] * std::sin(a[1][0] + a[2][0]))
232 204331 : * std::cos(a[0][0]);
233 204331 : jacobian[2][1] =
234 204331 : (l[0][0] * std::cos(a[1][0]) + l[1][0] * std::cos(a[1][0] + a[2][0]))
235 204331 : * std::sin(a[0][0]);
236 204331 : jacobian[2][2] = l[1][0] * std::cos(a[1][0] + a[2][0]) * std::sin(a[0][0]);
237 :
238 204331 : return jacobian;
239 : }
240 :
241 205699 : bool Kinematics::angles_within_limits(const Matrix<double, 5, 1>& angles)
242 : {
243 1199891 : for (std::size_t i = 0; i < angles.get_m(); ++i)
244 : {
245 2011510 : if (servo_limits[i][0] > angles[i][0]
246 1005755 : || servo_limits[i][1] < angles[i][0])
247 : {
248 11563 : return false;
249 : }
250 : }
251 :
252 194136 : return true;
253 : }
254 :
255 : } // namespace Kinematics
256 3 : } // namespace HLR
|
