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.

  1. Planning with Parameterizations
    1. Properties of parameterizations
    2. Formal problem statement
  2. Building Parameterizations with Analytic IK
    1. Resolving the "many-to-one" problem
    2. Reachability constraints
  3. Better Usability, Thanks to Geometry
    1. Gradients through IK
    2. Domain extensions
    3. 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:

  1. Compute value \(q=\operatorname{IK}({}^W\!X^G)\)
  2. Compute Jacobian \(J=D\operatorname{FK}(q)\)
  3. 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:

  1. Sample reachable configurations
  2. Sample random partial derivative vectors
  3. 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