ik.cpp

00001 /* Inverse kinetics, Nick Moriarty May 2014
00002 This code is provided under the terms of the MIT license.
00003 The MIT License (MIT)
00004 Copyright (c) 2014 Nick Moriarty
00005 Permission is hereby granted, free of charge, to any person obtaining a copy 
00006 of this software and associated documentation files (the "Software"), to deal 
00007 in the Software without restriction, including without limitation the rights 
00008 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 
00009 copies of the Software, and to permit persons to whom the Software is 
00010 furnished to do so, subject to the following conditions:
00011 The above copyright notice and this permission notice shall be included in 
00012 all copies or substantial portions of the Software.
00013 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 
00014 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 
00015 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 
00016 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 
00017 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 
00018 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 
00019 SOFTWARE.
00020  */
00021 #include "math.h"
00022 #include "ik.h"
00023 #include "config.h"
00024 
00025 const float PI=3.14159265359;
00026 
00027 // Get polar coords from cartesian ones
00028 void cart2polar(float a, float b, float& r, float& theta)
00029 {
00030     // Determine magnitude of cartesian coords
00031     r = sqrt(a*a + b*b);
00032 
00033     // Don't try to calculate zero-magnitude vectors' angles
00034     if(r == 0) return;
00035 
00036     float c = a / r;
00037     float s = b / r;
00038 
00039     // Safety!
00040     if(s > 1) s = 1;
00041     if(c > 1) c = 1;
00042     if(s < -1) s = -1;
00043     if(c < -1) c = -1;
00044 
00045     // Calculate angle in 0..PI
00046     theta = acos(c);
00047 
00048     // Convert to full range
00049     if(s < 0) theta *= -1;
00050 }
00051 
00052 // Get angle from a triangle using cosine rule
00053 bool cosangle(float opp, float adj1, float adj2, float& theta)
00054 {
00055     // Cosine rule:
00056     // C^2 = A^2 + B^2 - 2*A*B*cos(angle_AB)
00057     // cos(angle_AB) = (A^2 + B^2 - C^2)/(2*A*B)
00058     // C is opposite
00059     // A, B are adjacent
00060     float den = 2*adj1*adj2;
00061 
00062     if(den==0) return false;
00063     float c = (adj1*adj1 + adj2*adj2 - opp*opp)/den;
00064 
00065     if(c>1 || c<-1) return false;
00066 
00067     theta = acos(c);
00068 
00069     return true;
00070 }
00071 
00072 // Solve angles!
00073 bool solve(float x, float y, float z, float& a0, float& a1, float& a2)
00074 {
00075     // Solve top-down view
00076     float r, th0;
00077     cart2polar(y, x, r, th0);
00078 
00079     // Account for the wrist length!
00080     r -= L3;
00081 
00082     // In arm plane, convert to polar
00083     float ang_P, R;
00084     cart2polar(r, z, R, ang_P);
00085 
00086     // Solve arm inner angles as required
00087     float B, C;
00088     if(!cosangle(L2, L1, R, B)) return false;
00089     if(!cosangle(R, L1, L2, C)) return false;
00090 
00091     // Solve for servo angles from horizontal
00092     a0 = th0;
00093     a1 = ang_P + B;
00094     a2 = C + a1 - PI;
00095 
00096     return true;
00097 }