|
楼主 |
发表于 2023-7-24 00:25:15
|
显示全部楼层
本帖最后由 n_cell 于 2023-7-24 01:13 编辑
- #include "math.h"
- #define M_PI 3.14159265358979323846f
- #define M_PI_2 1.57079632679489661923f
- float DH_LTAngle1 = -90; // link Twist
- float DH_LTAngle2 = 0;
- float DH_LTAngle3 = 90;
- float DH_LTAngle4 = -90;
- float DH_LTAngle5 = 90;
- float DH_LTAngle6 = 0;
- float DH_LLength1 = 30.0; // Link Length
- float DH_LLength2 = 314.5;
- float DH_LLength3 = 0;
- float DH_LLength4 = 0;
- float DH_LLength5 = 0;
- float DH_LLength6 = 0;
- float DH_LOffset1 = 308.5; // Link Offset
- float DH_LOffset2 = 0;
- float DH_LOffset3 = 0;
- float DH_LOffset4 = -268.5;
- float DH_LOffset5 = 0;
- float DH_LOffset6 = -100.3;
- float DH_JAngle1 = 0; // Joint Angle
- float DH_JAngle2 = 0;
- float DH_JAngle3 = -90;
- float DH_JAngle4 = 0;
- float DH_JAngle5 = 0;
- float DH_JAngle6 = 180;
- float J1Angle = 0;
- float J2Angle = 0;
- float J3Angle = 0;
- float J4Angle = 0;
- float J5Angle = 0;
- float J6Angle = 0;
- float deg2Rad(float deg)
- {
- return (deg * M_PI) / 180.0f;
- }
- void inverseKine(float x, float y, float z, float rx, float ry, float rz)
- {
- float poseX = x;
- float poseY = y;
- float poseZ = z;
- float poseRx = rx;
- float poseRy = ry;
- float poseRz = rz;
- float A0 = deg2Rad(DH_LTAngle1);
- float A1 = deg2Rad(DH_LTAngle2);
- float A2 = deg2Rad(DH_LTAngle3);
- float A3 = deg2Rad(DH_LTAngle6);
- float A4 = DH_LOffset1;
- float A5 = DH_LOffset2;
- float A6 = DH_LOffset4;
- float A7 = DH_LOffset6;
- float A8 = DH_LLength1;
- float A9 = DH_LLength2;
- float AA = DH_LLength3;
- float XB0 =-(cosf(poseRz)*cosf(poseRx)-cosf(poseRy))*sinf(poseRz)*sinf(poseRx);
- float YB0 =cosf(poseRx)*sinf(poseRz)+cosf(poseRz)*cosf(poseRy)*sinf(poseRx);
- float ZB0 =sinf(poseRy)*sinf(poseRx);
- float PX =poseX;
- float XB1 =cosf(poseRy)*cosf(poseRx)*sinf(poseRz)+cosf(poseRz)*sinf(poseRx);
- float YB1 =cosf(poseRz)*cosf(poseRy)*cosf(poseRx)-sinf(poseRz)*sinf(poseRx);
- float ZB1 =cosf(poseRx)*sinf(poseRy);
- float PY =poseY;
- float XB2 =sinf(poseRz)*sinf(poseRy);
- float YB2 =cosf(poseRz)*sinf(poseRy);
- float ZB2 =-cosf(poseRy);
- float PZ =poseZ;
- float YC1 =- cosf(A3);
- float ZC1 =sinf(A3);
- float YC2 =sinf(A3);
- float ZC2 =cosf(A3);
- float C3 = -A7;
- float XD1 =-XB0;
- float YD1 =(YB0*YC1)+(ZB0*YC2);
- float ZD1 =(YB0*ZC1)+(ZB0*ZC2);
- float D1 =(ZB0*C3)+PX;
- float XD2 =-XB1;
- float YD2 =(YB1*YC1)+(ZB1*YC2);
- float ZD2 =(YB1*ZC1)+(ZB1*ZC2);
- float D2 =(ZB1*C3)+PY;
- float XD3 =-XB2;
- float YD3 =(YB2*YC1)+(ZB2*YC2);
- float ZD3 =(YB2*ZC1)+(ZB2*ZC2);
- float D3 =(ZB2*C3)+PZ;
- float E1 = atan2f((D2), (D1));
- float E2 = E1;
- float F1 = sqrtf(powf(fabs(D2), 2)+powf(fabs(D1), 2));
- float F2 = D3-A4;
- float F3 = F1-A8;
- float F4 = sqrtf(powf(F2, 2) + powf(F3, 2));
- float F5 = sqrtf(powf(A6, 2) + powf(AA, 2));
- float F6 = atan2f(F2, F3);
- float F7 = acosf((powf(A9, 2)+powf(F4, 2)-powf(fabs(F5), 2)) / (2*A9*F4));
- float G1 = 0;
- if(!isinf(fabs(A6)/AA))
- {
- G1 = atan(fabs(A6)/AA);
- }
- else
- {
- G1 = M_PI_2;
- }
- float G2 = M_PI-acosf((powf(fabs(F5), 2)+powf(A9, 2)-powf(F4, 2))/(2*fabs(F5)*A9))+(M_PI_2-G1);
- float G3 = -(F6+F7);
- float G4 = G2;
- float H1 = -F3;
- float H2 = sqrtf(powf(F2, 2)+powf(H1, 2));
- float H3 = acosf((powf(A9, 2)+powf(H2, 2)-powf(fabs(F5), 2))/(2*A9*H2));
- float H4 = atan2f(H1, F2);
- float H5 = M_PI-acosf((powf(fabs(F5), 2)+powf(A9, 2)-powf(H2, 2))/(2*fabs(F5)*A9))+(M_PI_2-G1);
- float H6 = M_PI_2-(H3+H4);
- float H7 = -M_PI+H6;
- float H8 = H5;
- float L1 = E2;
- float L2 = 0, L3 = 0;
-
- if (F3<0)
- {
- L2 = H7;
- L3 = H8;
- }
- else
- {
- L2 = G3;
- L3 = G4;
- }
- float M1 =cosf(L2);
- float M2 =-sinf(L2)*cosf(A1);
- float M3 =sinf(L2)*sinf(A1);
- float M4 =A9*cosf(L2);
- float M5 =sinf(L2);
- float M6 =cosf(L2)*cosf(A1);
- float M7 =-cosf(L2)*sinf(A1);
- float M8 =A9*sinf(L2);
- float M9 = 0;
- float MA =sinf(A1);
- float MB =cosf(A1);
- float MC =A5;
- float MD = 0;
- float ME = 0;
- float MF = 0;
- float M10 = 1;
- float N0 =cosf((L3)-M_PI_2);
- float N1 =-sinf((L3)-M_PI_2)*cosf(A2);
- float N2 =sinf((L3)-M_PI_2)*sinf(A2);
- float N3 =sinf((L3)-M_PI_2);
- float N4 =cosf((L3)-M_PI_2)*cosf(A2);
- float N5 =-cosf((L3)-M_PI_2)*sinf(A2);
- float N6 =0;
- float N7 =sinf(A2);
- float N8 =cosf(A2);
- float N9 =0;
- float NA =0;
- float NB =0;
- float O1 =cosf(L1);
- float O2 =-sinf(L1)*cosf(A0);
- float O3 =sinf(L1)*sinf(A0);
- float O4 =A8*cosf(L1);
- float O5 =sinf(L1);
- float O6 =cosf(L1)*cosf(A0);
- float O7 =-cosf(L1)*sinf(A0);
- float O8 =A8*sinf(L1);
- float O9 =0;
- float OA =sinf(A0);
- float OB =cosf(A0);
- float OC =A4;
- float P1 =(O1*M1)+(O2*M5)+(O3*M9)+(O4*MD);
- float P2 =(O1*M2)+(O2*M6)+(O3*MA)+(O4*ME);
- float P3 =(O1*M3)+(O2*M7)+(O3*MB)+(O4*MF);
- float P4 =(O1*M4)+(O2*M8)+(O3*MC)+(O4*M10);
- float P5 =(O5*M1)+(O6*M5)+(O7*M9)+(O8*MD);
- float P6 =(O5*M2)+(O6*M6)+(O7*MA)+(O8*ME);
- float P7 =(O5*M3)+(O6*M7)+(O7*MB)+(O8*MF);
- float P8 =(O5*M4)+(O6*M8)+(O7*MC)+(O8*M10);
- float P9 =(O9*M1)+(OA*M5)+(OB*M9)+(OC*MD);
- float PA =(O9*M2)+(OA*M6)+(OB*MA)+(OC*ME);
- float PB =(O9*M3)+(OA*M7)+(OB*MB)+(OC*MF);
- float PC =(O9*M4)+(OA*M8)+(OB*MC)+(OC*M10);
- float Q1 =(P1*N0)+(P2*N3)+(P3*N6)+(P4*N9);
- float Q2 =(P5*N0)+(P6*N3)+(P7*N6)+(P8*N9);
- float Q3 =(P9*N0)+(PA*N3)+(PB*N6)+(PC*N9);
- float Q4 =(P1*N1)+(P2*N4)+(P3*N7)+(P4*NA);
- float Q5 =(P5*N1)+(P6*N4)+(P7*N7)+(P8*NA);
- float Q6 =(P9*N1)+(PA*N4)+(PB*N7)+(PC*NA);
- float Q7 =(P1*N2)+(P2*N5)+(P3*N8)+(P4*NB);
- float Q8 =(P5*N2)+(P6*N5)+(P7*N8)+(P8*NB);
- float Q9 =(P9*N2)+(PA*N5)+(PB*N8)+(PC*NB);
- float R1 =(Q1*ZD1)+(Q2*ZD2)+(Q3*ZD3);
- float R2 =(Q4*ZD1)+(Q5*ZD2)+(Q6*ZD3);
- float R3 =(Q7*XD1)+(Q8*XD2)+(Q9*XD3);
- float R4 =(Q7*YD1)+(Q8*YD2)+(Q9*YD3);
- float R5 =(Q7*ZD1)+(Q8*ZD2)+(Q9*ZD3);
- float R7 = atan2f(R2,R1);
- float R8 = atan2f(+sqrtf(1-powf(R5, 2)),R5);
- float R9 = 0;
- if (R4 < 0)
- {
- R9 = atan2f(-R4,R3)-M_PI;
- }
- else
- {
- R9 = atan2f(-R4,R3)+M_PI;
- }
-
- float S7 = atan2f(-R2,-R1);
- float S8 = atan2f(-sqrtf(1-powf(R5, 2)),R5);
- float S9 = 0;
- if (R4 < 0)
- S9 = atan2f(R4,-R3)+M_PI;
- else
- S9 = atan2f(R4,-R3)-M_PI;
- float L5 = 0;
- if (1) // front config
- L5 = R8;
- else
- L5 = S8;
- float L4 = 0;
- if(L5>0)
- {
- L4 = R7;
- }
- else
- {
- L4 = S7;
- }
- float L6 = 0;
- if(L5<0)
- L6 = S9;
- else
- L6 = R9;
- J1Angle = L1;
- J2Angle = -M_PI_2-L2;
- J3Angle = M_PI_2-L3;
- J4Angle = L4;
- J5Angle = L5;
- J6Angle = -L6;
- }
复制代码 |
|