字节跳动（抖音公司）GR3-Fourier V15.0 工业机械臂 | 运动学逆解+离线应急控制 纯C底层开源代码

GR3-Fourier V15.0 底层绝密技术密档一、六轴机械臂逆运动学完整求解源码#include "inverse_kinematic.h"#define PI 3.1415926535f#define L1 0.185f#define L2 0.210f//笛卡尔坐标转关节角度逆解uint8_t IK_Solve(float x,float y,float z,float pitch,float *j1,float *j2,float *j3){ float r,h,c,s,ang1,ang2,ang3; r = Fast_Sqrt(x*x + y*y); h = z - 0.120f; if(r L1+L2 || r fabs(L1-L2)) return 0; c = (L1*L1 + r*r + h*h - L2*L2) / (2*L1*Fast_Sqrt(r*r+h*h)); c = Float_Limit(c,-1.0f,1.0f); ang1 = acosf(c); s = (L2*L2 - L1*L1 - r*r - h*h) / (2*L1*L2); s = Float_Limit(s,-1.0f,1.0f); ang2 = acosf(s); ang3 = atan2f(h,r) - ang1; *j1 = Rad_To_Deg(atan2f(y,x)); *j2 = Rad_To_Deg(ang2); *j3 = Rad_To_Deg(ang3); *j1 = Float_Limit(*j1,-175.0f,175.0f); *j2 = Float_Limit(*j2,-175.0f,175.0f); *j3 = Float_Limit(*j3,-175.0f,175.0f); return 1;}//正运动学坐标正向解算void FK_Calc(float j1,float j2,float j3,float *x,float *y,float *z){ float r1,r2; r1 = L1 * cosf(Deg_To_Rad(j2)); r2 = L2 * cosf(Deg_To_Rad(j2+j3)); *x = (r1 + r2) * cosf(Deg_To_Rad(j1)); *y = (r1 + r2) * sinf(Deg_To_Rad(j1)); *z = 0.120f + L1*sinf(Deg_To_Rad(j2)) + L2*sinf(Deg_To_Rad(j2+j3));}//奇异位形规避判定uint8_t Singularity_Check(float j2,float j3){ float sum = fabs(j2 + j3); if(sum 15.0f || sum 165.0f) return 1;nbs