Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems
Thomas Cohn
July 17, 2026
Presentation for the RSS 2026 Workshop on The Geometry of Motion: Physics-Informed Structures for Learning and Control
Robot Locomotion Group
Which Kinematically-Constrained Systems?
Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems
Thomas Cohn, Robot Locomomtion 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 Locomomtion 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 with Applications to \(SO(3)\) and \(\mathbb{R}^3 \times S^2\), Watterson et. al.
Direct Collocation Methods for Trajectory Optimization in Constrained Robotic Systems, Bordabla et al.
Thomas Cohn, Robot Locomomtion Group, MIT CSAIL
An "Intrinsic" Alternative?
Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems
Thomas Cohn, Robot Locomomtion 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 with Parameterizations
- Properties of parameterizations
- Formal problem statement
- Building Parameterizations with Analytic IK
- Resolving the "many-to-one" problem
- Reachability constraints
- Better Usability, Thanks to Geometry
- Gradients through IK
- Domain extensions
- Boundary reachability
Outline for the Remainder of the Talk
Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems
Thomas Cohn, Robot Locomomtion 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 Locomomtion 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\}\)
- Parametrization \(\xi:\mathbb R^m\to\mathbb R^n\), reachable domain \(\mathcal U\subseteq\mathbb R^m\)
- Require:
- \(\xi\) is differentiable almost everywhere
- \(\xi(\mathcal U)\subseteq\mathcal M\)
Assume a Parameterization
Thomas Cohn, Robot Locomomtion 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\}\)
- Parametrization \(\xi:\mathbb R^m\to\mathbb R^n\), reachable domain \(\mathcal U\subseteq\mathbb R^m\)
- Require:
- \(\xi\) is differentiable almost everywhere
- \(\xi(\mathcal U)\subseteq\mathcal M\)
\[\begin{array}{rl}\min_{\tilde\gamma}& L(\xi\circ\tilde\gamma)\\ \operatorname{s.t.} & \tilde\gamma:[0,1]\to\mathcal U\\ & \tilde\gamma(0)=\tilde q_0,\tilde\gamma(1)=\tilde q_1\\ & g((\xi\circ\tilde\gamma)(t))\le 0,\forall t\in[0,1]\\ & h((\xi\circ\tilde\gamma)(t))=0,\forall t\in[0,1]\end{array}\]
The Parameterized Planning Problem
Thomas Cohn, Robot Locomomtion Group, MIT CSAIL
Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems
\[\begin{array}{rl}\min_{\tilde\gamma}& L(\xi\circ\tilde\gamma)\\ \operatorname{s.t.} & \tilde\gamma:[0,1]\to\mathcal U\\ & \tilde\gamma(0)=\tilde q_0,\tilde\gamma(1)=\tilde q_1\\ & g((\xi\circ\tilde\gamma)(t))\le 0,\forall t\in[0,1]\\ & h((\xi\circ\tilde\gamma)(t))=0,\forall t\in[0,1]\end{array}\]
"Reachability Constraint"
Assume we can construct \(\tilde q_i\) such that \(\xi(\tilde q_i)=q_i\), \(i\in\{0,1\}\).
Becomes more Complicated
Eliminated by Construction: \(\tilde\gamma(t)\in\mathcal U\Rightarrow h((\xi\circ\tilde\gamma)(t))=0\)
The Parameterized Planning Problem
Thomas Cohn, Robot Locomomtion Group, MIT CSAIL
Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems
Planning Examples
Thomas Cohn, Robot Locomomtion 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 Locomomtion 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 Locomomtion 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 Locomomtion Group, MIT CSAIL
Analytic Inverse Kinematics and Self-Motion
Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems
Thomas Cohn, Robot Locomomtion 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.
Reachability Constraints
Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems
Thomas Cohn, Robot Locomomtion 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
A Straightforward Domain Extension
Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems
Thomas Cohn, Robot Locomomtion 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
"Direct" Reachability Constraint
Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems
Thomas Cohn, Robot Locomomtion 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!
"Probing" Reachability Constraint
Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems
Thomas Cohn, Robot Locomomtion 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\mathcal{C}\times\mathcal{D}\to\R\]
such that
\[\mathcal{D}_k\ge 0,\forall k\Leftrightarrow \textrm{reachable}\]
Constraint only active on boundary: much better for optimization!
Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems
Thomas Cohn, Robot Locomomtion Group, MIT CSAIL
Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems
Thomas Cohn, Robot Locomomtion Group, MIT CSAIL
Planning with Graphs of Convex Sets
Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems
Thomas Cohn, Robot Locomomtion Group, MIT CSAIL
Geodesically-Convex Collision-Free Regions
Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems
Thomas Cohn, Robot Locomomtion Group, MIT CSAIL
The Catch: You Need a Bespoke IK Function
Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems
Thomas Cohn, Robot Locomomtion Group, MIT CSAIL
- We want to use our method with all the robots
- We want to allow people to use any IK function they have
- Main target: codegen tools like IKFast
Can we use geometry to ease this burden?
The Inverse Function Theorem
Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems
Thomas Cohn, Robot Locomomtion Group, MIT CSAIL
Intuition: to differentiate through inverse kinematics, we just need to differentiate through forward kinematics!
(Spivak) Suppose that \(f:\mathbb{R}^n\to\mathbb{R}^n\) is continuously differentiable in an open set containing \(a\), and \(\det f'(a)\ne 0\). Then there is an open set \(V\) containing \(a\) and an open set \(W\) containing \(f(a)\) such that \(f:V\to W\) has a continuous inverse \(f^{-1}:W\to V\) which is differentiable and for all \(y\in W\) satisfies \((f^{-1})'(y)=[f'(f^{-1}(y))]^{-1}\).
Gradients from the IFT
Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems
Thomas Cohn, Robot Locomomtion Group, MIT CSAIL
Rough procedure:
- Compute value \(q=\operatorname{IK}({}^W\!X^G)\)
- Compute Jacobian \(J=D\operatorname{FK}(q)\)
- Invert: \(D\operatorname{IK}({}^W\!X^G)=J^{-1}\)
For invertibility, need the augmented forward kinematics
\[\operatorname{FK}_A:q\mapsto({}^W\!X^G,\psi),\]
which returns the end-effector pose and the self-motion parameters.
Direct Numerical Comparison
Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems
Thomas Cohn, Robot Locomomtion Group, MIT CSAIL
Procedure:
- Sample reachable configurations
- Sample random partial derivative vectors
- Compute derivatives via autodiff and IFT, compare
Domain Extensions (More Rigorously)
Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems
Thomas Cohn, Robot Locomomtion Group, MIT CSAIL
Ignore self-motion parameter for now. Suppose we write
\[\hat{\operatorname{IK}}({}^W\!X^G) = \left\{ \begin{array}{ll} \begin{array}{rl} \displaystyle \arg\min_{q} & \|\operatorname{FK}(q) - {}^W\!X^G\|^2 \\ \text{s.t.} & q \in \mathbb{T}^n \end{array} & {}^W\!X^G\;\text{not reachable} \\ \operatorname{IK}({}^W\!X^G) & {}^W\!X^G\;\text{reachable} \end{array} \right. \]
Note: this is not actually what our extensions are doing.
- Our clipping is a greedy projection in some space
- IK-Geo does an iterative least squares projection
- Worst-case naive strategy: bisection to a canonical reachable configuration
But perhaps it's a reasonable approximation?
Derivative via Sensitivity Analysis
Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems
Thomas Cohn, Robot Locomomtion Group, MIT CSAIL
\[\begin{array}{rl} \displaystyle \arg\min_{q} & \|\operatorname{FK}(q) - {}^W\!X^G\|^2 \\ \text{s.t.} & q \in \mathbb{T}^n \end{array}\]
- Obtain gradient \[D\hat{\operatorname{IK}}({}^W\!X^G)=\left(J(q)^TJ(q)+\sum_{i=1}^nr_iH_i(x)\right)^{-1}J(q)^T,\] \(r_i\) is the \(i\)th component of the residual \(r=\operatorname{FK}(q)-{}^W\!X^G\)
- If reachable, \(r_i=0\), so we recover the inverse function theorem when \(J(q)\) is nonsingular \[D\hat{\operatorname{IK}}({}^W\!X^G)=\left(J(q)^TJ(q)\right)^{-1}J(q)^T=J(q)^\dagger=J(q)^{-1}\]
Amenable to approximation (Levenberg-Marquardt, residual damping, etc.)
Boundary Reachability
Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems
Thomas Cohn, Robot Locomomtion 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 Locomomtion 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 that \(-\log\det(J(q)J(q)^T+\epsilon I)\le\tau\) for problem-dependent constants \(\epsilon,\tau\).
- Only active on the boundary (like probing reachability)
Boundary Reachability Results
Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems
Thomas Cohn, Robot Locomomtion Group, MIT CSAIL
- IrisNp2 is a reachability optimization "challenge" problem
- Can match autodiff performance, with
- IFT gradients
- Boundary reachability
- Gradient regularization
Box Pickup with Stability Constraints
Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems
Thomas Cohn, Robot Locomomtion Group, MIT CSAIL
What's Next?
Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems
Thomas Cohn, Robot Locomomtion Group, MIT CSAIL
Switching Between IK Branches
- Self motion manifolds are not homotopic when switching branches
- Leads to numerical stability or invalid representations
Branch Tracking with Generic IK Methods
- Generic IK methods (e.g. polynomial elimination) do not "sort" solutions
- Cuspidal manipulators may not even be "sortable"
Path planning and optimization for cuspidal 6R manipulators, Elias and Wen (2025).
Try it Out for Yourself!
Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems
Thomas Cohn, Robot Locomomtion Group, MIT CSAIL
Constrained bimanual planning tutorial repository covers
- Parameterization construction
- Costs and constraints (including reachability constraints)
- Motion planning with RRT, Trajopt, and GCS
- TOPPRA retiming for dynamics limits
Built upon Drake, includes C++ and Python implementations.
Thanks to My Collaborators on this Project
Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems
Thomas Cohn, Robot Locomomtion Group, MIT CSAIL
Motion Planning in Minimal Coordinates for Kinematically-Constrained Systems
Thomas Cohn
July 17, 2026
Presentation for the RSS 2026 Workshop on The Geometry of Motion: Physics-Informed Structures for Learning and Control
Robot Locomotion Group
Tutorial repository
RSS 2026 Talk: Workshop on Geometry of Robot Motion
By tcohn
