Planning along Differentiable Charts of Constraint Manifolds with General-Purpose IK Solvers

Kinematic equality constraints — carrying a box with two hands, opening a door, keeping feet planted, or any policy that reasons about the robot as a floating hand — confine a robot to a measure-zero constraint manifold. Rather than carry that constraint through the optimization, we parameterize the manifold with analytic inverse kinematics: optimize over end-effector pose and self-motion, and let IK map back to joint angles. The constraint then holds by construction, leaving a positive-measure feasible set bounded only by inequalities.

This needs derivatives of the IK map, and almost every manipulator gets its IK from a meta-solver like IKFast, whose machine-generated code is hopeless to differentiate through. We never touch it. Analytic IK is the local inverse of the augmented forward kinematics \(\mathrm{FK}_A\), which is trivially differentiable, so the inverse function theorem recovers the chart's derivative from a linear solve:

\[ D\mathrm{FK}_A(q)\, \frac{\partial q}{\partial y} = \left( \frac{\partial X}{\partial y},\ \frac{\partial \psi}{\partial y} \right)^{\!\top} \]

This is the same linear algebra that differential IK (DiffIK) uses, but put to a different purpose. DiffIK resolves redundancy locally, one step at a time, and can get stuck tracking a difficult end-effector path. Because the augmented Jacobian pins the self-motion down explicitly, our charts stay globally consistent, so the optimizer can plan motions that avoid joint limits, workspace boundaries, and kinematic singularities outright.

Analytic IK is also undefined outside the reachable workspace, where optimizers routinely wander. We extend it as a least-squares projection whose exact derivative collapses to the inverse function theorem result whenever the target is reachable, and replace the usual reachability constraint — active at every feasible point — with one that only activates near the workspace boundary:

\[ -\log\det\!\left( J J^\top + \epsilon I \right) \le \tau \]

A UR5e must grasp a mug among shelf obstacles, with IK derived automatically by EAIK. We generate 100 problem instances by rejection sampling collision-free goal states, and solve each from 10 randomly sampled initializations shared across all three formulations. Reformulating the problem in minimal coordinates raises SNOPT's single-initialization success rate by nearly 17 points over the configuration-space baseline; with three-initialization multistart, the parameterized formulations reach 90–93% against the baseline's 86%. The IFT solutions cost slightly more, likely because the change of variables distorts the joint-centering objective.