6轴机器人运动学正解,逆解2

逆解
逆解计算方法可以参考以下书籍
机器人学导论——分析、系统及应用 电子工业出版社
机器人学导论第3版 机械工业出版社
机器人学建模、规划与控制 西安交通大学出版社

对于关节1,2,3可以从运动方程手工推导出各个关节旋转角度的计算公式

逆解求解的结果并不是唯一的 可能有多组解

/*计算逆解 根据机器人坐标计算机器人关节角度 
 *关节参数在文件 param_table中
 *机器人坐标在文件 xyzrpy中
 *计算结果在屏幕输出 */



#include <stdio.h>
#include <math.h>
#include <string.h>

#define XYZ_F_3D "./xyzrpy"
#define DESIGN_DT "./param_table"
#define XYZ_F_TOOL "./tool_xyz"

#define PI (3.1415926535898)
#define ANG2RAD_EQU(N) (N *= (180.0/3.1415926535898) )
#define ANG2RAD(N) ( (N) * (180.0/3.1415926535898) )
#define RAD2ANG (3.1415926535898/180.0)
#define IS_ZERO(var) if(var < 0.0000000001 && var > -0.0000000001){var = 0;} 
// #define IS_ZERO(var) ( (var) < 0.0000000001 && (var) > -0.0000000001 )?0 :1 
#define JUDGE_ZERO(var) ( (var) < 0.0000000001 && (var) > -0.0000000001 )

#define MATRIX_1 1
#define MATRIX_M 4
#define MATRIX_N 4

#define ANGLE_OFFSET_J2 90
#define ANGLE_OFFSET_J3 90


typedef struct {
    double joint_v;  //joint variable
    double length;
    double d;
    double angle;
}param_t;

param_t param_table[6] ={0};
double worldx =0, worldy =0, worldz =0, 
       worldrr =0, worldrp =0, worldry =0;
double z_offset=0;

void printmatrix(double matrix[MATRIX_N][MATRIX_N], int m, int n);

int matrix_mul(double matrix_a[MATRIX_N][MATRIX_N],
            double matrix_b[MATRIX_N][MATRIX_N],
            double matrix_result[MATRIX_N][MATRIX_N], int m, int n);
void matrix_copy(double matrix_a[MATRIX_N][MATRIX_N],
            double matrix_b[MATRIX_N][MATRIX_N], int m, int n);

void calculate_matrix_R(double worldrr, double worldrp, double worldry,
            double (*matrix_R)[MATRIX_N]);
void calculate_matrix_A(double matrix[MATRIX_N][MATRIX_N], 
            param_t *p_param);
int judge(double j1, double j2, double j3);
void matrix_translate(double matrix[MATRIX_M][MATRIX_N], int m, int n);
void fun_zyz(double matrix_R[MATRIX_N][MATRIX_N], 
            double *p_r,  double *p_p, double *p_y);
int fun_j456(double  j1, double j2, double j3,
            param_t *p_table,double p_matrix_R[MATRIX_N][MATRIX_N],
            double *p_j4, double *p_j5, double *p_j6);

int fun_j2(double j1, double *p_j2, 
            double a1, double a2, double a3, double d4,   
            double px, double py, double pz )
{
  //计算关节2的角度 
    double v1_c, v1_s, v2_c, v2_s;
    double var_M, var_K, tmp;
    double var_sqrt[2] = {0};

    v1_c =cos(j1);
    IS_ZERO(v1_c);
    v1_s =sin(j1);
    IS_ZERO(v1_s);
    var_M = v1_c*px + v1_s*py - a1;
    var_K = (d4*d4 + a3*a3 - a2*a2 - pz*pz - var_M*var_M) / (-2 * a2);
    tmp = var_M*var_M + pz*pz - var_K*var_K;
    IS_ZERO(tmp);
    if( tmp >=0 ){
    //if( (var_M*var_M + pz*pz - var_K*var_K) >=0){
        //var_sqrt[0] = sqrt(var_M*var_M + pz*pz - var_K*var_K);
        var_sqrt[0] = sqrt(tmp);
        var_sqrt[1] = -var_sqrt[0];
    }else{
        printf("m^2 + z^2 - k^2 <0 : %lf\n", tmp);
        p_j2[0] =0, p_j2[1] =0;
        return 0;
    }

    p_j2[0] = -atan2(var_M, pz) + atan2(var_K, var_sqrt[0]);
    p_j2[1] = -atan2(var_M, pz) + atan2(var_K, var_sqrt[1]);
    return 1;
}

int fun_j3(double j1, double j2, double *p_j3,
            double a1, double a3, double d4,
            double px, double py, double pz)
{
  //计算关节3的角度 
    double var_K, tmp;
    double var_sqrt[2];
    double v1_c, v1_s, v2_c, v2_s;
    v1_c = cos(j1);
    IS_ZERO(v1_c);
    v1_s = sin(j1);
    IS_ZERO(v1_s);
    v2_c = cos(j2);
    IS_ZERO(v2_c);
    v2_s = sin(j2);
    IS_ZERO(v2_s);

    var_K = -v2_s*v1_c*px - v1_s*v2_s*py + v2_c*pz + v2_s*a1;
    IS_ZERO(var_K);

    tmp = d4*d4 + a3*a3 - var_K*var_K;
    IS_ZERO(tmp);

    if