IIMT-CR 3510 MDH参数

clear,clc,close all;

%% 建立机器人MDH参数，初始状态为竖直状态

% 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 结构参数

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 结构参数

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 结构参数

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 结构参数

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(电机转角,绕轴)

% 连杆偏距d(沿轴)  连杆长度a(沿x)  连杆转角alpha(绕x)  MDH  关节转角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可调界面