Revision 0:9805c9d36254, committed 2015-06-25

Comitter:

ikrase

Date:

Thu Jun 25 07:08:02 2015 +0000

Child:

1:031a0c78d8d6

Commit message:

Made into a library.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ArthropodIK.cpp	Thu Jun 25 07:08:02 2015 +0000
@@ -0,0 +1,99 @@
+#include "mbed.h"
+#include "quadrapod_defs.h"
+#include "ArthropodIK.h"
+
+float sq(float n1){
+    float res = n1 * n1;
+    return res;
+    }
+    
+    
+const float ArthropodSolver::LEG_ANGLES[4] = {0.78539816339, 2.35619449019, -2.35619449019, -0.78539816339, };
+
+
+ArthropodSolver::ArthropodSolver()  // Constuctor initializes thing. 
+{ 
+
+
+}
+
+
+ leg_angles_t ArthropodSolver::SolveLeg(float LegTargetXYZ[], int LegNum)
+    {
+        leg_angles_t angles_out; 
+        
+        angles_out.gamma = atan2(LegTargetXYZ[1], LegTargetXYZ[0]) - LEG_ANGLES[LegNum]; 
+        
+        float lp = sqrt(sq(LegTargetXYZ[0]) + sq(LegTargetXYZ[1]));
+        
+        float L2 = sq(LegTargetXYZ[2]) + sq(lp - COXA_L);
+        float a1 = acos(-LegTargetXYZ[2] / sqrt(L2));
+        float a2 = acos((sq(TIBIA_L)-sq(FEMUR_L)-L2)/(-2*FEMUR_L*sqrt(L2)));
+        angles_out.alpha = a1 + a2;
+        angles_out.beta = acos((-sq(TIBIA_L)-sq(FEMUR_L)+L2)/(-2*FEMUR_L*TIBIA_L));
+        
+        
+        // IK code, the most basic section. 
+        
+        
+        return angles_out; // THIS WILL OVERWRITE WITH MULTIPLE CALLS!!!!!!       
+}
+    
+
+
+float * ArthropodSolver::SolveBody(sixDOF_t BodyTarget6D[], float LegPriorPos[], int LegNum) {
+        static float foot_pos_out[NUM_LEGS];
+        
+        // IK code 
+        
+        // Loop calls SolveLeg()
+        
+        return foot_pos_out; 
+}
+    
+    
+    
+    
+float * ArthropodSolver::SolveLegFwd(leg_angles_t LegAngles, int LegNum){
+        static float footpos_out[3]; //x y z
+             
+         // code
+         
+        float L; float b1;  
+        
+        b1 = LegAngles.beta - (PI - LegAngles.alpha); // The part of beta that matters. 
+        
+        L = sin(b1) * TIBIA_L + COXA_L + sin(LegAngles.alpha) * FEMUR_L; // the extension length. 
+        
+        footpos_out[0] = L * cos(LEG_ANGLES[LegNum] + LegAngles.gamma); 
+        footpos_out[1] = L * sin(LEG_ANGLES[LegNum] + LegAngles.gamma); 
+        footpos_out[2] = -cos(LegAngles.alpha) - TIBIA_L*cos(b1) ;
+         
+         
+    
+        return footpos_out; 
+}        
+
+void  ArthropodSolver::YawXform(float invec[], float outvec[], float angle) {
+    outvec[0] = invec[0]*cos(angle)- invec[1]*sin(angle); 
+    outvec[1] = invec[0] * sin(angle) + outvec[1] * cos(angle);
+    outvec[2] = invec[2];        
+         
+         
+         
+}
+     
+void  ArthropodSolver::PitchXform(float invec[], float outvec[], float angle){
+    outvec[1] = invec[1]*cos(angle)- invec[2]*sin(angle); 
+    outvec[2] = invec[1] * sin(angle) + outvec[2] * cos(angle);
+    outvec[0] = invec[0]; 
+     
+}
+     
+void  ArthropodSolver::RollXform(float invec[], float outvec[], float angle){
+    outvec[2] = invec[2]*cos(angle)- invec[0]*sin(angle); 
+    outvec[0] = invec[0] * cos(angle) + outvec[2] * sin(angle);
+    outvec[1] = invec[1];  
+     
+}
+     

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ArthropodIK.h	Thu Jun 25 07:08:02 2015 +0000
@@ -0,0 +1,111 @@
+#ifndef ARTHROPODIK_H
+#define ARTHROPODIK_H
+
+
+#define NUM_LEGS  4
+#define COXA_L 30.0 //mm
+#define FEMUR_L 70.0
+#define TIBIA_L 100.0         // NEED TO ACTUALLY MEASURE. 
+#define HIP_DISP_ORTHO 50.0     // Orthogonal distance from center of robot to hip vertical joint. 
+
+
+//MASSIVE CREDITS to http://blog.oscarliang.net/inverse-kinematics-and-trigonometry-basics/: Oscar Liang
+
+// Probably a few typdef structs?
+
+
+
+typedef struct leg_angles {     // ALL OF THIS IS IN RADIANS. ALL ANGLES IN THIS LIBRARY ARE IN RADIANS. 
+    float gamma;            // CCW *displacement* angle of the hip from above
+    float beta;             // Included angle of the knee. Is pi/2 when knee is bent at a right angle (this is the resting position.)
+    float alpha;            // Angle of the hip above vertical axis. Is pi/2 when leg is sticking out straight (i.e. the resting position). 
+} leg_angles_t; 
+
+typedef struct sixDOF {
+    float xyz[3]; 
+    float ypr[3]; // that's yaw, pitch, and roll.
+} sixDOF_t; 
+
+float sq(float n1);
+
+/* this library assumes that all legs will be identical in lengths. */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+class ArthropodSolver {
+    
+public: 
+
+    static const float LEG_ANGLES[4];
+
+    //int numLegs;
+    
+    
+    
+    /**
+     *         Instantiate a new Arthropod Solver, entering the leg parameters
+     */
+    
+    ArthropodSolver();    // constructor. 
+        
+
+    
+ 
+    
+    
+    /** 
+     *  Determine the joint angles for a leg with the toe at a specified position
+     *
+     * @Returns a leg angle struct in float degrees. 
+     */
+    
+    //leg_angles_t SolveLeg(float LegTargetXYZ[], int LegNum); 
+    
+    leg_angles_t SolveLeg(float LegTargetXYZ[], int LegNum);
+    
+    
+    
+    
+    
+    /**
+     *  Deterimine the joint angles to shift the body to a different position, keeping the legs planted. 
+     */
+    
+    //leg_angles_t * SolveBody(sixDOF_t BodyTarget6D[], leg_angles_t LegPriorPos[]); 
+    
+    float * SolveBody(sixDOF_t BodyTarget6D[], float LegPriorPos[], int LegNum);
+    
+    
+    
+    
+    
+    
+    
+    
+    /**
+     * Determine the position of a foot, given the leg angles. 
+     */
+     
+     float * SolveLegFwd(leg_angles_t LegAngles, int LegNum);
+     
+     void  YawXform(float invec[], float outvec[], float angle);
+     
+     void  PitchXform(float invec[], float outvec[], float angle);
+     
+     void  RollXform(float invec[], float outvec[], float angle);
+    
+    
+    
+    
+    
+    
+};
+
+
+
+//const float ArthropodSolver::LEG_ANGLES[4] = {0.78539816339, 2.35619449019, -2.35619449019, -0.78539816339, }; /// moved to cpp file. 
+
+
+
+#endif
\ No newline at end of file