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