机械臂末端执行器Wrench动力学计算与实现

1. 项目概述：机械臂末端执行器Wrench的动力学计算

在机器人控制领域，末端执行器的力/力矩（Wrench）获取是许多高级控制算法的基础。以Franka Emika Panda机械臂为例，当我们需要实现笛卡尔空间阻抗控制、力反馈或协作拖动等功能时，准确计算末端6维力/力矩向量至关重要。

Wrench由3维力向量和3维力矩向量组成，表示为[Fx, Fy, Fz, Tx, Ty, Tz]。在Mujoco仿真环境中，我们无法直接读取这个量，而是需要通过关节空间力矩结合雅可比矩阵转换得到。这个转换过程涉及几个关键环节：

1. 获取关节空间总力矩（包括驱动扭矩、被动力和外力）
2. 计算末端执行器相对于基坐标系的雅可比矩阵
3. 通过阻尼伪逆方法将关节力矩映射到笛卡尔空间

注意：LOCAL_WORLD_ALIGNED坐标系的选择特别重要，它保证了力矩分量是在与基座标系对齐的局部坐标系中表示，这在机械臂控制中是最常用的表示方式。

2. 核心原理与技术实现

2.1 关节空间力矩的组成

在Mujoco中，关节空间总力矩由三部分组成：

python复制tau = self.data.qfrc_actuator + self.data.qfrc_passive + self.data.qfrc_applied

• qfrc_actuator：执行器输出的主动力矩，通常来自PID控制器或直接力矩指令
• qfrc_passive：被动力学效应，包括阻尼力、弹簧力和关节摩擦等
• qfrc_applied：外部施加的力矩，如碰撞或人为施加的力，默认情况下为0

2.2 雅可比矩阵计算与坐标系选择

使用Pinocchio库计算雅可比矩阵时，坐标系的选择直接影响最终Wrench的物理意义：

python复制J = pin.computeFrameJacobian(
    model, data, q, end_effector_id, pin.LOCAL_WORLD_ALIGNED
)

LOCAL_WORLD_ALIGNED坐标系的特点是：

• 原点位于末端执行器坐标系原点
• 坐标轴方向与世界坐标系保持一致
• 这种表示方式最符合人类直觉，也便于后续控制算法设计

2.3 从