matlab机器人画圆轨迹规划

在这里插入图片描述

function Untitled()clear;clc;
L(1)=Link([0 0.15055 0 pi/2]);    
L(2)=Link([0 -0.115 0.246 0]);
L(3)=Link([0 0.1163 0.190 0]);
L(4)=Link([0 -0.1175 0 pi/2]);
L(5)=Link([0 0.1175 0 -pi/2]);
L(6)=Link([0 0.105 0 0]);ZU3=SerialLink(L,'name','JAKA ZU3');     %确定画图平面
p0=[0,0,0];       
p1=[0.4,0.4,0.4];
p2=[-0.5,0.5,0.5];
X=p1-p0;B=p2-p0;  
Z=cross(X,B);
Y=cross(Z,X);   
x1=X/norm(X);
y1=Y/norm(Y);
z1=Z/norm(Z); 
R=[x1.',y1.',z1.'];
Js=[R ,p0.'];
Js=[Js;0 0 0 1];
r=norm(p1-p0);  %圆的轨迹
t=0:0.05:5;
[s,sd,~] = tpoly(0,2*pi,t);       
q1=[-12.7,98.9,-107,158,-114,11]; 
k = q1;
vs =[]; % qs  关节速度
pd=[];%ps  笛卡尔坐标系下的坐标位置
for i = 1:length(s)T=[1 0 0 r*cos(s(i));0 1 0 r*sin(s(i));0 0 1 0;0 0 0 1];T1=Js*T;       q=ZU3.ikunc(T1,k); %求解逆运动k=q;pd=[pd;T1(1:3,4).'];vs=[vs;q];
end
close all;
figure('NumberTitle', 'off', 'Name', '运动图像');
plot2(pd,'r','LineWidth',2);%圆的线宽
hold on;
ZU3.plot(vs,'fps',20);%输出运动图像
%绘制运动曲线
figure('NumberTitle', 'off', 'Name', '关节值随时间变化曲线');subplot(4,1,1);%图像布局plot(t,pd);  %输出空间位置曲线title('空间位置曲线')xlabel('秒(s)');ylabel('路程(m)')subplot(4,1,2);   plot(t,vs);%输出关节角度曲线title('关节角度曲线')  xlabel('秒(s)');  ylabel('角度(rad)');legend('θ1','θ2','θ3','θ4','θ5','θ6');%计算空间速度并输出空间速度曲线vx = -r*sin(s).*sd;vy = r*cos(s).*sd;vv = r*sd;subplot(4,1,3);   plot(t,[vx,vy,vv]);title('空间速度曲线')xlabel('秒(s)');   ylabel('速度(m/s)')%计算关节速度omiga=[];for i =1:length(s)J0=ZU3.jacob0(vs(i,:)');dq=J0\[vx(i);vy(i);0;0;0;0];omiga=[omiga;dq'];endsubplot(4,1,4);  plot(t,rad2deg(omiga));title('关节速度曲线')%输出关节速度曲线xlabel('秒(s)');  ylabel('角速度(rad/s)')