💻 computer science

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

This paper proposes a method to enable gradient-based trajectory planning for robot manipulators under kinematic constraints by computing gradients of general-purpose analytic inverse kinematics parameterizations via the inverse function theorem, while also extending their validity beyond the reachable workspace through least-squares domain extension and optimization-friendly reachability constraints.

Original authors: Thomas Cohn, Seiji Shaw, Harel Biggie, Travis Manderson, Nicholas Roy, Russ Tedrake

Published 2026-09-11

📖 6 min read🧠 Deep dive

CC BY 4.0

Original authors: Thomas Cohn, Seiji Shaw, Harel Biggie, Travis Manderson, Nicholas Roy, Russ Tedrake

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Robots that move with human-like dexterity face a fundamental puzzle: how to plan a path when their hands must stay locked to a specific position or orientation, such as holding a cup steady while walking or carrying a box with two arms. In the world of robotics, the space of all possible positions a robot can take is vast, but when a task demands that the robot's hand stay in a fixed pose, the valid paths shrink to a tiny, invisible slice of that space. Finding a route through this narrow slice is notoriously difficult for computers. Traditional planning tools often stumble here, either getting stuck or requiring the robot to be programmed with custom, hand-written math for every single new task. This limitation has kept many advanced robots from performing complex manipulation tasks in unstructured environments, as the software needed to guide them is often too rigid or too specific to be reused.

A team of researchers at MIT has developed a new way to guide robots through these tight constraints without needing to rewrite the underlying math for each machine. Instead of trying to force the robot to follow a rigid set of rules, they created a method that treats the robot's movement like a flexible map. The core of their approach relies on a mathematical principle that allows them to work backward from a desired hand position to the joint angles needed to get there, even when the computer program doing the calculation is a "black box" that cannot be easily modified. By using a standard rule of calculus known as the inverse function theorem, the researchers showed that they could extract the necessary guidance signals from the robot's forward movement equations. This means they can take any existing, automated tool that calculates how a robot moves and instantly make it useful for complex, constrained planning tasks, provided they can also handle the moments when the robot tries to reach for something just out of its physical range.

The researchers demonstrated that their method works by testing it on a real-world robot, the RB-Y1, a dual-armed machine designed to mimic human movement. In a series of twenty trials, the robot successfully picked up a box from various spots on the floor and placed it onto a nearby table. The robot had to navigate a path where both arms moved in perfect coordination to keep the box steady, a task that requires the robot to constantly adjust its joints while maintaining a strict relationship between its two hands. The system planned these movements by first generating a rough path and then refining it using the new mathematical technique. The result was a smooth, continuous motion where the robot lifted the box, stood upright, and placed it on the table without dropping it or violating the physical limits of its joints. The constraint that the hands must stay aligned was satisfied to a precision of less than a single millimeter, proving that the robot could follow the plan with high accuracy.

What makes this achievement significant is that the researchers did not have to build a new, custom mathematical model for the RB-Y1 robot. They used a standard, automated solver that generates movement plans for many different types of robots, a tool that typically cannot be used for these kinds of delicate, constrained tasks because it lacks the necessary mathematical "gradients" or guidance signals. The team's innovation was to create a bridge that allowed them to pull those signals out of the black-box solver. They also solved a second problem: what happens when the robot tries to calculate a path to a spot that is physically impossible to reach? In the past, the computer would simply crash or return an error. The new method allows the system to gracefully handle these impossible requests by finding the closest possible position and continuing the calculation, effectively keeping the planning process alive even when the robot wanders into unreachable territory.

The experiments showed that this general approach is nearly as fast as the specialized, hand-written math that experts have traditionally used for specific robots. In tests involving complex pathfinding and obstacle avoidance, the new method took only slightly longer to compute a solution than the custom versions, while offering the flexibility to work with a much wider variety of machines. The researchers found that by using specific techniques to smooth out the calculations when the robot is near the edge of its reach, the system remains stable and efficient. This suggests that the barrier to entry for advanced robot planning is much lower than previously thought. Engineers can now take a generic robot and a standard movement solver and immediately apply them to tasks that require holding objects, opening doors, or walking while carrying a load, without needing a team of mathematicians to derive new equations for every new application.

