107 lines
4.7 KiB
C
107 lines
4.7 KiB
C
//
|
|
// Created by 24871 on 2024/10/2.
|
|
//
|
|
|
|
#include "kinematics.h"
|
|
#include "RobotAlgorithmModule.h"
|
|
#include "arm_math.h"
|
|
#include "dm_motor_ctrl.h"
|
|
Link_Parameter_s Link[4];
|
|
double curr = -2.8,curr_3=-0.5;
|
|
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() {
|
|
//计算通用其次变换矩阵
|
|
int num = 0;
|
|
Link[0].angle = motor[Motor1].para.pos;
|
|
Link[1].angle = motor[Motor2].para.pos;
|
|
Link[2].angle = motor[Motor3].para.pos;
|
|
Link[3].angle = motor[Motor4].para.pos;
|
|
for (num = 0; num<4; num++) {
|
|
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_set2 = -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_set2 = 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));
|
|
Link[1].angle_set2 = -asin(Total_Transformation_Matrix[2][2] / sin(Link[2].angle_set));
|
|
|
|
|
|
motor[Motor1].ctrl.pos_set = Link[0].angle_set;
|
|
|
|
if(Link[2].angle>=0) motor[Motor2].ctrl.pos_set = Link[1].angle_set;
|
|
else motor[Motor2].ctrl.pos_set = Link[1].angle_set2;
|
|
|
|
if(Link[2].angle>=0) motor[Motor3].ctrl.pos_set = Link[2].angle_set;
|
|
else motor[Motor3].ctrl.pos_set = Link[2].angle_set2;
|
|
motor[Motor4].ctrl.pos_set = Link[3].angle_set;
|
|
|
|
motor[Motor1].ctrl.tor_set = curr * sin(Link[0].angle);
|
|
motor[Motor3].ctrl.tor_set = curr_3 * sin(Link[2].angle);
|
|
|
|
} |