220 const bool &verbose)
const
224 for (
unsigned int i = 0; i < 8; i++)
227 double c1[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
228 double s1[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
229 double c3[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
230 double s3[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
231 double c23[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
232 double s23[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
233 double c4[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
234 double s4[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
235 double c5[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
236 double s5[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
237 double c6[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
238 double s6[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
245 for (
unsigned int i = 0; i < 8; i++)
248 double px = fMw[0][3];
249 double py = fMw[1][3];
250 double pz = fMw[2][3];
253 double a_2 = px * px + py * py;
255 if (std::fabs(a_2) <= std::numeric_limits<double>::epsilon()) {
258 c1[4] = cos(q[0] + M_PI);
259 s1[4] = sin(q[0] + M_PI);
261 double a = sqrt(a_2);
269 for (
unsigned int i = 0; i < 8; i += 4) {
270 q_sol[i][0] = atan2(s1[i], c1[i]);
271 if (convertJointPositionInLimits(0, q_sol[i][0], q1_mod, verbose) ==
true) {
272 q_sol[i][0] = q1_mod;
273 for (
unsigned int j = 1; j < 4; j++) {
276 q_sol[i + j][0] = q_sol[i][0];
279 for (
unsigned int j = 1; j < 4; j++)
286 for (
unsigned int i = 0; i < 8; i += 4) {
289 2 * (
a1 * c1[i] * px +
a1 * s1[i] * py +
d1 * pz)) /
291 double d4_a3_K =
d4 *
d4 +
a3 *
a3 - K * K;
293 q_sol[i][2] = atan2(
a3,
d4) + atan2(K, sqrt(d4_a3_K));
294 q_sol[i + 2][2] = atan2(
a3,
d4) + atan2(K, -sqrt(d4_a3_K));
296 for (
unsigned int j = 0; j < 4; j += 2) {
298 for (
unsigned int k = 0; k < 2; k++)
299 ok[i + j + k] =
false;
301 if (convertJointPositionInLimits(2, q_sol[i + j][2], q3_mod, verbose) ==
true) {
302 for (
unsigned int k = 0; k < 2; k++) {
303 q_sol[i + j + k][2] = q3_mod;
304 c3[i + j + k] = cos(q3_mod);
305 s3[i + j + k] = sin(q3_mod);
308 for (
unsigned int k = 0; k < 2; k++)
309 ok[i + j + k] =
false;
321 double q23[8], q2_mod;
322 for (
unsigned int i = 0; i < 8; i += 2) {
325 c23[i] = (-(
a3 -
a2 * c3[i]) * (c1[i] * px + s1[i] * py -
a1) - (
d1 - pz) * (
d4 +
a2 * s3[i])) /
326 ((c1[i] * px + s1[i] * py -
a1) * (c1[i] * px + s1[i] * py -
a1) + (
d1 - pz) * (
d1 - pz));
327 s23[i] = ((
d4 +
a2 * s3[i]) * (c1[i] * px + s1[i] * py -
a1) - (
d1 - pz) * (
a3 -
a2 * c3[i])) /
328 ((c1[i] * px + s1[i] * py -
a1) * (c1[i] * px + s1[i] * py -
a1) + (
d1 - pz) * (
d1 - pz));
329 q23[i] = atan2(s23[i], c23[i]);
333 q_sol[i][1] = q23[i] - q_sol[i][2];
335 if (convertJointPositionInLimits(1, q_sol[i][1], q2_mod, verbose) ==
true) {
336 for (
unsigned int j = 0; j < 2; j++) {
337 q_sol[i + j][1] = q2_mod;
342 for (
unsigned int j = 0; j < 2; j++)
353 double r13 = fMw[0][2];
354 double r23 = fMw[1][2];
355 double r33 = fMw[2][2];
356 double s4s5, c4s5, q4_mod, q5_mod;
357 for (
unsigned int i = 0; i < 8; i += 2) {
359 s4s5 = -s1[i] * r13 + c1[i] * r23;
360 c4s5 = c1[i] * c23[i] * r13 + s1[i] * c23[i] * r23 - s23[i] * r33;
363 c5[i] = c1[i] * s23[i] * r13 + s1[i] * s23[i] * r23 + c23[i] * r33;
370 if (convertJointPositionInLimits(4, q_sol[i][4], q5_mod, verbose) ==
true) {
371 for (
unsigned int j = 0; j < 2; j++) {
372 q_sol[i + j][3] = q[3];
373 q_sol[i + j][4] = q5_mod;
374 c4[i] = cos(q_sol[i + j][3]);
375 s4[i] = sin(q_sol[i + j][3]);
378 for (
unsigned int j = 0; j < 2; j++)
385 if (std::fabs(c4s5) <= std::numeric_limits<double>::epsilon()) {
389 q_sol[i][3] = atan2(s4s5, c4s5);
392 q_sol[i][3] = atan2(s4s5, c4s5);
395 q_sol[i][3] = atan2(s4s5, c4s5);
397 if (convertJointPositionInLimits(3, q_sol[i][3], q4_mod, verbose) ==
true) {
398 q_sol[i][3] = q4_mod;
404 if (q_sol[i][3] > 0.)
405 q_sol[i + 1][3] = q_sol[i][3] + M_PI;
407 q_sol[i + 1][3] = q_sol[i][3] - M_PI;
408 if (convertJointPositionInLimits(3, q_sol[i + 1][3], q4_mod, verbose) ==
true) {
409 q_sol[i + 1][3] = q4_mod;
410 c4[i + 1] = cos(q4_mod);
411 s4[i + 1] = sin(q4_mod);
417 for (
unsigned int j = 0; j < 2; j++) {
418 if (ok[i + j] ==
true) {
419 c5[i + j] = c1[i + j] * s23[i + j] * r13 + s1[i + j] * s23[i + j] * r23 + c23[i + j] * r33;
420 s5[i + j] = (c1[i + j] * c23[i + j] * c4[i + j] - s1[i + j] * s4[i + j]) * r13 +
421 (s1[i + j] * c23[i + j] * c4[i + j] + c1[i + j] * s4[i + j]) * r23 -
422 s23[i + j] * c4[i + j] * r33;
424 q_sol[i + j][4] = atan2(s5[i + j], c5[i + j]);
425 if (convertJointPositionInLimits(4, q_sol[i + j][4], q5_mod, verbose) ==
true) {
426 q_sol[i + j][4] = q5_mod;
439 double r12 = fMw[0][1];
440 double r22 = fMw[1][1];
441 double r32 = fMw[2][1];
443 for (
unsigned int i = 0; i < 8; i++) {
444 c6[i] = -(c1[i] * c23[i] * s4[i] + s1[i] * c4[i]) * r12 + (c1[i] * c4[i] - s1[i] * c23[i] * s4[i]) * r22 +
445 s23[i] * s4[i] * r32;
446 s6[i] = -(c1[i] * c23[i] * c4[i] * c5[i] - c1[i] * s23[i] * s5[i] - s1[i] * s4[i] * c5[i]) * r12 -
447 (s1[i] * c23[i] * c4[i] * c5[i] - s1[i] * s23[i] * s5[i] + c1[i] * s4[i] * c5[i]) * r22 +
448 (c23[i] * s5[i] + s23[i] * c4[i] * c5[i]) * r32;
450 q_sol[i][5] = atan2(s6[i], c6[i]);
451 if (convertJointPositionInLimits(5, q_sol[i][5], q6_mod, verbose) ==
true) {
452 q_sol[i][5] = q6_mod;
459 unsigned int nbsol = 0;
460 unsigned int sol = 0;
462 for (
unsigned int i = 0; i < 8; i++) {
469 weight[1] = weight[2] = 4;
471 for (
unsigned int j = 0; j < 6; j++) {
472 double rought_dist = q[j] - q_sol[i][j];
473 double modulo_dist = rought_dist;
474 if (rought_dist > 0) {
475 if (fabs(rought_dist - 2 * M_PI) < fabs(rought_dist))
476 modulo_dist = rought_dist - 2 * M_PI;
478 if (fabs(rought_dist + 2 * M_PI) < fabs(rought_dist))
479 modulo_dist = rought_dist + 2 * M_PI;
489 for (
unsigned int i = 0; i < 8; i++) {
491 if (dist[i] < dist[sol])