双臂协同与相对 / 全身约束控制
把『两臂共持的闭链相对约束』与『全身层面的限位、自碰撞约束』统一进同一个 QP，在双七轴仿真平台上闭环验证。
两臂共持一个物体时，系统构成闭链：两个末端的相对位姿必须严格保持， 否则内力会把物体挤坏或把误差转嫁给夹持。与此同时，全身层面还有关节限位、 速度上界与臂间自碰撞这些不等式约束。单臂控制里这两类需求可以分开处理， 双臂场景下它们耦合在同一组关节变量上，拆开处理会互相打架。
这个模块把问题统一成一个 QP：绝对任务（物体位姿跟踪）进目标函数， 相对位姿进等式约束，限位 / 速度 / 自碰撞距离进不等式约束， 在 MuJoCo 双七轴平台上闭环验证共持搬运与对称装配两类场景。 博客里的双臂 QP 协同控制一文推导了这套 formulation 的数学细节， 本模块是它的可运行实现与系统评测。
协同任务空间建模（绝对坐标 + 相对坐标） ↓QP 构造： 目标 —— 绝对任务跟踪 + 正则项 等式 —— 末端相对位姿约束（闭链） 不等式 —— 限位 / 速度 / 自碰撞距离 ↓逐控制周期求解 → 关节指令 → MuJoCo 闭环 ↓全量日志 → 相对误差 / 内力估计 / 约束裕度 / 求解时间- 协同任务空间而非主从。用绝对 + 相对坐标参数化双臂任务， 两臂对称，不预设主从——相对约束的精度要求与绝对跟踪解耦，可以分别设定。
- 约束分层进 QP。相对位姿是等式约束（闭链不许破），全身安全是不等式约束， 任务跟踪只进目标函数；三者的『谁让步』顺序在 formulation 里是显式的。
- 不可行时的行为是设计项。不等式约束把可行域压空时，QP 无解不是异常退出的理由： 相对约束按设定松弛并记录松弛量，保证控制器始终有输出。松弛何时发生、多大， 本身就是评测指标。
- 内力作为一等公民观测。共持物体上的内力（由两侧施力的对消分量估计） 全程记录——相对误差小不代表内力健康，两者必须分开看。
动力学模型复用动力学建模与参数辨识项目的产出，本模块不重做辨识。
首批实验尚未跑完，下表先固定指标定义与验收口径，数字与图表待实验报告产出后原位填入：
| 指标 | 口径 | 当前状态 |
|---|---|---|
| 相对位姿误差 | 共持运动全程两末端相对位姿误差的上界与分布 | 待首批报告 |
| 内力水平 | 共持物体内力估计的均值与峰值，对照无协调基线 | 待首批报告 |
| 约束违例与松弛 | 不等式约束违例计数（目标为零）；相对约束松弛的触发次数与幅值 | 待首批报告 |
| 求解时间 | 单周期 QP 求解时间分布与超时率（对照目标控制频率） | 待首批报告 |
图表位预留：共持搬运轨迹的相对误差 / 内力时间线、约束裕度曲线、 求解时间直方图，由评测脚本从日志成套生成后替换本节占位。
评测场景里专门安排了把 QP 逼到边界的用例，失败案例与正常结果同等留档：
- 奇异与病态：某臂接近奇异时相对约束的雅可比病态，考察松弛策略是否按预期接管；
- 可行域压空：自碰撞约束与相对约束冲突的构型，记录控制器的实际降级路径；
- 内力漂移：长时程共持下内力是否缓慢累积——相对误差指标抓不住这类失败。
同时声明边界：MuJoCo 的夹持接触是软接触模型，内力估计的绝对量值 对接触参数敏感，报告里只对照相对变化与趋势；物体与夹爪之间不建模打滑的真实摩擦谱。
QP formulation 与约束定义不依赖仿真器；接实机时替换状态源与指令下发层， 自碰撞距离改由实机标定后的几何模型提供。松弛行为的评测在仿真里先把边界画清楚， 是双臂系统上真机前必须过的一道关。
仓库正在从 Origin 草稿整理发布，公开后此处与上方按钮会替换为具体地址。 发布内容包含任务与约束定义、QP 控制器、全部评测场景与日志，报告可一键重建。