计算机械臂运动空间

计算机械臂运动空间(随机一亿个数据)

clear
close all
clc
syms t1 t2 t3   ;
syms a b l1 l2   ;
t=[1        0         0   0
    0    0   -1         0
    0    1    0         0
    0         0         0    1];
T1=trotz(t1)*transl(a,0,b)*t; 
T2=trotz(t2)*transl(l1,0,0);
T3=trotz(t3)*transl(l2,0,0);
T=T1*T2*T3;
T=T(1:3,4);
T
a=0.6
b=0.8
l1=2.1
l2=2.5
tt=1000
t1=(rand(tt,1)*220-110)/180*pi    ;
t2=(rand(tt,1)*135-45)/180*pi    ;
t3=(rand(tt,1)*60-80)/180*pi    ;
x= a*cos(t1) + l1*cos(t1).*cos(t2) + l2*cos(t1).*cos(t2).*cos(t3) - l2*cos(t1).*sin(t2).*sin(t3);
y= a*sin(t1) + l1*cos(t2).*sin(t1) + l2*cos(t2).*cos(t3).*sin(t1) - l2*sin(t1).*sin(t2).*sin(t3);
z=            b + l1.*sin(t2) + l2*cos(t2).*sin(t3) + l2.*cos(t3).*sin(t2);
plot3(x,y,z,'.')
figure
tt=0
n=(abs(z-tt)<0.01);
plot(x(n),y(n),'.')
tt=num2str(tt)
tt=['当z=',tt,'时，所能到达区域']
title(tt)
figure
tt=0.8
n=(abs(z-tt)<0.01);
plot(x(n),y(n),'.')
tt=num2str(tt)
tt=['当z=',tt,'时，所能到达区域']
title(tt)
figure
tt=4.5
n=(abs(z-tt)<0.01);
plot(x(n),y(n),'.')
tt=num2str(tt)
tt=['当z=',tt,'时，所能到达区域']
title(tt)