Matlab求解机械臂可达工作空间（动画版本）(基于robotics-toolbox)

1、做了一个可视化的蒙特卡洛求解工作空间的程序

代码：(注意9和10两个版本的toolbox代码有区别)

%arm_solve.m
%机械臂可达空间动画求解
%using  Robotic Toolbox 9.10

clc;
clear;
L(1) = Link([0,0.08,0,-pi/2]);
L(2) = Link([0,0.455,0,pi/2]);
L(3) = Link([0,0.0,0,-pi/2]);
L(4) = Link([0,0.145,0,pi/2]);
L(1).qlim=[0,pi/2];
L(2).qlim=[-pi/2,pi/2];
L(3).qlim=[-pi/2,pi/2];
L(4).qlim=[-pi,pi];
base=[ 1 0 0 0;0 0 -1 0;0 1 0 0;0 0 0 1];

four_arm = SerialLink(L,'name','fourarm','base',base);

teach(four_arm); 
four_arm.plot([0 0 pi/3 0])
hold on;
N=30000;    %随机次数

    %关节角度限制
limitmax_1 = 0.0;
limitmin_1 = 90.0;
limitmax_2 = -90.0;
limitmin_2 = 90.0;
limitmax_3 = -90.0;
limitmin_3 = 90.0;
limitmax_4 = -180.0;
limitmin_4 = 180.0;

theta1=(limitmin_1+(limitmax_1-limitmin_1)*rand(N,1))*pi/180; %关节1限制
theta2=(limitmin_2+(limitmax_2-limitmin_2)*rand(N,1))*pi/180; %关节2限制
theta3=(limitmin_3+(limitmax_3-limitmin_3)*rand(N,1))*pi/180; %关节3限制
theta4=(limitmin_4+(limitmax_4-limitmin_4)*rand(N,1))*pi/180; %关节4限制

for n=1:1:3000
qq=[theta1(n),theta2(n),theta3(n),theta4(n)];
four_arm.plot(qq);%动画显示
Mricx=four_arm.fkine(qq);
plot3(Mricx(1,4),Mricx(2,4),Mricx(3,4),'b.','MarkerSize',0.5);%画出落点
hold on;
end

%arm_solve.m
%机械臂可达空间动画求解
%using  Robotic Toolbox 10.4
%update 2020.1
clc;
clear;
L(1) = Link([0,0.08,0,-pi/2]);
L(2) = Link([0,0.455,0,pi/2]);
L(3) = Link([0,0.0,0,-pi/2]);
L(4) = Link([0,0.145,0,pi/2]);
L(1).qlim=[0,pi/2];
L(2).qlim=[-pi/2,pi/2];
L(3).qlim=[-pi/2,pi/2];
L(4).qlim=[-pi,pi];
base=[ 1 0 0 0;0 0 -1 0;0 1 0 0;0 0 0 1];

four_arm = SerialLink(L,'name','fourarm','base',base);

teach(four_arm); 
four_arm.plot([0 0 pi/3 0])
hold on;
N=30000;    %随机次数

    %关节角度限制
limitmax_1 = 0.0;
limitmin_1 = 90.0;
limitmax_2 = -90.0;
limitmin_2 = 90.0;
limitmax_3 = -90.0;
limitmin_3 = 90.0;
limitmax_4 = -180.0;
limitmin_4 = 180.0;

theta1=(limitmin_1+(limitmax_1-limitmin_1)*rand(N,1))*pi/180; %关节1限制
theta2=(limitmin_2+(limitmax_2-limitmin_2)*rand(N,1))*pi/180; %关节2限制
theta3=(limitmin_3+(limitmax_3-limitmin_3)*rand(N,1))*pi/180; %关节3限制
theta4=(limitmin_4+(limitmax_4-limitmin_4)*rand(N,1))*pi/180; %关节4限制

for n=1:1:3000
qq=[theta1(n),theta2(n),theta3(n),theta4(n)];
four_arm.plot(qq);%动画显示
Mricx=four_arm.fkine(qq);
plot3(Mricx.t(1),Mricx.t(2),Mricx.t(3),'b.','MarkerSize',0.5);%画出落点
hold on;
end

