11、递归牛顿-欧拉动力学原理和代码

11、递归牛顿-欧拉动力学原理和代码

这段代码实现了机器人学中经典的递归牛顿-欧拉算法（Recursive Newton-Euler Algorithm, RNEA）。RNEA 是计算机械臂逆动力学的核心算法，用于求解在给定关节位置、速度和加速度的情况下，各关节需要施加的力矩。

1. 前向与反向递归

• 前向递归 (Forward Recursion)：从基座（Link 0）向末端（Link n）遍历。利用运动学公式，依次计算每个连杆的角速度、线速度、角加速度、线加速度和质心线加速度。这为计算惯性力做好了准备。
• 反向递归 (Backward Recursion)：从末端（Link n）向基座（Link 0）遍历。利用牛顿-欧拉方程（F=maF=maF=ma, N=Iω˙+ω×IωN=I\dot{\omega}+\omega \times I\omegaN=Iω˙+ω×Iω），结合前向递归计算出的加速度，依次计算每个连杆受到的惯性力、惯性力矩以及相邻连杆间的相互作用力和力矩。

2. 前向递归：运动学传播 (Forward Recursion)

从前一个连杆（i−1i-1i−1）向当前连杆（iii）依次计算角速度、线速度、角加速度和线加速度。

方向：从基座（i=0i=0i=0）向末端（i=ni=ni=n）

1. 角速度递推 ：

   ωi=Ri−1iωi−1+θ˙izi\omega_i = R_{i-1}^{i} \omega_{i-1} + \dot{\theta}_i z_iωi=Ri−1iωi−1+θ˙izi

2. 线速度递推 （计算连杆 iii 坐标系原点的线速度）：

   vi=Ri−1i(vi−1+ωi−1×pi)v_i = R_{i-1}^{i} (v_{i-1} + \omega_{i-1} \times p_i)vi=Ri−1i(vi−1+ωi−1×pi)

   (其中 pip_ipi 是从坐标系 {i−1}\{i-1\}{i−1} 原点到 {i}\{i\}{i} 原点的位置矢量)

3. 角加速度递推 ：

   ω˙i=Ri−1iω˙i−1+Ri−1iωi−1×(θ˙izi)+θ¨izi\dot{\omega}i = R{i-1}^{i} \dot{\omega}{i-1} + R{i-1}^{i} \omega_{i-1} \times (\dot{\theta}_i z_i) + \ddot{\theta}_i z_iω˙i=Ri−1iω˙i−1+Ri−1iωi−1×(θ˙izi)+θ¨izi

4. 线加速度递推 （计算连杆 iii 坐标系原点的线加速度）：

   v˙i=Ri−1iv˙i−1+ω˙i−1×pi+ωi−1×(ωi−1×pi)\dot{v}i = R{i-1}^{i} \\dot{v}_{i-1} + \\dot{\\omega}_{i-1} \\times p_i + \\omega_{i-1} \\times (\\omega_{i-1} \\times p_i)v˙i=Ri−1iv˙i−1+ω˙i−1×pi+ωi−1×(ωi−1×pi)

   (这一步是对线速度公式求导得出的。方括号内包含了上一连杆的平移加速度、切向加速度和向心加速度，最后通过旋转矩阵 RRR 转换到当前坐标系下。)

5. 质心线加速度递推 ：

   设 rcir_{ci}rci 为从关节 iii 原点到连杆 iii 质心的位置矢量。

   v¨ci=v˙i+ω˙i×rci+ωi×(ωi×rci)\ddot{v}_{ci} = \dot{v}i + \dot{\omega}i \times r{ci} + \omega_i \times (\omega_i \times r{ci})v¨ci=v˙i+ω˙i×rci+ωi×(ωi×rci)

   (这一步是在算出原点线加速度 v˙i\dot{v}_iv˙i 的基础上，叠加质心相对于原点的切向加速度和向心加速度。)

总结一下完整链条：

前向递归的运动学推导顺序是：角速度 →\rightarrow→ 线速度 →\rightarrow→ 角加速度 →\rightarrow→ 原点线加速度 →\rightarrow→ 质心线加速度。

