puma-simple.asv

来自「PUMA560机器人正解程序包含六个关节」· ASV 代码 · 共 36 行

function puma-simple()
syms x1 x2 x3 x4 x5 x6 d1 d2 d3 d4 d5 d6 a1 a2 a3 a4 a5 a6

                        A1 = [cos(x1) 0 -sin(x1) 0;sin(x1) 0 cos(x1) 0;0 -1 0 d1;0 0 0 1];
                        A2 = [cos(x2) -sin(x2) 0 a2*cos(x2);sin(x2) cos(x2) 0 a2*sin(x2);0 0 1 d2;0 0 0 1];
                        A3 = [cos(x3) 0 -sin(x3) a3*cos(x3);sin(x3) 0 cos(x3) a3*sin(x3);0 -1 0 0;0 0 0 1];
                        A4 = [cos(x4) 0 sin(x4) 0;sin(x4) 0 -cos(x4) 0;0 1 0 d4;0 0 0 1];
                        A5 = [cos(x5) 0 -sin(x5) 0;sin(x5) 0 cos(x5) 0;0 -1 0 0;0 0 0 1];
                        A6 = [cos(x6) -sin(x6) 0 0;sin(x6) cos(x6) 0 0;0 0 1 d6;0 0 0 1];
                        T = A1*A2*A3*A4*A5*A6
                        
clear
clc
syms x1 x2 x3 x4 x5 x6 d1 d2 d3 d4 d5 d6 a1 a2 a3 a4 a5 a6 nx ox ax px ny oy ay nz oz az pz py 

                        A1 = [cos(x1) 0 -sin(x1) 0;sin(x1) 0 cos(x1) 0;0 -1 0 d1;0 0 0 1];
                        A2 = [cos(x2) -sin(x2) 0 a2*cos(x2);sin(x2) cos(x2) 0 a2*sin(x2);0 0 1 d2;0 0 0 1];
                        A3 = [cos(x3) 0 -sin(x3) a3*cos(x3);sin(x3) 0 cos(x3) a3*sin(x3);0 -1 0 0;0 0 0 1];
                        A4 = [cos(x4) 0 sin(x4) 0;sin(x4) 0 -cos(x4) 0;0 1 0 d4;0 0 0 1];
                        A5 = [cos(x5) 0 -sin(x5) 0;sin(x5) 0 cos(x5) 0;0 -1 0 0;0 0 0 1];
                        A6 = [cos(x6) -sin(x6) 0 0;sin(x6) cos(x6) 0 0;0 0 1 0;0 0 0 1];
                        TA=[nx ox ax px;ny oy ay py;nz oz az pz;0 0 0 1]
                        A11 = simple(inv(A1));
                        A12 = simple(inv(A2));
                        A13 = simple(inv(A3));
                        A14 = simple(inv(A4));
                        A15 = simple(inv(A5));
                        A16 = simple(inv(A6));
                        A11
                        A12
                        A13
                        A14
                        A15
                        A16