82 lines
3.2 KiB
C
82 lines
3.2 KiB
C
|
//
|
||
|
|
// Created by 24871 on 2024/10/2.
|
||
|
|
//
|
||
|
|
|
||
|
|
#include "kinematics.h"
|
||
|
|
#include "RobotAlgorithmModule.h"
|
||
|
|
#include "arm_math.h"
|
||
|
|
Link_Parameter_s Link[4];
|
||
|
|
float 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]=cosf(Link[num].angle);
|
||
|
|
Link[num].transformation_matrix[0][1]=-sinf(Link[num].angle)*cosf(Link[num].twist);
|
||
|
|
Link[num].transformation_matrix[0][2]=sinf(Link[num].angle)*sinf(Link[num].twist);
|
||
|
|
Link[num].transformation_matrix[0][3]=Link[num].length*cosf(Link[num].angle);
|
||
|
|
Link[num].transformation_matrix[1][0]=sinf(Link[num].angle);
|
||
|
|
Link[num].transformation_matrix[1][1]=cosf(Link[num].angle)*cosf(Link[num].twist);
|
||
|
|
Link[num].transformation_matrix[1][2]=-cosf(Link[num].angle)*sinf(Link[num].twist);
|
||
|
|
Link[num].transformation_matrix[1][3]=Link[num].length*sinf(Link[num].angle);
|
||
|
|
Link[num].transformation_matrix[2][0]=0;
|
||
|
|
Link[num].transformation_matrix[2][1]=sinf(Link[num].twist);
|
||
|
|
Link[num].transformation_matrix[2][2]=cosf(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
|
||
|
|
float m=Total_Transformation_Matrix[1][3]-Link[3].length*Total_Transformation_Matrix[1][0]-Link[3].offset*Total_Transformation_Matrix[1][2];
|
||
|
|
float n=Total_Transformation_Matrix[0][3]-Link[3].length*Total_Transformation_Matrix[0][0]-Link[3].offset*Total_Transformation_Matrix[0][2];
|
||
|
|
//TODO:多解，需要一个判断位来选择解
|
||
|
|
Link[0].angle_set = atan2f(m,n)-atan2f(Link[1].offset, sqrtf(m*m+n*n-Link[2].offset*Link[2].offset));
|
||
|
|
Link[0].angle_set = atan2f(m,n)-atan2f(Link[1].offset, -sqrtf(m*m+n*n-Link[2].offset*Link[2].offset));
|
||
|
|
|
||
|
|
//关节3
|
||
|
|
//TODO:多解问题
|
||
|
|
Link[2].angle_set = acosf(Total_Transformation_Matrix[1][2]*cosf(Link[0].angle_set)-Total_Transformation_Matrix[0][2]*sinf(Link[0].angle));
|
||
|
|
Link[2].angle_set = -acosf(Total_Transformation_Matrix[1][2]*cosf(Link[0].angle_set)-Total_Transformation_Matrix[0][2]*sinf(Link[0].angle));
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|