3. 反向递归：动力学传播 (Backward Recursion)

从末端连杆向基座依次计算惯性力、力矩以及连杆间的相互作用力。

方向：从末端（i=ni=ni=n）向基座（i=1i=1i=1）

1. 计算连杆 iii 的惯性力与惯性力矩（在质心坐标系下）

根据牛顿第二定律和欧拉方程，首先计算作用在当前连杆质心上的惯性力和力矩：

• 惯性力 ：Fi=miv¨ciF_i = m_i \ddot{v}_{ci}Fi=miv¨ci （质量 × 质心线加速度）
• 惯性力矩 ：Ni=Iiω˙i+ωi×(Iiωi)N_i = I_i \dot{\omega}_i + \omega_i \times (I_i \omega_i)Ni=Iiω˙i+ωi×(Iiωi) （转动惯量 × 角加速度 + 陀螺力矩）
  (注：这里的 IiI_iIi 是连杆 iii 在其质心坐标系下的惯性张量)

2. 力与力矩的向后传递（在连杆坐标系原点处平衡）

这是最容易出错的一步。我们需要计算连杆 iii 的坐标系原点 处受到的合力 fif_ifi 和合力矩 nin_ini。

设 rciir_{ci}^ircii 为从连杆 iii 坐标系原点到其质心的矢量，ri+1ir_{i+1}^iri+1i 为从连杆 iii 坐标系原点到连杆 i+1i+1i+1 坐标系原点的矢量。

• 受力平衡公式 ：

  fi=Ri+1ifi+1+Fif_i = R_{i+1}^{i} f_{i+1} + F_ifi=Ri+1ifi+1+Fi

  (物理意义：当前关节原点受到的力 = 后一个关节传递过来的力（经旋转矩阵 RRR 转换） + 当前连杆自身的惯性力)

• 受力矩平衡公式 ：

  ni=Ri+1ini+1+Ni+rcii×Fi+ri+1i×(Ri+1ifi+1)n_i = R_{i+1}^{i} n_{i+1} + N_i + r_{ci}^{i} \times F_i + r_{i+1}^{i} \times (R_{i+1}^{i} f_{i+1})ni=Ri+1ini+1+Ni+rcii×Fi+ri+1i×(Ri+1ifi+1)

  (物理意义：当前关节原点受到的力矩 = 后一个关节传递过来的力矩 + 当前连杆自身的惯性力矩 + 当前关节惯性力 对当前关节原点的力矩 + 后一个关节传递过来的力对当前关节原点的力矩)

3. 计算关节驱动力矩 τi\tau_iτi

最终，驱动关节 iii 所需的力矩是合力矩 nin_ini 在关节旋转轴 ziz_izi 上的投影：

τi=niT⋅zi\tau_i = n_i^T \cdot z_iτi=niT⋅zi

4. 计算动力学方程的系数

标准的刚体动力学方程为：

τ=M(q)q¨+C(q,q˙)q˙+G(q) \tau = M(q)\ddot{q} + C(q, \dot{q})\dot{q} + G(q) τ=M(q)q¨+C(q,q˙)q˙+G(q)

注意，将C(q,q˙)q˙C(q, \dot{q})\dot{q}C(q,q˙)q˙ 简化为 C(q,q˙)C(q, \dot{q})C(q,q˙)，这里用下式表示：

τ=M(q)q¨+C(q,q˙)+G(q) \tau = M(q)\ddot{q} + C(q, \dot{q}) + G(q) τ=M(q)q¨+C(q,q˙)+G(q)

代码通过巧妙地控制输入变量（加速度 q¨\ddot{q}q¨ 和重力 ggg 变化, 输入变量 qqq和q˙\dot{q}q˙不变）， 分解出了方程的各项：

• 计算 C (科氏力/离心力)：令 q¨=0,g=0\ddot{q}=0, g=0q¨=0,g=0，此时 τ\tauτ 仅由速度项产生， τ=C(q,q˙)\tau = C(q, \dot{q})τ=C(q,q˙) 。
  C=RNEA(q,q˙,q¨=0,g=0) C = \text{RNEA}(q, \dot{q}, \ddot{q}=0, g=0) C=RNEA(q,q˙,q¨=0,g=0)
