六自由度机械臂正向运动学与姿态绘制with matlab

        如果不依赖机器人工具箱，希望自己通过作图显示机械臂某一时刻的工作姿态怎么来实现呢。首先我们知道原理是通过姿态的旋转变换以及平移变换来实现末端坐标的计算。计算完成后的将关节点连接起来便构成了机械臂在某一时刻的姿态位置。更进一步，可以通过旋转变换做出末端的坐标以方便分析。具体实现方式：

Code/matlab:

function MyCoordinateComputeDrawing
%工具：Matrix Compute
%时间：2019/2/12
%更新：easy_R
%核算：compare with RobotToolBox
 
%%
%parameters of coordinate
clear all; 
clc;
d1=0.25; d4=0.25;  %base and toolpoint transplante
a2=0.21; a3=0.165; a4=0.21;
alpha2=pi/2;  alpha6=pi/2;
 
%% 
%MDH Table of RobotToolBox
%********************************************************
%         θ   d   a   α       offset  
% L(1)=Link([0   0   0    0          0  ],'modified');
% L(2)=Link([0   0   0    alpha2     0  ],'modified');
% L(3)=Link([0   0   a2    0         0  ],'modified');
% L(4)=Link([0   0   a3    0         0  ],'modified');
% L(5)=Link([0   0   a4    0         0  ],'modified');
% L(6)=Link([0   0   0    alpha6     0  ],'modified');
%%********************************************************
 
%%
%compute the TransformMatrix
% syms theta1 theta2 theta3 theta4 theta5 theta6 real
% q={theta1,theta2,theta3,theta4,theta5,theta6};
theta1=0; theta2=pi/3; theta3=-pi/3; theta4=-pi/3; theta5=pi/3; theta6=0;
T10=[       cos(theta1),      -sin(theta1),      0,        0;
    sin(theta1)*cos(0),cos(theta1)*cos(0),-sin(0),-sin(0)*0;
    sin(theta1)*sin(0),cos(theta1)*sin(0), cos(0), cos(0)*0;
                     0,                 0,      0,         1
    ];
T21=[       cos(theta2),      -sin(theta2),      0,        0;
    sin(theta2)*cos(pi/2),cos(theta2)*cos(pi/2),-sin(pi/2),-sin(pi/2)*0;
    sin(theta2)*sin(pi/2),cos(theta2)*sin(pi/2), cos(pi/2), cos(pi/2)*0;
                     0,                 0,      0,         1
    ];
T32=[       cos(theta3),      -sin(theta3),      0,       a2;
    sin(theta3)*cos(0),cos(theta3)*cos(0),-sin(0),-sin(0)*0;
    sin(theta3)*sin(0),cos(theta3)*sin(0), cos(0), cos(0)*0;
                     0,                 0,      0,         1
    ];
T43=[       cos(theta4),      -sin(theta4),      0,       a3;
    sin(theta4)*cos(0),cos(theta4)*cos(0),-sin(0),-sin(0)*0;
    sin(theta4)*sin(0),cos(theta4)*sin(0), cos(0), cos(0)*0;
                     0,                 0,      0,         1
    ];
T54=[       cos(theta5),      -sin(theta5),      0,       a4;
    sin(theta5)*cos(0),cos(theta5)*cos(0),-sin(0),-sin(0)*0;
    sin(theta5)*sin(0),cos(theta5)*sin(0), cos(0), cos(0)*0;
                     0,                 0,      0,         1
    ];
T65=[       cos(theta6),      -sin(theta6),      0,        0;
    sin(theta6)*cos(pi/2),cos(theta6)*cos(pi/2),-sin(pi/2),-sin(pi/2)*0;
    sin(theta6)*sin(pi/2),cos(theta6)*sin(pi/2), cos(pi/2), cos(pi/2)*0;
                     0,                 0,      0,         1
    ];
 
%%
%Matrix compute
% Tb=[1,0,0,0;0,1,0,0;0,0,1,d1;0,0,0,1];   %respect to basement transform matrix
% T10=Tb*T10;
T20=T10*T21;T30=T20*T32;T40=T30*T43;T50=T40*T54;
T10;
T20;
T30;
T40;
T50;
T60=T50*T65;
 
%%
%number compute
% qz={0,0,0,0,0,0}; %ready state 
% T6=vpa(subs(T60,q,qz),3)  %compute simplify
disp('output transform matrix of the endpoint respect to basement T6:')
T60
% qu={0 pi/3 -pi/3 -pi/3 pi/3 0};%standup state
% T6=vpa(subs(T60,q,qz),3)
 
%%
%draw the posture of robot
x = [T10(1,4) T20(1,4) T30(1,4) T40(1,4) T50(1,4) T60(1,4)];
y = [T10(2,4) T20(2,4) T30(2,4) T40(2,4) T50(2,4) T60(2,4)];
z = [T10(3,4) T20(3,4) T30(3,4) T40(3,4) T50(3,4) T60(3,4)]; 
 
%draw the toolpoint coordinate
px=T60*[0.05;0;0;1];     py=T60*[0;0.05;0;1];     pz=T60*[0;0;0.05;1]; %'50'is properties of coordinate
px1=[T60(1,4),px(1,1)];py1=[T60(2,4),px(2,1)];pz1=[T60(3,4),px(3,1)];
px2=[T60(1,4),py(1,1)];py2=[T60(2,4),py(2,1)];pz2=[T60(3,4),py(3,1)];
px3=[T60(1,4),pz(1,1)];py3=[T60(2,4),pz(2,1)];pz3=[T60(3,4),pz(3,1)];
 
%%
%drawing figures
plot3(x,y,z,'o','linewidth',8);
hold on 
%set coordinate
plot3(px1,py1,pz1,'r','LineWidth',3)
hold on 
plot3(px2,py2,pz2,'g','LineWidth',3)
hold on 
plot3(px3,py3,pz3,'b','LineWidth',3)
 
title("Forward Kinematics")
xlabel("x(m)")
ylabel("y(m)")
zlabel("z(m)")
plot3(x,y,z, 'y','Linewidth',5);
grid on;
%%
 
 
 
end

运行结果并运用RTB核算：

其中，第一部分为RTB计算结果，第二部分为计算结果。

在此刻机械臂的姿态显示：