3D打印机械臂，开源六轴联动核心代码【已经开源】
打印机械臂项目会持续进行，预计2周后会在本贴开源六轴联动最核心的代码：运动学逆解（C代码，可在大多数平台移值），尽情期待运动学逆解，即给出位置姿态矩阵，计算出6轴机器人各个关节的角度。我认为这是六轴机器人最复杂也是计算量最大的部分之一，大部分人都是被这一步给难住了。
运动学正解就不开源了，因为这个难度小，并且大多数时候正解并不要求计算效率，有初中数学知识花点精力基本就可以自己推导出来 用这个代码做机床五轴五联动很轻松哦，所谓机床真五轴和假五轴的区别，就是看机床是五轴3联动还是五轴五联动，国内大多数机床五轴五联动是要另外掏钱的{:lol:} 非常期待 不明觉厉，顶下大牛 期待大牛 先奖励1年VIP++。已经充值给楼主位，原日期2023-10-24，新日期：2024-10-24 非常期待 大牛厉害。 楼主厉害了，不知道6轴逆解给出的是解析解，还是数值解。 非常期待，加油，大牛！ armok. 发表于 2023-7-17 10:48
先奖励1年VIP++。已经充值给楼主位，原日期2023-10-24，新日期：2024-10-24
(引用自6楼)
感谢坛主{:handshake:} 楼主除了一开始对成本的预算估计有些草率。
最近放出来的信息都是大牛所为啊。~ 不错不错，又有好东西可以学习了，活到老学到老啊 大牛厉害，非常期待。 不明觉厉。不知大牛有没有后续提供套件出售的计划。 支持并感谢你的开源！
不明觉厉，楼主的几个帖子质量很高 niaojingxin 发表于 2023-7-17 13:09
不明觉厉。不知大牛有没有后续提供套件出售的计划。
(引用自15楼)
这个项目周期太长了，现在还不好说 非常期待，膜拜一下 嵌入之梦2 发表于 2023-7-17 20:01
非常期待，膜拜一下
(引用自19楼)
{:lol:} 期待期待 非常期待，膜拜一下 非常期待，膜拜一下
大牛厉害，非常期待。 本帖最后由 n_cell 于 2023-7-24 01:13 编辑
#include "math.h"
#define M_PI 3.14159265358979323846f
#define M_PI_21.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;
} 本帖最后由 n_cell 于 2023-7-24 00:46 编辑
本算法基于DH参数模型，配置好关节参数即可正常使用
类似下图的库卡机器人，只需要按下图所示修改对应的关节参数即可
说明一下：inverseKine(float x, float y, float z, float rx, float ry, float rz)
其中x y z 为机械臂末端坐标，单位mm，rx ry rz为末端姿态绕各轴的旋转角度，单为rad
各关节角度JxAngle返回值单位为rad 今日打卡，我有个3D打印的五轴，都吃灰四五年了，/笑哭 非常支持，有空会测试一下楼主的代码 老兄，你这个开源是闷不做声的啊，刷新到了才发现已经开源了 honami520 发表于 2023-8-8 22:14
老兄，你这个开源是闷不做声的啊，刷新到了才发现已经开源了
(引用自30楼)
标题无法编辑了{:sweat:} 支持lz开源精神 佩服楼主的开源精神 跟着学习学习 跟着学习学习 nice
页:
[1]