% clc;
% clear all; %描述六轴机器人D-H参数xz类转换关系;
L(1)=Link('revolute', 'd',587.5, 'a',0, 'alpha', 0,'modified');
L(2)=Link('revolute', 'd', -157.5, 'a', 150, 'alpha', -pi/2,'modified');
L(3)=Link('revolute', 'd', -207.5, 'a',730.5, 'alpha', 0,'modified');
L(4)=Link('revolute', 'd',720, 'a',0, 'alpha', pi/2,'modified');
L(5)=Link('revolute', 'd',-97, 'a',0, 'alpha', -pi/2,'modified');
L(6)=Link('revolute', 'd',122, 'a',0, 'alpha', pi/2,'modified');
CO605=SerialLink(L,'name','HSR_CO605'); %串联六轴机器人
xita0=[0,-90,180,0,0,0]; %定义机器人零点
CO605.plot(xita0);
hold on
CO605.teach(CO605,'degress');
T1=transl(900,600,350); %绘制给定起始点点位,得到起点位姿
T2=transl(900,-600,700); %根据给定终点,得到终止位姿
T=ctraj(T1,T2,50);
Tj=transl(T);
plot3(Tj(:,1), Tj(:,2), Tj(:,3));%输出末端轨迹
grid on;
q = CO605.ikine(T);
CO605.plot(q); %动画演示
q = q*180/pi; %输出运动关节角
%绘制机器人直线轨迹