Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn

September 23, 2026

Presentation for the MIT Robotic Exploration Lab group meeting

Robot Locomotion Group

Which Kinematically-Constrained Systems?

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

Toyota Research Institute

Boston Dynamics

The Configuration-Space Picture

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Learning the Metric of Task Constraint Manifolds for Constrained Motion Planning, Zha et al.

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

The Predominant, "Extrinsic" Approach

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Sampling-Based Methods for Motion Planning with Constraints, Kingston et al.

Sampling-Based Planners

Trajectory Optimizers

Trajectory Optimization on Manifolds: A Theoretically-Guaranteed Embedded Sequential Convex Programming Approach, Bonalli et al. (RSS 2019)

Direct Collocation Methods for Trajectory Optimization in Constrained Robotic Systems, Bordalba et al.

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

An "Intrinsic" Alternative?

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

"Would be great if we could do it, but difficult or impossible in practice"

Paraphrased from the textbooks of LaValle, Latombe, Siciliano and Khatib, Lynch and Park.

Trajectory Optimization On Manifolds with
Applications to \(SO(3)\) and \(\mathbb{R}^3 \times S^2\), Watterson et al.

Riemannian Direct Trajectory Optimization of Rigid Bodies on Matrix Lie Groups, Teng et al.

A Constrained Motion Planning Method Exploiting Learned Latent Space for High-Dimensional State and Constraint Spaces, Park et al.

Planning and IK on Constraint Manifolds
Change of Variables with Analytic IK
1. Resolving the "many-to-one" problem
2. Reachability: the constraint that replaces the equality
Gradients through Any IK Solver
1. The inverse function theorem
2. Domain extensions and boundary reachability
Results
The Benchmark Suite: Where the Solvers Struggle
Why a Trust-Region Solver?

Outline for the Remainder of the Talk

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

Collision-Free Motion Planning Problem:

\[\begin{array}{rl}\min_\gamma & L(\gamma)\\ \operatorname{s.t.} & \gamma:[0,1]\to\mathbb R^n\\ & \gamma(0)=q_0,\gamma(1)=q_1\\ & g(\gamma(t))\le 0,\forall t\in[0,1]\end{array}\]

Constrained Collision-Free Motion Planning Problem:

\[\begin{array}{rl}\min_\gamma & L(\gamma)\\ \operatorname{s.t.} & \gamma:[0,1]\to\mathbb R^n\\ & \gamma(0)=q_0,\gamma(1)=q_1\\ & g(\gamma(t))\le 0,\forall t\in[0,1]\\ & h(\gamma(t))=0,\forall t\in[0,1]\end{array}\]

The Constrained Planning Problem

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Constrained Collision-Free Motion Planning Problem:

Constraint manifold \(\mathcal M=\{q\in\mathbb R^n:h(q)=0\}\)
Parameterization \(\phi:\mathbb R^m\to\mathbb R^n\), reachable domain \(\mathcal V\subseteq\mathbb R^m\)
Require:
- \(\phi\) is differentiable almost everywhere
- \(\phi(\mathcal V)\subseteq\mathcal M\)

Assume a Parameterization

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Constraint manifold \(\mathcal M=\{q\in\mathbb R^n:h(q)=0\}\)
Parameterization \(\phi:\mathbb R^m\to\mathbb R^n\), reachable domain \(\mathcal V\subseteq\mathbb R^m\)
Require:
- \(\phi\) is differentiable almost everywhere
- \(\phi(\mathcal V)\subseteq\mathcal M\)

\[\begin{array}{rl}\min_{\tilde\gamma}& L(\phi\circ\tilde\gamma)\\ \operatorname{s.t.} & \tilde\gamma:[0,1]\to\mathcal V\\ & \tilde\gamma(0)=\tilde q_0,\tilde\gamma(1)=\tilde q_1\\ & g((\phi\circ\tilde\gamma)(t))\le 0,\forall t\in[0,1]\\ & h((\phi\circ\tilde\gamma)(t))=0,\forall t\in[0,1]\end{array}\]

The Parameterized Planning Problem

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

"Reachability Constraint"

Assume we can construct \(\tilde q_i\) such that \(\phi(\tilde q_i)=q_i\), \(i\in\{0,1\}\).

Becomes more Complicated

Eliminated by Construction: \(\tilde\gamma(t)\in\mathcal V\Rightarrow h((\phi\circ\tilde\gamma)(t))=0\)

