kinecalc.cc
Go to the documentation of this file.
00001 /*
00002  *  P2OS for ROS
00003  *  Copyright (C) 2009
00004  *     David Feil-Seifer, Brian Gerkey, Kasper Stoy, 
00005  *      Richard Vaughan, & Andrew Howard
00006  *
00007  *
00008  *  This program is free software; you can redistribute it and/or modify
00009  *  it under the terms of the GNU General Public License as published by
00010  *  the Free Software Foundation; either version 2 of the License, or
00011  *  (at your option) any later version.
00012  *
00013  *  This program is distributed in the hope that it will be useful,
00014  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00015  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016  *  GNU General Public License for more details.
00017  *
00018  *  You should have received a copy of the GNU General Public License
00019  *  along with this program; if not, write to the Free Software
00020  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00021  *
00022  */
00023 
00024 
00025 #include <p2os_driver/kinecalc.h>
00026 
00027 #include <math.h>
00028 
00029 #include <stdio.h>
00030 
00031 KineCalc::KineCalc (void)
00032 {
00033   link1 = 0.06875f;
00034   link2 = 0.16f;
00035   link3 = 0.0f;
00036   link4 = 0.13775f;
00037   link5 = 0.11321f;
00038 
00039   endEffector.p.x = 0.0f; endEffector.p.y = 0.0f; endEffector.p.z = 0.0f;
00040   endEffector.n.x = 0.0f; endEffector.n.y = 0.0f; endEffector.n.z = 0.0f;
00041   endEffector.o.x = 0.0f; endEffector.o.y = -1.0f; endEffector.o.z = 1.0f;
00042   endEffector.a.x = 1.0f; endEffector.a.y = 0.0f; endEffector.a.z = 0.0f;
00043 
00044   for (int ii = 0; ii < 5; ii++)
00045   {
00046     joints[ii] = 0.0f;
00047     jointOffsets[ii] = 0.0f;
00048     jointMin[ii] = 0.0f;
00049     jointMax[ii] = 0.0f;
00050   }
00051 }
00052 
00053 
00055 //  Accessor functions
00057 
00058 void KineCalc::SetP (double newPX, double newPY, double newPZ)
00059 {
00060   endEffector.p.x = newPX;
00061   endEffector.p.y = newPY;
00062   endEffector.p.z = newPZ;
00063 }
00064 
00065 void KineCalc::SetN (double newNX, double newNY, double newNZ)
00066 {
00067   endEffector.n.x = newNX;
00068   endEffector.n.y = newNY;
00069   endEffector.n.z = newNZ;
00070 }
00071 
00072 void KineCalc::SetO (double newOX, double newOY, double newOZ)
00073 {
00074   endEffector.o.x = newOX;
00075   endEffector.o.y = newOY;
00076   endEffector.o.z = newOZ;
00077 }
00078 
00079 void KineCalc::SetA (double newAX, double newAY, double newAZ)
00080 {
00081   endEffector.a.x = newAX;
00082   endEffector.a.y = newAY;
00083   endEffector.a.z = newAZ;
00084 }
00085 
00086 double KineCalc::GetTheta (unsigned int index)
00087 {
00088   return joints[index];
00089 }
00090 
00091 void KineCalc::SetTheta (unsigned int index, double newVal)
00092 {
00093   joints[index] = newVal;
00094 }
00095 
00096 void KineCalc::SetLinkLengths (double newLink1, double newLink2, double newLink3, double newLink4, double newLink5)
00097 {
00098   link1 = newLink1;
00099   link2 = newLink2;
00100   link3 = newLink3;
00101   link4 = newLink4;
00102   link5 = newLink5;
00103 }
00104 
00105 void KineCalc::SetOffset (unsigned int joint, double newOffset)
00106 {
00107   jointOffsets[joint] = newOffset;
00108 }
00109 
00110 void KineCalc::SetJointRange (unsigned int joint, double min, double max)
00111 {
00112   jointMin[joint] = fmin(min, max);   // So that if min > max we reverse them
00113   jointMax[joint] = fmax(min, max);
00114 }
00115 
00116 
00117 
00119 //  Utility helper functions
00121 
00122 KineVector KineCalc::Normalise (const KineVector &vector)
00123 {
00124   KineVector result;
00125   double length = sqrt (vector.x * vector.x + vector.y * vector.y + vector.z * vector.z);
00126   if (length != 0)
00127   {
00128     result.x = vector.x / length;
00129     result.y = vector.y / length;
00130     result.z = vector.z / length;
00131   }
00132   else
00133   {
00134     printf ("P2OS: Tried to normalise a vector of zero length.");
00135     result.x = 0;
00136     result.y = 0;
00137     result.z = 0;
00138   }
00139   return result;
00140 }
00141 
00142 KineVector KineCalc::CalculateN (const EndEffector &pose)
00143 {
00144   KineVector result;
00145   result.x = pose.o.y * pose.a.z - pose.a.y * pose.o.z;
00146   result.y = pose.o.z * pose.a.x - pose.a.z * pose.o.x;
00147   result.z = pose.o.x * pose.a.y - pose.a.x * pose.o.y;
00148   if (result.x == 0 && result.y == 0 && result.z == 0)
00149   {
00150     printf ("P2OS: Approach and orientation cannot be the same vector - their cross product cannot be zero.");
00151     // Ensures that a different orientation vector is created
00152     KineVector orient;
00153     if (pose.a.y == 0 && pose.a.z == 0)
00154     {
00155       orient.x = 0;
00156       orient.y = 1;
00157       orient.z = 0;
00158     }
00159     else
00160     {
00161       orient.x = 1;
00162       orient.y = 0;
00163       orient.z = 0;
00164     }
00165     result.x = orient.y * pose.a.z - pose.a.y * orient.z;
00166     result.y = orient.z * pose.a.x - pose.a.z * orient.x;
00167     result.z = orient.x * pose.a.y - pose.a.x * orient.y;
00168   }
00169   return Normalise (result);
00170 }
00171 
00172 void KineCalc::PrintEndEffector (const EndEffector &endEffector)
00173 {
00174   printf ("P: (%f, %f, %f)\tA: (%f, %f, %f)\tO: (%f, %f, %f)\tN: (%f, %f, %f)\n",
00175           endEffector.p.x, endEffector.p.y, endEffector.p.z,
00176           endEffector.a.x, endEffector.a.y, endEffector.a.z,
00177           endEffector.o.x, endEffector.o.y, endEffector.o.z,
00178           endEffector.n.x, endEffector.n.y, endEffector.n.z);
00179 }
00180 
00181 
00183 //  The important functions
00185 
00186 //  Calculate the forward kinematics
00187 //  The result is stored in endEffector
00188 //  fromJoints[]:   An array of 5 joint values
00189 void KineCalc::CalculateFK (const double fromJoints[])
00190 {
00191   double adjustedJoints[5];
00192 
00193   adjustedJoints[0] = (fromJoints[0] - jointOffsets[0]) * -1;
00194   adjustedJoints[1] = fromJoints[1] - jointOffsets[1];
00195   adjustedJoints[2] = fromJoints[2] - jointOffsets[2];
00196   adjustedJoints[3] = (fromJoints[3] - jointOffsets[3]) * -1;;
00197   adjustedJoints[4] = (fromJoints[4] - jointOffsets[4]) * -1;;
00198 
00199   endEffector = CalcFKForJoints (adjustedJoints);
00200 //  printf ("Result of FK:\n");
00201 //  PrintEndEffector (endEffector);
00202 }
00203 
00204 //  Calculate the inverse kinematics
00205 //  The result is stored in joints
00206 //  fromPosition:   An EndEffector structure describing the pose
00207 //                  of the end effector
00208 bool KineCalc::CalculateIK (const EndEffector &fromPosition)
00209 {
00210   // Some references to make the code neater
00211   const KineVector &p = fromPosition.p;
00212   const KineVector &a = fromPosition.a;
00213   // These are the four possible solutions to the IK
00214   // solution1 = {1a, 2a, 3a, 4a, 5a}
00215   // solution2 = {1a, 2b, 3b, 4b, 5b}
00216   // solution3 = {1b, 2c, 3c, 4c, 5c}
00217   // solution4 = {1b, 2d, 3d, 4d, 5d}
00218   double solutions[4][5];
00219   double temp = 0.0f;
00220 
00221   // First calculate the two possible values for theta1, theta1a and theta1b
00222   temp = atan2 (p.y - link5 * a.y, p.x - link5 * a.x);
00223   solutions[0][0] = solutions[1][0] = temp;
00224   temp = atan2 (link5 * a.y - p.y, link5 * a.x - p.x);
00225   solutions[2][0] = solutions[3][0] = temp;
00226 
00227   // Next, using theta1_a, calculate thetas 2 and 3 (a and b)
00228   // First up is calculating r and rz
00229   double r = 0.0f, rz = 0.0f;
00230   if (sin (solutions[0][0]) < 0.1f && sin(solutions[0][0]) > -0.1f)
00231   {
00232     r = ((p.x - (link5 * a.x)) / cos (solutions[0][0])) - link1;
00233   }
00234   else
00235   {
00236     r = ((p.y - (link5 * a.y)) / sin (solutions[0][0])) - link1;
00237   }
00238   rz = p.z - (link5 * a.z);
00239   // Then calculate theta2a and 3a
00240   temp = (r * r + rz * rz + link2 * link2 - link4 * link4) / (2 * link2 * sqrt (r * r + rz * rz));
00241   temp = fmin (fmax (temp, -1.0f), 1.0f);
00242   temp = atan2 (rz, r) - acos (temp);
00243   int m1 = -1;
00244   do
00245   {
00246     if (m1 > 1)
00247     {
00248       printf ("m1 > 1!\n");
00249       break;
00250     }
00251     solutions[0][1] = temp + 2 * m1 * M_PI;
00252     m1 += 1;  // So that within the 3 iterations we get m1 = -1, 0, 1
00253   } // Put a catchall here to prevent infinite loops by checking if m1 has gone past 1 (shouldn't happen)
00254   while ((solutions[0][1] < -(M_PI) || solutions[0][1] > M_PI));// && m1 < 1);
00255   temp = (link2 * link2 + link4 * link4 - r * r - rz * rz) / (2 * link2 * link4);
00256   temp = fmin (fmax (temp, -1.0f), 1.0f);
00257   solutions[0][2] = M_PI - acos (temp);
00258   // Followed by theta2b and 3b
00259   temp = (r * r + rz * rz + link2 * link2 - link4 * link4) / (2 * link2 * sqrt (r * r + rz * rz));
00260   temp = fmin (fmax (temp, -1.0f), 1.0f);
00261   temp = atan2 (rz, r) + acos (temp);
00262   m1 = -1;
00263   do
00264   {
00265     if (m1 > 1)
00266     {
00267       break;
00268     }
00269     solutions[1][1] = temp + 2 * m1 * M_PI;
00270     m1 += 1;  // So that within the 3 iterations we get m1 = -1, 0, 1
00271   }
00272   while ((solutions[1][1] < -(M_PI) || solutions[1][1] > M_PI));// && m1 < 1);
00273   temp = (link2 * link2 + link4 * link4 - r * r - rz * rz) / (2 * link2 * link4);
00274   temp = fmin (fmax (temp, -1.0f), 1.0f);
00275   solutions[1][2] = -(M_PI) + acos (temp);
00276 
00277   // Using theta2a and 3a, calculate 4a and 5a to complete solution1
00278   CalcTheta4and5 (solutions[0], fromPosition);
00279   // Using theta2b and 3b, calculate 4b and 5b to complete solution2
00280   CalcTheta4and5 (solutions[1], fromPosition);
00281 
00282   // That's two of the possible solutions. To get the other two, repeat with theta1b
00283   // First up is calculating r and rz
00284   r = 0.0f;
00285   rz = 0.0f;
00286   if (sin (solutions[2][0]) < 0.1f && sin(solutions[2][0]) > -0.1f)
00287   {
00288     r = (p.x - link5 * a.x) / cos (solutions[2][0]) - link1;
00289   }
00290   else
00291   {
00292     r = (p.y - link5 * a.y) / sin (solutions[2][0]) - link1;
00293   }
00294   rz = p.z - (link5 * a.z);
00295   // Then calculate theta2c and 3c
00296   temp = (r * r + rz * rz + link2 * link2 - link4 * link4) / (2 * link2 * sqrt (r * r + rz * rz));
00297   temp = fmin (fmax (temp, -1.0f), 1.0f);
00298   temp = atan2 (rz, r) - acos (temp);
00299   m1 = -1;
00300   do
00301   {
00302     if (m1 > 1)
00303     {
00304       break;
00305     }
00306     solutions[2][1] = temp + 2 * m1 * M_PI;
00307     m1 += 1;  // So that within the 3 iterations we get m1 = -1, 0, 1
00308   } // Put a catchall here to prevent infinite loops by checking if m1 has gone past 1 (shouldn't happen)
00309   while ((solutions[2][1] < -(M_PI) || solutions[2][1] > M_PI));// && m1 < 1);
00310   temp = (link2 * link2 + link4 * link4 - r * r - rz * rz) / (2 * link2 * link4);
00311   temp = fmin (fmax (temp, -1.0f), 1.0f);
00312   solutions[2][2] = M_PI - acos (temp);
00313   // Followed by theta2d and 3d
00314   temp = (r * r + rz * rz + link2 * link2 - link4 * link4) / (2 * link2 * sqrt (r * r + rz * rz));
00315   temp = fmin (fmax (temp, -1.0f), 1.0f);
00316   temp = atan2 (rz, r) + acos (temp);
00317   m1 = -1;
00318   do
00319   {
00320     if (m1 > 1)
00321     {
00322       break;
00323     }
00324     solutions[3][1] = temp + 2 * m1 * M_PI;
00325     m1 += 1;  // So that within the 3 iterations we get m1 = -1, 0, 1
00326   }
00327   while ((solutions[3][1] < -(M_PI) || solutions[3][1] > M_PI));// && m1 < 1);
00328   temp = (link2 * link2 + link4 * link4 - r * r - rz * rz) / (2 * link2 * link4);
00329   temp = fmin (fmax (temp, -1.0f), 1.0f);
00330   solutions[3][2] = -(M_PI) + acos (temp);
00331 
00332   // Using theta2c and 3c, calculate 4c and 5c to complete solution1
00333   CalcTheta4and5 (solutions[2], fromPosition);
00334   // Using theta2d and 3d, calculate 4d and 5d to complete solution2
00335   CalcTheta4and5 (solutions[3], fromPosition);
00336 
00337   // Choose the best of the four solutions
00338   int chosenSolution = ChooseSolution (fromPosition, solutions);
00339   if (chosenSolution == -1)
00340     // Couldn't find a valid solution
00341     return false;
00342 
00343   // Offsets and so forth
00344   joints[0] = (solutions[chosenSolution][0] * -1) + jointOffsets[0];
00345   joints[1] = solutions[chosenSolution][1] + jointOffsets[1];
00346   joints[2] = solutions[chosenSolution][2] + jointOffsets[2];
00347   joints[3] = (solutions[chosenSolution][3] * -1) + jointOffsets[3];
00348   joints[4] = (solutions[chosenSolution][4] * -1) + jointOffsets[4];
00349 
00350   return true;
00351 }
00352 
00353 //  Calculates thetas 4 and 5 based on supplied thetas 1, 2 and 3 and the desired end effector pose
00354 //  angles[]:       A 5-element array, of which elements 0, 1 and 2 should be filled already
00355 //  fromPosition:   The desired end effector pose
00356 void KineCalc::CalcTheta4and5 (double angles[], const EndEffector &fromPosition)
00357 {
00358   const KineVector &n = fromPosition.n;
00359   const KineVector &o = fromPosition.o;
00360   const KineVector &a = fromPosition.a;
00361 
00362   double cos1 = cos (angles[0]);
00363   double cos23 = cos (angles[1] + angles[2]);
00364   double sin1 = sin (angles[0]);
00365   double sin23 = sin (angles[1] + angles[2]);
00366 
00367   if (cos23 != 0.0f)
00368   {
00369     if (sin1 < -0.1f || sin1 > 0.1f)
00370     {
00371       angles[3] = atan2 (n.z / cos23, -(n.x + ((n.z * cos1 * sin23) / cos23)) / sin1);
00372     }
00373     else
00374     {
00375       angles[3] = atan2 (n.z / cos23, (n.y + ((n.z * sin1 * sin23) / cos23)) / cos1);
00376     }
00377 
00378     double cos4 = cos (angles[3]);
00379     double sin4 = sin (angles[3]);
00380     if (cos4 != 0 || sin23 != 0)
00381     {
00382       angles[4] = atan2 (a.z * cos23 * cos4 - o.z * sin23, o.z * cos23 * cos4 + a.z * sin23);
00383     }
00384     else
00385     {
00386       angles[4] = atan2 (-(o.x * cos1 + o.y * sin1) / cos23, (o.x * sin1 - o.y * cos1) / sin4);
00387     }
00388   }
00389   else
00390   {
00391     angles[4] = atan2 (-o.z / sin23, a.z / sin23);
00392 
00393     double cos5 = cos (angles[4]);
00394     double sin5 = sin (angles[4]);
00395     if (cos5 > -0.1f || cos5 < 0.1f)
00396     {
00397       angles[3] = atan2 ((a.x * sin1 - a.y * cos1) / sin5, -(n.x * sin1) + n.y * cos1);
00398     }
00399     else
00400     {
00401       angles[3] = atan2 ((o.x * sin1 - o.y * cos1) / cos5, -(n.x * sin1) + n.y * cos1);
00402     }
00403   }
00404 }
00405 
00406 //  Choose the best solution from the 4 available based on error and reachability
00407 //  fromPosition:   The desired end effector pose
00408 //  solutions[][]:  The four solutions (each with 5 angles) in an array
00409 int KineCalc::ChooseSolution (const EndEffector &fromPosition, const double solutions[][5])
00410 {
00411   double errors[4];
00412   int order[4], jj;
00413 
00414   // We have 4 solutions, calculate the error for each one
00415   errors[0] = CalcSolutionError (solutions[0], fromPosition);
00416   errors[1] = CalcSolutionError (solutions[1], fromPosition);
00417   errors[2] = CalcSolutionError (solutions[2], fromPosition);
00418   errors[3] = CalcSolutionError (solutions[3], fromPosition);
00419 
00420   for (int ii = 0; ii < 4; ii++)
00421   {
00422     double min = fmin (errors[0], fmin (errors[1], fmin(errors[2], errors[3])));
00423     for (jj = 0; min != errors[jj]; jj++);  // Find the index at which the min is at
00424     errors[jj] = 999999;
00425     order[ii] = jj;
00426   }
00427 
00428   for (int ii = 0; ii < 4; ii++)
00429   {
00430     if (SolutionInRange (solutions[order[ii]]))
00431     {
00432       return order[ii];
00433     }
00434   }
00435 
00436   return -1;
00437 }
00438 
00439 //  Calculate the error for a solution from the desired pose
00440 //  solution[]:       An array of 5 angles
00441 //  fromPosition[]:   The end effector pose
00442 double KineCalc::CalcSolutionError (const double solution[], const EndEffector &fromPosition)
00443 {
00444   EndEffector solutionPos;
00445   double error = 0.0f;
00446 
00447   // Calculate the position of the end effector this solution gives using FK
00448   solutionPos = CalcFKForJoints (solution);
00449   // Calculate the distance from this to the desired position
00450   double xOffset = solutionPos.p.x - fromPosition.p.x;
00451   double yOffset = solutionPos.p.y - fromPosition.p.y;
00452   double zOffset = solutionPos.p.z - fromPosition.p.z;
00453 
00454   error = sqrt (xOffset * xOffset + yOffset * yOffset + zOffset * zOffset);
00455   if (isnan (error))
00456     error = 9999;
00457 
00458   return error;
00459 }
00460 
00461 //  Calculates the forward kinematics of a set of joint angles
00462 //  angles[]:   The 5 angles to calculate from
00463 EndEffector KineCalc::CalcFKForJoints (const double angles[])
00464 {
00465   EndEffector result;
00466 
00467   double cos1 = cos (angles[0]);
00468   double cos2 = cos (angles[1]);
00469   double cos23 = cos (angles[1] + angles[2]);
00470   double cos4 = cos (angles[3]);
00471   double cos5 = cos (angles[4]);
00472   double sin1 = sin (angles[0]);
00473   double sin2 = sin (angles[1]);
00474   double sin23 = sin (angles[1] + angles[2]);
00475   double sin4 = sin (angles[3]);
00476   double sin5 = sin (angles[4]);
00477 
00478   result.p.x = link5 * ((cos1 * cos23 * cos5) + (sin1 * sin4 * sin5) - (cos1 * sin23 * cos4 * sin5)) +
00479       cos1 * ((link4 * cos23) + (link2 * cos2) + link1);
00480   result.p.y = link5 * ((sin1 * cos23 * cos5) + (cos1 * sin4 * sin5) - (sin1 * sin23 * cos4 * sin5)) +
00481       sin1 * ((link4 * cos23) + (link2 * cos2) + link1);
00482   result.p.z = link5 * ((sin23 * cos5) + (cos23 * cos4 * sin5)) + (link4 * sin23) + (link2 * sin2);
00483 
00484   result.n.x = -(sin1 * cos4) - (cos1 * sin23 * sin4);
00485   result.n.y = (cos1 * cos4) - (sin1 * sin23 * sin4);
00486   result.n.z = (cos23 * sin4);
00487 
00488   result.o.x = -(cos1 * cos23 * sin5) + (sin1 * sin4 * cos5) - (cos1 * sin23 * cos4 * cos5);
00489   result.o.y = -(sin1 * cos23 * sin5) - (cos1 * sin4 * cos5) - (sin1 * sin23 * cos4 * cos5);
00490   result.o.z = -(sin23 * sin5) + (cos23 * cos4 * cos5);
00491 
00492   result.a.x = (cos1 * cos23 * cos5) + (sin1 * sin4 * sin5) - (cos1 * sin23 * cos4 * sin5);
00493   result.a.y = (sin1 * cos23 * cos5) + (cos1 * sin4 * sin5) - (sin1 * sin23 * cos4 * sin5);
00494   result.a.z = (sin23 * cos5) + (cos23 * cos4 * sin5);
00495 
00496   return result;
00497 }
00498 
00499 //  Checks if the angles for a solution are reachable by the arm
00500 //  angles[]:   The 5 angles to check
00501 bool KineCalc::SolutionInRange (const double angles[])
00502 {
00503   for (int ii = 0; ii < 5; ii++)
00504   {
00505     if (angles[ii] < jointMin[ii] || angles[ii] > jointMax[ii] || isnan(angles[ii]))
00506       return false;
00507   }
00508 
00509   return true;
00510 }


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 Mon Oct 6 2014 03:12:45