Ian Krase / ArthropodIK

Library for analyitical inverse kinematics on 4-legged 3DOF per leg quadrupod robots. Easily modifyable for more legs.

Dependents:   Quadrapod NeoQuadrapod

ArthropodIK.cpp@0:9805c9d36254, 2015-06-25 (annotated)

Committer:
ikrase
Date:
Thu Jun 25 07:08:02 2015 +0000
Revision:
0:9805c9d36254
Child:
1:031a0c78d8d6
Made into a library.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ikrase 0:9805c9d36254 1 #include "mbed.h"
ikrase 0:9805c9d36254 2 #include "quadrapod_defs.h"
ikrase 0:9805c9d36254 3 #include "ArthropodIK.h"
ikrase 0:9805c9d36254 4
ikrase 0:9805c9d36254 5 float sq(float n1){
ikrase 0:9805c9d36254 6 float res = n1 * n1;
ikrase 0:9805c9d36254 7 return res;
ikrase 0:9805c9d36254 8 }
ikrase 0:9805c9d36254 9
ikrase 0:9805c9d36254 10
ikrase 0:9805c9d36254 11 const float ArthropodSolver::LEG_ANGLES[4] = {0.78539816339, 2.35619449019, -2.35619449019, -0.78539816339, };
ikrase 0:9805c9d36254 12
ikrase 0:9805c9d36254 13
ikrase 0:9805c9d36254 14 ArthropodSolver::ArthropodSolver() // Constuctor initializes thing.
ikrase 0:9805c9d36254 15 {
ikrase 0:9805c9d36254 16
ikrase 0:9805c9d36254 17
ikrase 0:9805c9d36254 18 }
ikrase 0:9805c9d36254 19
ikrase 0:9805c9d36254 20
ikrase 0:9805c9d36254 21 leg_angles_t ArthropodSolver::SolveLeg(float LegTargetXYZ[], int LegNum)
ikrase 0:9805c9d36254 22 {
ikrase 0:9805c9d36254 23 leg_angles_t angles_out;
ikrase 0:9805c9d36254 24
ikrase 0:9805c9d36254 25 angles_out.gamma = atan2(LegTargetXYZ[1], LegTargetXYZ[0]) - LEG_ANGLES[LegNum];
ikrase 0:9805c9d36254 26
ikrase 0:9805c9d36254 27 float lp = sqrt(sq(LegTargetXYZ[0]) + sq(LegTargetXYZ[1]));
ikrase 0:9805c9d36254 28
ikrase 0:9805c9d36254 29 float L2 = sq(LegTargetXYZ[2]) + sq(lp - COXA_L);
ikrase 0:9805c9d36254 30 float a1 = acos(-LegTargetXYZ[2] / sqrt(L2));
ikrase 0:9805c9d36254 31 float a2 = acos((sq(TIBIA_L)-sq(FEMUR_L)-L2)/(-2*FEMUR_L*sqrt(L2)));
ikrase 0:9805c9d36254 32 angles_out.alpha = a1 + a2;
ikrase 0:9805c9d36254 33 angles_out.beta = acos((-sq(TIBIA_L)-sq(FEMUR_L)+L2)/(-2*FEMUR_L*TIBIA_L));
ikrase 0:9805c9d36254 34
ikrase 0:9805c9d36254 35
ikrase 0:9805c9d36254 36 // IK code, the most basic section.
ikrase 0:9805c9d36254 37
ikrase 0:9805c9d36254 38
ikrase 0:9805c9d36254 39 return angles_out; // THIS WILL OVERWRITE WITH MULTIPLE CALLS!!!!!!
ikrase 0:9805c9d36254 40 }
ikrase 0:9805c9d36254 41
ikrase 0:9805c9d36254 42
ikrase 0:9805c9d36254 43
ikrase 0:9805c9d36254 44 float * ArthropodSolver::SolveBody(sixDOF_t BodyTarget6D[], float LegPriorPos[], int LegNum) {
ikrase 0:9805c9d36254 45 static float foot_pos_out[NUM_LEGS];
ikrase 0:9805c9d36254 46
ikrase 0:9805c9d36254 47 // IK code
ikrase 0:9805c9d36254 48
ikrase 0:9805c9d36254 49 // Loop calls SolveLeg()
ikrase 0:9805c9d36254 50
ikrase 0:9805c9d36254 51 return foot_pos_out;
ikrase 0:9805c9d36254 52 }
ikrase 0:9805c9d36254 53
ikrase 0:9805c9d36254 54
ikrase 0:9805c9d36254 55
ikrase 0:9805c9d36254 56
ikrase 0:9805c9d36254 57 float * ArthropodSolver::SolveLegFwd(leg_angles_t LegAngles, int LegNum){
ikrase 0:9805c9d36254 58 static float footpos_out[3]; //x y z
ikrase 0:9805c9d36254 59
ikrase 0:9805c9d36254 60 // code
ikrase 0:9805c9d36254 61
ikrase 0:9805c9d36254 62 float L; float b1;
ikrase 0:9805c9d36254 63
ikrase 0:9805c9d36254 64 b1 = LegAngles.beta - (PI - LegAngles.alpha); // The part of beta that matters.
ikrase 0:9805c9d36254 65
ikrase 0:9805c9d36254 66 L = sin(b1) * TIBIA_L + COXA_L + sin(LegAngles.alpha) * FEMUR_L; // the extension length.
ikrase 0:9805c9d36254 67
ikrase 0:9805c9d36254 68 footpos_out[0] = L * cos(LEG_ANGLES[LegNum] + LegAngles.gamma);
ikrase 0:9805c9d36254 69 footpos_out[1] = L * sin(LEG_ANGLES[LegNum] + LegAngles.gamma);
ikrase 0:9805c9d36254 70 footpos_out[2] = -cos(LegAngles.alpha) - TIBIA_L*cos(b1) ;
ikrase 0:9805c9d36254 71
ikrase 0:9805c9d36254 72
ikrase 0:9805c9d36254 73
ikrase 0:9805c9d36254 74 return footpos_out;
ikrase 0:9805c9d36254 75 }
ikrase 0:9805c9d36254 76
ikrase 0:9805c9d36254 77 void ArthropodSolver::YawXform(float invec[], float outvec[], float angle) {
ikrase 0:9805c9d36254 78 outvec[0] = invec[0]*cos(angle)- invec[1]*sin(angle);
ikrase 0:9805c9d36254 79 outvec[1] = invec[0] * sin(angle) + outvec[1] * cos(angle);
ikrase 0:9805c9d36254 80 outvec[2] = invec[2];
ikrase 0:9805c9d36254 81
ikrase 0:9805c9d36254 82
ikrase 0:9805c9d36254 83
ikrase 0:9805c9d36254 84 }
ikrase 0:9805c9d36254 85
ikrase 0:9805c9d36254 86 void ArthropodSolver::PitchXform(float invec[], float outvec[], float angle){
ikrase 0:9805c9d36254 87 outvec[1] = invec[1]*cos(angle)- invec[2]*sin(angle);
ikrase 0:9805c9d36254 88 outvec[2] = invec[1] * sin(angle) + outvec[2] * cos(angle);
ikrase 0:9805c9d36254 89 outvec[0] = invec[0];
ikrase 0:9805c9d36254 90
ikrase 0:9805c9d36254 91 }
ikrase 0:9805c9d36254 92
ikrase 0:9805c9d36254 93 void ArthropodSolver::RollXform(float invec[], float outvec[], float angle){
ikrase 0:9805c9d36254 94 outvec[2] = invec[2]*cos(angle)- invec[0]*sin(angle);
ikrase 0:9805c9d36254 95 outvec[0] = invec[0] * cos(angle) + outvec[2] * sin(angle);
ikrase 0:9805c9d36254 96 outvec[1] = invec[1];
ikrase 0:9805c9d36254 97
ikrase 0:9805c9d36254 98 }
ikrase 0:9805c9d36254 99