【六自由度机器人工作空间绘制】

#六自由度机器人工作空间绘制
先建立一个变换矩阵函数（MD-H参数法）

function T=myf(alpha,a,d,theta)
T=[            cosd(theta),           -sind(theta),           0,             a;
cosd(alpha)*sind(theta), cosd(alpha)*cosd(theta), -sind(alpha), -d*sind(alpha);
sind(alpha)*sind(theta), sind(alpha)*cosd(theta),  cosd(alpha),  d*cosd(alpha);
                     0,                     0,           0,             1];
end % end myf

本来很理想地想用subs()函数代入变量theta, 但是好像没用对。

clear; 
clc;
syms theta1 theta2 theta3 theta4 theta5 theta6;
T0_1=myf(0,0,0,theta1);
T1_2=myf(-90,230,0,theta2);
T2_3=myf(0,850,0,theta3);
T3_4=myf(-90,170,0,theta4);
T4_5=myf(90,0,0,theta5);
T5_6=myf(-90,0,190,theta6);
T0_4=T0_1*T1_2*T2_3*T3_4*T4_5;
T0_6=T0_1*T1_2*T2_3*T3_4*T4_5*T5_6;
x=T0_6(1,4);
y=T0_6(2,4);
z=T0_6(3,4);
% 创建三维矩阵
XYZ = zeros(3,80000);
k=0;
for theta1=-180:5:180
    for theta2=-90:5:135
        for theta3=-185:5:80 
            for theta4=-180:5:180
                for theta5=-130:60:130  
                    for theta6=-180:60:180
                        k=k+1;
                        % 第一二三行分别为xyz坐标
                        XYZ(1,k) =subs(x);
                        XYZ(2,k) =subs(y);
                        XYZ(3,k) =subs(z);
                    end
                end
            end
        end
    end
end
subplot(1, 2, 1);
figure1 = scatter(XYZ(1,:),XYZ(2,:));
title("机械臂末端工作空间的俯视图")
subplot(1, 2, 2);
figure2=scatter3(XYZ(1,:),XYZ(2,:),XYZ(3,:));
title("机械臂末端工作空间的3D视图")

于是改用粗暴的方法，直接打印出函数x,y,z, copy一下，替换循环里面的subs(x),subs(y),subs(z)，很快跑完了。。。