蒙特卡洛法求解机械臂工作空间（AUBO-C5）

博客地址：https://www.cnblogs.com/zylyehuo/

参考链接：蒙特卡洛法求解机械臂工作空间（以IRB4600型工业机械臂为例）

clc; clear; close all;

% ========== 定义DH参数 ==========
% 关节变量（θ），初始都为0，d、a、α按典型六轴结构设定
% 单位为毫米和弧度
d1 = 0.1215
d2 = 0.1405
d3 = 0
d4 = -0.1025
d5 = -0.1025
d6 = -0.094

theta1 = pi/2
theta2 = -pi/2
theta3 = 0
theta4 = -pi/2
theta5 = 0
theta6 = 0

a0 = 0
a1 = 0
a2 = 0.408
a3 = 0.376
a4 = 0
a5 = 0

alpha0 = 0
alpha1 = -pi/2
alpha2 = 0
alpha3 = pi
alpha4 = pi/2
alpha5 = -pi/2

L(1)=Link([theta1, d1, a0, alpha0],  'modified')
L(2)=Link([theta2, d2, a1, alpha1, 0, -pi/2],  'modified')
L(3)=Link([theta3, d3, a2, alpha2],  'modified')
L(4)=Link([theta4, d4, a3, alpha3, 0, -pi/2],  'modified')
L(5)=Link([theta5, d5, a4, alpha4],  'modified')
L(6)=Link([theta6, d6, a5, alpha5],  'modified')

L1.qlim =[-175*pi/180, 175*pi/180];
L2.qlim =[-175*pi/180, 175*pi/180];
L3.qlim =[-175*pi/180, 175*pi/180];
L4.qlim =[-175*pi/180, 175*pi/180];
L5.qlim =[-175*pi/180, 175*pi/180];
L6.qlim =[-175*pi/180, 175*pi/180];

% ========== 连接连杆为一个机器人 ==========
robot = SerialLink(L,'name','AUBO-C5')
% 蒙特卡洛法确定工作空间
NUM = 10000;% 随机点数
J = zeros(NUM,6);
POS = zeros(NUM,3);%x,y,z
for i = 1:NUM
    J(i,1) = L1.qlim(1) + (L1.qlim(2)-L1.qlim(1))*rand;
    J(i,2) = L2.qlim(1) + (L2.qlim(2)-L2.qlim(1))*rand;
    J(i,3) = L3.qlim(1) + (L3.qlim(2)-L3.qlim(1))*rand;
    J(i,4) = L4.qlim(1) + (L4.qlim(2)-L4.qlim(1))*rand;
    J(i,5) = L5.qlim(1) + (L5.qlim(2)-L5.qlim(1))*rand;
    J(i,6) = L6.qlim(1) + (L6.qlim(2)-L6.qlim(1))*rand;
    T = robot.fkine(J(i,:));
    POS(i,:) = transl(T);
    % 绘制结果
end
% 绘制结果
robot.plot(J(NUM,:));%最后一个点
hold on
plot3(POS(:,1),POS(:,2),POS(:,3),'b.','MarkerSize',0.5);%随机轨迹
grid on;