• 计算 G (重力)：令 q¨=0,g≠0\ddot{q}=0, g \neq 0q¨=0,g=0，算出 τCG=C(q,q˙)+G(q)\tau_{CG} = C(q, \dot{q}) + G(q)τCG=C(q,q˙)+G(q) ，然后 G=τCG−CG = \tau_{CG} - CG=τCG−C。
  G=RNEA(q,q˙,q¨=0,g)−C G = \text{RNEA}(q, \dot{q}, \ddot{q}=0, g) - C G=RNEA(q,q˙,q¨=0,g)−C
• 计算 M (质量矩阵)：令 q¨\ddot{q}q¨ 的第 iii 个元素为 1（其余为 0），此时算出的 τ\tauτ 减去 τCG\tau_{CG}τCG 即为质量矩阵 MMM 的第 iii 列。
  M:,i=RNEA(q,q˙,q¨i=1,g)−(C+G) M_{:,i} = \text{RNEA}(q, \dot{q}, \ddot{q}_i=1, g) - (C + G) M:,i=RNEA(q,q˙,q¨i=1,g)−(C+G)

5. 动力学方程用法

在机器人的动力学方程中MMM 、 CCC 、GGG其实充当了**"当前状态下的物理系数"**。

• 实际工程做实时控制时 ：直接用牛顿-欧拉算法 RNEA 算 τ\tauτ，不用计算 M,C,GM, C, GM,C,G。
• 什么时候才需要显式计算 M、C、G？ ：
  • 模型预测控制：某些高级控制算法需要知道系统的解析结构来进行优化求解。
  • 仿真器开发 ：如果你要写一个正向动力学仿真器（已知 τ\tauτ 求 q¨\ddot{q}q¨），你需要解方程 q¨=M−1(τ−Cq˙−G)\ddot{q} = M^{-1}(\tau - C\dot{q} - G)q¨=M−1(τ−Cq˙−G)，这时必须求 MMM 及其逆矩阵。
  • 参数辨识与调试：为了验证算法正确性，或者分析机器人的惯性特性，需要把各项分离开来对比。

6. 实际应用

1. 计算力矩控制（Computed Torque Control）

这是该公式最直接、最经典的应用，目的是将非线性的机器人系统"线性化"。利用完整的动力学模型 对机器人进行线性化和解耦，实现高精度的轨迹跟踪。

• 原理 ：如果我们想让机器人达到某个期望加速度 q¨\ddot{q}q¨，我们可以直接利用动力学模型反算出需要的力矩 τ\tauτ：
  τ=M(q)q¨+C(q,q˙)q˙+G(q)\tau = M(q)\ddot{q} + C(q, \dot{q})\dot{q} + G(q)τ=M(q)q¨+C(q,q˙)q˙+G(q)
• 作用 ：通过这种方式，控制器主动抵消了重力 GGG、科氏力 CCC 和惯性力 MMM 的影响。这就好比给机器人装了一个"超级大脑"，它能预判自己身体的重量和惯性，从而精准地执行动作。
• 结果 ：原本复杂的非线性机器人动力学方程被简化为一个简单的双重积分器系统（e¨+Kde˙+Kpe=0\ddot{e} + K_d \dot{e} + K_p e = 0e¨+Kde˙+Kpe=0），使得我们可以直接使用简单的 PID 控制器就能实现极高的跟踪精度。

2. 重力补偿与零力示教

在协作机器人中，在机械臂低速或静止时，直接施加 G(q)G(q)G(q) 力矩来抵消重力，防止机械臂下垂。

