项目二：MuJoCo 下 SO-101 机械臂上面3个关节部分的集成与控制

[BoBoDai]

项目二：MuJoCo 下 SO-101 机械臂上面3个关节的集成与控制

📖 用户故事

作为 机器人硬件工程师与仿真测试人员，
我希望 在 MuJoCo 中加载 SO-101 机械臂的上半部分模型，并实现基本的位置控制和运动学解算，
以便 在仿真中验证控制算法后，快速迁移到真实的 SO-101 硬件上运行。

✅ 验收标准

1. SO-101 上半部分模型加载

• 将 SO-101 上半部分的 MJCF/URDF 模型文件放入项目中
• 在 MuJoCo 中正确加载模型，包含以下关节（以上半部分实际结构为准）：
  • 肘部俯仰 (elbow pitch)
  • 腕部俯仰 (wrist pitch)
  • 腕部横滚 (wrist roll)
• 各关节限位、力矩上限、连杆质量等物理参数与真实硬件一致
• 模型可在 MuJoCo viewer 中正常渲染和交互

2. 位置控制接口

• 提供关节空间位置控制：传入各关节目标角度，调用 mj_step 驱动
• 提供笛卡尔空间位置控制：传入末端目标位姿 (x, y, z, roll, pitch, yaw)，内部通过 IK 解算为关节角度后执行

3. 正向/逆向运动学

• FK：给定关节角度，返回末端相对于基座的 3-DOF 位姿 (x, y, z, roll, pitch, yaw)
• IK：给定末端目标位姿，返回关节角度解（使用 Jacobian pseudo-inverse 数值方法）
• IK 求解失败时返回明确错误状态（如 success=False），不静默失败

4. 末端执行器接口

• 预留末端执行器安装位（end-effector site），方便后续挂载夹爪或工具