opw_kinematics_impl.h
Go to the documentation of this file.
1 #ifndef OPW_KINEMATICS_IMPL_H
2 #define OPW_KINEMATICS_IMPL_H
3 
4 #include <cmath>
5 #include <iostream>
6 #include <Eigen/Dense>
7 
8 namespace opw_kinematics
9 {
10 template <typename T>
11 bool comparePoses(const Eigen::Transform<T, 3, Eigen::Isometry>& Ta,
12  const Eigen::Transform<T, 3, Eigen::Isometry>& Tb,
13  T tolerance)
14 {
15  T translation_distance = (Ta.translation() - Tb.translation()).norm();
16  T angular_distance = Eigen::Quaternion<T>(Ta.linear()).angularDistance(Eigen::Quaternion<T>(Tb.linear()));
17 
18  if (std::abs(translation_distance) > tolerance)
19  {
20  std::cout << "Translation Error: " << translation_distance << std::endl;
21  return false;
22  }
23 
24  if (std::abs(angular_distance) > tolerance)
25  {
26  std::cout << "Angular Error: " << angular_distance << std::endl;
27  return false;
28  }
29 
30  return true;
31 }
32 
33 template <typename T>
34 Transform<T> forward(const Parameters<T>& p, const std::array<T, 6>& qs) noexcept
35 {
36  using Matrix = Eigen::Matrix<T, 3, 3>;
37  using Vector = Eigen::Matrix<T, 3, 1>;
38 
39  std::array<T, 6> q;
40  q[0] = qs[0] * p.sign_corrections[0] - p.offsets[0];
41  q[1] = qs[1] * p.sign_corrections[1] - p.offsets[1];
42  q[2] = qs[2] * p.sign_corrections[2] - p.offsets[2];
43  q[3] = qs[3] * p.sign_corrections[3] - p.offsets[3];
44  q[4] = qs[4] * p.sign_corrections[4] - p.offsets[4];
45  q[5] = qs[5] * p.sign_corrections[5] - p.offsets[5];
46 
47  T psi3 = std::atan2(p.a2, p.c3);
48  T k = std::sqrt(p.a2 * p.a2 + p.c3 * p.c3);
49 
50  T cx1 = p.c2 * std::sin(q[1]) + k * std::sin(q[1] + q[2] + psi3) + p.a1;
51  T cy1 = p.b;
52  T cz1 = p.c2 * std::cos(q[1]) + k * std::cos(q[1] + q[2] + psi3);
53 
54  T cx0 = cx1 * std::cos(q[0]) - cy1 * std::sin(q[0]);
55  T cy0 = cx1 * std::sin(q[0]) + cy1 * std::cos(q[0]);
56  T cz0 = cz1 + p.c1;
57 
58  T s1 = std::sin(q[0]);
59  T s2 = std::sin(q[1]);
60  T s3 = std::sin(q[2]);
61  T s4 = std::sin(q[3]);
62  T s5 = std::sin(q[4]);
63  T s6 = std::sin(q[5]);
64 
65  T c1 = std::cos(q[0]);
66  T c2 = std::cos(q[1]);
67  T c3 = std::cos(q[2]);
68  T c4 = std::cos(q[3]);
69  T c5 = std::cos(q[4]);
70  T c6 = std::cos(q[5]);
71 
72  Matrix r_0c;
73  r_0c(0, 0) = c1 * c2 * c3 - c1 * s2 * s3;
74  r_0c(0, 1) = -s1;
75  r_0c(0, 2) = c1 * c2 * s3 + c1 * s2 * c3;
76 
77  r_0c(1, 0) = s1 * c2 * c3 - s1 * s2 * s3;
78  r_0c(1, 1) = c1;
79  r_0c(1, 2) = s1 * c2 * s3 + s1 * s2 * c3;
80 
81  r_0c(2, 0) = -s2 * c3 - c2 * s3;
82  r_0c(2, 1) = 0;
83  r_0c(2, 2) = -s2 * s3 + c2 * c3;
84 
85  Matrix r_ce;
86  r_ce(0, 0) = c4 * c5 * c6 - s4 * s6;
87  r_ce(0, 1) = -c4 * c5 * s6 - s4 * c6;
88  r_ce(0, 2) = c4 * s5;
89 
90  r_ce(1, 0) = s4 * c5 * c6 + c4 * s6;
91  r_ce(1, 1) = -s4 * c5 * s6 + c4 * c6;
92  r_ce(1, 2) = s4 * s5;
93 
94  r_ce(2, 0) = -s5 * c6;
95  r_ce(2, 1) = s5 * s6;
96  r_ce(2, 2) = c5;
97 
98  Matrix r_oe = r_0c * r_ce;
99 
100  // Note: do not use auto here, leads to lazy evaluation which
101  // seems to be buggy on at least some setups and uses uninitialized data
102  Vector u = Vector(cx0, cy0, cz0) + p.c4 * r_oe * Vector::UnitZ();
103 
104  Transform<T> i;
105  i.setIdentity();
106  i.translation() = u;
107  i.linear() = r_oe;
108 
109  return i;
110 }
111 
112 template <typename T>
113 Solutions<T> inverse(const Parameters<T>& params, const Transform<T>& pose) noexcept
114 {
115  using Vector = Eigen::Matrix<T, 3, 1>;
116  using Matrix3 = Eigen::Matrix<T, 3, 3>;
117 
118  // The container for solutions
119  Solutions<T> solutions;
120 
121  // Adjust to wrist center
122  const auto& matrix = pose.linear();
123  Vector c = pose.translation() - params.c4 * pose.linear() * Vector::UnitZ();
124 
125  T nx1 = std::sqrt(c.x() * c.x() + c.y() * c.y() - params.b * params.b) - params.a1;
126 
127  // Compute theta1_i, theta1_ii
128  T tmp1 = std::atan2(c.y(), c.x());
129  T tmp2 = std::atan2(params.b, nx1 + params.a1);
130  T theta1_i = tmp1 - tmp2;
131  T theta1_ii = tmp1 + tmp2 - T(M_PI);
132 
133  // theta2 i through iv
134  T tmp3 = (c.z() - params.c1);
135  T s1_2 = nx1 * nx1 + tmp3 * tmp3;
136 
137  T tmp4 = nx1 + T(2.0) * params.a1;
138  T s2_2 = tmp4 * tmp4 + tmp3 * tmp3;
139  T kappa_2 = params.a2 * params.a2 + params.c3 * params.c3;
140 
141  T c2_2 = params.c2 * params.c2;
142 
143  T tmp5 = s1_2 + c2_2 - kappa_2;
144 
145  T s1 = std::sqrt(s1_2);
146  T s2 = std::sqrt(s2_2);
147 
148  T tmp13 = std::acos(tmp5 / (T(2.0) * s1 * params.c2));
149  T tmp14 = std::atan2(nx1, c.z() - params.c1);
150  T theta2_i = -tmp13 + tmp14;
151  T theta2_ii = tmp13 + tmp14;
152 
153  T tmp6 = s2_2 + c2_2 - kappa_2;
154 
155  T tmp15 = std::acos(tmp6 / (T(2.0) * s2 * params.c2));
156  T tmp16 = std::atan2(nx1 + T(2.0) * params.a1, c.z() - params.c1);
157  T theta2_iii = -tmp15 - tmp16;
158  T theta2_iv = tmp15 - tmp16;
159 
160  // theta3
161  T tmp7 = s1_2 - c2_2 - kappa_2;
162  T tmp8 = s2_2 - c2_2 - kappa_2;
163  T tmp9 = T(2) * params.c2 * std::sqrt(kappa_2);
164  T tmp10 = std::atan2(params.a2, params.c3);
165  T tmp11 = std::acos(tmp7 / tmp9);
166 
167  T theta3_i = tmp11 - tmp10;
168  T theta3_ii = -tmp11 - tmp10;
169 
170  T tmp12 = std::acos(tmp8 / tmp9);
171  T theta3_iii = tmp12 - tmp10;
172  T theta3_iv = -tmp12 - tmp10;
173 
174  // Now for the orientation part...
175  std::array<T, 4> s23;
176  std::array<T, 4> c23;
177  std::array<T, 4> sin1;
178  std::array<T, 4> cos1;
179 
180  sin1[0] = std::sin(theta1_i);
181  sin1[1] = std::sin(theta1_i);
182  sin1[2] = std::sin(theta1_ii); // ???
183  sin1[3] = std::sin(theta1_ii);
184 
185  cos1[0] = std::cos(theta1_i);
186  cos1[1] = std::cos(theta1_i);
187  cos1[2] = std::cos(theta1_ii); // ???
188  cos1[3] = std::cos(theta1_ii);
189 
190  s23[0] = std::sin(theta2_i + theta3_i);
191  s23[1] = std::sin(theta2_ii + theta3_ii);
192  s23[2] = std::sin(theta2_iii + theta3_iii);
193  s23[3] = std::sin(theta2_iv + theta3_iv);
194 
195  c23[0] = std::cos(theta2_i + theta3_i);
196  c23[1] = std::cos(theta2_ii + theta3_ii);
197  c23[2] = std::cos(theta2_iii + theta3_iii);
198  c23[3] = std::cos(theta2_iv + theta3_iv);
199 
200  std::array<T, 4> m;
201  m[0] = matrix(0, 2) * s23[0] * cos1[0] + matrix(1, 2) * s23[0] * sin1[0] + matrix(2, 2) * c23[0];
202  m[1] = matrix(0, 2) * s23[1] * cos1[1] + matrix(1, 2) * s23[1] * sin1[1] + matrix(2, 2) * c23[1];
203  m[2] = matrix(0, 2) * s23[2] * cos1[2] + matrix(1, 2) * s23[2] * sin1[2] + matrix(2, 2) * c23[2];
204  m[3] = matrix(0, 2) * s23[3] * cos1[3] + matrix(1, 2) * s23[3] * sin1[3] + matrix(2, 2) * c23[3];
205 
206  T theta5_i = std::atan2(std::sqrt(1 - m[0] * m[0]), m[0]);
207  T theta5_ii = std::atan2(std::sqrt(1 - m[1] * m[1]), m[1]);
208  T theta5_iii = std::atan2(std::sqrt(1 - m[2] * m[2]), m[2]);
209  T theta5_iv = std::atan2(std::sqrt(1 - m[3] * m[3]), m[3]);
210 
211  T theta5_v = -theta5_i;
212  T theta5_vi = -theta5_ii;
213  T theta5_vii = -theta5_iii;
214  T theta5_viii = -theta5_iv;
215 
216  // The derived equations in the paper are geometric and break down when joint 5 is equal to zero.
217  // When joint 5 is equal to zeros the values passed to std::atan2 are both zero which is undefined.
218  // This can result in significant rotation error up to PI between joint 4 and joint 6 each.
219  // In the paper it defines an equation for Rc which is the rotation matrix of joint 5 relative to the base.
220  // If you set joint 4 to zero and joint 5 is zero it results in the matrix below where the z-axis is the
221  // same as the provided pose. Next, it takes the poses x-axis and projects it onto Rc and then leverage
222  // std::atan2 to calculate the joint 6 angle.
223  T zero_threshold = static_cast<T>(1e-6);
224  T theta4_i, theta6_i;
225  if (std::abs(theta5_i) < zero_threshold)
226  {
227  theta4_i = 0;
228  Vector xe(matrix(0, 0), matrix(1, 0), matrix(2, 0));
229  Matrix3 Rc;
230  Rc.col(1) = Vector(-std::sin(theta1_i), std::cos(theta1_i), 0); // yc
231  Rc.col(2) = matrix.col(2); // zc and ze are equal
232  Rc.col(0) = Rc.col(1).cross(Rc.col(2)); // xc
233  Vector xec = Rc.transpose() * xe;
234  theta6_i = std::atan2(xec(1), xec(0));
235  }
236  else
237  {
238  T theta4_iy = matrix(1, 2) * cos1[0] - matrix(0, 2) * sin1[0];
239  T theta4_ix = matrix(0, 2) * c23[0] * cos1[0] + matrix(1, 2) * c23[0] * sin1[0] - matrix(2, 2) * s23[0];
240  theta4_i = std::atan2(theta4_iy, theta4_ix);
241 
242  T theta6_iy = matrix(0, 1) * s23[0] * cos1[0] + matrix(1, 1) * s23[0] * sin1[0] + matrix(2, 1) * c23[0];
243  T theta6_ix = -matrix(0, 0) * s23[0] * cos1[0] - matrix(1, 0) * s23[0] * sin1[0] - matrix(2, 0) * c23[0];
244  theta6_i = std::atan2(theta6_iy, theta6_ix);
245  }
246 
247  T theta4_ii, theta6_ii;
248  if (std::abs(theta5_ii) < zero_threshold)
249  {
250  theta4_ii = 0;
251  Vector xe(matrix(0, 0), matrix(1, 0), matrix(2, 0));
252  Matrix3 Rc;
253  Rc.col(1) = Vector(-std::sin(theta1_i), std::cos(theta1_i), 0); // yc
254  Rc.col(2) = matrix.col(2); // zc and ze are equal
255  Rc.col(0) = Rc.col(1).cross(Rc.col(2)); // xc
256  Vector xec = Rc.transpose() * xe;
257  theta6_ii = std::atan2(xec(1), xec(0));
258  }
259  else
260  {
261  T theta4_iiy = matrix(1, 2) * cos1[1] - matrix(0, 2) * sin1[1];
262  T theta4_iix = matrix(0, 2) * c23[1] * cos1[1] + matrix(1, 2) * c23[1] * sin1[1] - matrix(2, 2) * s23[1];
263  theta4_ii = std::atan2(theta4_iiy, theta4_iix);
264 
265  T theta6_iiy = matrix(0, 1) * s23[1] * cos1[1] + matrix(1, 1) * s23[1] * sin1[1] + matrix(2, 1) * c23[1];
266  T theta6_iix = -matrix(0, 0) * s23[1] * cos1[1] - matrix(1, 0) * s23[1] * sin1[1] - matrix(2, 0) * c23[1];
267  theta6_ii = std::atan2(theta6_iiy, theta6_iix);
268  }
269 
270  T theta4_iii, theta6_iii;
271  if (std::abs(theta5_iii) < zero_threshold)
272  {
273  theta4_iii = 0;
274  Vector xe(matrix(0, 0), matrix(1, 0), matrix(2, 0));
275  Matrix3 Rc;
276  Rc.col(1) = Vector(-std::sin(theta1_ii), std::cos(theta1_ii), 0); // yc
277  Rc.col(2) = matrix.col(2); // zc and ze are equal
278  Rc.col(0) = Rc.col(1).cross(Rc.col(2)); // xc
279  Vector xec = Rc.transpose() * xe;
280  theta6_iii = std::atan2(xec(1), xec(0));
281  }
282  else
283  {
284  T theta4_iiiy = matrix(1, 2) * cos1[2] - matrix(0, 2) * sin1[2];
285  T theta4_iiix = matrix(0, 2) * c23[2] * cos1[2] + matrix(1, 2) * c23[2] * sin1[2] - matrix(2, 2) * s23[2];
286  theta4_iii = std::atan2(theta4_iiiy, theta4_iiix);
287 
288  T theta6_iiiy = matrix(0, 1) * s23[2] * cos1[2] + matrix(1, 1) * s23[2] * sin1[2] + matrix(2, 1) * c23[2];
289  T theta6_iiix = -matrix(0, 0) * s23[2] * cos1[2] - matrix(1, 0) * s23[2] * sin1[2] - matrix(2, 0) * c23[2];
290  theta6_iii = std::atan2(theta6_iiiy, theta6_iiix);
291  }
292 
293  T theta4_iv, theta6_iv;
294  if (std::abs(theta5_iv) < zero_threshold)
295  {
296  theta4_iv = 0;
297  Vector xe(matrix(0, 0), matrix(1, 0), matrix(2, 0));
298  Matrix3 Rc;
299  Rc.col(1) = Vector(-std::sin(theta1_ii), std::cos(theta1_ii), 0); // yc
300  Rc.col(2) = matrix.col(2); // zc and ze are equal
301  Rc.col(0) = Rc.col(1).cross(Rc.col(2)); // xc
302  Vector xec = Rc.transpose() * xe;
303  theta6_iv = std::atan2(xec(1), xec(0));
304  }
305  else
306  {
307  T theta4_ivy = matrix(1, 2) * cos1[3] - matrix(0, 2) * sin1[3];
308  T theta4_ivx = matrix(0, 2) * c23[3] * cos1[3] + matrix(1, 2) * c23[3] * sin1[3] - matrix(2, 2) * s23[3];
309  theta4_iv = std::atan2(theta4_ivy, theta4_ivx);
310 
311  T theta6_ivy = matrix(0, 1) * s23[3] * cos1[3] + matrix(1, 1) * s23[3] * sin1[3] + matrix(2, 1) * c23[3];
312  T theta6_ivx = -matrix(0, 0) * s23[3] * cos1[3] - matrix(1, 0) * s23[3] * sin1[3] - matrix(2, 0) * c23[3];
313  theta6_iv = std::atan2(theta6_ivy, theta6_ivx);
314  }
315 
316  T theta4_v = theta4_i + T(M_PI);
317  T theta4_vi = theta4_ii + T(M_PI);
318  T theta4_vii = theta4_iii + T(M_PI);
319  T theta4_viii = theta4_iv + T(M_PI);
320 
321  T theta6_v = theta6_i - T(M_PI);
322  T theta6_vi = theta6_ii - T(M_PI);
323  T theta6_vii = theta6_iii - T(M_PI);
324  T theta6_viii = theta6_iv - T(M_PI);
325 
326  Eigen::Map<const Eigen::Array<T, 6, 1>> offsets(params.offsets.data());
327  Eigen::Array<T, 6, 1> signs;
328  signs[0] = params.sign_corrections[0];
329  signs[1] = params.sign_corrections[1];
330  signs[2] = params.sign_corrections[2];
331  signs[3] = params.sign_corrections[3];
332  signs[4] = params.sign_corrections[4];
333  signs[5] = params.sign_corrections[5];
334 
335  Eigen::Array<T, 6, 8> theta;
336  // clang-format off
337  theta << theta1_i, theta1_i, theta1_ii, theta1_ii, theta1_i, theta1_i, theta1_ii, theta1_ii,
338  theta2_i, theta2_ii, theta2_iii, theta2_iv, theta2_i, theta2_ii, theta2_iii, theta2_iv,
339  theta3_i, theta3_ii, theta3_iii, theta3_iv, theta3_i, theta3_ii, theta3_iii, theta3_iv,
340  theta4_i, theta4_ii, theta4_iii, theta4_iv, theta4_v, theta4_vi, theta4_vii, theta4_viii,
341  theta5_i, theta5_ii, theta5_iii, theta5_iv, theta5_v, theta5_vi, theta5_vii, theta5_viii,
342  theta6_i, theta6_ii, theta6_iii, theta6_iv, theta6_v, theta6_vi, theta6_vii, theta6_viii;
343  // clang-format on
344 
345  // Perform the computation
346  Eigen::Map<Eigen::Array<T, 6, 8>> output(solutions[0].data());
347  output = (theta.colwise() + offsets).colwise() * signs;
348 
349  // If debug check
350 #ifndef NDEBUG
351  Eigen::IOFormat heavy_fmt(Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]");
352  for (std::size_t i = 0; i < solutions.size(); ++i)
353  {
354  if (std::isfinite(solutions[i][0]) && std::isfinite(solutions[i][1]) && std::isfinite(solutions[i][2]) &&
355  std::isfinite(solutions[i][3]) && std::isfinite(solutions[i][4]) && std::isfinite(solutions[i][5]))
356  {
357  Transform<T> check_pose = forward<T>(params, solutions[i]);
358  if (!comparePoses<T>(pose, check_pose, T(1e-3)))
359  {
360  double t{ 0 };
361  if (i == 0)
362  t = theta5_i;
363  else if (i == 1)
364  t = theta5_ii;
365  else if (i == 2)
366  t = theta5_iii;
367  else if (i == 3)
368  t = theta5_iv;
369  else if (i == 4)
370  t = theta5_v;
371  else if (i == 5)
372  t = theta5_vi;
373  else if (i == 6)
374  t = theta5_vii;
375  else if (i == 7)
376  t = theta5_viii;
377 
378  std::cout << "*********************************" << std::endl
379  << "********** Pose Failure *********" << std::endl
380  << "*********************************" << std::endl
381  << "Theta 5 value: " << t << std::endl
382  << "Determinant: " << pose.matrix().determinant() << std::endl
383  << "Pose:" << std::endl
384  << pose.matrix().format(heavy_fmt) << std::endl;
385  }
386  }
387  }
388 #endif
389 
390  return solutions;
391 }
392 } // namespace opw_kinematics
393 #endif
opw_kinematics
Definition: opw_io.h:7
opw_kinematics::Transform
Eigen::Transform< T, 3, Eigen::Isometry > Transform
Definition: opw_kinematics.h:15
opw_kinematics::Solutions
std::array< std::array< T, 6 >, 8 > Solutions
Definition: opw_kinematics.h:22
opw_kinematics::forward
Transform< T > forward(const Parameters< T > &p, const std::array< T, 6 > &qs) noexcept
Computes the tool pose of the robot described by when said robot has the joint positions given by qs,...
Definition: opw_kinematics_impl.h:34
opw_kinematics::inverse
Solutions< T > inverse(const Parameters< T > &params, const Transform< T > &pose) noexcept
Computes up to 8 kinematically unique joint solutions that put the tool flange of the robot described...
Definition: opw_kinematics_impl.h:113
opw_kinematics::comparePoses
bool comparePoses(const Eigen::Transform< T, 3, Eigen::Isometry > &Ta, const Eigen::Transform< T, 3, Eigen::Isometry > &Tb, T tolerance)
Definition: opw_kinematics_impl.h:11
opw_kinematics::Parameters
Definition: opw_parameters.h:10

opw_kinematics
Author(s): Jon Meyer , Jeroen De Maeyer
autogenerated on Thu Jan 16 2025 03:40:37