clear,clc,close all;

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

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

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

iimt03=0;

iimt05=1;

iimt10=0;

iimt16=0;

% 運行前檢查

if iimt03+iimt05+iimt10+iimt16>1

? ? error('run only one robot each time, 一次運行一個選項')

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(臂桿長,沿x)? alpha(電機夾角,繞x)? MDH? theta(電機轉(zhuǎn)角,繞軸)

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

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

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

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

L4=Link('d',? d4,'a', -a3, 'alpha',? ? 0,'qlim',[-4.68,1.54],'modified','offset',0);

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

L6=Link('d',? d6,'a',? ?0, 'alpha', -pi/2,'qlim',[-3.11,3.11],'modified','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.2];? ? ? ? ? ? ? ? ? ? ? ? ? ?% workspace limit

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

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

forwarda=robot.fkine(Theta)? ? ? %求正解的齊次變換矩陣，對比示教器來驗證

robot.teach();

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