Robot workspace（机器人工作空间求解）
Robot workspace
图片
% Main program
clear;
clc;
%建立机器人模型
% theta d a alpha offset
ML1=Link([0.0 0.0 0.0 0.0 0.0 ],'modified');
ML2=Link([0 0 0.180 -pi/6 0 ],'modified');
ML3=Link([0 0 0.600 0 0 ],'modified');
ML4=Link([0 0.630 0.130 -pi/4 0 ],'modified');
ML5=Link([0 0 0 pi/2 0 ],'modified');
ML6=Link([0 0 0 -pi/2 0 ],'modified');
modrobot=SerialLink([ML1 ML2 ML3 ML4 ML5 ML6],'name','modified');
modrobot.plot([0,0,0,0,0,0]);
hold on;
N=500000; %随机次数
theta1=-165/180*pi+(165/180*pi)*rand(N,1); %关节1限制
theta2=-90/180*pi+(155/180*pi)*rand(N,1); %关节2限制
theta3=-200/180*pi+(70/180*pi)*rand(N,1); %关节3限制
theta4=-170/180*pi+(170/180*pi)*rand(N,1); %关节4限制
theta5=-135/180*pi+(135/180*pi)*rand(N,1); %关节5限制
theta6=-360/180*pi+(360/180*pi)*rand(N,1); %关节6限制
for n=1:1:N
modmyt06=mystafkine(theta1(n),theta2(n),theta3(n),theta4(n),theta5(n),theta6(n));
plot3(modmyt06(1,4),modmyt06(2,4),modmyt06(3,4),'b*','MarkerSize',1);
% hold om
X(n,1)=modmyt06(1,4);
Y(n,1)=modmyt06(2,4);
Z(n,1)=modmyt06(3,4);
end
figure(2)
x = double(X); y=double(Y); z= double(Z); %获取点云坐标
alp = 0.2;region = 0.75;%hole = 1; region = 0.75;
shp = alphaShape(x,y,z,alp); %生产点云的包络数据
plot(shp) %显示点云包络
v= volume(shp);
title(['v= ',num2str(v) ,'m3']) %计算体积并显示
% Function
function [T06]=mystafkine(theta1,theta2,theta3,theta4,theta5,theta6)
SDH=[theta1 0 0.180 -pi/2;
theta2 0 0.600 0;
theta3 0 0.130 -pi/2;
theta4 0.630 0 pi/2;
theta5 0 0 -pi/2;
theta6 0.1075 0 0];
T01=[cos(SDH(1,1)) -sin(SDH(1,1))*cos(SDH(1,4)) sin(SDH(1,1))*sin(SDH(1,4)) SDH(1,3)*cos(SDH(1,1));
sin(SDH(1,1)) cos(SDH(1,1))*cos(SDH(1,4)) -cos(SDH(1,1))*sin(SDH(1,4)) SDH(1,3)*sin(SDH(1,1));
0 sin(SDH(1,4)) cos(SDH(1,4)) SDH(1,2);
0 0 0 1];
T12=[cos(SDH(2,1)) -sin(SDH(2,1))*cos(SDH(2,4)) sin(SDH(2,1))*sin(SDH(2,4)) SDH(2,3)*cos(SDH(2,1));
sin(SDH(2,1)) cos(SDH(2,1))*cos(SDH(2,4)) -cos(SDH(2,1))*sin(SDH(2,4)) SDH(2,3)*sin(SDH(2,1));
0 sin(SDH(2,4)) cos(SDH(2,4)) SDH(2,2);
0 0 0 1];
T23=[cos(SDH(3,1)) -sin(SDH(3,1))*cos(SDH(3,4)) sin(SDH(3,1))*sin(SDH(3,4)) SDH(3,3)*cos(SDH(3,1));
sin(SDH(3,1)) cos(SDH(3,1))*cos(SDH(3,4)) -cos(SDH(3,1))*sin(SDH(3,4)) SDH(3,3)*sin(SDH(3,1));
0 sin(SDH(3,4)) cos(SDH(3,4)) SDH(3,2);
0 0 0 1];
T34=[cos(SDH(4,1)) -sin(SDH(4,1))*cos(SDH(4,4)) sin(SDH(4,1))*sin(SDH(4,4)) SDH(4,3)*cos(SDH(4,1));
sin(SDH(4,1)) cos(SDH(4,1))*cos(SDH(4,4)) -cos(SDH(4,1))*sin(SDH(4,4)) SDH(4,3)*sin(SDH(4,1));
0 sin(SDH(4,4)) cos(SDH(4,4)) SDH(4,2);
0 0 0 1];
T45=[cos(SDH(5,1)) -sin(SDH(5,1))*cos(SDH(5,4)) sin(SDH(5,1))*sin(SDH(5,4)) SDH(5,3)*cos(SDH(5,1));
sin(SDH(5,1)) cos(SDH(5,1))*cos(SDH(5,4)) -cos(SDH(5,1))*sin(SDH(5,4)) SDH(5,3)*sin(SDH(5,1));
0 sin(SDH(5,4)) cos(SDH(5,4)) SDH(5,2);
0 0 0 1];
T56=[cos(SDH(6,1)) -sin(SDH(6,1))*cos(SDH(6,4)) sin(SDH(6,1))*sin(SDH(6,4)) SDH(6,3)*cos(SDH(6,1));
sin(SDH(6,1)) cos(SDH(6,1))*cos(SDH(6,4)) -cos(SDH(6,1))*sin(SDH(6,4)) SDH(6,3)*sin(SDH(6,1));
0 sin(SDH(6,4)) cos(SDH(6,4)) SDH(6,2);
0 0 0 1];
T06=T01*T12*T23*T34*T45*T56;