Martin Smith
IK_Engine.cpp@0:0965dacb3caf, 2011-08-21 (annotated)
- Committer:
- ms523
- Date:
- Sun Aug 21 08:25:00 2011 +0000
- Revision:
- 0:0965dacb3caf
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| ms523 | 0:0965dacb3caf | 1 | /******************************************************************************** |
| ms523 | 0:0965dacb3caf | 2 | This function finds E.R.I.C.'s joint angles for a given foot position |
| ms523 | 0:0965dacb3caf | 3 | Notebook: http://mbed.org/users/ms523/notebook/eric-leg-inverse-kinematics/ |
| ms523 | 0:0965dacb3caf | 4 | ********************************************************************************/ |
| ms523 | 0:0965dacb3caf | 5 | #include "ERIC.h" |
| ms523 | 0:0965dacb3caf | 6 | |
| ms523 | 0:0965dacb3caf | 7 | Joint_Angles IK_Engine(int X, int Y, int Z){ |
| ms523 | 0:0965dacb3caf | 8 | |
| ms523 | 0:0965dacb3caf | 9 | // Adjust X to account for the X-axis offset |
| ms523 | 0:0965dacb3caf | 10 | if(X > 0) |
| ms523 | 0:0965dacb3caf | 11 | X = X - X_OFFSET; |
| ms523 | 0:0965dacb3caf | 12 | else |
| ms523 | 0:0965dacb3caf | 13 | X = X + X_OFFSET; |
| ms523 | 0:0965dacb3caf | 14 | |
| ms523 | 0:0965dacb3caf | 15 | // Calculate Leg length |
| ms523 | 0:0965dacb3caf | 16 | float A = X*X+Z*Z-B*B; |
| ms523 | 0:0965dacb3caf | 17 | A = sqrt(A); |
| ms523 | 0:0965dacb3caf | 18 | |
| ms523 | 0:0965dacb3caf | 19 | // Calculate beta |
| ms523 | 0:0965dacb3caf | 20 | float beta = A/B; |
| ms523 | 0:0965dacb3caf | 21 | beta = atan(beta); |
| ms523 | 0:0965dacb3caf | 22 | beta = beta * 180 / PI; |
| ms523 | 0:0965dacb3caf | 23 | |
| ms523 | 0:0965dacb3caf | 24 | // Calculate C |
| ms523 | 0:0965dacb3caf | 25 | float C = X*X+Z*Z; |
| ms523 | 0:0965dacb3caf | 26 | C = sqrt(C); |
| ms523 | 0:0965dacb3caf | 27 | |
| ms523 | 0:0965dacb3caf | 28 | // Calculate gamma |
| ms523 | 0:0965dacb3caf | 29 | float gamma = (float)Z / C; |
| ms523 | 0:0965dacb3caf | 30 | gamma = acos(gamma); |
| ms523 | 0:0965dacb3caf | 31 | gamma = gamma * 180 / PI; |
| ms523 | 0:0965dacb3caf | 32 | |
| ms523 | 0:0965dacb3caf | 33 | // Calculate alpha (i.e. lateral servo angle) |
| ms523 | 0:0965dacb3caf | 34 | float alpha = gamma - beta; |
| ms523 | 0:0965dacb3caf | 35 | |
| ms523 | 0:0965dacb3caf | 36 | // Calculate the length of the leg |
| ms523 | 0:0965dacb3caf | 37 | float D = A*A+Y*Y; |
| ms523 | 0:0965dacb3caf | 38 | D = sqrt(D); |
| ms523 | 0:0965dacb3caf | 39 | |
| ms523 | 0:0965dacb3caf | 40 | // Work out phi (i.e. knee servo angle) |
| ms523 | 0:0965dacb3caf | 41 | float phi = E*E+F*F-D*D; |
| ms523 | 0:0965dacb3caf | 42 | phi = phi / (2*E*F); |
| ms523 | 0:0965dacb3caf | 43 | phi = acos(phi); |
| ms523 | 0:0965dacb3caf | 44 | phi = phi * 180 / PI; |
| ms523 | 0:0965dacb3caf | 45 | |
| ms523 | 0:0965dacb3caf | 46 | // Work out epsilon |
| ms523 | 0:0965dacb3caf | 47 | float epsilon = E*E-F*F+D*D; |
| ms523 | 0:0965dacb3caf | 48 | epsilon = epsilon / (2*D*E); |
| ms523 | 0:0965dacb3caf | 49 | epsilon = acos(epsilon); |
| ms523 | 0:0965dacb3caf | 50 | epsilon = epsilon * 180 / PI; |
| ms523 | 0:0965dacb3caf | 51 | |
| ms523 | 0:0965dacb3caf | 52 | // Work out delta |
| ms523 | 0:0965dacb3caf | 53 | float delta = (float) Y/ A; |
| ms523 | 0:0965dacb3caf | 54 | delta = atan(delta); |
| ms523 | 0:0965dacb3caf | 55 | delta = delta * 180 / PI; |
| ms523 | 0:0965dacb3caf | 56 | |
| ms523 | 0:0965dacb3caf | 57 | // Work out theta (i.e. hip servo angle) |
| ms523 | 0:0965dacb3caf | 58 | float theta = 180 - delta - epsilon; |
| ms523 | 0:0965dacb3caf | 59 | |
| ms523 | 0:0965dacb3caf | 60 | // Return calculated joint angles |
| ms523 | 0:0965dacb3caf | 61 | Joint_Angles angles; // Local variable |
| ms523 | 0:0965dacb3caf | 62 | angles.lateral = alpha; |
| ms523 | 0:0965dacb3caf | 63 | angles.hip = theta; |
| ms523 | 0:0965dacb3caf | 64 | angles.knee = phi; |
| ms523 | 0:0965dacb3caf | 65 | return(angles); |
| ms523 | 0:0965dacb3caf | 66 | } |