• 原理 ：当机器人处于静止或低速状态时（q˙≈0,q¨≈0\dot{q} \approx 0, \ddot{q} \approx 0q˙≈0,q¨≈0），公式简化为 τ≈G(q)\tau \approx G(q)τ≈G(q)。
• 作用：控制器实时计算出维持当前姿态所需的重力力矩，并驱动电机输出完全相等的反向力矩来抵消重力。
• 结果 ：操作者会感觉机械臂像悬浮在空中一样轻，可以随意拖动机械臂进行轨迹规划（即"拖拽示教"）。如果没有这个公式算出的 G(q)G(q)G(q)，机械臂在断电或松开刹车时会瞬间砸向地面。

3. 正向动力学仿真

当你需要在计算机中模拟机器人的运动（例如训练强化学习算法或验证新设计的机械臂）时，需要用到公式的逆运算形式：

q¨=M(q)−1(τ−C(q,q˙)q˙−G(q))\ddot{q} = M(q)^{-1} (\tau - C(q, \dot{q})\dot{q} - G(q))q¨=M(q)−1(τ−C(q,q˙)q˙−G(q))

• 作用 ：已知当前的关节位置 qqq、速度 q˙\dot{q}q˙ 和电机输出的力矩 τ\tauτ，通过求解这个方程，可以得到下一时刻的加速度 q¨\ddot{q}q¨。
• 结果：结合数值积分方法（如龙格-库塔法），就可以推算出机器人未来的运动轨迹。这是所有物理引擎（如 MuJoCo, PyBullet）的核心算法基础。

4. 高级阻抗与导纳控制

当机器人与环境发生接触（如打磨、装配）时，不仅要控制位置，还要控制力。

• 原理 ：阻抗控制的目标是让机器人表现出类似弹簧-阻尼系统的特性：Fext=Md(x¨d−x¨)+Bd(x˙d−x˙)+Kd(xd−x)F_{ext} = M_d(\ddot{x}_d - \ddot{x}) + B_d(\dot{x}_d - \dot{x}) + K_d(x_d - x)Fext=Md(x¨d−x¨)+Bd(x˙d−x˙)+Kd(xd−x)。
• 作用 ：为了实现这种虚拟的柔顺性，控制器必须非常清楚机器人自身的真实惯性 M(q)M(q)M(q) 和非线性项 C,GC, GC,G。只有精确地补偿掉机器人自身的动力学特性，才能准确地呈现出我们想要设定的"虚拟质量"和"虚拟阻尼"。
• 结果：机器人在接触硬物时不会刚性碰撞，而是像按在弹簧上一样柔顺地退让，保证加工质量和安全性。

7 armature

armature 是机器人动力学模型（特别是在 MuJoCo 和 URDF/SDF 格式中）的一个重要参数。

armature 代表了电机转子和传动机构（如减速箱）的等效惯量。

<joint axis="0 1 0" range="-6.28319 6.28319" armature="0.0"/>

物理含义

当转动一个电机的轴时，除了要克服外部负载的惯量外，还需要克服电机内部转子本身以及减速箱齿轮转动所需的力。armature 就是用来模拟这部分**"内部转动惯量"**的。

• 单位 ：千克·平方米 (kg⋅m2kg \cdot m^2kg⋅m2)。
• 位置：它被加在**关节（Joint）**上，而不是连杆（Link）上。

在动力学方程中的作用

在机器人动力学方程 τ=M(q)q¨+C(q,q˙)+G(q)\tau = M(q)\ddot{q} + C(q, \dot{q}) + G(q)τ=M(q)q¨+C(q,q˙)+G(q) 中：

• 质量矩阵 M(q)M(q)M(q)：主要由连杆的质量和几何分布决定。
• Armature 的作用：armature 会被直接加到质量矩阵 M(q)M(q)M(q) 的对角线上。

公式 ：

Mtotal(q)=Mlinks(q)+IarmatureM_{total}(q) = M_{links}(q) + I_{armature}Mtotal(q)=Mlinks(q)+Iarmature

其中 IarmatureI_{armature}Iarmature 是一个对角矩阵，对角线上的值就是你在 XML 中定义的 armature 值。