The equality is gone and the feasible set has positive measure in \(\tilde q\). The price: a new inequality (reachability), and a nonlinear map \(\phi\) inside every remaining cost and constraint.

The Parameterized Planning Problem

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Planning Examples

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Trajectory Optimization

Sampling-Based Planning

Graphs of Convex Sets

Parameterizing Kinematic Graphs

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

Key idea (Xie and Amato 2001):

Split into active and passive chains
Specify active chains as input
Solve for passive chains with IK

Parameterizing Kinematic Graphs

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

Key idea (Xie and Amato 2001):

Split into active and passive chains
Specify active chains as input
Solve for passive chains with IK

A Worked Example: Bimanual

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

\(\phi(q_c,\psi_s)=\big(q_c,\ \operatorname{IK}({}^W\!X^{E_s}(q_c),\psi_s)\big)\): 7 controlled joints + 1 self-motion parameter \(\mapsto\) 14 joint angles

Analytic Inverse Kinematics and Self-Motion

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

Analytic IK can be written as a function

\[\operatorname{IK}:\operatorname{SE}(3)\times\Psi\times\mathscr{K}\to\mathbb{R}^n,\]

where \(\Psi\) are continuous redundancy parameters and \(\mathscr{K}\) are discrete redundancy parameters.

Choosing \(\psi\in\Psi\) and \(\kappa\in\mathscr{K}\) uniquely specifies an IK solution, resolving the "many-to-one" problem. This is the map we will differentiate through.

Reachability Constraints

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

For most robots, \(\operatorname{IK}\) is not defined on all of \(\operatorname{SE}(3)\)
- (Obvious example: a robot arm bolted to a table has limited reach)
Our framework really requires two key ingredients
- A parameterization (i.e. an IK function)
- An optimization-amenable description of its domain: an inequality with an informative gradient, active only on the boundary

A Straightforward Domain Extension

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

The analytic IK function will have domain limited functions

\(\arccos(\cdot):[-1,1]\to\mathbb{R}\)
\(\sqrt{\cdot}:[0,\infty)\to\mathbb{R}\)

Position-based kinematics for 7-DoF serial manipulators with global configuration control, joint limit and singularity avoidance, Faria et al. 2018

Replace with domain-extended versions, e.g.,

