62 lines
1.6 KiB
Mathematica
62 lines
1.6 KiB
Mathematica
|
clear;
|
|||
|
|
clc;
|
|||
|
|
|
|||
|
|
L1=0.151;L3=0.350; L4=0.139;
|
|||
|
|
|
|||
|
|
theta_1=-0.02;
|
|||
|
|
theta_2=-1.473;
|
|||
|
|
theta_3=-0.024;
|
|||
|
|
theta_4=-1.559;
|
|||
|
|
theta_5=-2.98966;
|
|||
|
|
|
|||
|
|
q = [theta_1,theta_2,theta_3,theta_4,theta_5]
|
|||
|
|
|
|||
|
|
|
|||
|
|
%定义关节角度限制
|
|||
|
|
lim1_min = -pi/2; lim1_max = pi/2; %关节1(-90,90)
|
|||
|
|
lim2_min = -pi/2; lim2_max = pi/2; %关节2(0，90)
|
|||
|
|
lim3_min = -pi; lim3_max = pi; %关节4(-180，180)
|
|||
|
|
lim4_min = -pi; lim4_max = pi; %关节5(-180，180)
|
|||
|
|
lim5_min = -pi/2; lim5_max = pi/2; %关节6(-90，90)
|
|||
|
|
|
|||
|
|
|
|||
|
|
%定义关节旋转范围
|
|||
|
|
lim1 = lim1_max - lim1_min;
|
|||
|
|
lim2 = lim2_max - lim2_min;
|
|||
|
|
lim3 = lim3_max - lim3_min;
|
|||
|
|
lim4 = lim4_max - lim4_min;
|
|||
|
|
lim5 = lim5_max - lim5_min;
|
|||
|
|
|
|||
|
|
%定义关节
|
|||
|
|
r(1)=Link([0 L1 0 pi/2]);r(1).qlim=[lim1_min,lim1_max];
|
|||
|
|
r(2)=Link([0 0 L3 pi/2]);r(2).qlim=[lim2_min,lim2_max];
|
|||
|
|
r(3)=Link([0 0 0 -pi/2]);r(3).qlim=[lim3_min,lim3_max];
|
|||
|
|
r(4)=Link([0 0 0 pi/2]);r(4).qlim=[lim4_min,lim4_max];
|
|||
|
|
r(5)=Link([0 L4 0 0]);r(5).qlim=[lim5_min,lim5_max];
|
|||
|
|
|
|||
|
|
%组合关节
|
|||
|
|
robot=SerialLink([r(1),r(2),r(3),r(4),r(5)],'name','robot');
|
|||
|
|
robot.fkine(q)
|
|||
|
|
|
|||
|
|
robot.display();
|
|||
|
|
|
|||
|
|
%
|
|||
|
|
%使用蒙特卡洛法绘制机械臂的工作空间
|
|||
|
|
N=5000;
|
|||
|
|
theta1 = ( lim1_min + (lim1 * rand(N,1)) );
|
|||
|
|
theta2 = ( lim2_min + (lim2 * rand(N,1)) );
|
|||
|
|
theta3 = ( lim3_min + (lim3 * rand(N,1)) );
|
|||
|
|
theta4 = ( lim4_min + (lim4 * rand(N,1)) );
|
|||
|
|
theta5 = ( lim5_min + (lim5 * rand(N,1)) );
|
|||
|
|
|
|||
|
|
% for n = 1:N
|
|||
|
|
% theta = [theta1(n),theta2(n),theta3(n),theta4(n),theta5(n)];
|
|||
|
|
% workspace = robot.fkine(theta);
|
|||
|
|
% plot3(workspace.t(1),workspace.t(2),workspace.t(3),'b.','markersize',1);
|
|||
|
|
% hold on;
|
|||
|
|
% end
|
|||
|
|
% robot.plot(theta); %动画显示
|
|||
|
|
|
|||
|
|
robot.teach;
|
|||
|
|
|