import numpy as np
import utility as ram
from robot_data import robot
from forward_kinematics import forward_kinematics



 
def RecursiveNewtonEulerAlgorithm(q, qdot, qddot, g):
    """
     RNEA 递归牛顿-欧拉算法的核心实现，包含前向递归（计算运动学量）和反向递归（计算动力学量）。
    """
    # 执行正运动学，获取当前姿态下所有连杆的全局位姿信息
    _, robot = forward_kinematics(q)

    # 将连杆的惯性张量从"惯性主轴坐标系"转换到"连杆本体坐标系"
    # 公式：I_global = R * diag(I_local) * R.T
    for i in range(1, len(robot.body) + 1):
        iquat = robot.body[i].iquat
        R_inertia = ram.quat2rotation(iquat)  # 描述了惯性主轴坐标系相对于连杆本体坐标系的旋转姿态
        robot.body[i].I_body = R_inertia @ np.diag(robot.body[i].inertia) @ R_inertia.T

    # ==================== 前向递归 (Forward Recursion) ====================
    # 目标：从基座（Link 0）向末端依次计算各连杆的角速度(w)、线速度(v)、角加速度(wdot)和线加速度(vdot)
    w00 = np.array([0,0,0])  # 基座角速度
    v00 = np.array([0,0,0])  # 基座线速度
    wdot00 = np.array([0,0,0])  # 基座角加速度
    # vdot00 = np.array([0,0,0])  # 基座线加速度 未考虑重力
    vdot00 = np.array([0,0,-g])  # 基座线加速度 考虑重力

    for i in range(1, len(robot.body) + 1):
        joint_axis = robot.body[i].joint_axis  # 关节旋转轴
        ipos = robot.body[i].ipos  # 连杆质心在局部坐标系下的位置
        o = robot.body[i].o_local  # 连杆坐标系原点相对于上一连杆的偏移
        R = robot.body[i].R_local.T  # 上一连杆到当前连杆的旋转矩阵

        # 计算当前连杆的角速度 (w_i = R * w_{i-1} + qdot * z_i)
        robot.body[i].w = R @ w00 + qdot[i - 1] * joint_axis
        # 计算当前连杆原点的线速度
        robot.body[i].v = R @ (v00 + ram.vec2skew(w00) @ o)
        # 计算当前连杆的角加速度 (包含科氏加速度项)
        robot.body[i].wdot = R @ wdot00 + ram.vec2skew((R @ w00)) @ (qdot[i - 1] * joint_axis) + qddot[i - 1] * joint_axis
        # 计算当前连杆原点的线加速度 (包含向心加速度和切向加速度)
        robot.body[i].vdot = R @ (vdot00 + ram.vec2skew(wdot00) @ o + ram.vec2skew(w00) @ (ram.vec2skew(w00) @ o))
        # 计算当前连杆质心的线加速度 (vdotC)
        robot.body[i].vdotC = robot.body[i].vdot + ram.vec2skew(robot.body[i].wdot) @ ipos + ram.vec2skew(robot.body[i].w) @ (ram.vec2skew(robot.body[i].w) @ ipos)

        # 更新状态，传递给下一个连杆
        w00 = robot.body[i].w
        v00 = robot.body[i].v
        wdot00 = robot.body[i].wdot
        vdot00 = robot.body[i].vdot
        vdotC = robot.body[i].vdotC

    # ==================== 反向递归 (Backward Recursion) ====================
    # 目标：从末端向基座依次计算各连杆之间的相互作用力(f)和力矩(n)
    fn = np.array([0,0,0])  # 末端执行器受到的外部力（此处设为0）
    nn = np.array([0,0,0])  # 末端执行器受到的外部力矩（此处设为0）
    Rn = np.eye(3)
    o = robot.params.end_eff_pos_local

    for i in range(len(robot.body), 0, -1):
        m = robot.body[i].mass
        I = robot.body[i].I_body
        w = robot.body[i].w
        ipos = robot.body[i].ipos

        # 牛顿方程：计算惯性力 F = m * a_c
        F = m * robot.body[i].vdotC
        # 欧拉方程：计算惯性力矩 N = I * wdot + w x (I * w)
        N = I @ robot.body[i].wdot + ram.vec2skew(w) @ (I @ robot.body[i].w)
        
        # 力的平衡：计算上一连杆对当前连杆的作用力
        robot.body[i].f = Rn @ fn + F
        # 力矩的平衡：计算上一连杆对当前连杆的作用力矩
        robot.body[i].n = N + Rn @ nn + ram.vec2skew(ipos) @ F + ram.vec2skew(o) @ (Rn @ fn)

        # 更新状态，传递给上一个连杆
        fn = robot.body[i].f
        nn = robot.body[i].n
        Rn = robot.body[i].R_local
        o = robot.body[i].o_local

    # ==================== 提取关节力矩 ====================
    tau = np.zeros(len(robot.body))
    count = 0
    for i in range(1, len(robot.body) + 1):
        joint_axis = robot.body[i].joint_axis
        # 将作用力矩投影到关节旋转轴上，得到驱动该关节所需的标量力矩
        j = np.dot(joint_axis, np.array([1,2,3]))
        tau[count] = robot.body[i].n[j - 1]
        count += 1

    return tau


