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 
6 template <typename T>
7 void inverse(const Parameters<T>& params, const Transform<T>& pose, T* out) noexcept
8 {
9  using Vector = Eigen::Matrix<T, 3, 1>;
10 
11  // Adjust to wrist center
12  Vector c = pose.translation() - params.c4 * pose.linear() * Vector::UnitZ();
13  const auto& matrix = pose.matrix();
14 
15  T nx1 = std::sqrt(c.x() * c.x() + c.y() * c.y() - params.b * params.b) - params.a1;
16 
17  // Compute theta1_i, theta1_ii
18  T tmp1 = std::atan2(c.y(), c.x());
19  T tmp2 = std::atan2(params.b, nx1 + params.a1);
20  T theta1_i = tmp1 - tmp2;
21  T theta1_ii = tmp1 + tmp2 - T(M_PI);
22 
23  // theta2 i through iv
24  T tmp3 = (c.z() - params.c1);
25  T s1_2 = nx1 * nx1 + tmp3 * tmp3;
26 
27  T tmp4 = nx1 + T(2.0) * params.a1;
28  T s2_2 = tmp4 * tmp4 + tmp3 * tmp3;
29  T kappa_2 = params.a2 * params.a2 + params.c3 * params.c3;
30 
31  T c2_2 = params.c2 * params.c2;
32 
33  T tmp5 = s1_2 + c2_2 - kappa_2;
34 
35  T s1 = std::sqrt(s1_2);
36  T s2 = std::sqrt(s2_2);
37  T theta2_i = -std::acos(tmp5 / (T(2.0) * s1 * params.c2)) +
38  std::atan2(nx1, c.z() - params.c1);
39  T theta2_ii = std::acos(tmp5 / (T(2.0) * s1 * params.c2)) +
40  std::atan2(nx1, c.z() - params.c1);
41 
42  T tmp6 = s2_2 + c2_2 - kappa_2;
43 
44  T theta2_iii = -std::acos(tmp6 / (T(2.0) * s2 * params.c2)) - std::atan2(nx1 + T(2.0) * params.a1, c.z() - params.c1);
45  T theta2_iv = std::acos(tmp6 / (T(2.0) * s2 * params.c2)) - std::atan2(nx1 + T(2.0) * params.a1, c.z() - params.c1);
46 
47  // theta3
48  T tmp7 = s1_2 - c2_2 - kappa_2;
49  T tmp8 = s2_2 - c2_2 - kappa_2;
50  T tmp9 = T(2) * params.c2 * std::sqrt(kappa_2);
51  T theta3_i = std::acos(tmp7 / tmp9) - std::atan2(params.a2, params.c3);
52 
53  T theta3_ii = -std::acos(tmp7 / tmp9) - std::atan2(params.a2, params.c3);
54 
55  T theta3_iii = std::acos(tmp8 / tmp9) - std::atan2(params.a2, params.c3);
56  T theta3_iv = -std::acos(tmp8 / tmp9) - std::atan2(params.a2, params.c3);
57 
58  // Now for the orientation part...
59  T s23[4];
60  T c23[4];
61  T sin1[4];
62  T cos1[4];
63 
64  sin1[0] = sin(theta1_i);
65  sin1[1] = sin(theta1_i);
66  sin1[2] = sin(theta1_ii); // ???
67  sin1[3] = sin(theta1_ii);
68 
69  cos1[0] = cos(theta1_i);
70  cos1[1] = cos(theta1_i);
71  cos1[2] = cos(theta1_ii); // ???
72  cos1[3] = cos(theta1_ii);
73 
74  s23[0] = sin(theta2_i + theta3_i);
75  s23[1] = sin(theta2_ii + theta3_ii);
76  s23[2] = sin(theta2_iii + theta3_iii);
77  s23[3] = sin(theta2_iv + theta3_iv);
78 
79  c23[0] = cos(theta2_i + theta3_i);
80  c23[1] = cos(theta2_ii + theta3_ii);
81  c23[2] = cos(theta2_iii + theta3_iii);
82  c23[3] = cos(theta2_iv + theta3_iv);
83 
84  T m[4];
85  m[0] = matrix(0,2) * s23[0] * cos1[0] + matrix(1,2) * s23[0] * sin1[0] + matrix(2,2) * c23[0];
86  m[1] = matrix(0,2) * s23[1] * cos1[1] + matrix(1,2) * s23[1] * sin1[1] + matrix(2,2) * c23[1];
87  m[2] = matrix(0,2) * s23[2] * cos1[2] + matrix(1,2) * s23[2] * sin1[2] + matrix(2,2) * c23[2];
88  m[3] = matrix(0,2) * s23[3] * cos1[3] + matrix(1,2) * s23[3] * sin1[3] + matrix(2,2) * c23[3];
89 
90  T theta4_i = std::atan2(matrix(1,2) * cos1[0] - matrix(0,2) * sin1[0],
91  matrix(0,2) * c23[0] * cos1[0] + matrix(1,2) * c23[0] * sin1[0] - matrix(2,2) * s23[0]);
92 
93  T theta4_ii = std::atan2(matrix(1,2) * cos1[1] - matrix(0,2) * sin1[1],
94  matrix(0,2) * c23[1] * cos1[1] + matrix(1,2) * c23[1] * sin1[1] - matrix(2,2) * s23[1]);
95 
96  T theta4_iii = std::atan2(matrix(1,2) * cos1[2] - matrix(0,2) * sin1[2],
97  matrix(0,2) * c23[2] * cos1[2] + matrix(1,2) * c23[2] * sin1[2] - matrix(2,2) * s23[2]);
98 
99  T theta4_iv = std::atan2(matrix(1,2) * cos1[3] - matrix(0,2) * sin1[3],
100  matrix(0,2) * c23[3] * cos1[3] + matrix(1,2) * c23[3] * sin1[3] - matrix(2,2) * s23[3]);
101 
102  T theta4_v = theta4_i + T(M_PI);
103  T theta4_vi = theta4_ii + T(M_PI);
104  T theta4_vii = theta4_iii + T(M_PI);
105  T theta4_viii = theta4_iv + T(M_PI);
106 
107  T theta5_i = std::atan2(sqrt(1 - m[0] * m[0]), m[0]);
108  T theta5_ii = std::atan2(sqrt(1 - m[1] * m[1]), m[1]);
109  T theta5_iii = std::atan2(sqrt(1 - m[2] * m[2]), m[2]);
110  T theta5_iv = std::atan2(sqrt(1 - m[3] * m[3]), m[3]);
111 
112  T theta5_v = -theta5_i;
113  T theta5_vi = -theta5_ii;
114  T theta5_vii = -theta5_iii;
115  T theta5_viii = -theta5_iv;
116 
117  T theta6_i = std::atan2(matrix(0,1) * s23[0] * cos1[0] + matrix(1,1) * s23[0] * sin1[0] + matrix(2,1) * c23[0],
118  -matrix(0,0) * s23[0] * cos1[0] - matrix(1, 0) * s23[0] * sin1[0] - matrix(2,0) * c23[0]);
119 
120  T theta6_ii = std::atan2(matrix(0,1) * s23[1] * cos1[1] + matrix(1,1) * s23[1] * sin1[1] + matrix(2,1) * c23[1],
121  -matrix(0,0) * s23[1] * cos1[1] - matrix(1, 0) * s23[1] * sin1[1] - matrix(2,0) * c23[1]);
122 
123  T theta6_iii = std::atan2(matrix(0,1) * s23[2] * cos1[2] + matrix(1,1) * s23[2] * sin1[2] + matrix(2,1) * c23[2],
124  -matrix(0,0) * s23[2] * cos1[2] - matrix(1, 0) * s23[2] * sin1[2] - matrix(2,0) * c23[2]);
125 
126  T theta6_iv = std::atan2(matrix(0,1) * s23[3] * cos1[3] + matrix(1,1) * s23[3] * sin1[3] + matrix(2,1) * c23[3],
127  -matrix(0,0) * s23[3] * cos1[3] - matrix(1, 0) * s23[3] * sin1[3] - matrix(2,0) * c23[3]);
128 
129  T theta6_v = theta6_i - T(M_PI);
130  T theta6_vi = theta6_ii - T(M_PI);
131  T theta6_vii = theta6_iii - T(M_PI);
132  T theta6_viii = theta6_iv - T(M_PI);
133 
134  out[6 * 0 + 0] = (theta1_i + params.offsets[0]) * params.sign_corrections[0];
135  out[6 * 0 + 1] = (theta2_i + params.offsets[1]) * params.sign_corrections[1];
136  out[6 * 0 + 2] = (theta3_i + params.offsets[2]) * params.sign_corrections[2];
137  out[6 * 0 + 3] = (theta4_i + params.offsets[3]) * params.sign_corrections[3];
138  out[6 * 0 + 4] = (theta5_i + params.offsets[4]) * params.sign_corrections[4];
139  out[6 * 0 + 5] = (theta6_i + params.offsets[5]) * params.sign_corrections[5];
140 
141  out[6 * 1 + 0] = (theta1_i + params.offsets[0]) * params.sign_corrections[0];
142  out[6 * 1 + 1] = (theta2_ii + params.offsets[1]) * params.sign_corrections[1];
143  out[6 * 1 + 2] = (theta3_ii + params.offsets[2]) * params.sign_corrections[2];
144  out[6 * 1 + 3] = (theta4_ii + params.offsets[3]) * params.sign_corrections[3];
145  out[6 * 1 + 4] = (theta5_ii + params.offsets[4]) * params.sign_corrections[4];
146  out[6 * 1 + 5] = (theta6_ii + params.offsets[5]) * params.sign_corrections[5];
147 
148  out[6 * 2 + 0] = (theta1_ii + params.offsets[0]) * params.sign_corrections[0];
149  out[6 * 2 + 1] = (theta2_iii + params.offsets[1]) * params.sign_corrections[1];
150  out[6 * 2 + 2] = (theta3_iii + params.offsets[2]) * params.sign_corrections[2];
151  out[6 * 2 + 3] = (theta4_iii + params.offsets[3]) * params.sign_corrections[3];
152  out[6 * 2 + 4] = (theta5_iii + params.offsets[4]) * params.sign_corrections[4];
153  out[6 * 2 + 5] = (theta6_iii + params.offsets[5]) * params.sign_corrections[5];
154 
155  out[6 * 3 + 0] = (theta1_ii + params.offsets[0]) * params.sign_corrections[0];
156  out[6 * 3 + 1] = (theta2_iv + params.offsets[1]) * params.sign_corrections[1];
157  out[6 * 3 + 2] = (theta3_iv + params.offsets[2]) * params.sign_corrections[2];
158  out[6 * 3 + 3] = (theta4_iv + params.offsets[3]) * params.sign_corrections[3];
159  out[6 * 3 + 4] = (theta5_iv + params.offsets[4]) * params.sign_corrections[4];
160  out[6 * 3 + 5] = (theta6_iv + params.offsets[5]) * params.sign_corrections[5];
161 
162  out[6 * 4 + 0] = (theta1_i + params.offsets[0]) * params.sign_corrections[0];
163  out[6 * 4 + 1] = (theta2_i + params.offsets[1]) * params.sign_corrections[1];
164  out[6 * 4 + 2] = (theta3_i + params.offsets[2]) * params.sign_corrections[2];
165  out[6 * 4 + 3] = (theta4_v + params.offsets[3]) * params.sign_corrections[3];
166  out[6 * 4 + 4] = (theta5_v + params.offsets[4]) * params.sign_corrections[4];
167  out[6 * 4 + 5] = (theta6_v + params.offsets[5]) * params.sign_corrections[5];
168 
169  out[6 * 5 + 0] = (theta1_i + params.offsets[0]) * params.sign_corrections[0];
170  out[6 * 5 + 1] = (theta2_ii + params.offsets[1]) * params.sign_corrections[1];
171  out[6 * 5 + 2] = (theta3_ii + params.offsets[2]) * params.sign_corrections[2];
172  out[6 * 5 + 3] = (theta4_vi + params.offsets[3]) * params.sign_corrections[3];
173  out[6 * 5 + 4] = (theta5_vi + params.offsets[4]) * params.sign_corrections[4];
174  out[6 * 5 + 5] = (theta6_vi + params.offsets[5]) * params.sign_corrections[5];
175 
176  out[6 * 6 + 0] = (theta1_ii + params.offsets[0]) * params.sign_corrections[0];
177  out[6 * 6 + 1] = (theta2_iii + params.offsets[1]) * params.sign_corrections[1];
178  out[6 * 6 + 2] = (theta3_iii + params.offsets[2]) * params.sign_corrections[2];
179  out[6 * 6 + 3] = (theta4_vii + params.offsets[3]) * params.sign_corrections[3];
180  out[6 * 6 + 4] = (theta5_vii + params.offsets[4]) * params.sign_corrections[4];
181  out[6 * 6 + 5] = (theta6_vii + params.offsets[5]) * params.sign_corrections[5];
182 
183  out[6 * 7 + 0] = (theta1_ii + params.offsets[0]) * params.sign_corrections[0];
184  out[6 * 7 + 1] = (theta2_iv + params.offsets[1]) * params.sign_corrections[1];
185  out[6 * 7 + 2] = (theta3_iv + params.offsets[2]) * params.sign_corrections[2];
186  out[6 * 7 + 3] = (theta4_viii + params.offsets[3]) * params.sign_corrections[3];
187  out[6 * 7 + 4] = (theta5_viii + params.offsets[4]) * params.sign_corrections[4];
188  out[6 * 7 + 5] = (theta6_viii + params.offsets[5]) * params.sign_corrections[5];
189 }
190 
191 template <typename T>
192 Transform<T> forward(const Parameters<T>& p, const T* qs) noexcept
193 {
194  using Matrix = Eigen::Matrix<T, 3, 3>;
195  using Vector = Eigen::Matrix<T, 3, 1>;
196 
197  T q[6];
198  q[0] = qs[0] * p.sign_corrections[0] - p.offsets[0];
199  q[1] = qs[1] * p.sign_corrections[1] - p.offsets[1];
200  q[2] = qs[2] * p.sign_corrections[2] - p.offsets[2];
201  q[3] = qs[3] * p.sign_corrections[3] - p.offsets[3];
202  q[4] = qs[4] * p.sign_corrections[4] - p.offsets[4];
203  q[5] = qs[5] * p.sign_corrections[5] - p.offsets[5];
204 
205  T psi3 = std::atan2(p.a2, p.c3);
206  T k = std::sqrt(p.a2 * p.a2 + p.c3 * p.c3);
207 
208  T cx1 = p.c2 * std::sin(q[1]) + k * std::sin(q[1] + q[2] + psi3) + p.a1;
209  T cy1 = p.b;
210  T cz1 = p.c2 * std::cos(q[1]) + k * std::cos(q[1] + q[2] + psi3);
211 
212  T cx0 = cx1 * std::cos(q[0]) - cy1 * std::sin(q[0]);
213  T cy0 = cx1 * std::sin(q[0]) + cy1 * std::cos(q[0]);
214  T cz0 = cz1 + p.c1;
215 
216  T s1 = std::sin(q[0]);
217  T s2 = std::sin(q[1]);
218  T s3 = std::sin(q[2]);
219  T s4 = std::sin(q[3]);
220  T s5 = std::sin(q[4]);
221  T s6 = std::sin(q[5]);
222 
223  T c1 = std::cos(q[0]);
224  T c2 = std::cos(q[1]);
225  T c3 = std::cos(q[2]);
226  T c4 = std::cos(q[3]);
227  T c5 = std::cos(q[4]);
228  T c6 = std::cos(q[5]);
229 
230  Matrix r_0c;
231  r_0c(0,0) = c1 * c2 * c3- c1 * s2 * s3;
232  r_0c(0,1) = -s1;
233  r_0c(0,2) = c1*c2*s3+c1*s2*c3;
234 
235  r_0c(1,0) = s1*c2*c3-s1*s2*s3;
236  r_0c(1,1) = c1;
237  r_0c(1,2) = s1*c2*s3+s1*s2*c3;
238 
239  r_0c(2,0) = -s2*c3-c2*s3;
240  r_0c(2,1) = 0;
241  r_0c(2,2) = -s2*s3+c2*c3;
242 
243  Matrix r_ce;
244  r_ce(0,0) = c4*c5*c6-s4*s6;
245  r_ce(0,1) = -c4*c5*s6-s4*c6;
246  r_ce(0,2) = c4 * s5;
247 
248  r_ce(1,0) = s4*c5*c6+c4*s6;
249  r_ce(1,1) = -s4*c5*s6+c4*c6;
250  r_ce(1,2) = s4*s5;
251 
252  r_ce(2,0) = -s5*c6;
253  r_ce(2,1) = s5*s6 ;
254  r_ce(2,2) = c5;
255 
256  Matrix r_oe = r_0c * r_ce;
257 
258  //Note: do not use auto here, leads to lazy evalutation which
259  // seems to be buggy on at least some setups and uses uninitialized data
260  Vector u = Vector(cx0, cy0, cz0) + p.c4 * r_oe * Vector::UnitZ();
261 
262  Transform<T> i;
263  i.setIdentity();
264  i.translation() = u;
265  i.linear() = r_oe;
266 
267  return i;
268 }
269 
270 #endif
M_PI
#define M_PI
sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
inverse
void inverse(const Parameters< T > &params, const Transform< T > &pose, T *out) noexcept
Definition: opw_kinematics_impl.h:7
forward
Transform< T > forward(const Parameters< T > &p, const T *qs) noexcept
Definition: opw_kinematics_impl.h:192
cos
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
sin
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Transform
opw_kinematics::Transform< double > Transform
Definition: abb2400_ikfast_tests.cpp:11

moveit_opw_kinematics_plugin
Author(s): Jeroen De Maeyer, Simon Schmeisser (isys vision)
autogenerated on Wed Jun 3 2020 03:17:14