The work also highlights the importance of how robots handle failure. When the planning algorithm suggests a move that is physically impossible, the system does not stop; instead, it uses a least-squares approach to find the best possible approximation and continues to refine the path. This ability to recover from impossible states is crucial for real-world applications where the robot might not know exactly where an object is or where its environment might change. By keeping the gradient signal flowing even in these difficult zones, the optimizer can steer the robot back toward a feasible solution. The researchers noted that while some older methods might struggle to converge on a solution when the math gets messy, their approach, combined with a new type of boundary constraint, proved robust enough to handle the full range of motions required for the box-picking task.

Ultimately, this research shifts the paradigm of how robots are taught to move. Rather than treating every new task as a unique mathematical challenge requiring a bespoke solution, the field can move toward a more universal approach where standard tools are adapted on the fly. The ability to differentiate through any inverse kinematics function means that the vast library of existing robot movement software can be unlocked for complex, constrained planning. The successful hardware trials with the RB-Y1 robot serve as a concrete proof that this theoretical framework works in the physical world, allowing a machine to navigate the delicate balance between its physical limits and the demands of a task. The result is a step toward robots that can be more easily deployed in homes and factories, capable of performing intricate manipulation tasks with the same ease as a human, guided by software that understands the geometry of movement without needing to be rewritten for every new job.

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

Problem Statement

Motion planning for robot manipulators under kinematic equality constraints (e.g., dual-arm coordination, door opening, or floating-hand manipulation) restricts feasible motions to a measure-zero submanifold of the configuration space (C-space). Traditional sampling-based planners struggle with these constraints, often requiring specialized rejection sampling or interpolation. Trajectory optimizers face similar difficulties, as they must handle challenging nonlinear equality constraints that are highly sensitive to initial guesses.

A promising strategy to simplify this problem is to parameterize the constraint manifold using analytic Inverse Kinematics (IK). By representing the robot's path in a minimal coordinate space (the parameterized space) rather than the full C-space, equality constraints are eliminated, allowing standard planning algorithms to be applied directly. However, this approach requires the IK mapping to be differentiable to support gradient-based trajectory optimization. While bespoke, handwritten analytic IK solutions can be made differentiable, the vast majority of modern robots rely on automated meta-solvers (e.g., IKFast, EAIK) that generate highly optimized but non-differentiable code. Consequently, most robots lack the gradients necessary for parameterized optimization frameworks. Furthermore, even when gradients are available, standard analytic IK functions are undefined outside the reachable workspace, causing catastrophic failures in nonlinear optimizers that may propose infeasible end-effector poses during intermediate iterations.

Methodology

The authors propose a framework to enable gradient-based optimization through general-purpose, black-box IK solvers without modifying the solvers themselves. The methodology consists of three primary components:

1. Differentiation via the Inverse Function Theorem (IFT)

Instead of attempting to differentiate the complex, optimized code of an IK solver, the authors leverage the Inverse Function Theorem. They construct an Augmented Forward Kinematics (FK) function, $F_{KA}$ , which maps joint angles $q$ to both the end-effector pose and the continuous self-motion parameters ( $\psi$ ).

The Jacobian of the IK mapping, $D\text{IK}$ , is recovered from the Jacobian of the augmented FK, $D F_{KA}$ , using the relationship $D F_{KA} = [D\text{IK}]^{-1}$ .
To compute the gradient of the IK mapping with respect to decision variables, the system solves a linear system involving $D F_{KA}$ rather than explicitly inverting the matrix. This approach is globally consistent and relies only on standard forward kinematics, which are easily differentiable.

2. Least-Squares Domain Extension

To address the issue of undefined gradients for end-effector poses outside the reachable workspace, the authors extend the domain of the analytic IK function. They define a continuous extension, $\hat{\text{IK}}$ , that returns the configuration minimizing the least-squares error between the achieved and desired end-effector poses when the target is unreachable.