def ComputeMCG(q, qdot, flag, armature=0.0):
    """
    利用 RNEA 算法分解计算机械臂动力学方程中的各项：M(q)qdd + C(q,qdot) + G(q) = tau

    armature 代表了电机转子和传动机构（如减速箱）的等效惯量,UR5e的XML中有。
    """
    nv = len(qdot)
    g = robot.params.gravity[2]

    # 1. 计算科氏力和离心力项 C(q, qdot)
    # 令 qddot = 0, g = 0，此时计算出的力矩纯粹由速度引起的科氏力和离心力产生
    qddot = np.zeros(nv)
    tau_C = RecursiveNewtonEulerAlgorithm(q, qdot, qddot,  g=0)
    C = np.zeros((nv, 1))
    C[:, 0] = tau_C

    # 2. 计算科氏力 + 重力项 (C + G)
    # 令 qddot = 0, g != 0，此时计算出的力矩包含速度项和重力项
    qddot = np.zeros(nv)
    tau_CG = RecursiveNewtonEulerAlgorithm(q, qdot, qddot,  g)
    CG = np.zeros((nv, 1))
    CG[:, 0] = tau_CG

    # 3. 计算重力项 G(q)
    # 通过 (C+G) - C 得到纯粹的重力补偿力矩
    G = np.zeros((nv, 1))
    G[:, 0] = CG[:, 0] - C[:, 0]

    # 4. 计算质量矩阵 M(q)
    M = np.zeros((nv, nv))

    if (flag == 1):
        # 通过多次调用 RNEA 来提取质量矩阵的每一列
        for i in range(nv):
            qddot[i] = 1  # 令第 i 个关节的加速度为 1，其余为 0
            # 此时计算出的力矩 tau = M * [0...1...0] + CG = M的第i列 + CG
            tau = RecursiveNewtonEulerAlgorithm(q, qdot, qddot, g)
            M[:, i] = tau - tau_CG  # 减去 CG 项，得到质量矩阵的第 i 列
            qddot[i] = 0  # 重置加速度


        # ==========================================
        # 【核心修改】：将 armature 叠加到质量矩阵对角线上
        # ==========================================
        # 如果 armature 是标量，则所有关节使用相同的等效惯量
        if np.isscalar(armature):
            M += np.eye(nv) * armature
        # 如果 armature 是数组，则按对角线元素分别叠加
        elif isinstance(armature, np.ndarray) and len(armature) == nv:
            M += np.diag(armature)
        else:
            raise ValueError("armature 必须是标量或长度等于关节数 nv 的 numpy 数组")

    return C, G, CG, M

import numpy as np
from jac_torques import jac_torques  # 导入计算雅可比矩阵和获取机器人参数的函数
import utility as ram
from scipy.linalg import block_diag  # 用于构建分块对角矩阵

