Drake轨迹优化中如何为笛卡尔位置添加线性约束？

我目前正在尝试线性化Drake的轨迹优化问题，希望得到高层面的指导：

• 背景：已基于KinematicTrajectoryOptimization实现代码，通过SNOPT求解出乒乓球挥拍运动。目标是将该非线性问题线性化为QP，以便使用Linear MPC结合相机的乒乓球估计结果进行实时更新。
• 当前非线性约束设置：通过AddPathPositionConstraint添加了PositionConstraint和OrientationConstraint，通过AddVelocityConstraintAtNormalizedTime添加了SpatialVelocityConstraint，这些约束主要作用于预测的击球接触时刻，同时还包含一些次要的起止约束。
• 遇到的问题：正尝试将程序迁移至DirectCollocation或普通MathematicalProgram框架，但难以找到约束机器人末端执行器笛卡尔位置的线性替代方案。

请问是否可以添加末端执行器位置的线性笛卡尔约束？我自己想到了两种可能的方案：

1. 预先求解逆运动学（IK），转而约束关节位置，但这种方式会限制求解器只能使用我指定的姿态（若存在多种可达的末端执行器姿态）；
2. 添加连杆坐标系位置的松弛变量，引入约束linkposition_k = linkposition_(k-1) + jacobian_(k-1) @ joint_velocities_(k-1)（其中雅可比矩阵基于每个配点的初始猜测），再对这些松弛变量施加所需的位置约束。

内容的提问来源于stack exchange，提问作者louis