kinecalc.cc
Go to the documentation of this file.
1 /*
2  * P2OS for ROS
3  * Copyright (C) 2009
4  * David Feil-Seifer, Brian Gerkey, Kasper Stoy,
5  * Richard Vaughan, & Andrew Howard
6  *
7  *
8  * This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with this program; if not, write to the Free Software
20  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
21  *
22  */
23 
24 
25 #include <p2os_driver/kinecalc.h>
26 
27 #include <math.h>
28 
29 #include <stdio.h>
30 
32 {
33  link1 = 0.06875f;
34  link2 = 0.16f;
35  link3 = 0.0f;
36  link4 = 0.13775f;
37  link5 = 0.11321f;
38 
39  endEffector.p.x = 0.0f; endEffector.p.y = 0.0f; endEffector.p.z = 0.0f;
40  endEffector.n.x = 0.0f; endEffector.n.y = 0.0f; endEffector.n.z = 0.0f;
41  endEffector.o.x = 0.0f; endEffector.o.y = -1.0f; endEffector.o.z = 1.0f;
42  endEffector.a.x = 1.0f; endEffector.a.y = 0.0f; endEffector.a.z = 0.0f;
43 
44  for (int ii = 0; ii < 5; ii++)
45  {
46  joints[ii] = 0.0f;
47  jointOffsets[ii] = 0.0f;
48  jointMin[ii] = 0.0f;
49  jointMax[ii] = 0.0f;
50  }
51 }
52 
53 
55 // Accessor functions
57 
58 void KineCalc::SetP (double newPX, double newPY, double newPZ)
59 {
60  endEffector.p.x = newPX;
61  endEffector.p.y = newPY;
62  endEffector.p.z = newPZ;
63 }
64 
65 void KineCalc::SetN (double newNX, double newNY, double newNZ)
66 {
67  endEffector.n.x = newNX;
68  endEffector.n.y = newNY;
69  endEffector.n.z = newNZ;
70 }
71 
72 void KineCalc::SetO (double newOX, double newOY, double newOZ)
73 {
74  endEffector.o.x = newOX;
75  endEffector.o.y = newOY;
76  endEffector.o.z = newOZ;
77 }
78 
79 void KineCalc::SetA (double newAX, double newAY, double newAZ)
80 {
81  endEffector.a.x = newAX;
82  endEffector.a.y = newAY;
83  endEffector.a.z = newAZ;
84 }
85 
86 double KineCalc::GetTheta (unsigned int index)
87 {
88  return joints[index];
89 }
90 
91 void KineCalc::SetTheta (unsigned int index, double newVal)
92 {
93  joints[index] = newVal;
94 }
95 
96 void KineCalc::SetLinkLengths (double newLink1, double newLink2, double newLink3, double newLink4, double newLink5)
97 {
98  link1 = newLink1;
99  link2 = newLink2;
100  link3 = newLink3;
101  link4 = newLink4;
102  link5 = newLink5;
103 }
104 
105 void KineCalc::SetOffset (unsigned int joint, double newOffset)
106 {
107  jointOffsets[joint] = newOffset;
108 }
109 
110 void KineCalc::SetJointRange (unsigned int joint, double min, double max)
111 {
112  jointMin[joint] = fmin(min, max); // So that if min > max we reverse them
113  jointMax[joint] = fmax(min, max);
114 }
115 
116 
117 
119 // Utility helper functions
121 
123 {
124  KineVector result;
125  double length = sqrt (vector.x * vector.x + vector.y * vector.y + vector.z * vector.z);
126  if (length != 0)
127  {
128  result.x = vector.x / length;
129  result.y = vector.y / length;
130  result.z = vector.z / length;
131  }
132  else
133  {
134  printf ("P2OS: Tried to normalise a vector of zero length.");
135  result.x = 0;
136  result.y = 0;
137  result.z = 0;
138  }
139  return result;
140 }
141 
143 {
144  KineVector result;
145  result.x = pose.o.y * pose.a.z - pose.a.y * pose.o.z;
146  result.y = pose.o.z * pose.a.x - pose.a.z * pose.o.x;
147  result.z = pose.o.x * pose.a.y - pose.a.x * pose.o.y;
148  if (result.x == 0 && result.y == 0 && result.z == 0)
149  {
150  printf ("P2OS: Approach and orientation cannot be the same vector - their cross product cannot be zero.");
151  // Ensures that a different orientation vector is created
152  KineVector orient;
153  if (pose.a.y == 0 && pose.a.z == 0)
154  {
155  orient.x = 0;
156  orient.y = 1;
157  orient.z = 0;
158  }
159  else
160  {
161  orient.x = 1;
162  orient.y = 0;
163  orient.z = 0;
164  }
165  result.x = orient.y * pose.a.z - pose.a.y * orient.z;
166  result.y = orient.z * pose.a.x - pose.a.z * orient.x;
167  result.z = orient.x * pose.a.y - pose.a.x * orient.y;
168  }
169  return Normalise (result);
170 }
171 
172 void KineCalc::PrintEndEffector (const EndEffector &endEffector)
173 {
174  printf ("P: (%f, %f, %f)\tA: (%f, %f, %f)\tO: (%f, %f, %f)\tN: (%f, %f, %f)\n",
175  endEffector.p.x, endEffector.p.y, endEffector.p.z,
176  endEffector.a.x, endEffector.a.y, endEffector.a.z,
177  endEffector.o.x, endEffector.o.y, endEffector.o.z,
178  endEffector.n.x, endEffector.n.y, endEffector.n.z);
179 }
180 
181 
183 // The important functions
185 
186 // Calculate the forward kinematics
187 // The result is stored in endEffector
188 // fromJoints[]: An array of 5 joint values
189 void KineCalc::CalculateFK (const double fromJoints[])
190 {
191  double adjustedJoints[5];
192 
193  adjustedJoints[0] = (fromJoints[0] - jointOffsets[0]) * -1;
194  adjustedJoints[1] = fromJoints[1] - jointOffsets[1];
195  adjustedJoints[2] = fromJoints[2] - jointOffsets[2];
196  adjustedJoints[3] = (fromJoints[3] - jointOffsets[3]) * -1;;
197  adjustedJoints[4] = (fromJoints[4] - jointOffsets[4]) * -1;;
198 
199  endEffector = CalcFKForJoints (adjustedJoints);
200 // printf ("Result of FK:\n");
201 // PrintEndEffector (endEffector);
202 }
203 
204 // Calculate the inverse kinematics
205 // The result is stored in joints
206 // fromPosition: An EndEffector structure describing the pose
207 // of the end effector
208 bool KineCalc::CalculateIK (const EndEffector &fromPosition)
209 {
210  // Some references to make the code neater
211  const KineVector &p = fromPosition.p;
212  const KineVector &a = fromPosition.a;
213  // These are the four possible solutions to the IK
214  // solution1 = {1a, 2a, 3a, 4a, 5a}
215  // solution2 = {1a, 2b, 3b, 4b, 5b}
216  // solution3 = {1b, 2c, 3c, 4c, 5c}
217  // solution4 = {1b, 2d, 3d, 4d, 5d}
218  double solutions[4][5];
219  double temp = 0.0f;
220 
221  // First calculate the two possible values for theta1, theta1a and theta1b
222  temp = atan2 (p.y - link5 * a.y, p.x - link5 * a.x);
223  solutions[0][0] = solutions[1][0] = temp;
224  temp = atan2 (link5 * a.y - p.y, link5 * a.x - p.x);
225  solutions[2][0] = solutions[3][0] = temp;
226 
227  // Next, using theta1_a, calculate thetas 2 and 3 (a and b)
228  // First up is calculating r and rz
229  double r = 0.0f, rz = 0.0f;
230  if (sin (solutions[0][0]) < 0.1f && sin(solutions[0][0]) > -0.1f)
231  {
232  r = ((p.x - (link5 * a.x)) / cos (solutions[0][0])) - link1;
233  }
234  else
235  {
236  r = ((p.y - (link5 * a.y)) / sin (solutions[0][0])) - link1;
237  }
238  rz = p.z - (link5 * a.z);
239  // Then calculate theta2a and 3a
240  temp = (r * r + rz * rz + link2 * link2 - link4 * link4) / (2 * link2 * sqrt (r * r + rz * rz));
241  temp = fmin (fmax (temp, -1.0f), 1.0f);
242  temp = atan2 (rz, r) - acos (temp);
243  int m1 = -1;
244  do
245  {
246  if (m1 > 1)
247  {
248  printf ("m1 > 1!\n");
249  break;
250  }
251  solutions[0][1] = temp + 2 * m1 * M_PI;
252  m1 += 1; // So that within the 3 iterations we get m1 = -1, 0, 1
253  } // Put a catchall here to prevent infinite loops by checking if m1 has gone past 1 (shouldn't happen)
254  while ((solutions[0][1] < -(M_PI) || solutions[0][1] > M_PI));// && m1 < 1);
255  temp = (link2 * link2 + link4 * link4 - r * r - rz * rz) / (2 * link2 * link4);
256  temp = fmin (fmax (temp, -1.0f), 1.0f);
257  solutions[0][2] = M_PI - acos (temp);
258  // Followed by theta2b and 3b
259  temp = (r * r + rz * rz + link2 * link2 - link4 * link4) / (2 * link2 * sqrt (r * r + rz * rz));
260  temp = fmin (fmax (temp, -1.0f), 1.0f);
261  temp = atan2 (rz, r) + acos (temp);
262  m1 = -1;
263  do
264  {
265  if (m1 > 1)
266  {
267  break;
268  }
269  solutions[1][1] = temp + 2 * m1 * M_PI;
270  m1 += 1; // So that within the 3 iterations we get m1 = -1, 0, 1
271  }
272  while ((solutions[1][1] < -(M_PI) || solutions[1][1] > M_PI));// && m1 < 1);
273  temp = (link2 * link2 + link4 * link4 - r * r - rz * rz) / (2 * link2 * link4);
274  temp = fmin (fmax (temp, -1.0f), 1.0f);
275  solutions[1][2] = -(M_PI) + acos (temp);
276 
277  // Using theta2a and 3a, calculate 4a and 5a to complete solution1
278  CalcTheta4and5 (solutions[0], fromPosition);
279  // Using theta2b and 3b, calculate 4b and 5b to complete solution2
280  CalcTheta4and5 (solutions[1], fromPosition);
281 
282  // That's two of the possible solutions. To get the other two, repeat with theta1b
283  // First up is calculating r and rz
284  r = 0.0f;
285  rz = 0.0f;
286  if (sin (solutions[2][0]) < 0.1f && sin(solutions[2][0]) > -0.1f)
287  {
288  r = (p.x - link5 * a.x) / cos (solutions[2][0]) - link1;
289  }
290  else
291  {
292  r = (p.y - link5 * a.y) / sin (solutions[2][0]) - link1;
293  }
294  rz = p.z - (link5 * a.z);
295  // Then calculate theta2c and 3c
296  temp = (r * r + rz * rz + link2 * link2 - link4 * link4) / (2 * link2 * sqrt (r * r + rz * rz));
297  temp = fmin (fmax (temp, -1.0f), 1.0f);
298  temp = atan2 (rz, r) - acos (temp);
299  m1 = -1;
300  do
301  {
302  if (m1 > 1)
303  {
304  break;
305  }
306  solutions[2][1] = temp + 2 * m1 * M_PI;
307  m1 += 1; // So that within the 3 iterations we get m1 = -1, 0, 1
308  } // Put a catchall here to prevent infinite loops by checking if m1 has gone past 1 (shouldn't happen)
309  while ((solutions[2][1] < -(M_PI) || solutions[2][1] > M_PI));// && m1 < 1);
310  temp = (link2 * link2 + link4 * link4 - r * r - rz * rz) / (2 * link2 * link4);
311  temp = fmin (fmax (temp, -1.0f), 1.0f);
312  solutions[2][2] = M_PI - acos (temp);
313  // Followed by theta2d and 3d
314  temp = (r * r + rz * rz + link2 * link2 - link4 * link4) / (2 * link2 * sqrt (r * r + rz * rz));
315  temp = fmin (fmax (temp, -1.0f), 1.0f);
316  temp = atan2 (rz, r) + acos (temp);
317  m1 = -1;
318  do
319  {
320  if (m1 > 1)
321  {
322  break;
323  }
324  solutions[3][1] = temp + 2 * m1 * M_PI;
325  m1 += 1; // So that within the 3 iterations we get m1 = -1, 0, 1
326  }
327  while ((solutions[3][1] < -(M_PI) || solutions[3][1] > M_PI));// && m1 < 1);
328  temp = (link2 * link2 + link4 * link4 - r * r - rz * rz) / (2 * link2 * link4);
329  temp = fmin (fmax (temp, -1.0f), 1.0f);
330  solutions[3][2] = -(M_PI) + acos (temp);
331 
332  // Using theta2c and 3c, calculate 4c and 5c to complete solution1
333  CalcTheta4and5 (solutions[2], fromPosition);
334  // Using theta2d and 3d, calculate 4d and 5d to complete solution2
335  CalcTheta4and5 (solutions[3], fromPosition);
336 
337  // Choose the best of the four solutions
338  int chosenSolution = ChooseSolution (fromPosition, solutions);
339  if (chosenSolution == -1)
340  // Couldn't find a valid solution
341  return false;
342 
343  // Offsets and so forth
344  joints[0] = (solutions[chosenSolution][0] * -1) + jointOffsets[0];
345  joints[1] = solutions[chosenSolution][1] + jointOffsets[1];
346  joints[2] = solutions[chosenSolution][2] + jointOffsets[2];
347  joints[3] = (solutions[chosenSolution][3] * -1) + jointOffsets[3];
348  joints[4] = (solutions[chosenSolution][4] * -1) + jointOffsets[4];
349 
350  return true;
351 }
352 
353 // Calculates thetas 4 and 5 based on supplied thetas 1, 2 and 3 and the desired end effector pose
354 // angles[]: A 5-element array, of which elements 0, 1 and 2 should be filled already
355 // fromPosition: The desired end effector pose
356 void KineCalc::CalcTheta4and5 (double angles[], const EndEffector &fromPosition)
357 {
358  const KineVector &n = fromPosition.n;
359  const KineVector &o = fromPosition.o;
360  const KineVector &a = fromPosition.a;
361 
362  double cos1 = cos (angles[0]);
363  double cos23 = cos (angles[1] + angles[2]);
364  double sin1 = sin (angles[0]);
365  double sin23 = sin (angles[1] + angles[2]);
366 
367  if (cos23 != 0.0f)
368  {
369  if (sin1 < -0.1f || sin1 > 0.1f)
370  {
371  angles[3] = atan2 (n.z / cos23, -(n.x + ((n.z * cos1 * sin23) / cos23)) / sin1);
372  }
373  else
374  {
375  angles[3] = atan2 (n.z / cos23, (n.y + ((n.z * sin1 * sin23) / cos23)) / cos1);
376  }
377 
378  double cos4 = cos (angles[3]);
379  double sin4 = sin (angles[3]);
380  if (cos4 != 0 || sin23 != 0)
381  {
382  angles[4] = atan2 (a.z * cos23 * cos4 - o.z * sin23, o.z * cos23 * cos4 + a.z * sin23);
383  }
384  else
385  {
386  angles[4] = atan2 (-(o.x * cos1 + o.y * sin1) / cos23, (o.x * sin1 - o.y * cos1) / sin4);
387  }
388  }
389  else
390  {
391  angles[4] = atan2 (-o.z / sin23, a.z / sin23);
392 
393  double cos5 = cos (angles[4]);
394  double sin5 = sin (angles[4]);
395  if (cos5 > -0.1f || cos5 < 0.1f)
396  {
397  angles[3] = atan2 ((a.x * sin1 - a.y * cos1) / sin5, -(n.x * sin1) + n.y * cos1);
398  }
399  else
400  {
401  angles[3] = atan2 ((o.x * sin1 - o.y * cos1) / cos5, -(n.x * sin1) + n.y * cos1);
402  }
403  }
404 }
405 
406 // Choose the best solution from the 4 available based on error and reachability
407 // fromPosition: The desired end effector pose
408 // solutions[][]: The four solutions (each with 5 angles) in an array
409 int KineCalc::ChooseSolution (const EndEffector &fromPosition, const double solutions[][5])
410 {
411  double errors[4];
412  int order[4], jj;
413 
414  // We have 4 solutions, calculate the error for each one
415  errors[0] = CalcSolutionError (solutions[0], fromPosition);
416  errors[1] = CalcSolutionError (solutions[1], fromPosition);
417  errors[2] = CalcSolutionError (solutions[2], fromPosition);
418  errors[3] = CalcSolutionError (solutions[3], fromPosition);
419 
420  for (int ii = 0; ii < 4; ii++)
421  {
422  double min = fmin (errors[0], fmin (errors[1], fmin(errors[2], errors[3])));
423  for (jj = 0; min != errors[jj]; jj++); // Find the index at which the min is at
424  errors[jj] = 999999;
425  order[ii] = jj;
426  }
427 
428  for (int ii = 0; ii < 4; ii++)
429  {
430  if (SolutionInRange (solutions[order[ii]]))
431  {
432  return order[ii];
433  }
434  }
435 
436  return -1;
437 }
438 
439 // Calculate the error for a solution from the desired pose
440 // solution[]: An array of 5 angles
441 // fromPosition[]: The end effector pose
442 double KineCalc::CalcSolutionError (const double solution[], const EndEffector &fromPosition)
443 {
444  EndEffector solutionPos;
445  double error = 0.0f;
446 
447  // Calculate the position of the end effector this solution gives using FK
448  solutionPos = CalcFKForJoints (solution);
449  // Calculate the distance from this to the desired position
450  double xOffset = solutionPos.p.x - fromPosition.p.x;
451  double yOffset = solutionPos.p.y - fromPosition.p.y;
452  double zOffset = solutionPos.p.z - fromPosition.p.z;
453 
454  error = sqrt (xOffset * xOffset + yOffset * yOffset + zOffset * zOffset);
455  if (isnan (error))
456  error = 9999;
457 
458  return error;
459 }
460 
461 // Calculates the forward kinematics of a set of joint angles
462 // angles[]: The 5 angles to calculate from
463 EndEffector KineCalc::CalcFKForJoints (const double angles[])
464 {
465  EndEffector result;
466 
467  double cos1 = cos (angles[0]);
468  double cos2 = cos (angles[1]);
469  double cos23 = cos (angles[1] + angles[2]);
470  double cos4 = cos (angles[3]);
471  double cos5 = cos (angles[4]);
472  double sin1 = sin (angles[0]);
473  double sin2 = sin (angles[1]);
474  double sin23 = sin (angles[1] + angles[2]);
475  double sin4 = sin (angles[3]);
476  double sin5 = sin (angles[4]);
477 
478  result.p.x = link5 * ((cos1 * cos23 * cos5) + (sin1 * sin4 * sin5) - (cos1 * sin23 * cos4 * sin5)) +
479  cos1 * ((link4 * cos23) + (link2 * cos2) + link1);
480  result.p.y = link5 * ((sin1 * cos23 * cos5) + (cos1 * sin4 * sin5) - (sin1 * sin23 * cos4 * sin5)) +
481  sin1 * ((link4 * cos23) + (link2 * cos2) + link1);
482  result.p.z = link5 * ((sin23 * cos5) + (cos23 * cos4 * sin5)) + (link4 * sin23) + (link2 * sin2);
483 
484  result.n.x = -(sin1 * cos4) - (cos1 * sin23 * sin4);
485  result.n.y = (cos1 * cos4) - (sin1 * sin23 * sin4);
486  result.n.z = (cos23 * sin4);
487 
488  result.o.x = -(cos1 * cos23 * sin5) + (sin1 * sin4 * cos5) - (cos1 * sin23 * cos4 * cos5);
489  result.o.y = -(sin1 * cos23 * sin5) - (cos1 * sin4 * cos5) - (sin1 * sin23 * cos4 * cos5);
490  result.o.z = -(sin23 * sin5) + (cos23 * cos4 * cos5);
491 
492  result.a.x = (cos1 * cos23 * cos5) + (sin1 * sin4 * sin5) - (cos1 * sin23 * cos4 * sin5);
493  result.a.y = (sin1 * cos23 * cos5) + (cos1 * sin4 * sin5) - (sin1 * sin23 * cos4 * sin5);
494  result.a.z = (sin23 * cos5) + (cos23 * cos4 * sin5);
495 
496  return result;
497 }
498 
499 // Checks if the angles for a solution are reachable by the arm
500 // angles[]: The 5 angles to check
501 bool KineCalc::SolutionInRange (const double angles[])
502 {
503  for (int ii = 0; ii < 5; ii++)
504  {
505  if (angles[ii] < jointMin[ii] || angles[ii] > jointMax[ii] || isnan(angles[ii]))
506  return false;
507  }
508 
509  return true;
510 }
double jointOffsets[5]
Definition: kinecalc.h:97
double CalcSolutionError(const double solution[], const EndEffector &fromPosition)
Definition: kinecalc.cc:442
void CalculateFK(const double fromJoints[])
Definition: kinecalc.cc:189
double x
Definition: kinecalc.h:30
double joints[5]
Definition: kinecalc.h:95
bool CalculateIK(const EndEffector &fromPosition)
Definition: kinecalc.cc:208
void SetO(const KineVector &newO)
Definition: kinecalc.h:59
bool SolutionInRange(const double angles[])
Definition: kinecalc.cc:501
double link1
Definition: kinecalc.h:105
void SetJointRange(unsigned int joint, double min, double max)
Definition: kinecalc.cc:110
void SetOffset(unsigned int joint, double newOffset)
Definition: kinecalc.cc:105
double link4
Definition: kinecalc.h:105
void SetLinkLengths(double newLink1, double newLink2, double newLink3, double newLink4, double newLink5)
Definition: kinecalc.cc:96
double link2
Definition: kinecalc.h:105
void SetTheta(unsigned int index, double newVal)
Definition: kinecalc.cc:91
double jointMax[5]
Definition: kinecalc.h:100
KineVector p
Definition: kinecalc.h:37
void SetN(const KineVector &newN)
Definition: kinecalc.h:58
double GetTheta(unsigned int index)
Definition: kinecalc.cc:86
KineVector n
Definition: kinecalc.h:38
KineVector o
Definition: kinecalc.h:39
KineVector CalculateN(const EndEffector &pose)
Definition: kinecalc.cc:142
double link5
Definition: kinecalc.h:105
void SetP(const KineVector &newP)
Definition: kinecalc.h:57
double y
Definition: kinecalc.h:30
void PrintEndEffector(const EndEffector &endEffector)
Definition: kinecalc.cc:172
EndEffector endEffector
Definition: kinecalc.h:90
double jointMin[5]
Definition: kinecalc.h:99
EndEffector CalcFKForJoints(const double angles[])
Definition: kinecalc.cc:463
double z
Definition: kinecalc.h:30
void CalcTheta4and5(double angles[], const EndEffector &fromPosition)
Definition: kinecalc.cc:356
int ChooseSolution(const EndEffector &fromPosition, const double solutions[][5])
Definition: kinecalc.cc:409
KineVector a
Definition: kinecalc.h:40
void SetA(const KineVector &newA)
Definition: kinecalc.h:60
double link3
Definition: kinecalc.h:105
KineCalc(void)
Definition: kinecalc.cc:31
KineVector Normalise(const KineVector &vector)
Definition: kinecalc.cc:122


p2os_driver
Author(s): Hunter Allen , David Feil-Seifer , Aris Synodinos , Brian Gerkey, Kasper Stoy, Richard Vaughan, Andrew Howard, Tucker Hermans, ActivMedia Robotics LLC, MobileRobots Inc.
autogenerated on Wed Jun 25 2014 09:37:15