image.png

定义常量

  1. #define PI 3.1415926535
  2. #define deg2rad(deg) ((deg)*PI/180)
  3. #define rad2deg(rad) ((rad)*180/PI)
  5. #define AF 12.5
  6. #define AB 21.5
  7. #define EF 21.5
  8. #define CE 21.5
  9. #define BC 34.19
  10. #define CD 34.95
  11. #define bcd_angle deg2rad(120)
  12. #define half_AF (AF/2.0)

反解实现

  1. double AD2 = 0;
  2. double AD = 0;
  3. double angle_DAF = 0;
  4. double BD2 = 0;
  5. double BD = 0;
  6. double angle_BAD = 0;
  7. double angle_DBA = 0;
  8. double angle_DBC = 0;
  9. double angle_ABC = 0;
  10. double AC2 = 0;
  11. double AC = 0;
  12. double angle_BAC = 0;
  13. double angle_CAF = 0;
  14. double CF2 = 0;
  15. double CF = 0;
  16. double angle_AFC = 0;
  17. double angle_CFE = 0;
  18. //步态相关
  19. double sigma = 0;
  20. double zep = 0;
  21. double xep_b = 0;
  22. double xep_z = 0;
  24. double alpha = 0;
  25. double beta = 0;
  26. //8个舵机的转动角度
  27. double steering_angles[8] = {0};
  29. void leg_inverse(double fx,double fy,double *alpha,double *beta){
  30.     //AD长度
  31.     AD2 = pow((half_AF+fx),2.0)+pow(fy,2.0); 
  32.     AD = sqrt(AD2);
  33.     //DAF夹角
  34.     angle_DAF = asin(fy/AD);
  35.     //BD长度
  36.     BD2 = pow(BC,2.0)+pow(CD,2.0)-2*BC*CD*cos(bcd_angle);
  37.     BD = sqrt(BD2);
  38.     //BAD夹角
  39.     angle_BAD = acos((pow(AB,2.0)+AD2-BD2)/(2*AB*AD));
  40.     //alpha角度
  41.     *alpha = PI-angle_BAD-angle_DAF;
  43.     angle_DBA = acos((pow(AB,2.0)+BD2-AD2)/(2*AB*BD));
  44.     angle_DBC = acos((pow(BC,2.0)+BD2-pow(CD,2.0))/(2*BC*BD));
  45.     angle_ABC = angle_DBA-angle_DBC;
  47.     //AC长度
  48.     AC2 = pow(AB,2.0)+pow(BC,2.0)-2*AB*BC*cos(angle_ABC);
  49.     AC = sqrt(AC2);
  51.     angle_BAC = acos((pow(AB,2.0)+AC2-pow(BC,2.0))/(2*AB*AC));
  52.     angle_CAF = PI-(*alpha)-angle_BAC;
  54.     CF2 = pow(AF,2.0)+AC2-2*AF*AC*cos(angle_CAF);
  55.     CF = sqrt(CF2);
  57.     angle_AFC = acos((pow(AF,2.0)+CF2-AC2)/(2*AF*CF));
  58.     angle_CFE = acos((CF2+pow(EF,2.0)-pow(CE,2.0))/(2*CF*EF));
  60.     *beta = PI-angle_AFC-angle_CFE;
  61. //    printf("======================\n");
  62. //    printf("al:%lf ,beta:%lf\n",*alpha,*beta);
  63. }