六自由度机械臂三维仿真程序完整版

clear
clc
L1=link（[-pi/2 0 0 255 0]）;
%L =link（[alpha A theta D sigma]）
%link.alpha	   返回扭转角
%link.A        返回杆件长度
%link.theta    返回关节角
%link.D        返回横距
%link.sigma    返回关节类型
L2= link（[pi/2 0 0 255 0]）;
L3=link（[pi/2 0 0 0 0]）;
L4=link（[-pi/2 0 0 300 0]）;
L5=link（[pi/2 0 0 0 0]）;
L6=link（[0 0 0 120 0]）;
r=robot（{L1 L2 L3 L4 L5 L6}）;
r.name=‘MY ROBOT‘;%模型的名称
drivebot（r）;

q1=[0 0 0 0 0 0];%起始点关节空间矢量          
q2=[pi/12 -pi/6 pi/4 pi*5/12 pi*5/9 -pi*11/18]; %终止点关节空间矢量   
t=[0:0.05:10];%仿真时间              
q=jtraj（q1q2t）;%关节空间规划         
p=fkine（rq）;

x = squeeze（p（14:））;
y = squeeze（p（24:））;
z = squeeze（p（34:））;
s1 = squeeze（q（:1:））;
s2 = squeeze（q（:2:））;
s3 = squeeze（q（:3:））;
s4 = squeeze（q（:4:））;
s5 = squeeze（q（:5:））;
s6 = squeeze（q（:6:））;
%存入XYZ值
xlswrite（‘e:\测量件坐标.xlsx‘x‘sheet1‘‘a‘）;
xlswrite（‘e:\测量件坐标.xlsx‘y‘sheet1‘‘b‘）;
xlswrite（‘e:\测量件坐标.xlsx‘z‘sheet1‘‘c‘）;
%存入s1s2s3s4s5s6
xlswrite（‘e:\测量件旋转角.xlsx‘s1‘sheet1‘‘a‘）;
xlswrite