The gradient of this extension is derived via sensitivity analysis.
Recognizing that exact least-squares solutions are computationally expensive, the paper proposes several efficient approximations inspired by numerical IK literature, including:
- Zero gradients.
- Pseudoinverse of the augmented Jacobian.
- Levenberg-Marquardt (LM) damping (constant or singular-value thresholding).
- Residual damping and Anisotropic damping.
- Full Newton methods (using a subset of the Hessian).
In practice, the authors often rely on efficient approximations (e.g., geometric subproblem least-squares or bisection searches) rather than computing the full least-squares solution, while still utilizing the derived Jacobian formulas for gradient flow.

3. Boundary Reachability Constraints

Standard reachability constraints (enforcing that the FK of the IK solution matches the target) are active everywhere, which can hinder optimizer convergence. The authors propose a new boundary reachability constraint that is only active near the boundary of the reachable workspace.

This constraint enforces the non-singularity of the kinematic Jacobian at the projected configuration.
Formulated as $b := -\log \det(JJ^T + \epsilon I) \leq \tau$ , it provides well-behaved gradients and allows the optimizer to recover feasibility when iterates drift outside the workspace, outperforming direct reachability constraints in numerical experiments.

Key Contributions

General-Purpose Differentiation: A novel method to compute gradients for any analytic IK function (including those from black-box meta-solvers) using the Inverse Function Theorem and augmented forward kinematics, eliminating the need for bespoke, differentiable IK code.
Robust Domain Extension: A least-squares formulation and a suite of efficient approximations that provide valid gradients for end-effector poses outside the reachable workspace, preventing optimizer failure during intermediate steps.
Optimization-Amenable Constraints: A boundary reachability constraint formulation that is active only at the workspace boundary, improving the convergence properties of trajectory optimizers compared to traditional direct reachability constraints.
Hardware Validation: Successful demonstration of the framework on a Rainbow RB-Y1 bimanual robot performing 20 pick-and-place trials, moving boxes from the floor to a table while maintaining strict kinematic constraints.

Experimental Results

The paper validates the approach through numerical experiments and hardware trials:

Gradient Accuracy and Speed: Comparisons with forward-mode automatic differentiation (autodiff) on a bespoke IK solution show that the IFT method achieves high accuracy (median error $< 10^{-13}$ ). While autodiff is faster for small partial sizes, the IFT method becomes faster for larger partial sizes due to the constant cost of building the augmented Jacobian.
Downstream Planning Performance: In tasks involving graph-of-convex-sets (IrisNp2), trajectory optimization (Trajopt), and time-parameterization (TOPPRA), the proposed IFT approach with boundary reachability constraints and residual damping strategies achieved runtimes comparable to bespoke autodiff baselines. Notably, boundary reachability significantly reduced the runtime gap for IrisNp2 compared to direct reachability constraints.
Generic Solver Integration: The framework was successfully applied to the EAIK solver (for UR5e grasp selection) and IKFast (for the RB-Y1 dual-arm system).
- In the UR5e grasp selection task, the IFT-based formulation achieved higher success rates (90-93%) than the C-space baseline (86%), though with slightly higher median costs.
- In the RB-Y1 hardware experiments, the robot successfully planned and executed 20 pick-and-place trajectories. The measured constraint violations were minimal (0.555 mm position, 1.597 mrad orientation), demonstrating that the robot could track the plan using joint position control without dropping objects.

Significance and Claims

The paper claims to significantly lower the barrier to entry for constrained trajectory optimization in minimal coordinates. By decoupling the requirement for differentiable IK code from the optimization framework, the method allows researchers and engineers to utilize the vast ecosystem of automated IK solvers for complex manipulation tasks.

The authors emphasize that their approach provides the necessary gradients to reason about robots as "floating hands," enabling global planning that avoids joint limits, workspace boundaries, and kinematic singularities—capabilities that local differential IK (DiffIK) often lacks. While the method introduces a minor runtime overhead compared to bespoke autodiff implementations, the trade-off is justified by the ability to use general-purpose solvers and the robustness provided by the domain extension and boundary constraints. The work bridges the gap between bespoke analytic solutions and automatically generated ones, making gradient-based planning accessible to a broader class of robotic systems.