IIMT-CR 3510 SDH参数

clear,clc,close all;

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

% 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', 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); %显示三维动画

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

forwarda=robot.fkine(Theta)      %求正解的齐次变换矩阵，对比示教器来验证

robot.teach();

% robot.teach(forwarda,'rpy')    %显示roll/pitch/yaw angles，GUI可调界面