机器人动力学 拉格朗日乘子法求解动力学方程

clear
clc

syms m1 m2 m3 real
syms L1 L2 L3 h1 h2 h3 w1 w2 w3 real
syms theta1 theta2 theta3 real
syms g real

%% 初始化 需要自己修改的地方
% DH 参数
DH=[0,0,0,theta1;0,L1,0,theta2];    % DH参数
FreeV = [theta1,theta2];             % 自由变量
[Linen,covn] = size(DH);
dof = Linen;                        % 自由度
% 伪惯性矩矩阵
I1 = [m1*(L1^2)/3 0 0 m1*L1/2;0 0 0 0; 0 0 0 0;m1*L1/2 0 0 m1];
I2 = [m2*(L2^2)/3 0 0 m2*L2/2;0 0 0 0; 0 0 0 0;m2*L2/2 0 0 m2];
I = cat(3,I1,I2);
% 质心位置
C1 =  [1/2*L1 0 0 1];
C2 =  [1/2*L2 0 0 1];
C = cat(3,C1,C2);
% 重力加速度
G0 = [0,-g,0,0];
% 连杆质量
M = [m1,m2];
%% 0-i 变换矩阵
T = sym(zeros(4,4,dof));
for i = 1:dof
    if (i == 1)
        T(:,:,i) = simplify(generateTransFor(DH(i,1),DH(i,2),DH(i,3),DH(i,4)));
    else
        T(:,:,i) = simplify(T(:,:,i-1) * generateTransFor(DH(i,1),DH(i,2),DH(i,3),DH(i,4)));
    end
end
%% 求变换矩阵的一阶导数 DT每一行是从0到n的变换矩阵 每一列是求导
DT = sym(zeros(4,4,dof,dof));
for i = 1:dof
    for j = 1:dof
        DT(:,:,i,j) = simplify(diff(T(:,:,i),FreeV(1,j)));
    end
end
%% DT2 第三维是 0到n的变换矩阵 第四维是第一次求导 第五维度是第二次求导
DT2 = sym(zeros(4,4,dof,dof,dof));
for i = 1:dof
    for j = 1:dof
        for m = 1:dof
            DT2(:,:,i,j,m) = simplify(diff(DT(:,:,i,j),FreeV(1,m)));
        end
    end
end
%% 计算D
D = sym(zeros(dof,dof));
for i = 1:dof
    for j = 1:dof
        for m = max(i,j):dof
            D(i,j) = simplify(D(i,j)+trace(DT(:,:,m,i)*I(:,:,m)*DT(:,:,m,j)'));
        end
    end
end
%%  计算H
H = sym(zeros(dof,dof,dof));
for i = 1:dof
    for k = 1:dof
        for m = 1:dof
            for n = max([i,k,m]):dof
                H(i,k,m) = simplify(H(i,k,m)+trace(DT(:,:,n,i)*I(:,:,n)*DT2(:,:,n,k,m)'));
            end
        end
    end
end
H = reshape(H,dof,dof*dof);
%%    计算G
G = sym(zeros(1,dof));
for i = 1:dof
    for k = i:dof
        G(1,i) = simplify(G(1,i)+M(1,k)*G0*DT(:,:,k,i)*C(:,:,k)');
    end
end
G = reshape(G,dof,1);
%% 
symdisp(D);
symdisp(H);
symdisp(G);