使用Lagrange-Euler计算给定轨迹的所需扭矩

我有一个带有2个旋转关节的2DOF机器人，如下图所示。我正在尝试（使用MATLAB）计算移动它所需的扭矩，但我的答案与我的预期不符。
br /> $$
\ begin {array} {c | cccc}
关节＆a＆\ alpha＆d＆\ theta \\
\ hline
1＆0＆ \ pi / 2＆0＆\ theta_1 \\
2＆1＆0＆0＆\ theta_2 \\
\ end {array}

我正在尝试使用本文第5/6页中所述的Euler-Lagrange技术来计算产生给定加速度所需的扭矩。
特别是，
$$ T_i（inertial）= \ sum_ {j = 0} ^ nD_ {ij} \ ddot q_i $$
其中
$$ D_ {ij} = \ sum_ {p = max（i，j）} ^ n Trace（U_ { pj} J_pU_ {pi} ^ T）$$

$$
J_i = \ begin {bmatrix}
{（-I_ {xx} + I_ {yy} + I_ {zz}）\ over 2}和I_ {xy}＆I_ {xz}＆m_i \ bar x_i \\
I_ {xy}＆{（I_ {xx} -I_ {yy} + I_ {zz }）\ over 2}＆I_ {yz}＆m_i \ bar y_i \\
I_ {xz}＆I_ {yz}＆{（I_ {xx} + I_ {yy} -I_ {zz}）\超过2}和m_i \ bar z_i \\
m_i \ bar x_i和m_i \ bar y_i＆m_i \ bar z_i＆m_i \ e nd {bmatrix}
$$

由于遇到麻烦，我试图创建一个最简单的示例，但我仍然出错了。为此，我试图计算以恒定的1 $ {rad \ over s ^ 2} $加速$ \ theta_1 $所需的惯性转矩。由于$ \ theta_2 $恒定为0，我认为这应该消除所有陀螺/科里奥利力。我使链接1失重，因此其伪惯性矩阵为0。我已经为链接2计算了我的伪惯性矩阵：
$$
I_ {xx} = {mr ^ 2 \ over 2} = 0.0025 \\ I_ {yy} = I_ {zz} = {ml ^ 2 \ over 3} = 2/3
$$
$$
J_2 = \开始{bmatrix }
1.3308＆0＆0＆-1 \\
0＆0.0025＆0＆0 \\
0＆0＆0.0025＆0 \\
-1＆0＆ 0＆2 \\
\ end {bmatrix}
$$

关节1的预期扭矩：
$$
T_1 = I \ ddot \ omega \\
T_1 = {ml ^ 2 \ over 3} \ times \ ddot \ omega \\
T_1 = {2 \ times1 \ over3} \ times1 \\
T_1 = {2 \ over3} Nm
$$

我的关节1代码计算出的扭矩：

q = [0 0];
qdd = [1 0];
T = calcT(q);
calc_inertial_torque(1, T, J, qdd) 

$$
T_1 = {4 \ over3} Nm
$$

所以这是我的问题，我的代码$ T_1 $与我的简单机制$不符T_1 $。

被调用的关键功能如下所示。但我很高兴能将其扩展到更多自由度。