机器人工作空间求解编程matlab

%四轴机械臂工作空间计算
clc;
clear all;
format short;%保留精度

%角度转换
du=pi/180; %度
radian=180/pi; %弧度


%% 求取工作空间
%关节角限位
q1_s=-180*du; q1_end=180*du;
d2_s=0; d2_end=60;
q3_s=-270*du; q3_end=-90*du;
q4_s=-180*du; q4_end=180*du;


%设置step
%step越大，点越稀疏，运行时间越快
step=20;%计算步距
step1= (q1_end -q1_s) / step;
step2= (d2_end -d2_s) / step;
step3= (q3_end -q3_s) / step;
step4= (q4_end -q4_s) / step;

%计算工作空间
tic;%tic1
i=1;
% T = zeros(3,1);
% T_x = zeros(1,step1*step2*step3*step4*step5);
% T_y = zeros(1,step1*step2*step3*step4*step5);
% T_z = zeros(1,step1*step2*step3*step4*step5);
for q1=q1_s:step1:q1_end
for d2=d2_s:step2:d2_end
for q3=q3_s:step3:q3_end
for q4=q4_s:step4:q4_end

% T=p560.fkine([q1*du d2 q3*du q4*du ]);%正向运动学仿真函数
T10=[cos(q1) -sin(q1) 0 0; sin(q1) cos(q1) 0 0; 0 0 1 0;0 0 0 1];
T21=[1 0 0 0; 0 0 1 d2; 0 -1 0 0; 0 0 0 1];
T32=[cos(q3) -sin(q3) 0 0; 0 0 -1 0; sin(q3) cos(q3) 0 0;0 0 0 1];
T43=[cos(q4) -sin(q4) 0 0; 0 0 -1 -60; sin(q4) cos(q4) 0 0; 0 0 0 1];

T=T10*T21*T32*T43
T_x(1,i) = T(1,4);
T_y(1,i) = T(2,4);
T_z(1,i) = T(3,4);
i=i+1;
end
end
end
end
disp(['循环运行时间：',num2str(toc)]);
t1=clock;

%% 绘制工作空间
% figure('name','四轴机械臂工作空间')
% hold on
% plotopt = {'noraise', 'nowrist', 'nojaxes', 'delay',0};
% p560.plot([0 20*du 0 0 0 0], plotopt{:});

plot3(T_x,T_y,T_z,'r.','MarkerSize',3);
disp(['绘制工作空间运行时间：',num2str(etime(clock,t1))]);

%获取X,Y,Z空间坐标范围
Point_range=[min(T_x) max(T_x) min(T_y) max(T_y) min(T_z) max(T_z)]
hold off