Robot workspace
【MATLAB|Robot workspace（机器人工作空间求解）】图片
文章图片
% Main program
clear;
clc;
%建立机器人模型
%thetadaalphaoffset
ML1=Link([0.00.00.00.00.0],'modified');
ML2=Link([000.180-pi/60],'modified');
ML3=Link([000.60000],'modified');
ML4=Link([00.6300.130-pi/40],'modified');
ML5=Link([000pi/20],'modified');
ML6=Link([000-pi/20],'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);
endfigure(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=[theta100.180-pi/2;
theta200.6000;
theta300.130-pi/2;
theta40.6300pi/2;
theta500-pi/2;
theta60.107500];
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));
0sin(SDH(1,4))cos(SDH(1,4))SDH(1,2);
0001];
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));
0sin(SDH(2,4))cos(SDH(2,4))SDH(2,2);
0001];
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));
0sin(SDH(3,4))cos(SDH(3,4))SDH(3,2);
0001];
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));
0sin(SDH(4,4))cos(SDH(4,4))SDH(4,2);
0001];
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));
0sin(SDH(5,4))cos(SDH(5,4))SDH(5,2);
0001];
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));
0sin(SDH(6,4))cos(SDH(6,4))SDH(6,2);
0001];
T06=T01*T12*T23*T34*T45*T56;
推荐阅读
- 最优化问题|改进交叉算子的自适应人工蜂群黏菌算法
- matlab|嵌入均衡池的黏菌优化算法
- 最优化问题|加入领导者的黏菌优化算法
- MATLAB图形界面|基于Matlab的汽车出入库计时计费系统
- Matlab旅程|MATLAB的结构化程序设计