绘制三个自由度的机械臂

clc
t=0:0.1:2*pi;
x=cos(t);%圆的x坐标
y=sin(t);%圆的y坐标
r = 0.05; % 圆半径
set(gcf,'doublebuffer','on'); %gcf 返回当前 Figure 对象的句柄值；打开双缓冲
hold on; % 这样就可以在一个图上画多条线
fill(r*x,r*y,'c'); % 画中心点
axis([-2,5,-2,5]); 
q=0;w=pi/3;Z=1.0*exp(i*q);
cd=abs(4-Z);ad=1.5;ab=1.5;bc=2;
ac=sqrt(ab^2+bc^2-2*ab*bc*cos(w));
jbac=acos((ab^2+ac^2-bc^2)/(2*ab*ac));
jd=acos((ad^2+cd^2-ab^2-bc^2+2*ab*bc*cos(w))/(2*ad*cd));
A=Z+ad*exp((jd+angle(4-Z))*i);
B=A+ab*exp((jbac+angle(4-A))*i);
h1 = plot(real([Z,A]),imag([Z,A]),'y','linewidth',3,'linestyle','--');%real是实部， imag是虚部 画出黄色的连杆
h2 = plot(real([A,B]),imag([A,B]),'b','linewidth',3,'linestyle',':'); %画出蓝色的连杆。
h3 = plot(real([B,4]),imag([B,4]),'c','linewidth',3,'linestyle',':');
grid on;
ga = plot(re