写出matlab六自由度机械臂正运动学仿真

好的，以下是一个MATLAB六自由度机械臂正运动学仿真的代码示例：

clc
clear
close all

%% DH参数
a1 = 0;
a2 = 0;
a3 = 0.1;
a4 = 0.1;
a5 = 0.1;
a6 = 0;
d1 = 0.1;
d2 = 0;
d3 = 0;
d4 = 0.2;
d5 = 0;
d6 = 0.1;
alpha1 = -pi/2;
alpha2 = 0;
alpha3 = -pi/2;
alpha4 = pi/2;
alpha5 = -pi/2;
alpha6 = 0;

%% 机械臂关节角度
theta1 = 0;
theta2 = pi/2;
theta3 = 0;
theta4 = 0;
theta5 = 0;
theta6 = 0;

%% 计算转换矩阵
T1 = [cos(theta1), -sin(theta1)*cos(alpha1), sin(theta1)*sin(alpha1), a1*cos(theta1);
      sin(theta1), cos(theta1)*cos(alpha1), -cos(theta1)*sin(alpha1), a1*sin(theta1);
      0, sin(alpha1), cos(alpha1), d1;
      0, 0, 0, 1];
T2 = [cos(theta2), -sin(theta2)*cos(alpha2), sin(theta2)*sin(alpha2), a2*cos(theta2);
      sin(theta2), cos(theta2)*cos(alpha2), -cos(theta2)*sin(alpha2), a2*sin(theta2);
      0, sin(alpha2), cos(alpha2), d2;
      0, 0, 0, 1];
T3 = [cos(theta3), -sin(theta3)*cos(alpha3), sin(theta3)*sin(alpha3), a3*cos(theta3);
      sin(theta3), cos(theta3)*cos(alpha3), -cos(theta3)*sin(alpha3), a3*sin(theta3);
      0, sin(alpha3), cos(alpha3), d3;
      0, 0, 0, 1];
T4 = [cos(theta4), -sin(theta4)*cos(alpha4), sin(theta4)*sin(alpha4), a4*cos(theta4);
      sin(theta4), cos(theta4)*cos(alpha4), -cos(theta4)*sin(alpha4), a4*sin(theta4);
      0, sin(alpha4), cos(alpha4), d4;
      0, 0, 0, 1];
T5 = [cos(theta5), -sin(theta5)*cos(alpha5), sin(theta5)*sin(alpha5), a5*cos(theta5);
      sin(theta5), cos(theta5)*cos(alpha5), -cos(theta5)*sin(alpha5), a5*sin(theta5);
      0, sin(alpha5), cos(alpha5), d5;
      0, 0, 0, 1];
T6 = [cos(theta6), -sin(theta6)*cos(alpha6), sin(theta6)*sin(alpha6), a6*cos(theta6);
      sin(theta6), cos(theta6)*cos(alpha6), -cos(theta6)*sin(alpha6), a6*sin(theta6);
      0, sin(alpha6), cos(alpha6), d6;
      0, 0, 0, 1];

T = T1*T2*T3*T4*T5*T6;

%% 获得末端执行器的位置和姿态
position = T(1:3, 4);
orientation = tform2quat(T);

%% 可视化机械臂
figure;
plot3(position(1), position(2), position(3), 'ro');
hold on;
quiver3(position(1), position(2), position(3), orientation(2), orientation(3), orientation(4), 'b');
xlabel('X');
ylabel('Y');
zlabel('Z');
axis equal;
grid on;

请注意，在实际应用中，机械臂的DH参数和关节角度可能会根据具体的机械臂模型而有所不同。因此，您需要根据具体的机械臂模型进行参数的调整。