2、当然上面那个还是太慢了，真正想要迅速得到结果还是用这个整体矩阵求解比较方便,1s就有结果。

%机械臂可达空间迅速求解
% Robotic Toolbox 9.10

clc;
clear;
L(1) = Link([0,0.08,0,-pi/2]);
L(2) = Link([0,0.455,0,pi/2]);
L(3) = Link([0,0.0,0,-pi/2]);
L(4) = Link([0,0.145,0,pi/2]);
L(1).qlim=[0,pi/2];
L(2).qlim=[-pi/2,pi/2];
L(3).qlim=[-pi/2,pi/2];
L(4).qlim=[-pi,pi];
base=[ 1 0 0 0;0 0 -1 0;0 1 0 0;0 0 0 1];

four_arm = SerialLink(L,'name','fourarm','base',base);

teach(four_arm); 
four_arm.plot([0 0 pi/3 0])
hold on;
N=30000;    %随机次数

    %关节角度限制
limitmax_1 = 0.0;
limitmin_1 = 90.0;
limitmax_2 = -90.0;
limitmin_2 = 90.0;
limitmax_3 = -90.0;
limitmin_3 = 90.0;
limitmax_4 = -180.0;
limitmin_4 = 180.0;

theta1=(limitmin_1+(limitmax_1-limitmin_1)*rand(N,1))*pi/180; %关节1限制
theta2=(limitmin_2+(limitmax_2-limitmin_2)*rand(N,1))*pi/180; %关节2限制
theta3=(limitmin_3+(limitmax_3-limitmin_3)*rand(N,1))*pi/180; %关节3限制
theta4=(limitmin_4+(limitmax_4-limitmin_4)*rand(N,1))*pi/180; %关节4限制


qq=[theta1,theta2,theta3,theta4];

Mricx=four_arm.fkine(qq);

x=reshape(Mricx(1,4,:),N,1);
y=reshape(Mricx(2,4,:),N,1);
z=reshape(Mricx(3,4,:),N,1);
plot3(x,y,z,'b.','MarkerSize',0.5);%画出落点
hold on;

%机械臂可达空间迅速求解
% Robotic Toolbox 10.4

clc;
clear;
L(1) = Link([0,0.08,0,-pi/2]);
L(2) = Link([0,0.455,0,pi/2]);
L(3) = Link([0,0.0,0,-pi/2]);
L(4) = Link([0,0.145,0,pi/2]);
L(1).qlim=[0,pi/2];
L(2).qlim=[-pi/2,pi/2];
L(3).qlim=[-pi/2,pi/2];
L(4).qlim=[-pi,pi];
base=[ 1 0 0 0;0 0 -1 0;0 1 0 0;0 0 0 1];

four_arm = SerialLink(L,'name','fourarm','base',base);

teach(four_arm); 
four_arm.plot([0 0 pi/3 0])
hold on;
N=30000;    %随机次数

    %关节角度限制
limitmax_1 = 0.0;
limitmin_1 = 90.0;
limitmax_2 = -90.0;
limitmin_2 = 90.0;
limitmax_3 = -90.0;
limitmin_3 = 90.0;
limitmax_4 = -180.0;
limitmin_4 = 180.0;

theta1=(limitmin_1+(limitmax_1-limitmin_1)*rand(N,1))*pi/180; %关节1限制
theta2=(limitmin_2+(limitmax_2-limitmin_2)*rand(N,1))*pi/180; %关节2限制
theta3=(limitmin_3+(limitmax_3-limitmin_3)*rand(N,1))*pi/180; %关节3限制
theta4=(limitmin_4+(limitmax_4-limitmin_4)*rand(N,1))*pi/180; %关节4限制


qq=[theta1,theta2,theta3,theta4];

Mricx=four_arm.fkine(qq);
X=zeros(N,1);
Y=zeros(N,1);
Z=zeros(N,1);
for n=1:1:N
    X(n)=Mricx(n).t(1);
    Y(n)=Mricx(n).t(2);
    Z(n)=Mricx(n).t(3);

end
plot3(X,Y,Z,'b.','MarkerSize',0.5);%画出落点
hold on;