MATLAB做机器人仿真（实验六）

x1=-pi/2; a1=121.16; d1=191.99;
x2=0; a2=850.94; th2=82.19*pi/180;
x3=0; a3=482.6;
x4=pi/2; a4=133.35;
x5=-pi/2; a5=0; th5=pi/2;
x6=0; d6=380.9;
L(1)=Link([0 d1 a1 x1]); L(1).qlim=[-pi,pi];
L(2)=Link([0 0 a2 x2]); L(2).qlim=[-pi/6,pi/2];
L(3)=Link([0 0 a3 x3]); L(3).qlim=[-pi/2,pi/2];
L(4)=Link([0 0 a4 x4]); L(4).qlim=[-pi/2,pi/2];
L(5)=Link([0 0 a5 x5]); L(5).qlim=[-pi/2,pi/2];
L(6)=Link([0 d6 0 x6]); L(6).qlim=[-pi,pi];
robot6=SerialLink(L,'name','robot6','offset',[0 -th2 th2 0 -th5 0]);
%% 轨迹规划-圆
    Th=0:pi/60:2*pi;R=400;
    for I=1:length(Th)
    j=1:length(Th);
    x(1,j)=1500;
    y(1,j)=R*sin(Th);
    z(1,j)=0+R*cos(Th);
    end

for ko=1:length(Th)
TTr=[x(ko),y(ko),z(ko)];
TO(:,:,ko)=transl(TTr)*trotz(-90)*troty(0)*trotx(-90);
end
qz=ikine(robot6,TO,'pinv');
plot3(x,y,z,'r','LineWidth',2);
hold on;
plot(robot6,qz,'loop');
hold off;