闭环位姿控制（Closed-loop Pose Control）是根据当前位姿与目标位姿之间的误差，持续更新机器人命令，使末端或某个 frame 逐步逼近目标的控制方式。
它强调“观测当前状态再修正”，不同于一次性生成目标关节值后直接执行的开环方式。
核心问题
闭环位姿控制要解决：
- 当前末端 pose 和目标 pose 相差多少？
- 位置误差和姿态误差如何组合或加权？
- 误差应该通过 IK、DLS、逆雅可比还是底层控制器转换成关节命令？
- 接近目标、接近限位或接近碰撞时如何降速和停止？
核心机制
典型循环是：
- 用 FK 或状态估计得到当前末端位姿。
- 计算目标位姿与当前位姿的误差 。
- 用 雅可比矩阵、DLS 或 IK plugin 求关节增量/速度。
- 执行关节限位、奇异点检查、碰撞检查和运动平滑。
- 发布命令，下一周期重新读取状态并继续修正。
MoveIt Servo 的 Pose command 可以被理解为一种面向实时 servo 的闭环位姿跟随入口。
工程用途
- 末端持续跟随外部系统给出的目标 Pose。
- 把视觉、力觉或人机输入产生的目标转换为机器人运动。
- 在目标缓慢变化或存在模型误差时不断修正残差。
- 为遥操作和视觉伺服提供统一的 pose-error 控制表达。
边界与常见坑
- 位姿误差的表达坐标系必须和 Jacobian 或控制器约定一致。
- 姿态误差不能简单用欧拉角差硬减，容易遇到奇异和角度跳变。
- 闭环控制会追踪误差，但不自动保证全局可达、无碰撞或最优路径。
- 反馈延迟过大时，闭环可能抖动、超调或感觉不跟手。