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.h@1:031a0c78d8d6, 2015-06-26 (annotated)

Committer:
ikrase
Date:
Fri Jun 26 04:21:43 2015 +0000
Revision:
1:031a0c78d8d6
Parent:
0:9805c9d36254 
Finished library, not yet tested.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ikrase 0:9805c9d36254 1 #ifndef ARTHROPODIK_H
ikrase 0:9805c9d36254 2 #define ARTHROPODIK_H
ikrase 0:9805c9d36254 3
ikrase 0:9805c9d36254 4
ikrase 0:9805c9d36254 5 #define NUM_LEGS 4
ikrase 0:9805c9d36254 6 #define COXA_L 30.0 //mm
ikrase 0:9805c9d36254 7 #define FEMUR_L 70.0
ikrase 0:9805c9d36254 8 #define TIBIA_L 100.0 // NEED TO ACTUALLY MEASURE.
ikrase 0:9805c9d36254 9 #define HIP_DISP_ORTHO 50.0 // Orthogonal distance from center of robot to hip vertical joint.
ikrase 0:9805c9d36254 10
ikrase 0:9805c9d36254 11
ikrase 0:9805c9d36254 12 //MASSIVE CREDITS to http://blog.oscarliang.net/inverse-kinematics-and-trigonometry-basics/: Oscar Liang
ikrase 0:9805c9d36254 13
ikrase 0:9805c9d36254 14 // Probably a few typdef structs?
ikrase 0:9805c9d36254 15
ikrase 0:9805c9d36254 16
ikrase 0:9805c9d36254 17
ikrase 0:9805c9d36254 18 typedef struct leg_angles { // ALL OF THIS IS IN RADIANS. ALL ANGLES IN THIS LIBRARY ARE IN RADIANS.
ikrase 0:9805c9d36254 19 float gamma; // CCW *displacement* angle of the hip from above
ikrase 0:9805c9d36254 20 float beta; // Included angle of the knee. Is pi/2 when knee is bent at a right angle (this is the resting position.)
ikrase 0:9805c9d36254 21 float alpha; // Angle of the hip above vertical axis. Is pi/2 when leg is sticking out straight (i.e. the resting position).
ikrase 0:9805c9d36254 22 } leg_angles_t;
ikrase 0:9805c9d36254 23
ikrase 0:9805c9d36254 24 typedef struct sixDOF {
ikrase 0:9805c9d36254 25 float xyz[3];
ikrase 0:9805c9d36254 26 float ypr[3]; // that's yaw, pitch, and roll.
ikrase 0:9805c9d36254 27 } sixDOF_t;
ikrase 0:9805c9d36254 28
ikrase 0:9805c9d36254 29 float sq(float n1);
ikrase 0:9805c9d36254 30
ikrase 0:9805c9d36254 31 /* this library assumes that all legs will be identical in lengths. */
ikrase 0:9805c9d36254 32 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ikrase 0:9805c9d36254 33 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ikrase 0:9805c9d36254 34
ikrase 0:9805c9d36254 35
ikrase 0:9805c9d36254 36 class ArthropodSolver {
ikrase 0:9805c9d36254 37
ikrase 0:9805c9d36254 38 public:
ikrase 0:9805c9d36254 39
ikrase 0:9805c9d36254 40 static const float LEG_ANGLES[4];
ikrase 0:9805c9d36254 41
ikrase 0:9805c9d36254 42 //int numLegs;
ikrase 0:9805c9d36254 43
ikrase 0:9805c9d36254 44
ikrase 0:9805c9d36254 45
ikrase 0:9805c9d36254 46 /**
ikrase 0:9805c9d36254 47 * Instantiate a new Arthropod Solver, entering the leg parameters
ikrase 0:9805c9d36254 48 */
ikrase 0:9805c9d36254 49
ikrase 0:9805c9d36254 50 ArthropodSolver(); // constructor.
ikrase 0:9805c9d36254 51
ikrase 0:9805c9d36254 52
ikrase 0:9805c9d36254 53
ikrase 0:9805c9d36254 54
ikrase 0:9805c9d36254 55
ikrase 0:9805c9d36254 56
ikrase 0:9805c9d36254 57 /**
ikrase 0:9805c9d36254 58 * Determine the joint angles for a leg with the toe at a specified position
ikrase 0:9805c9d36254 59 *
ikrase 0:9805c9d36254 60 * @Returns a leg angle struct in float degrees.
ikrase 0:9805c9d36254 61 */
ikrase 0:9805c9d36254 62
ikrase 0:9805c9d36254 63 //leg_angles_t SolveLeg(float LegTargetXYZ[], int LegNum);
ikrase 0:9805c9d36254 64
ikrase 0:9805c9d36254 65 leg_angles_t SolveLeg(float LegTargetXYZ[], int LegNum);
ikrase 0:9805c9d36254 66
ikrase 0:9805c9d36254 67
ikrase 0:9805c9d36254 68
ikrase 0:9805c9d36254 69
ikrase 0:9805c9d36254 70
ikrase 0:9805c9d36254 71 /**
ikrase 0:9805c9d36254 72 * Deterimine the joint angles to shift the body to a different position, keeping the legs planted.
ikrase 0:9805c9d36254 73 */
ikrase 0:9805c9d36254 74
ikrase 0:9805c9d36254 75 //leg_angles_t * SolveBody(sixDOF_t BodyTarget6D[], leg_angles_t LegPriorPos[]);
ikrase 0:9805c9d36254 76
ikrase 1:031a0c78d8d6 77 float * SolveBody(sixDOF_t BodyTarget6D, float LegPriorPos[], int LegNum);
ikrase 0:9805c9d36254 78
ikrase 0:9805c9d36254 79
ikrase 0:9805c9d36254 80
ikrase 0:9805c9d36254 81
ikrase 0:9805c9d36254 82
ikrase 0:9805c9d36254 83
ikrase 0:9805c9d36254 84
ikrase 0:9805c9d36254 85
ikrase 0:9805c9d36254 86 /**
ikrase 0:9805c9d36254 87 * Determine the position of a foot, given the leg angles.
ikrase 0:9805c9d36254 88 */
ikrase 0:9805c9d36254 89
ikrase 0:9805c9d36254 90 float * SolveLegFwd(leg_angles_t LegAngles, int LegNum);
ikrase 0:9805c9d36254 91
ikrase 0:9805c9d36254 92 void YawXform(float invec[], float outvec[], float angle);
ikrase 0:9805c9d36254 93
ikrase 0:9805c9d36254 94 void PitchXform(float invec[], float outvec[], float angle);
ikrase 0:9805c9d36254 95
ikrase 0:9805c9d36254 96 void RollXform(float invec[], float outvec[], float angle);
ikrase 0:9805c9d36254 97
ikrase 0:9805c9d36254 98
ikrase 0:9805c9d36254 99
ikrase 0:9805c9d36254 100
ikrase 0:9805c9d36254 101
ikrase 0:9805c9d36254 102
ikrase 0:9805c9d36254 103 };
ikrase 0:9805c9d36254 104
ikrase 0:9805c9d36254 105
ikrase 0:9805c9d36254 106
ikrase 0:9805c9d36254 107 //const float ArthropodSolver::LEG_ANGLES[4] = {0.78539816339, 2.35619449019, -2.35619449019, -0.78539816339, }; /// moved to cpp file.
ikrase 0:9805c9d36254 108
ikrase 0:9805c9d36254 109
ikrase 0:9805c9d36254 110
ikrase 0:9805c9d36254 111 #endif