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
- Resolving the "many-to-one" problem
- Reachability: the constraint that replaces the equality
- Gradients through Any IK Solver
- The inverse function theorem
- 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:
\[\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}\]
- 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
\[\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}\]
- 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
\[\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}\]
"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)
Small, nonconvex, expensive-to-evaluate problems with inexact gradients and a bounded validity region. What would you build for that?
Thanks to My Collaborators on this Project
Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems
Thomas Cohn, Robot Locomotion Group, MIT CSAIL
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