85 lines
3.9 KiB
C
85 lines
3.9 KiB
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
|
|
//TODO:多解问题
|
|
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));
|
|
|
|
} |