Library for analyitical inverse kinematics on 4-legged 3DOF per leg quadrupod robots. Easily modifyable for more legs.
Dependents: Quadrapod NeoQuadrapod
ArthropodIK.h
- Committer:
- ikrase
- Date:
- 2015-06-25
- Revision:
- 0:9805c9d36254
- Child:
- 1:031a0c78d8d6
File content as of revision 0:9805c9d36254:
#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