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