Diff: IK_Engine.cpp

Revision:

0:8a7c1e92d067

diff -r 000000000000 -r 8a7c1e92d067 IK_Engine.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IK_Engine.cpp	Sun Oct 16 16:01:08 2011 +0000
@@ -0,0 +1,89 @@
+/********************************************************************************
+ This function finds E.R.I.C.'s joint angles for a given foot position
+ Notebook: http://mbed.org/users/ms523/notebook/eric-leg-inverse-kinematics/
+********************************************************************************/
+#include "FK.h"
+
+Joint_Angles IK_Engine(int X, int Y, int Z, char legs){
+
+    // Adjust X to account for the X-axis offset
+        if(X > 0)
+            X = X - X_OFFSET;
+        else
+            X = X + X_OFFSET;     
+            
+    // Calculate Leg length
+        float A = X*X+Z*Z-B*B;
+        A = sqrt(A);
+        
+    // Calculate beta
+        float beta = A/B;
+        beta = atan(beta);
+        beta = beta * 180 / PI;
+        
+    // Calculate C
+        float C = X*X+Z*Z;
+        C = sqrt(C);
+         
+    // Calculate gamma
+        float gamma = (float)Z / C;
+        gamma = acos(gamma);
+        gamma = gamma * 180 / PI;
+        
+    // Calculate alpha (i.e. lateral servo angle)
+        float alpha = gamma - beta;
+
+    // Calculate the length of the leg
+        float D = A*A+Y*Y;
+        D = sqrt(D);
+
+        // Work out phi (i.e. knee servo angle)
+        float phi;
+        if(legs){           // Fore legs selected
+            phi = ULNA*ULNA+RADIUS*RADIUS-D*D;
+            phi = phi / (2*ULNA*RADIUS);
+            phi = acos(phi);
+            phi = phi * 180 / PI;
+            phi = 360 - phi;            // Because hind legs bend backwards
+        }
+        else{               // Hind legs selected
+            phi = FEMUR*FEMUR+TIBIA*TIBIA-D*D;
+            phi = phi / (2*FEMUR*TIBIA);
+            phi = acos(phi);
+            phi = phi * 180 / PI;
+        }
+
+    // Work out epsilon
+        float epsilon;
+        if(legs){           // Fore legs selected
+            epsilon = ULNA*ULNA-RADIUS*RADIUS+D*D;
+            epsilon = epsilon / (2*D*ULNA);
+        }
+        else{               // Hind legs selected
+            epsilon = FEMUR*FEMUR-TIBIA*TIBIA+D*D;
+            epsilon = epsilon / (2*D*FEMUR);
+        }
+        epsilon = acos(epsilon);
+        epsilon = epsilon * 180 / PI;
+        
+    // Work out delta
+        float delta = (float) Y/ A;                   
+        delta = atan(delta);
+        delta = delta * 180 / PI;
+        
+    // Work out theta (i.e. hip servo angle)
+        float theta;
+        if(legs){           // Fore legs selected
+            theta = 180 - delta + epsilon;      // Because fore legs bend backwards
+        }
+        else{               // Hind legs selected
+            theta = 180 - delta - epsilon;
+        }
+        
+    // Return calculated joint angles   
+        Joint_Angles angles;    // Local variable 
+        angles.lateral = alpha;
+        angles.hip = theta;
+        angles.knee = phi;
+        return(angles);   
+ }
\ No newline at end of file