Ian Krase / ArthropodIK

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   Quadrapod NeoQuadrapod

Files at this revision

API Documentation at this revision

Revision 1:031a0c78d8d6, committed 2015-06-26

Comitter:
ikrase
Date:
Fri Jun 26 04:21:43 2015 +0000
Parent:
0:9805c9d36254
Child:
2:6214ab5b94cb
Commit message:
Finished library, not yet tested.

Changed in this revision

ArthropodIK.cpp Show annotated file Show diff for this revision Revisions of this file
ArthropodIK.h Show annotated file Show diff for this revision Revisions of this file 
--- a/ArthropodIK.cpp	Thu Jun 25 07:08:02 2015 +0000
+++ b/ArthropodIK.cpp	Fri Jun 26 04:21:43 2015 +0000
@@ -41,12 +41,46 @@
     
 
 
-float * ArthropodSolver::SolveBody(sixDOF_t BodyTarget6D[], float LegPriorPos[], int LegNum) {
-        static float foot_pos_out[NUM_LEGS];
+float * ArthropodSolver::SolveBody(sixDOF_t BodyTarget6D, float LegPriorPos[], int LegNum) {
+        
+        static float foot_pos_out[3];
+        float xp= 0, yp = 0; 
         
-        // IK code 
+        float foot_pos_inter[3]; 
+        float foot_pos_new[3]; 
+        switch (LegNum) {                   // Get the coordinates relative to the robot center rather than the leg axis. 
+            case 0: 
+                xp = HIP_DISP_ORTHO;
+                yp = HIP_DISP_ORTHO;
+                break;
+            case 1: 
+                xp = -HIP_DISP_ORTHO;
+                yp = HIP_DISP_ORTHO;
+                break;
+            case 2: 
+                xp = -HIP_DISP_ORTHO;
+                yp = -HIP_DISP_ORTHO;
+                break;
+            case 3: 
+                xp = HIP_DISP_ORTHO;
+                yp = -HIP_DISP_ORTHO;
+                break;
+            }            
+        foot_pos_new[0] = LegPriorPos[0] + xp + BodyTarget6D.xyz[0];
+        foot_pos_new[1] = LegPriorPos[1] + yp + BodyTarget6D.xyz[1];
+        foot_pos_new[2] = LegPriorPos[2]  +  BodyTarget6D.xyz[2];
         
-        // Loop calls SolveLeg()
+        YawXform(foot_pos_new,foot_pos_inter,BodyTarget6D.ypr[0]); // apply yaw transformation first. 
+        PitchXform(foot_pos_inter, foot_pos_new,BodyTarget6D.ypr[1]);
+        RollXform(foot_pos_new, foot_pos_inter,BodyTarget6D.ypr[2]);
+        
+        foot_pos_out[0] = foot_pos_inter[0] - xp  - BodyTarget6D.xyz[0];
+        foot_pos_out[1] = foot_pos_inter[1] - yp - BodyTarget6D.xyz[0];
+        foot_pos_out[2] = foot_pos_inter[2] - BodyTarget6D.xyz[0];
+        
+       
+       
+       
         
         return foot_pos_out; 
 }

--- a/ArthropodIK.h	Thu Jun 25 07:08:02 2015 +0000
+++ b/ArthropodIK.h	Fri Jun 26 04:21:43 2015 +0000
@@ -74,7 +74,7 @@
     
     //leg_angles_t * SolveBody(sixDOF_t BodyTarget6D[], leg_angles_t LegPriorPos[]); 
     
-    float * SolveBody(sixDOF_t BodyTarget6D[], float LegPriorPos[], int LegNum);
+    float * SolveBody(sixDOF_t BodyTarget6D, float LegPriorPos[], int LegNum);