\[\arccos(\pi(t))=\left\{\begin{array}{ll}\arccos(-1) & t <=-1\\ \arccos(t) & -1<t<1\\ \arccos(1) & t\ge 1\end{array}\right.\]

\[\sqrt{\pi(t)}=\left\{\begin{array}{ll}\sqrt{0} & t\le 0\\ \sqrt{t} & 0 < t\end{array}\right.\qquad\qquad\]

Closed-Form Inverse Kinematic Joint Solution for Humanoid Robots, Ali et al. 2010

This is a greedy projection in some intermediate space. Later: it approximates a least-squares projection onto the reachable set.

"Direct" Reachability Constraint

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

This extends the domain of \(\operatorname{IK}\) to all of \(\operatorname{SE}(3)\times\Psi\times\mathscr K\), but now it is "incorrect" for nonreachable poses.

Impose the constraint

\[\|\operatorname{FK}(\operatorname{IK}(X,\psi,\kappa))-X\|=0\]

Some properties:

Positive-measure feasible set
Always active at a feasible \((X,\psi,\kappa)\) -- even if this isn't on the boundary of the reachable set!
So its gradient vanishes on the feasible set: LICQ fails everywhere. Interior-point methods handle this badly; SQP with elastic mode tolerates it.

UR5e grasp IK under IPOPT: ~1.1 s median with the direct constraint vs ~0.15 s with a boundary-only constraint (later). Squaring the norm does not help (1.10 → 1.11 s): a constraint-qualification problem, not a smoothness one.

"Probing" Reachability Constraint

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

Position-based kinematics for 7-DoF serial manipulators with global configuration control, joint limit and singularity avoidance, Faria et al. 2018

Closed-Form Inverse Kinematic Joint Solution for Humanoid Robots, Ali et al. 2010

The analytic IK function will have domain limited functions

\(\arccos(\cdot):[-1,1]\to\mathbb{R}\)
\(\sqrt{\cdot}:[0,\infty)\to\mathbb{R}\)

Define probing functions \(\mathcal{D}_k:\operatorname{SE}(3)\times\Psi\times\mathscr K\to\mathbb R\) from the pre-clip intermediate values, such that

\[\mathcal{D}_k\ge 0,\ \forall k\ \Leftrightarrow\ \textrm{reachable}\]

Same positive-measure feasible set, but only active on its boundary: much better for optimization!

Practical rule: impose \(\mathcal D_k\ge s\) slightly inside the clip margin \(c\), with \(s-c\ge\) solver feasibility tolerance (we use \(s=10^{-3},\ c=10^{-4},\ \mathrm{tol}=10^{-4}\)); otherwise the clip silently absorbs infeasibility.

Does the Reachability Constraint Matter?

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

Same parameterization, same (hand-written, autodiff) gradients, same solver (SNOPT); only the reachability constraint changes. Bimanual iiwa, seconds per solve:

Reachability constraint	IrisNp2	Trajopt
Direct \(\\|\operatorname{FK}(\operatorname{IK}(X,\psi))-X\\|=0\)	1.14	1.54
Probing \(\mathcal D_k\ge0\)	1.03	0.98
Boundary \(-\log\det(JJ^T+\epsilon I)\le\tau\) (later)	1.08	1.24

The constraint that is active everywhere costs ~10% on region generation and ~50% on trajectory optimization, with identical gradients. Constraint qualification is not a formality here.

Autodiff rows of Table I, arXiv 2609.10905 (IrisNp2 averaged over 17 seeds, Trajopt over 10 initial guesses).

Planning with Sequences of Convex Sets

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

Graphs of Convex Sets (Marcucci et al.) is one instance; any planner over a decomposition into convex pieces applies, given convex collision-free regions in the chart.

Convex Regions in the Chart (IRIS-NP2)

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

A random walk inside one region: a polytope in the 8-D chart whose image is collision-free and reachable.

What if I Just Want to Drop in an IK Solver?

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

We want to use our method with all the robots, with whatever IK function people already have, unmodified
Main target: codegen tools like IKFast (~100k lines of generated C++ for a UR arm)
No inspection of the code ⇒ no probing functions
Returns no solution off the reachable set ⇒ even the direct constraint can't be evaluated

Can we take whatever IK solver we have and make it work?

The Inverse Function Theorem

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

If \(f:\mathbb R^n\to\mathbb R^n\) is \(C^1\) near \(a\) and \(\det f'(a)\neq0\), then \(f\) has a local \(C^1\) inverse with

\[(f^{-1})'(y)=\big[f'(f^{-1}(y))\big]^{-1}.\]

Intuition: to differentiate through inverse kinematics, we just need to differentiate through forward kinematics!

Gradients from the IFT

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

The inverse of analytic IK is the augmented forward kinematics \(\operatorname{FK}_A:q\mapsto({}^W\!X^G,\psi)\), which returns the end-effector pose and the self-motion parameter.

Given incoming partials \(\big(\tfrac{\partial X}{\partial y},\tfrac{\partial\psi}{\partial y}\big)\):

Compute the value \(q=\operatorname{IK}({}^W\!X^G,\psi)\) (black box)
Compute the augmented Jacobian \(J_A=D\operatorname{FK}_A(q)\) (ours)
Never invert; solve the linear system \[D\operatorname{FK}_A(q)\,\frac{\partial q}{\partial y}=\Big(\frac{\partial X}{\partial y},\frac{\partial\psi}{\partial y}\Big)^{\!T}\]

\(D\operatorname{FK}_A\) is well structured: \(D\operatorname{FK}_A=\begin{bmatrix}D\operatorname{FK}\\ D\operatorname{FK}_\psi\end{bmatrix}\), and if \(\psi\) is a joint angle, \(D\operatorname{FK}_\psi=[\ldots,0,1,0,\ldots]\). (Shoulder–elbow–wrist parameterizations are known in closed form too.)

Direct Numerical Comparison

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

Procedure (bimanual iiwa chart, 10,000 reachable samples):

Sample reachable configurations and random partial-derivative vectors of size \(2^j\)
Compute \(\partial q/\partial y\) via forward-mode autodiff and via the IFT; compare

Median error \(<10^{-13}\), 95th percentile \(<10^{-12}\). Autodiff is faster up to \(2^6\) partials; the IFT is faster above (2.6× at \(2^{10}\)), because building \(D\operatorname{FK}_A\) is a constant cost.

Extending IK Outside the Workspace

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

Greedy projection (clipping) truncates gradients; empirically not good enough with IFT gradients
Probing functions would give external signal, but need the IK internals
Idealized answer: the nearest reachable solution \[\widehat{\operatorname{IK}}({}^W\!X^G,\psi)=\arg\min_q\big\|\operatorname{FK}_A(q)-({}^W\!X^G,\psi)\big\|^2,\qquad \widehat{\operatorname{IK}}=\operatorname{IK}\ \text{on the reachable set}\]

What we actually do is cheaper: some approximate projection onto the reachable set

Clipping in an intermediate space (iiwa, hand-written IK)
Least-squares roots from the IK solver (EAIK / IK-Geo, UR5e)
Bisection on \(\operatorname{SE}(3)\) toward a canonical reachable pose (IKFast, RB-Y1)

We only need a vaguely reasonable answer that helps the optimizer drag itself back to where things are actually defined. (For the iiwa chart, 84.5% of a sampled box domain is unreachable; problem-setup dependent.)

Derivative via Sensitivity Analysis

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

Differentiate \(\widehat{\operatorname{IK}}=\arg\min_q\|\operatorname{FK}_A(q)-(X,\psi)\|^2\) through its optimality conditions. With \(J_A=D\operatorname{FK}_A(q^*)\), residual \(r=\operatorname{FK}_A(q^*)-(X,\psi)\), Hessian slices \(H_i=D_q[J_A]_i\):

\[D\widehat{\operatorname{IK}}(X,\psi)=\Big(J_A^TJ_A+\sum_{i}r_iH_i\Big)^{-1}J_A^T\]

Reachable \(\Rightarrow r=0\Rightarrow (J_A^TJ_A)^{-1}J_A^T=J_A^\dagger=J_A^{-1}\): the IFT is recovered. Outside, the Hessian term survives.

Approximation ladder for the non-reachable case (cheap → expensive), borrowed from numerical IK:

Zero gradient · Pseudoinverse \(J_A^\dagger\)
Levenberg–Marquardt \((J_A^TJ_A+\lambda I)^{-1}J_A^T\), constant \(\lambda\) or singular-value-thresholded
Residual damping \((J_A^TJ_A+\lambda\|r\|^2I)^{-1}J_A^T\) · Anisotropic damping \(V\Lambda V^T,\ \Lambda_j=\lambda(\|r\|^2+3(u_j^Tr)^2)\)
Full Newton with the 6 closed-form end-effector Hessian slices, plus \(\lambda I\)

Residual damping is the workhorse: cheap, and it wins or ties everywhere we've measured.

Boundary Reachability

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

Recall:

Direct reachability is always active
⮑ Difficult for optimizers to use!
Probing reachability requires privileged information
⮑ Incompatible with black-box IFT treatment

Fun fact:

Configurations in the preimage of the workspace boundary have a singular Jacobian!

(Result follows by the open mapping corollary of the IFT.)

Singularity robust algorithm in serial manipulators, Oetomo and Ang (2009)

Boundary Reachability

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

Fun fact:

Configurations in the preimage of the workspace boundary have a singular Jacobian!

(Result follows by the open mapping corollary of the IFT.)

Singularity robust algorithm in serial manipulators, Oetomo and Ang (2009)

Enforce \(b:=-\log\det(J(q)J(q)^T+\epsilon I)\le\tau\); only active on the boundary (like probing), but needs no IK internals
Gradient \(\partial b/\partial q_i=-2\operatorname{tr}\!\big(J^T(JJ^T+\epsilon I)^{-1}\,\partial J/\partial q_i\big)\), chained through \(D\widehat{\operatorname{IK}}\): costs a kinematic Hessian
\(\epsilon,\tau\) are derived from task tolerances, not tuned (\(\epsilon=10^{-6}\); \(\tau=2.46\) from \(v_\text{task}/\dot q_{\max}\))

Boundary Reachability in Action

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

Boundary Reachability Results

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

Gradient method (bimanual iiwa)	IrisNp2 (s)	Trajopt (s)
Autodiff baselines (bespoke IK)
Autodiff, direct reachability	1.14	1.54
Autodiff, probing reachability	1.03	0.98
Autodiff, boundary reachability	1.08	1.24
IFT, direct reachability
Zero gradients	4.11	1.35
Pseudoinverse	4.12	1.31
LM constant / LM SVT	3.12 / 2.22	1.42 / 1.49
Residual damping	1.58	1.76
Anisotropic / full Newton	1.72 / 1.71	2.52 / 1.43
IFT, boundary reachability
Zero gradients / pseudoinverse	1.64 / 1.75	1.20 / 1.23
LM constant / LM SVT	2.51 / 1.29	1.31 / 1.21
Residual damping	1.15	1.24
Anisotropic / full Newton	1.15 / 1.17	1.23 / 1.20

IrisNp2's counterexample search must optimize outside the reachable set: a reachability "challenge" problem
Naive IFT gradients: 4× slower than bespoke autodiff
Can match autodiff performance with
- IFT gradients
- Boundary reachability
- Gradient regularization
Trajopt (feasible start) barely cares

Paper Table I (IrisNp2 averaged over 17 seeds; Trajopt over 10 RRT initial guesses). TOPPRA column omitted (0.6–0.8 s, no reachability constraint).

Box Pickup with Stability Constraints

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

RB-Y1 Hardware Results

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

23-DOF plant; both arms through unmodified IKFast; chart = (torso 6, two gripper poses, two self-motion angles), base pinned
Box from a 4×5 floor grid (3 cm pitch) to a table; 6 legs per plan, 2 of them constrained
20/20 planned, 20/20 executed with plain joint position control

Metric	Mean	Max
Total planning runtime (s)	56.1	135.0
Optimization IK runtime (s)	19.4	84.0
Trajectory optimization runtime (s)	5.6	29.6
Trajectory duration (s)	54.1	63.6
Measured constraint violation (mm)	0.555	2.235
Measured constraint violation (mrad)	1.597	6.809

Planned violation ≈ 6·10⁻¹² mm: everything measured is controller tracking. Planning time is dominated by optimization IK (grasp selection) and trajopt — the solver is the bottleneck.

The Benchmark Suite

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

Optimization IK (one iteration of the planning problem)

UR5e grasp selection, EAIK: 6 vars, SNOPT / IPOPT / NLopt, 100 targets × 10 guesses
RB-Y1 whole-body grasp IK, IKFast: 23 vars, IPOPT, stability + collision, ~3,400 solves
Framework suite (hand-written IK), IPOPT / SNOPT / NLopt:
arm on a table (7) · grasp selection (7) · mobile manipulator (10) · bimanual mobile manipulator (18) · humanoid stability (34)

Planning

★ IRIS-NP2 counterexample search, iiwa bimanual, 8-D chart
iiwa bimanual trajopt, autodiff and IFT gradients: ~160 vars, ~900 nonlinear rows (SNOPT / IPOPT)
RB-Y1 constrained trajopt, IKFast: 57–505 vars, ~1,100 rows (SNOPT, IPOPT escalation)

★ Highlights: IRIS-NP2 · UR5e grasp IK · iiwa autodiff trajopt

Arm on a table

Grasp selection

Mobile manipulator

Bimanual mobile manipulator

Humanoid stability

All instances except the framework suite are in the public code release (docker cohnt/constraint-manifold-charts-ift); happy to hand over any of them.

Highlight 1: IRIS-NP2 Counterexample Search

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

IRIS-NP2 grows a polytope \(P\) in the chart by repeatedly solving counterexample search programs:
\(\displaystyle\min_{\tilde q\in P}\ \|\tilde q-\tilde q_\text{seed}\|_E\ \ \text{s.t.}\ \phi(\tilde q)\ \text{in collision}\)
Initial guesses are sampled uniformly from \(P\): mostly unreachable points of the chart
So the solver runs where \(\operatorname{IK}\) is undefined and every gradient is an extended-domain approximation
Numerically challenging: many small nonconvex solves per region, tight budgets

Gradient outside the reachable set	IrisNp2 (s)
Bespoke autodiff, probing reach	1.03
IFT, zero gradient, direct reach	4.11
IFT, pseudoinverse, direct reach	4.12
IFT, residual damping, direct reach	1.58
IFT, residual damping, boundary reach	1.15

Residual damping is the most performant; full Newton is not much better than Levenberg–Marquardt
Pseudoinverse and zero gradients only succeed by relying on the random restarts
The one stage where gradient quality outside the workspace is decisive: 4× between best and worst

Highlight 2: UR5e Grasp IK (EAIK)

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

Pick a collision-free grasp on a mug: gripper point on the mug axis, orientation free, \(\min\|q\|^2\)
IK from EAIK (black box; returns least-squares roots off the workspace); 6 decision variables (gripper pose in the mug frame) vs 6 joint angles
100 targets × 10 shared random initial guesses; SNOPT, tolerances \(10^{-6}\), 10 s cap

Formulation	Success (1 init)	Success (3 init)	Mean cost	Mean t (s)	Median t (s)
C-space baseline	46.5%	86%	12.68	0.357	0.105
IFT, direct reach.	61.4%	93%	13.30	0.719	0.100
IFT, boundary reach.	63.2%	90%	13.53	0.776	0.110

Minimal coordinates: +15 pp success, slightly worse cost, same median time, 2× worse mean time
The tail: of 3,000 SNOPT solves, the wall clock was hit once; the major-iteration limit 177 / 137 / 114 times (baseline / direct / boundary); every failure is a constraint failure
It is easier for the solver to certify convergence in the old formulation; the tail suggests numerical difficulty, not throughput

Table II of arXiv 2609.10905; replicated bit-exactly on different hardware (0 success flips) on 2026-09-22.

Highlight 3: Bimanual Trajopt vs. Tolerances

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

Kinematic trajectory optimization for the bimanual iiwa in the 8-D chart, hand-written autodiff gradients (the best case for the formulation); one representative instance
137 decision variables (B-spline control points), 103 constraints (1,217 rows): collision, joint limits, reachability, endpoints, at sampled path points

Solver / setting	Converged	Feasible	Time (s)	Cost
SNOPT, major optimality tol 1e-1 / 1e-2 / 1e-3	yes	yes	2.03 – 2.06	1.588
SNOPT, major optimality tol 1e-6 (default)	no	yes	19.4	1.358
IPOPT, penalty line search, tol 1e-4	yes	yes	40.9	1.361
IPOPT, default (filter line search)	no	yes	60.1	1.381

Tightening SNOPT's optimality tolerance from 1e-3 to 1e-6 turns a 2 s solve into a 19 s failure, while the iterate stays feasible the whole time
IPOPT needs a non-default line search to converge at all, and is 20× slower (Drake's build: SPRAL linear solver; HSL untested)
Hacky probe, not a result: re-solving SNOPT inside a shrinking/expanding bounding box gave ~1.2 s at every tolerance, at a ~3% cost penalty. Suggestive at best.

Why Trust Regions? (1) Approximate Gradients

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

Outside the reachable set, the value we return comes from a cheap surrogate projection (clipping, EAIK least-squares roots, SE(3) bisection)…
…but the gradient we return is derived from the idealized least-squares \(\widehat{\operatorname{IK}}\) — and then approximated again (damping)
So even with exact sensitivity analysis, value and gradient belong to slightly different functions. Our gradients are inexact by construction, not by numerical accident
Line-search globalization assumes the directional derivative is consistent with the function it's searching along; trust-region methods only need the local model to be decent inside the radius, and are provably robust to relative gradient error (Carter, 1991)
A trust radius is also the natural way to keep iterates inside the region where the chart is valid: what if the initial guess is outside the domain? What if an intermediate iterate leaves it? Is clipping enough?
The direct reachability constraint's LICQ failure, the "no solution" cliff of black-box IK, and the \(\infty\)-with-zero-gradient returns when IK cannot resolve a sample are all problems of where the iterate is allowed to go

Why Trust Regions? (2) Line Searches Stall

Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems

Thomas Cohn, Robot Locomotion Group, MIT CSAIL

Evidence from the suite that the solvers stall rather than fail:

iiwa trajopt (autodiff, SNOPT): 2 s at tolerance 1e-3, 19 s and not converged at 1e-6, feasible throughout; IPOPT's default filter line search does not converge (60 s), the penalty line search does (41 s)
RB-Y1 constrained trajopt (IKFast, SNOPT): of 52 failures in one 20-point run, 42 hit the time limit and 8 the iteration limit; 0 were infeasibility exits, and the abandoned seeds were valid with 32–62 mm clearance. SNOPT routinely exits 34/41 while holding a feasible iterate. IPOPT's filter line search "strays from feasibility and sticks"; an \(\infty\) return with zero gradient when IK can't resolve a sample strangles both
UR5e grasp IK (EAIK, SNOPT): major-iteration-limit exits 177 / 137 / 114 per 1,000 vs. one wall-clock hit; mean time 0.72 s vs. median 0.10 s
RB-Y1 grasp IK (IKFast, IPOPT): boundary gradient with no damping (\(\lambda=0\)): 14% success; with damping: 100%. The Jacobian is singular exactly where the fallback engages
IRIS-NP2: 4.11 s vs. 1.15 s between the worst and best extended-domain gradient, same solver, same problem
(Framework suite, hand-written IK: SNOPT leaves elastic mode on 51% of bimanual-mobile trials in 0.1 s; IPOPT reports "infeasible" on 19% of trials whose returned point satisfies every constraint at our tolerance)