PaperlayerView exact revision on arXiv

Source-linked AI summary

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

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

arXiv:2609.10905v1cs.RO

TL;DR

Kinematic equality constraints restrict robot motion to measure-zero manifolds, while many automated analytic IK solvers are difficult to differentiate. The paper uses augmented forward kinematics and the inverse function theorem to obtain IK gradients, extends them with least-squares solutions outside reachability, and demonstrates constrained planning through numerical studies and RB-Y1 hardware trials.

  • Problem

    Equality-constrained manipulation requires planning on measure-zero manifolds, but automated analytic IK solvers generally do not expose differentiable mappings for gradient-based optimization.

  • Method

    The method computes IK gradients from augmented forward-kinematics Jacobians using the inverse function theorem and extends the IK domain with least-squares solutions and boundary reachability constraints.

  • Results

    The framework achieved close performance to bespoke IK solutions and successfully planned and executed RB-Y1 box pick-and-place motions satisfying the kinematic constraint.

  • Takeaways & Limitations

    Implementation-independent IK gradients lower the barrier to constrained trajectory optimization in minimal coordinates and support generic automated IK solvers.

Abstract

from arXiv · show

Planning trajectories for robot manipulators under kinematic equality constraints restricts feasible motions to a measure-zero submanifold of the configuration space, requiring special algorithmic treatment. A promising strategy is parametrizing the set of feasible configurations using analytic inverse kinematics (IK). Bespoke analytic IK functions can be written to be differentiable, a necessary property for gradient-based trajectory optimization. But the vast majority of IK functions are computed by automated meta-solvers like IKFast, and are difficult to modify for differentiability. We present a new approach for computing gradients of analytic IK parameterizations: we leverage the inverse function theorem to recover the desired gradients from the ordinary forward kinematic Jacobian. Furthermore, we present a least-squares domain extension and an optimization-amenable description of the reachability constraint, which preserves gradient signal outside the reachable workspace. We demonstrate the efficacy of our approach through numerical experiments and downstream tasks, including a hardware demonstration of an RB-Y1 picking up a box and placing it on a table. Project website: https://cohnt.github.io/inverse-function-theorem-parameterization/

I. INTRODUCTION

Equality-constrained manipulation confines feasible configurations to a measure-zero constraint manifold, challenging both sampling-based planning and trajectory optimization. The paper uses differentiable parameterizations from analytic IK and extends gradients and reachability handling to black-box IK solvers.

  • I. INTRODUCTION: Equality constraints on end-effector pose reduce feasible configurations to a measure-zero subset of configuration space.Examples include bimanual object carrying, door opening, and keeping both feet fixed on the ground.
  • I. INTRODUCTION: Constraint manifolds prevent rejection sampling and make trajectory optimizers highly dependent on good initial guesses.Planned trajectories may also require post-processing or compliant control to satisfy constraints accurately.
  • I. INTRODUCTION: Analytic IK parameterizations eliminate equality constraints and expose minimal coordinates for applying ordinary planning algorithms.Additional self-motion arguments resolve the one-to-many nature of inverse kinematics.
  • I. INTRODUCTION: The proposed method computes gradients for black-box IK by combining augmented forward kinematics with the inverse function theorem.This avoids modifying optimized analytic IK implementations generated by automated solvers.
  • I. INTRODUCTION: Least-squares domain extensions preserve gradient flow outside the reachable workspace and pair with an optimization-amenable reachability description.The framework is evaluated in numerical studies, downstream planning tasks, and 20 RB-Y1 hardware pick-and-place trials.
  • I. INTRODUCTION: Existing constrained-planning approaches rely on implicit manifold representations, specialized sampling, continuation, or local approximations for generic manifolds.These approaches include mature constrained-planning frameworks and direct optimization methods whose performance depends on initialization.

III. BACKGROUND

The background formulates constrained planning as optimization on an implicitly defined manifold and motivates replacing its hardest equality constraint with an analytic IK parameterization. Reachability remains an inequality-like domain issue that must be handled carefully.

  • III. BACKGROUND: An analytic IK mapping takes reachable end-effector poses and self-motion variables to configurations, with discrete self-motion choices handled separately.The paper fixes one discrete self-motion choice in practice.
  • III. BACKGROUND: Analytic IK charts certain constraint manifolds using minimal coordinates while retaining costs and constraints in configuration space.The parameterization can use end-effector pose and self-motion variables while imposing joint limits and other configuration-space constraints.
  • III. BACKGROUND: Robot workspace limits require parameterized optimization problems to impose reachability constraints.Direct reachability constraints match the achieved end-effector transform to the desired transform but remain active throughout the feasible region.
  • III. BACKGROUND: The constrained trajectory problem minimizes a cost over collision-free paths whose configurations remain on the implicitly defined manifold.A parameterized formulation replaces manifold-valued trajectories with trajectories in the parameter domain.
  • III. BACKGROUND: Eliminating the manifold equality constraint through an IK parameterization can make constrained trajectory optimization easier to solve.The same perspective applies to optimization IK problems by optimizing a single point instead of a trajectory.

