Abstract:

Safe robotic control in contact-rich environments requires navigating non-convex optimization landscapes while enforcing safety constraints and achieving task-level precision. Annealing-based sampling MPC methods enable fast exploration of complex solution spaces but lack principled constraint handling and struggle to reach the tight tolerances required by precise manipulation tasks. We propose Constrained Annealing-based Sampling MPC (CAS-MPC), which integrates inequality constraints via a primal–dual weight reshaping scheme that preserves sample diversity, and a hybrid refinement strategy that switches to Sequential Linear–Quadratic MPC for millimeter-level precision once near the goal. On a Unitree Go2 quadruped and a Fetch mobile manipulator, CAS-MPC maintains a 1.0 alive-trajectory ratio during obstacle avoidance, while the hybrid controller reduces average convergence time by 47% over SLQ-MPC and average position error by 71% over CAS-MPC alone across five SE(3) reaching tasks.