def print_matrix(Mat, decimals=6, name="Mat"):
    if not isinstance(Mat, np.ndarray):
        raise ValueError("Input must be a numpy.ndarray.")
    if Mat.ndim != 2:
        raise ValueError("Input must be a 2D numpy.ndarray (matrix).")

    format_string = f"{{:.{decimals}f}}"
    print(f"{name} = [ ...")
    for row in Mat:
        formatted_row = " ".join(format_string.format(elem) for elem in row)
        print(f"  {formatted_row};")
    print("];\n")


def TMT(q, armature=0.0):
    """
    使用 TMT (Transformed Mass Tensor) 方法计算机器人的质量矩阵 M(q)。
    核心公式：M = J^T * M_body * J

    armature 代表了电机转子和传动机构（如减速箱）的等效惯量,UR5e的XML中有。

    """

    # 1. 获取当前关节角度 q 下的机器人模型参数以及各连杆质心的雅可比矩阵
    # J_g1 到 J_g6 分别代表第1到第6个连杆质心在空间中的线速度和角速度相对于关节速度的映射关系
    robot, J_g1, J_g2, J_g3, J_g4, J_g5, J_g6 = jac_torques(q)

    # 初始化一个空的质量矩阵容器
    M_ = np.zeros((0, 0))  

    # 2. 循环构建所有连杆的"空间质量矩阵" (Spatial Mass Matrix)
    j = 0
    for i in range(1, len(robot.body) + 1):
        # 获取当前连杆的姿态四元数、局部姿态以及关节轴
        quat = robot.body[i].quat
        iquat = robot.body[i].iquat
        joint_axis = robot.body[i].joint_axis

        # --- 构建 3x3 质量块 ---
        m = robot.body[i].mass
        mass = m * np.eye(3)  # 线运动部分的质量矩阵（对角阵）

        # --- 构建 3x3 惯性张量块 ---
        # 第一步：将连杆主轴惯性矩转换到全局惯性坐标系下
        # R_inertia 表示连杆局部惯性系相对于全局惯性系的旋转
        R_inertia = ram.quat2rotation(iquat)
        # 计算在全局惯性系下的惯性张量 (注意：这里保留了原代码的 np.matrix，建议后续统一改为 @ 和 np.array)
        robot.body[i].I_body = R_inertia @ np.matrix(np.diag(robot.body[i].inertia)) @ R_inertia.T

        # 第二步：将惯性张量从全局惯性系转换到当前连杆的局部坐标系 (i-1 到 i 的局部系)
        R_local = robot.body[i].R_local
        # 计算在局部坐标系下的惯性张量
        inertia = R_local @ robot.body[i].I_body @ R_local.T  

        # --- 合并为 6x6 的空间质量矩阵 ---
        # 将 3x3 的质量块和 3x3 的惯性块组合成一个 6x6 的分块对角矩阵
        # 左上角是质量（影响线动量），右下角是转动惯量（影响角动量）
        M_local = block_diag(mass, inertia)

        # --- 将所有连杆的 6x6 矩阵拼接到总矩阵 M_ 中 ---
        if M_.size == 0:  # 如果是第一个连杆，直接初始化
            M_ = M_local
        else:  # 后续连杆通过分块对角的方式向右下角拼接
            M_ = block_diag(M_, M_local)

    # 3. 整合所有连杆的雅可比矩阵
    # 将各个连杆质心的雅可比矩阵在垂直方向堆叠，形成一个巨大的整体雅可比矩阵 J
    # 这个 J 描述了所有连杆的质心运动与整体关节速度之间的映射关系
    J = np.vstack([J_g1, J_g2, J_g3, J_g4, J_g5, J_g6])

    # 4. 计算最终的关节空间质量矩阵 (TMT 核心公式)
    # M_TMT = J^T * M_ * J
    # 物理意义：通过雅可比矩阵的转置，将所有连杆在笛卡尔空间下的质量与惯性"投影"或"映射"到关节空间中
    M_TMT = J.T @ M_ @ J  # 使用 @ 进行矩阵乘法

    # 确保最终输出的是标准的 numpy ndarray 格式
    M_TMT = np.asarray(M_TMT)



    # ==========================================
    # 【核心修改】：将 armature 叠加到质量矩阵对角线上
    # ==========================================
    nv = len(q)
    # 如果 armature 是标量，则所有关节使用相同的等效惯量
    if np.isscalar(armature):
        M_TMT += np.eye(nv) * armature
    # 如果 armature 是数组，则按对角线元素分别叠加
    elif isinstance(armature, np.ndarray) and len(armature) == nv:
        M_TMT += np.diag(armature)
    else:
        raise ValueError("armature 必须是标量或长度等于关节数 nv 的 numpy 数组")


    return M_TMT