IV. METHODOLOGY

The methodology recovers analytic IK derivatives from augmented forward-kinematics Jacobians using the inverse function theorem. It applies this construction to redundant manipulators while identifying rank loss as the boundary of validity.

  • IV. METHODOLOGY: The inverse function theorem computes the Jacobian of an analytic IK mapping from the derivative of forward kinematics.The required forward-kinematics derivative can be obtained with standard robotics toolboxes.
  • IV. METHODOLOGY: Augmented forward kinematics returns both end-effector pose and self-motion variables, making it the inverse of a fixed-branch redundant IK mapping.Its Jacobian includes pose and self-motion derivatives; for IKFast joint-angle self-motion, the latter are standard basis rows.
  • IV. METHODOLOGY: IFT-based gradients propagate derivatives through IK by solving with the augmented forward-kinematics Jacobian rather than differentiating the IK implementation directly.Symbolic simplification provides a more efficient strategy than directly inverting the full Jacobian.
  • IV. METHODOLOGY: The augmented Jacobian supports globally consistent gradients, whereas rank loss at non-reachable configurations or representational singularities causes the IFT to break down.The method therefore falls back to an approximate solution in those cases.
  • IV. METHODOLOGY: For bimanual constrained motion, the subordinate end-effector pose is derived from the controlled arm and passed with a self-motion parameter to IK.The resulting full configuration parameterizes the constraint manifold using controlled-arm joints and the subordinate self-motion variable.

B. Extending the Domain of IK

The domain-extension method replaces unavailable IK solutions for non-reachable targets with least-squares configurations and derives gradients through sensitivity analysis. On reachable targets, it recovers the ordinary IFT formulation when the augmented Jacobian has full rank.

  • B. Extending the Domain of IK: Meta-solver IK implementations may return no solution outside the reachable workspace, while the IFT also fails at the workspace boundary because it is singular.Approximate IK solutions additionally omit the approximation procedure’s contribution to the gradient.
  • B. Extending the Domain of IK: The extended IK returns the exact IK solution for reachable targets and a configuration achieving the closest end-effector pose in least-squares sense otherwise.This provides a continuous optimization target beyond the ordinary IK domain.
  • B. Extending the Domain of IK: Sensitivity analysis computes the extended IK Jacobian from the optimum, augmented Jacobian, residual, and Hessian slices.The nonzero residual outside the workspace means the Hessian term is not eliminated.
  • B. Extending the Domain of IK: When the target is reachable and the augmented Jacobian has full rank, the sensitivity formula reduces to the IFT formulation.The required matrix inversions can be implemented as linear-system solves.

1) Zero Gradients:

For non-reachable targets, derivative handling must preserve useful gradient flow without numerical instability. The approach matches prior derivative treatment while retaining gradients in non-singular directions.

  • Derivative handling for non-reachable end-effector targets follows the treatment used by prior work.
  • The desired behavior combines non-reachable-target derivative handling with stability near singular Jacobians.
  • Singular Jacobians can still preserve gradient flow in non-singular directions while avoiding numerical blowup.

2) Pseudoinverse:

The method uses damped least-squares regularization, with damping selected either constantly or through singular-value thresholding. Thresholding activates damping only when the Jacobian is ill-conditioned, while projected configurations can yield less informative Jacobians for larger residuals.

  • Damping can use either a constant factor or singular value thresholding based on the smallest singular value.
  • Singular value thresholding applies damping only when the Jacobian is ill-conditioned.
  • For larger residuals, the Jacobian evaluated at the projected configuration becomes less informative.

5) Anisotropic Damping:

The approach extends IK gradients outside the reachable workspace, enforces reachability through a boundary-focused constraint, and uses damping and efficient approximations to support optimization in practice.

  • 5) Anisotropic Damping: Damping is introduced independently along singular directions according to the residual magnitude in each direction.
  • C. New Reachability Constraints: Efficient approximations include arccos clipping, geometric subproblem least-squares, and bisection to a canonical reachable target.
  • C. New Reachability Constraints: The least-squares domain extension provides solutions and gradients when the current iterate is outside the feasible workspace.
  • C. New Reachability Constraints: The boundary reachability constraint relies on projected configurations and the ordinary kinematic Jacobian to enforce non-singularity.
  • C. New Reachability Constraints: The log-determinant formulation has well-behaved gradients but requires hyperparameters chosen per problem.
  • C. New Reachability Constraints: Although boundary reachability requires the more expensive kinematic Hessian, it outperforms direct reachability in practice.

V. EXPERIMENTS

