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;