Verified Task-Space Motion Planning Under Joint-Space Constraints

2026-05-21

AI Summary

This work addresses the limitations of traditional task-space planners such as Bug2, which often neglect joint limits and fail to reach targets when the Jacobian becomes ill-conditioned, leading to joint limit violations or unreachable goals. To overcome these issues, the authors propose an adaptive step-size planning method that leverages a second-order inverse kinematics approximation combined with an S-procedure to construct, at each step, a Cartesian hyper-rectangle reachable under joint constraints. This construction is formulated as a semidefinite program and solved efficiently via a novel fast bisection algorithm exploiting quadratic structure. For the first time, verifiable reachability analysis is integrated into real-time task-space planning. Evaluated on 94 adversarial scenarios, the approach achieves zero joint limit violations and a 100% target-reaching success rate, substantially outperforming Bug2.

Abstract

Reactive task-space planners such as Bug2 operate with fixed Cartesian step sizes and are unaware of the manipulator's joint-angle limits. When the Jacobian is poorly conditioned, even small Cartesian steps can demand joint changes that exceed admissible bounds; clipping the joints to their limits causes tracking drift and can prevent goal reaching entirely. We address this by computing, at each planning step, the largest Cartesian hyperrectangle that is \emph{certifiably reachable} under joint displacement bounds. Using a second-order polynomial approximation of the inverse kinematics and the S-procedure, we formulate a small semidefinite program whose solution yields the certified half-width~$λ^\star$. An equivalent bisection procedure exploiting the quadratic structure solves the certification in sub-millisecond time. Integrating this certificate with Bug2 yields a planner whose step size adapts to local kinematic conditioning. In a statistical evaluation over 94 adversarial scenarios spanning six joint-limit settings, the SOS-verified planner achieves \emph{zero} joint-limit violations with a 100\% goal-reaching rate, whereas a standard Bug2 planner violates joint limits in 6--11\% of steps and fails to reach the goal in up to 18\% of scenarios.

Problem

• task-space motion planning
• joint-space constraints
• kinematic conditioning
• joint limits
• goal reaching

Innovation

• certifiable motion planning
• joint-space constraints
• task-space planning
• semidefinite programming
• inverse kinematics approximation