Experiments evaluate IFT derivative accuracy and runtime, then test the gradients in constrained planning and grasp-selection tasks. The results show near-autodiff accuracy, favorable scaling for larger derivative vectors, and improved downstream performance with suitable regularization and reachability formulations.

  • Downstream planning: IFT gradients support trajectory optimization for a bimanual KUKA iiwa motion while maintaining a constant relative end-effector transform.
  • A. Comparison with Forward-Mode Autodiff: The IFT derivatives achieve median error below 10^-13 and 95th-percentile error below 10^-12 across sampled partial sizes.
  • A. Comparison with Forward-Mode Autodiff: For partial sizes above 26, IFT runs faster than autodiff because augmented-Jacobian construction has constant cost.
  • Downstream planning: Residual, anisotropic, and full-Newton damping strategies produce the smallest IrisNp2 runtime differences from the bespoke baseline, whereas direct reachability is much slower with IFT.
  • Overall results: With appropriate gradient regularization, IFT incurs only a minor runtime increase compared with bespoke autodiff implementations.

B. Using Generic IK Solvers

The framework applies existing generic IK solvers without modification by extending their domains and differentiating through them with IFT-based gradients. In the UR5e grasp-selection experiment, these formulations improved success rates over the C-space baseline but had higher costs and mean runtimes.

  • Using a Solution Generated by EAIK: EAIK provides least-squares solutions when targets are infeasible, making it compatible with the proposed analytic-IK domain extension.Its solution branches are generally sorted, and least-squares usage is reported so branch-index changes can be tracked.
  • Results: The IFT formulations achieved higher success rates than the C-space baseline, while producing slightly higher-cost solutions.The authors attribute the cost increase to objective distortion in minimal coordinates.
  • Experimental setup: The experiment compares ordinary C-space optimization with end-effector parameterizations using direct and boundary reachability constraints under SNOPT.IFT residual damping is used for the Jacobian of the extended IK function.
  • Results: Median runtimes were comparable, but mean runtimes were worse for IFT formulations, suggesting convergence certification was generally easier in the C-space formulation.Direct reachability incurred much higher runtime with IFT gradients on IrisNp2 because of gradient inaccuracy.
  • Limitation: Approximate gradients outside the reachable workspace may explain why IFT struggled to converge on some instances.The authors suggest trust-region methods as a possible alternative to line-search solvers such as SNOPT and IPOPT.

2) Using a Solution Generated by IKFast:

The authors use IKFast-generated solutions to parameterize constrained bimanual motion for an RB-Y1. Across 20 hardware trials, the robot consistently picked up boxes and placed them on a table while satisfying the kinematic constraint to floating-point tolerance.

  • Using a Solution Generated by IKFast: IKFast solutions parameterize both arms while base and torso variables complete the RB-Y1 bimanual configuration.The base configuration is held fixed in this experiment, and each arm contributes a self-motion parameter.
  • Planning procedure: Each pick-and-place trajectory uses six sequential stages, alternating unconstrained motions with constrained lift and placement motions.Unconstrained segments use C-space planning, whereas constrained segments use the parameterization framework.
  • Planning procedure: The constrained and unconstrained motions are planned with BiRRT and shortcutting, with straight-line attempts used for selected transitions before fallback planning.Grasps are selected through optimization IK, a meta-heuristic, and an expensive plannability check.
  • Hardware demonstration: Across 20 trials, the RB-Y1 consistently picked up boxes from varied configurations and placed them on a nearby table without dropping or damaging them.The starts formed a 4 × 5 grid with 3 cm spacing on the floor.
  • Hardware demonstration: The planned hardware trajectories satisfied the bimanual kinematic constraint up to floating-point tolerance.The object was moved while maintaining a constant relative end-effector transform.

VI. DISCUSSION

The discussion presents IFT-based differentiation and domain extensions as a general route to optimization with analytic IK parameterizations. It reports practical guidance while identifying IK branch tracking and approximate-gradient convergence as remaining boundaries.

  • Discussion: The IFT approach differentiates through IK mappings without modifying analytic IK functions, lowering the barrier to constrained trajectory optimization in minimal coordinates.The method is intended for contexts that reason about the robot as a floating hand.
  • Discussion: The approach reportedly achieves performance close to bespoke IK solutions while providing optimization-amenable domain extensions and reachability constraints.The global formulation is described as producing motions that avoid joint limits, workspace boundaries, and kinematic singularities.
  • Design guidance: Implementation-independent gradients allow any well-defined IK function with continuous solutions to be deployed in the framework.Residual-based regularization requires only limited hyperparameter tuning, and boundary reachability handles non-reachable optimizer iterates robustly.
  • Limitations: Open questions remain for IK solvers that do not globally sort solutions into branches, including IK-Geo subproblems 5 and 6 and polynomial/eigenvalue methods.Path planning in minimal coordinates also remains open for cuspidal manipulators.
Loading 2609.10905v1…