Customer_controller/USER/kinematics.c

//
// Created by 24871 on 2024/10/2.
//

#include "kinematics.h"
#include "RobotAlgorithmModule.h"
#include "arm_math.h"
Link_Parameter_s Link[4];
double Total_Transformation_Matrix[4][4],Process1_Transformation_Matrix[4][4],Process2_Transformation_Matrix[4][4];
void Kinematic_Init()
{
    Link[0].length = LINK_LENGTH1;
    Link[1].length = LINK_LENGTH2;
    Link[2].length = LINK_LENGTH3;
    Link[3].length = LINK_LENGTH4;

    Link[0].offset = LINK_OFFSET1;
    Link[1].offset = LINK_OFFSET2;
    Link[2].offset = LINK_OFFSET3;
    Link[3].offset = LINK_OFFSET4;

    Link[0].twist = LINK_TWIST1;
    Link[1].twist = LINK_TWIST2;
    Link[2].twist = LINK_TWIST3;
    Link[3].twist = LINK_TWIST4;
}

void Kinematic_Calculate() {
    //计算通用其次变换矩阵
    for (int num = 0; num++; num < 4) {
        Link[num].transformation_matrix[0][0] = cos(Link[num].angle);
        Link[num].transformation_matrix[0][1] = -sin(Link[num].angle) * cos(Link[num].twist);
        Link[num].transformation_matrix[0][2] = sin(Link[num].angle) * sin(Link[num].twist);
        Link[num].transformation_matrix[0][3] = Link[num].length * cos(Link[num].angle);
        Link[num].transformation_matrix[1][0] = sin(Link[num].angle);
        Link[num].transformation_matrix[1][1] = cos(Link[num].angle) * cos(Link[num].twist);
        Link[num].transformation_matrix[1][2] = -cos(Link[num].angle) * sin(Link[num].twist);
        Link[num].transformation_matrix[1][3] = Link[num].length * sin(Link[num].angle);
        Link[num].transformation_matrix[2][0] = 0;
        Link[num].transformation_matrix[2][1] = sin(Link[num].twist);
        Link[num].transformation_matrix[2][2] = cos(Link[num].twist);
        Link[num].transformation_matrix[2][3] = Link[num].offset;
        Link[num].transformation_matrix[3][0] = 0;
        Link[num].transformation_matrix[3][1] = 0;
        Link[num].transformation_matrix[3][2] = 0;
        Link[num].transformation_matrix[3][3] = 1;


    }
    //求出总变换（正运动学）
    Matrix4Mult(Link[0].transformation_matrix, Link[1].transformation_matrix, Process1_Transformation_Matrix);
    Matrix4Mult(Process1_Transformation_Matrix, Link[2].transformation_matrix, Process2_Transformation_Matrix);
    Matrix4Mult(Process2_Transformation_Matrix, Link[3].transformation_matrix, Total_Transformation_Matrix);

    //逆运动学
    //关节1
    double m = Total_Transformation_Matrix[1][3] - Link[3].length * Total_Transformation_Matrix[1][0] -
              Link[3].offset * Total_Transformation_Matrix[1][2];
    double n = Total_Transformation_Matrix[0][3] - Link[3].length * Total_Transformation_Matrix[0][0] -
              Link[3].offset * Total_Transformation_Matrix[0][2];

    Link[0].angle_set = atan2(m, n) - atan2(Link[1].offset, 0);


    //关节3
    Link[2].angle_set = acos(Total_Transformation_Matrix[1][2] * cos(Link[0].angle_set) -
                              Total_Transformation_Matrix[0][2] * sin(Link[0].angle));
//    Link[2].angle_set = -acos(Total_Transformation_Matrix[1][2] * cos(Link[0].angle_set) -
//                               Total_Transformation_Matrix[0][2] * sin(Link[0].angle));

    //关节4
    double mm = -Total_Transformation_Matrix[1][2];
    double nn = Total_Transformation_Matrix[0][2];
//    Link[3].angle_set = atan2(mm, nn) - atan2(cos(Link[1].angle_set), sqrt(mm * mm + nn * nn -
//                                                                               cos(Link[1].angle_set) *
//                                                                               cos(Link[1].angle_set)));
    Link[3].angle_set = atan2(mm, nn) - atan2(sin(Link[1].angle_set), -sqrt(mm * mm + nn * nn -
                                                                                cos(Link[1].angle_set) *
                                                                                cos(Link[1].angle_set)));

    //关节2
    Link[1].angle_set = asin(Total_Transformation_Matrix[2][2] / sin(Link[2].angle_set));

}