结论：TMT计算质量矩阵速度稍快一些，但精度也损失一些。

Elapsed time (ComputeMCG): 0.0078 seconds
Elapsed time (TMT): 0.0020 seconds
Gravity and Coriolis (重力与科氏力对比):
CG_mj = [ ...
  0.0000;
  -15.8571;
  -15.8572;
  -1.3812;
  -0.0000;
  -0.0000;
];

CG_py = [ ...
  -0.0699;
  -15.9788;
  -15.7898;
  -1.3827;
  -0.0027;
  -0.0006;
];

CG_py-CG_mj  = [ ...
  -0.069886;
  -0.121700;
  0.067438;
  -0.001558;
  -0.002719;
  -0.000562;
];

Mass matrix (质量矩阵对比):
M_mj = [ ...
  0.980825 -0.400542 0.001953 0.001953 0.007172 -0.000132;
  -0.400542 2.138931 0.646841 0.069417 0.000000 -0.000000;
  0.001953 0.646841 0.753038 0.075612 0.000000 -0.000000;
  0.001953 0.069417 0.075612 0.120423 0.000000 -0.000000;
  0.007172 0.000000 0.000000 0.000000 0.103418 -0.000000;
  -0.000132 -0.000000 -0.000000 -0.000000 -0.000000 0.100132;
];

M_py = [ ...
  0.980825 -0.400542 0.001953 0.001953 0.007172 -0.000132;
  -0.400542 2.138931 0.646841 0.069417 0.000000 -0.000000;
  0.001953 0.646841 0.753038 0.075612 0.000000 -0.000000;
  0.001953 0.069417 0.075612 0.120423 0.000000 -0.000000;
  0.007172 0.000000 0.000000 0.000000 0.103418 0.000000;
  -0.000132 -0.000000 -0.000000 -0.000000 0.000000 0.100132;
];

M_py_TMT = [ ...
  0.980061 -0.400542 0.001953 0.001953 0.007172 -0.000099;
  -0.400542 2.111846 0.619756 0.069417 0.000000 -0.000000;
  0.001953 0.619756 0.725954 0.075612 0.000000 -0.000000;
  0.001953 0.069417 0.075612 0.120423 0.000000 -0.000000;
  0.007172 0.000000 0.000000 0.000000 0.103817 0.000000;
  -0.000099 -0.000000 -0.000000 -0.000000 0.000000 0.100099;
];

M_mj-M_py_TMT = [ ...
  0.000764 0.000001 0.000000 0.000000 0.000000 -0.000033;
  0.000001 0.027085 0.027085 0.000000 -0.000000 -0.000000;
  0.000000 0.027085 0.027085 0.000000 -0.000000 -0.000000;
  0.000000 0.000000 0.000000 0.000000 -0.000000 -0.000000;
  0.000000 -0.000000 -0.000000 -0.000000 -0.000399 -0.000000;
  -0.000033 -0.000000 -0.000000 -0.000000 -0.000000 0.000033;
];

M_py-M_mj  = [ ...
  -0.000000 0.000000 -0.000000 -0.000000 -0.000000 -0.000000;
  0.000000 0.000000 0.000000 0.000000 0.000000 0.000000;
  0.000000 0.000000 0.000000 0.000000 0.000000 0.000000;
  0.000000 0.000000 0.000000 -0.000000 0.000000 -0.000000;
  -0.000000 0.000000 0.000000 -0.000000 0.000000 0.000000;
  0.000000 0.000000 0.000000 0.000000 0.000000 0.000000;
];