用Matlab写一个以6个关节角为输入变量的六自由度机械臂雅可比矩阵求解代码

以下是一个以6个关节角为输入变量的六自由度机械臂雅可比矩阵求解的 Matlab 代码示例：

function J = jacobian(theta, L)
% theta: 1x6 关节角度，单位为弧度
% L: 1x6 关节长度
% J: 6x6 雅可比矩阵

% 初始化变量
J = zeros(6, 6);
T = eye(4);

% 计算机械臂正向运动学
for i = 1:6
    T = T * dh_transform(theta(i), L(i));
end

% 计算雅可比矩阵
for i = 1:6
    % 计算机械臂正向运动学
    T_i = eye(4);
    for j = 1:i-1
        T_i = T_i * dh_transform(theta(j), L(j));
    end
    % 计算末端位姿相对于第 i 个关节的雅可比矩阵
    z_i_1 = T_i(1:3, 3);
    p_i_1 = T_i(1:3, 4);
    J(1:3, i) = cross(z_i_1, T(1:3, 4) - p_i_1);
    J(4:6, i) = z_i_1;
end

end

function T = dh_transform(theta, d)
% 计算 DH 参数法下的变换矩阵
a = 0;
alpha = -pi / 2;
T = [cos(theta), -sin(theta) * cos(alpha), sin(theta) * sin(alpha), a * cos(theta);
     sin(theta), cos(theta) * cos(alpha), -cos(theta) * sin(alpha), a * sin(theta);
     0, sin(alpha), cos(alpha), d;
     0, 0, 0, 1];
end

使用时，可以输入关节角度和长度，计算出雅可比矩阵：

theta = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6];
L = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6];
J = jacobian(theta, L);