clear,clc,close all;

%% 建立機(jī)器人SDH參數(shù)，初始狀態(tài)為豎直狀態(tài)

% https://blog.csdn.net/rouyu308/article/details/125014076?spm=1001.2014.3001.5502

% 選取所研究的機(jī)械臂類型(后兩位代表機(jī)械臂有效負(fù)載) by lixingjia 2021.2.3

% 參考資料為陳賽旋的博士大論文

iimt03=0;

iimt05=1;

iimt10=0;

iimt16=0;

% 運(yùn)行前檢查

if iimt03+iimt05+iimt10+iimt16>1

? ? error('run only one robot each time, 一次運(yùn)行一個(gè)選項(xiàng)')

end

if iimt03 % iimt03 結(jié)構(gòu)參數(shù)

d1=0.1505; a2=-0.2642; a3=-0.2375; d4=0.1018; d5=0.1018; d6=0.0948;

name='IIMT-CR-03';

end

if iimt05 % iimt05 結(jié)構(gòu)參數(shù)

d1=0.1375; a2=-0.4020; a3=-0.3760; d4=0.1205; d5=0.1205; d6=0.0885;

name='IIMT-CR-05';

end

if iimt10 % iimt10 結(jié)構(gòu)參數(shù)

d1=0.1310; a2=-0.6410; a3=-0.6010; d4=0.1205; d5=0.1205; d6=0.0885;

name='IIMT-CR-10';

end

if iimt16 % iimt16 結(jié)構(gòu)參數(shù)

d1=0.1310; a2=-0.5100; a3=-0.4200; d4=0.1205; d5=0.1205; d6=0.0885;

name='IIMT-CR-16';

end

% d(軸偏移,沿 軸)? a(臂桿長(zhǎng),沿x)? alpha(電機(jī)夾角,繞x)? MDH? theta(電機(jī)轉(zhuǎn)角,繞軸)

% 連桿偏距d(沿軸)? 連桿長(zhǎng)度a(沿x)? 連桿轉(zhuǎn)角alpha(繞x)? MDH? 關(guān)節(jié)轉(zhuǎn)角theta(繞軸)

L1=Link('d', d1,'a',? 0, 'alpha', pi/2,'qlim',[-3.11,3.11],'standard','offset',0);

L2=Link('d',? 0,'a', a2, 'alpha',? ? 0,'qlim',[-3.11,3.11],'standard','offset',0);

L3=Link('d',? 0,'a', a3, 'alpha',? ? 0,'qlim',[-3.11,3.11],'standard','offset',0);

L4=Link('d', d4,'a',? 0, 'alpha', pi/2,'qlim',[-4.68,1.54],'standard','offset',0);

L5=Link('d', d5,'a',? 0, 'alpha',-pi/2,'qlim',[-3.11,3.11],'standard','offset',0);

L6=Link('d', d6,'a',? 0, 'alpha',? ? 0,'qlim',[-3.11,3.11],'standard','offset',0);

?% case offset=[0 0 0 0 0 0], robot is in 0 pose; case offset=[0 -pi/2 0 -pi/2 0 0], robot is in I pose;

robot=SerialLink([L1 L2 L3 L4 L5 L6],'name',name)

Theta=[0 0 0 0 0 0];

% Theta=[0 90 0 -90 0 0];

W=[-1,+1,-1,+1,-1,1];? ? ? ? ? ? ? ? ? ? ? ? ? ?% workspace limit

robot.plot(Theta,'tilesize',0.2,'workspace',W); %顯示三維動(dòng)畫

Theta=Theta/180*pi;? ? ? ? ? ? ? %換算成弧度

forwarda=robot.fkine(Theta)? ? ? %求正解的齊次變換矩陣，對(duì)比示教器來(lái)驗(yàn)證

robot.teach();

% robot.teach(forwarda,'rpy')? ? %顯示roll/pitch/yaw angles，GUI可調(diào)界面