Robot workspace(机器人工作空间求解)

Robot workspace

图片
Robot workspace(机器人工作空间求解)_第1张图片

% 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;

你可能感兴趣的:(MATLAB,Pro,robot)