齿轮动力学方程

    %% Function   动力学方程推导  练习
    clc;clear;
    %% %%%%%%%%%%%%%%%%%%%%%%参数定义及赋值%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    syms Mp Mq Ip Iq rp rq 
    syms thita_p thita_q dthita_p dthita_q deta ddeta de e t Tp Tq km cm
    %% %%%%%%%%%%%%%%%%%%%%%%参数定义及赋值%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    dpt=[thita_p;thita_q];     %振动位移
    %% 方程推导
    e=0.02*sin(t)+cos(1+0.5*t);
    de=diff(e);
    deta=rp*thita_p-rq*thita_q+e;
    ddeta=rp*dthita_p-rq*dthita_q+de;
    T=1/2*(Ip*dthita_p^2+Iq*dthita_q^2);
    U=1/2*(km*deta^2);
    R=1/2*(cm*ddeta^2);
    L=T-U;
    dp(1,1)=Ip\(Tp+diff(L,thita_p)-diff(