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@2:6214ab5b94cb, 2015-06-27 (annotated)

Committer:
ikrase
Date:
Sat Jun 27 08:19:04 2015 +0000
Revision:
2:6214ab5b94cb
Parent:
1:031a0c78d8d6 
Library tweak (subtle)

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 2:6214ab5b94cb 5
ikrase 0:9805c9d36254 6 float sq(float n1){
ikrase 0:9805c9d36254 7 float res = n1 * n1;
ikrase 0:9805c9d36254 8 return res;
ikrase 0:9805c9d36254 9 }
ikrase 0:9805c9d36254 10
ikrase 0:9805c9d36254 11
ikrase 0:9805c9d36254 12 const float ArthropodSolver::LEG_ANGLES[4] = {0.78539816339, 2.35619449019, -2.35619449019, -0.78539816339, };
ikrase 0:9805c9d36254 13
ikrase 0:9805c9d36254 14
ikrase 0:9805c9d36254 15 ArthropodSolver::ArthropodSolver() // Constuctor initializes thing.
ikrase 0:9805c9d36254 16 {
ikrase 0:9805c9d36254 17
ikrase 0:9805c9d36254 18
ikrase 0:9805c9d36254 19 }
ikrase 0:9805c9d36254 20
ikrase 0:9805c9d36254 21
ikrase 0:9805c9d36254 22 leg_angles_t ArthropodSolver::SolveLeg(float LegTargetXYZ[], int LegNum)
ikrase 0:9805c9d36254 23 {
ikrase 0:9805c9d36254 24 leg_angles_t angles_out;
ikrase 0:9805c9d36254 25
ikrase 0:9805c9d36254 26 angles_out.gamma = atan2(LegTargetXYZ[1], LegTargetXYZ[0]) - LEG_ANGLES[LegNum];
ikrase 0:9805c9d36254 27
ikrase 0:9805c9d36254 28 float lp = sqrt(sq(LegTargetXYZ[0]) + sq(LegTargetXYZ[1]));
ikrase 0:9805c9d36254 29
ikrase 0:9805c9d36254 30 float L2 = sq(LegTargetXYZ[2]) + sq(lp - COXA_L);
ikrase 0:9805c9d36254 31 float a1 = acos(-LegTargetXYZ[2] / sqrt(L2));
ikrase 0:9805c9d36254 32 float a2 = acos((sq(TIBIA_L)-sq(FEMUR_L)-L2)/(-2*FEMUR_L*sqrt(L2)));
ikrase 0:9805c9d36254 33 angles_out.alpha = a1 + a2;
ikrase 0:9805c9d36254 34 angles_out.beta = acos((-sq(TIBIA_L)-sq(FEMUR_L)+L2)/(-2*FEMUR_L*TIBIA_L));
ikrase 0:9805c9d36254 35
ikrase 0:9805c9d36254 36
ikrase 0:9805c9d36254 37 // IK code, the most basic section.
ikrase 0:9805c9d36254 38
ikrase 0:9805c9d36254 39
ikrase 0:9805c9d36254 40 return angles_out; // THIS WILL OVERWRITE WITH MULTIPLE CALLS!!!!!!
ikrase 0:9805c9d36254 41 }
ikrase 0:9805c9d36254 42
ikrase 0:9805c9d36254 43
ikrase 0:9805c9d36254 44
ikrase 1:031a0c78d8d6 45 float * ArthropodSolver::SolveBody(sixDOF_t BodyTarget6D, float LegPriorPos[], int LegNum) {
ikrase 1:031a0c78d8d6 46
ikrase 1:031a0c78d8d6 47 static float foot_pos_out[3];
ikrase 1:031a0c78d8d6 48 float xp= 0, yp = 0;
ikrase 0:9805c9d36254 49
ikrase 1:031a0c78d8d6 50 float foot_pos_inter[3];
ikrase 1:031a0c78d8d6 51 float foot_pos_new[3];
ikrase 1:031a0c78d8d6 52 switch (LegNum) { // Get the coordinates relative to the robot center rather than the leg axis.
ikrase 1:031a0c78d8d6 53 case 0:
ikrase 1:031a0c78d8d6 54 xp = HIP_DISP_ORTHO;
ikrase 1:031a0c78d8d6 55 yp = HIP_DISP_ORTHO;
ikrase 1:031a0c78d8d6 56 break;
ikrase 1:031a0c78d8d6 57 case 1:
ikrase 1:031a0c78d8d6 58 xp = -HIP_DISP_ORTHO;
ikrase 1:031a0c78d8d6 59 yp = HIP_DISP_ORTHO;
ikrase 1:031a0c78d8d6 60 break;
ikrase 1:031a0c78d8d6 61 case 2:
ikrase 1:031a0c78d8d6 62 xp = -HIP_DISP_ORTHO;
ikrase 1:031a0c78d8d6 63 yp = -HIP_DISP_ORTHO;
ikrase 1:031a0c78d8d6 64 break;
ikrase 1:031a0c78d8d6 65 case 3:
ikrase 1:031a0c78d8d6 66 xp = HIP_DISP_ORTHO;
ikrase 1:031a0c78d8d6 67 yp = -HIP_DISP_ORTHO;
ikrase 1:031a0c78d8d6 68 break;
ikrase 1:031a0c78d8d6 69 }
ikrase 1:031a0c78d8d6 70 foot_pos_new[0] = LegPriorPos[0] + xp + BodyTarget6D.xyz[0];
ikrase 1:031a0c78d8d6 71 foot_pos_new[1] = LegPriorPos[1] + yp + BodyTarget6D.xyz[1];
ikrase 1:031a0c78d8d6 72 foot_pos_new[2] = LegPriorPos[2] + BodyTarget6D.xyz[2];
ikrase 0:9805c9d36254 73
ikrase 1:031a0c78d8d6 74 YawXform(foot_pos_new,foot_pos_inter,BodyTarget6D.ypr[0]); // apply yaw transformation first.
ikrase 1:031a0c78d8d6 75 PitchXform(foot_pos_inter, foot_pos_new,BodyTarget6D.ypr[1]);
ikrase 1:031a0c78d8d6 76 RollXform(foot_pos_new, foot_pos_inter,BodyTarget6D.ypr[2]);
ikrase 1:031a0c78d8d6 77
ikrase 1:031a0c78d8d6 78 foot_pos_out[0] = foot_pos_inter[0] - xp - BodyTarget6D.xyz[0];
ikrase 1:031a0c78d8d6 79 foot_pos_out[1] = foot_pos_inter[1] - yp - BodyTarget6D.xyz[0];
ikrase 1:031a0c78d8d6 80 foot_pos_out[2] = foot_pos_inter[2] - BodyTarget6D.xyz[0];
ikrase 1:031a0c78d8d6 81
ikrase 1:031a0c78d8d6 82
ikrase 1:031a0c78d8d6 83
ikrase 1:031a0c78d8d6 84
ikrase 0:9805c9d36254 85
ikrase 0:9805c9d36254 86 return foot_pos_out;
ikrase 0:9805c9d36254 87 }
ikrase 0:9805c9d36254 88
ikrase 0:9805c9d36254 89
ikrase 0:9805c9d36254 90
ikrase 0:9805c9d36254 91
ikrase 0:9805c9d36254 92 float * ArthropodSolver::SolveLegFwd(leg_angles_t LegAngles, int LegNum){
ikrase 0:9805c9d36254 93 static float footpos_out[3]; //x y z
ikrase 0:9805c9d36254 94
ikrase 0:9805c9d36254 95 // code
ikrase 0:9805c9d36254 96
ikrase 0:9805c9d36254 97 float L; float b1;
ikrase 0:9805c9d36254 98
ikrase 0:9805c9d36254 99 b1 = LegAngles.beta - (PI - LegAngles.alpha); // The part of beta that matters.
ikrase 0:9805c9d36254 100
ikrase 0:9805c9d36254 101 L = sin(b1) * TIBIA_L + COXA_L + sin(LegAngles.alpha) * FEMUR_L; // the extension length.
ikrase 0:9805c9d36254 102
ikrase 0:9805c9d36254 103 footpos_out[0] = L * cos(LEG_ANGLES[LegNum] + LegAngles.gamma);
ikrase 0:9805c9d36254 104 footpos_out[1] = L * sin(LEG_ANGLES[LegNum] + LegAngles.gamma);
ikrase 0:9805c9d36254 105 footpos_out[2] = -cos(LegAngles.alpha) - TIBIA_L*cos(b1) ;
ikrase 0:9805c9d36254 106
ikrase 0:9805c9d36254 107
ikrase 0:9805c9d36254 108
ikrase 0:9805c9d36254 109 return footpos_out;
ikrase 0:9805c9d36254 110 }
ikrase 0:9805c9d36254 111
ikrase 0:9805c9d36254 112 void ArthropodSolver::YawXform(float invec[], float outvec[], float angle) {
ikrase 0:9805c9d36254 113 outvec[0] = invec[0]*cos(angle)- invec[1]*sin(angle);
ikrase 0:9805c9d36254 114 outvec[1] = invec[0] * sin(angle) + outvec[1] * cos(angle);
ikrase 0:9805c9d36254 115 outvec[2] = invec[2];
ikrase 0:9805c9d36254 116
ikrase 0:9805c9d36254 117
ikrase 0:9805c9d36254 118
ikrase 0:9805c9d36254 119 }
ikrase 0:9805c9d36254 120
ikrase 0:9805c9d36254 121 void ArthropodSolver::PitchXform(float invec[], float outvec[], float angle){
ikrase 0:9805c9d36254 122 outvec[1] = invec[1]*cos(angle)- invec[2]*sin(angle);
ikrase 0:9805c9d36254 123 outvec[2] = invec[1] * sin(angle) + outvec[2] * cos(angle);
ikrase 0:9805c9d36254 124 outvec[0] = invec[0];
ikrase 0:9805c9d36254 125
ikrase 0:9805c9d36254 126 }
ikrase 0:9805c9d36254 127
ikrase 0:9805c9d36254 128 void ArthropodSolver::RollXform(float invec[], float outvec[], float angle){
ikrase 0:9805c9d36254 129 outvec[2] = invec[2]*cos(angle)- invec[0]*sin(angle);
ikrase 0:9805c9d36254 130 outvec[0] = invec[0] * cos(angle) + outvec[2] * sin(angle);
ikrase 0:9805c9d36254 131 outvec[1] = invec[1];
ikrase 0:9805c9d36254 132
ikrase 0:9805c9d36254 133 }
ikrase 0:9805c9d36254 134