Literature
Display
← All papers

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

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

Preprint posted · arXiv (preprint) ↗

Robotics8 min text · CondensedOriginal paper ↗
Contents

Reading guide

All eight pages, four complete figures, three exact tables, eighteen numbered equation groups, four unnumbered displays, the full inverse-function theorem and forty-nine references are covered. The method differentiates a chosen analytic IK branch through an augmented forward-kinematics Jacobian. Exact local derivatives inside regular chart regions are distinct from approximate gradients outside the reachable workspace. The practical contribution is wider solver compatibility, with task-dependent accuracy, runtime and reachability assumptions.

At a glance

The paper supplies gradients for black-box analytic IK without editing generated solver code. It augments forward kinematics with the chosen redundancy coordinates, solves a linear system for derivatives, and adds approximate workspace extensions plus a log-determinant reachability surrogate. Results include agreement with bespoke autodiff on sampled reachable configurations, downstream planning comparisons, and twenty RB-Y1 box trials. The method still needs a continuous branch and well-conditioned local coordinates; arbitrary branch-switching IK is not automatically differentiable.

I Introduction

Equality constraints such as a fixed two-hand grasp confine feasible robot configurations to a lower-dimensional set. Analytic IK can chart that set, allowing optimization in smaller coordinates while checking collision and joint limits after lifting back to joint space. Generated analytic solvers often lack autodiff support. The proposed derivative interface removes that implementation barrier, while local singularities, reachable-domain boundaries and branch tracking remain mathematical and practical constraints.

Complete original Fig. 1 : A constraint-satisfying motion planned with our minimal coordinates trajectory optimization framework, using the RB-Y1 bimanual manipulator to pick up a box from the floor and place it on a table.
Fig. 1: A constraint-satisfying motion planned with our minimal coordinates trajectory optimization framework, using the RB-Y1 bimanual manipulator to pick up a box from the floor and place it on a table.

II Related Work

Earlier constrained planners use projections, specialized manifold sampling, continuation or direct collocation. IK parameterization instead makes some equality constraints automatic within a chart. The method adds a way to differentiate such charts using familiar forward kinematics; it does not replace collision checking, initial-guess generation, stability constraints or downstream trajectory timing.

III Background

Equation1 separates reachable poses, continuous self-motion and discrete IK branches. The experiments hold a discrete branch fixed, and the continuous redundancy coordinates complete the local inverse. Equations3–4 express the same path problem in joint and minimal coordinates, provided the selected chart covers the required path. Direct reachability in Equation2 requires zero reconstruction residual everywhere feasible, motivating a less intrusive surrogate. Poses live on SE(3), so pose derivatives, Jacobian frames and local rotation coordinates must be consistent when implementing the formulas.

Original mathematical definitions, assumptions and experimental protocol

We use monogram notation [34, §3.1] to describe poses: XBA∈SE⁡(3){}^{A}\!X^{B}\in\SE(3) is the pose of frame BB relative to (and expressed in) frame AA. When this pose is a function of a variable qq, we write XBA​(q){}^{A}\!X^{B}(q). The forward kinematics are FK:ℝn→SE⁡(3)\FK:\mathbb{R}^{n}\to\SE(3). An analytic IK function is a mapping

Complete original mathematical display S3.E1
Original selectable mathematical display
Equation 1
IK:𝒰×Ψ×𝒦→ℝn,\IK:\mathcal{U}\times\Psi\times\mathscr{K}\to\mathbb{R}^{n}, (1)
Original mathematical definitions, assumptions and experimental protocol

where 𝒰⊆SE⁡(3)\mathcal{U}\subseteq\SE(3) is a set of reachable end-effector poses, Ψ\Psi describes the continuous self-motions and 𝒦\mathscr{K} enumerates the discrete self-motions (e.g. “elbow up” vs “elbow down”) [35]. In practice, we restrict ourselves to some κ∈𝒦\kappa\in\mathscr{K} and drop the discrete self-motion from the notation [10].

Complete original mathematical display S3.E2
Original selectable mathematical display
Equation 2
||FK⁡(IK⁡(XGW,ψ))−XGW||F2≤0,\left|\left|\FK(\IK({}^{W}\!X^{G},\psi))-{}^{W}\!X^{G}\right|\right|_{F}^{2}\leq 0, (2)
Original mathematical definitions, assumptions and experimental protocol

For a constraint manifold defined implicitly as ℳ={q∈ℝn:F⁡(q)=0}\mathcal{M}=\left\{q\in\mathbb{R}^{n}:F(q)=0\right\}, a minimum-cost path (with respect to a cost functional LL) between q0,q1∈ℳq_{0},q_{1}\in\mathcal{M} can be found via

Complete original mathematical display S3.E3
Original selectable mathematical display
Equation 3
argminL⁡(γ)s.t.γ∈𝒞1​([0,1],ℝn),γ⁡(s)∈ℳ,∀s∈[0,1],γ⁡(s)​ is collision free,∀s∈[0,1],γ(0)=q0,γ(1)=q1.\begin{array}[]{rlr}\argmin&L(\gamma)\\ \textrm{{s.t.}}&\gamma\in\mathcal{C}^{1}([0,1],\mathbb{R}^{n}),\\ &\gamma(s)\in\mathcal{M},&\forall s\in[0,1],\\ &\gamma(s)\textrm{{ is collision free}},&\forall s\in[0,1],\\ &\gamma(0)=q_{0},\gamma(1)=q_{1}.\end{array} (3)
Original mathematical definitions, assumptions and experimental protocol

Given a parameterization ϕ:𝒱→ℳ\phi:\mathcal{V}\to\mathcal{M} with 𝒱⊆ℝm\mathcal{V}\subseteq\mathbb{R}^{m}, m≤nm\leq n, and points q~0,q~1\tilde{q}_{0},\tilde{q}_{1} such that ϕ⁡(q~0)=q0\phi(\tilde{q}_{0})=q_{0} and ϕ⁡(q~1)=q1\phi(\tilde{q}_{1})=q_{1}, this problem can be rewritten as

Complete original mathematical display S3.E4
Original selectable mathematical display
Equation 4
argminL⁡(ϕ∘γ~)s.t.γ~∈𝒞1​([0,1],𝒱),(ϕ∘γ~)​(s)​ is collision free,∀s∈[0,1],γ~(0)=q~0,γ~(1)=q~1.\begin{array}[]{rlr}\argmin&L(\phi\circ\tilde{\gamma})\\ \textrm{{s.t.}}&\tilde{\gamma}\in\mathcal{C}^{1}([0,1],\mathcal{V}),\\ &(\phi\circ\tilde{\gamma})(s)\textrm{{ is collision free}},&\forall s\in[0,1],\\ &\tilde{\gamma}(0)=\tilde{q}_{0},\tilde{\gamma}(1)=\tilde{q}_{1}.\end{array} (4)
Original mathematical definitions, assumptions and experimental protocol

For many constrained trajectory optimization problems, the equality constraint γ⁡(s)∈ℳ\gamma(s)\in\mathcal{M} is the “hardest” constraint, so its elimination via an IK parameterization results in an easier-to-solve problem. We can interpret an optimization IK problem in the same way, by optimizing for a single point instead of a trajectory.

IV Methodology

The approach assumes access to analytic IK solutions with identifiable continuous/discrete self-motion and to forward-kinematics derivatives. It uses exact inverse differentiation where the augmented Jacobian is nonsingular, then numerical approximations where that reasoning fails. Approximate extension values and approximate derivatives are useful optimization guidance, not generally the derivative of the exact black-box projection procedure.

IV-A Applying the Inverse Function Theorem

The preserved theorem gives a local inverse near a regular point. Stacking the ordinary FK Jacobian with derivatives of self-motion coordinates makes the augmented map locally square and invertible. Equation6 obtains Jacobian-vector products by solving against this augmented Jacobian rather than differentiating generated IK code or explicitly forming its inverse. The bimanual example retains the complete block-Jacobian derivation and final linear solve. Fixed branch identity gives consistent redundancy choices, while rank loss, representation singularities and branch changes invalidate a single smooth inverse; the construction is not a global convergence theorem.

Original mathematical definitions, assumptions and experimental protocol

We can leverage the inverse function theorem (IFT) [37, Thm. 2.11] to compute the Jacobian of IK\IK in terms of the derivative of FK\FK, which can easily be computed with standard robotics toolboxes.

Theorem 1 (Inverse Function Theorem).

If f:ℝn→ℝnf:\mathbb{R}^{n}\to\mathbb{R}^{n} is continuously differentiable at a point pp and has a full-rank Jacobian, then there is an inverse function f−1f^{-1} defined on a neighborhood of f⁡(p)f(p), and [D​f​(p)]−1=D⁡(f−1)​f​(p)[Df(p)]^{-1}=D(f^{-1})f(p).

Original mathematical definitions, assumptions and experimental protocol

For a nonredundant manipulator, the EE pose uniquely identifies the joint angles up to the choice of discrete self-motion κ∈𝒦\kappa\in\mathscr{K}, so IK−1=FK\IK^{-1}=\FK. For a redundant manipulator, we call the inverse of the analytic IK function the augmented forward kinematics (inspired by the language of Elias and Wen [17]). We write FKA:ℝn→SE⁡(3)×Ψ×𝒦\FK_{A}:\mathbb{R}^{n}\to\SE(3)\times\Psi\times\mathscr{K} and restrict to a single discrete self-motion κ∈𝒦\kappa\in\mathscr{K}; its Jacobian will have the block structure

Complete original mathematical display S4.E5
Original selectable mathematical display
Equation 5
D​FKA​(q)=[D​FK⁡(q)D​FKψ​(q)],D\FK_{A}(q)=\begin{bmatrix}D\FK(q)\\ D\FK_{\psi}(q)\end{bmatrix}, (5)
Original mathematical definitions, assumptions and experimental protocol

where FKψ\FK_{\psi} maps the joint angles qq to their corresponding self-motion parameter. If joint angles are used to parameterize the self motion (as in IKFast [14]), the rows of D​FKψ​(q)D\FK_{\psi}(q) are standard basis vectors. For a 7DoF arm using the shoulder-elbow-wrist parameterization, it is still simple to compute this Jacobian [17].

Original mathematical definitions, assumptions and experimental protocol

Now, we leverage D​FKA​(q)D\FK_{A}(q) to compute gradients of the analytic IK mapping. Suppose we have decision variables (X,ψ)(X,\psi) and partials (∂X∂y,∂ψ∂y)(\frac{\partial X}{\partial y},\frac{\partial\psi}{\partial y}) for some other variable yy. We obtain q=IK⁡(X,ψ)q=\IK(X,\psi), and want to compute ∂q∂y=D​IK⁡(X,ψ)​(∂X∂y,∂ψ∂y)T\frac{\partial q}{\partial y}=D\IK(X,\psi)(\frac{\partial X}{\partial y},\frac{\partial\psi}{\partial y})^{T}. Due to the IFT, D​FKA⁡(q)=[D​IK⁡(X,ψ)]−1D\FK_{A}(q)=[D\IK(X,\psi)]^{-1}, so we find ∂q∂y\frac{\partial q}{\partial y} by solving the linear system

Complete original mathematical display S4.E6
Original selectable mathematical display
Equation 6
D​FKA⁡(q)​∂q∂y=(∂X∂y,∂ψ∂y)T.D\FK_{A}(q)\tfrac{\partial q}{\partial y}=\left(\tfrac{\partial X}{\partial y},\tfrac{\partial\psi}{\partial y}\right)^{T}. (6)
Original mathematical definitions, assumptions and experimental protocol

This is analogous to how differential inverse kinematics (DiffIK) resolves kinematic redundancy locally, but our use of the augmented Jacobian ensures global consistency. For non-reachable configurations (and representational singularities [17]), D​FKAD\FK_{A} loses rank, and the IFT breaks down. In this case, we fall back to an approximate solution (details deferred to Subsection IV-B).

Original mathematical definitions, assumptions and experimental protocol

A worked example. We now discuss how to apply the IFT machinery to a parameterization for constrained bimanual motion planning [10]. Let qc,qs∈ℝ7q_{c},q_{s}\in\mathbb{R}^{7} be the joint angles of the “controlled” and “subordinate” arms, EcE_{c} and EsE_{s} their end-effector frames, and let ψs∈ℝ\psi_{s}\in\mathbb{R} be the subordinate arm’s continuous self-motion parameter. Let FKc\FK_{c} be the forward kinematics of the controlled arm and FKA\FK_{A} refer to the augmented forward kinematics of the subordinate arm. We compute the desired pose of the subordinate arm’s end-effector in terms of qcq_{c} via XEsW​(qc)=XEcW​(qc)​XEsEc{}^{W}\!X^{E_{s}}(q_{c})={}^{W}\!X^{E_{c}}(q_{c}){}^{E_{c}}\!X^{E_{s}}, where XEsEc{}^{E_{c}}\!X^{E_{s}} is the fixed transform between the end-effectors. Then we compute qs=IK⁡(XEsW​(qc),ψs)q_{s}=\IK({}^{W}\!X^{E_{s}}(q_{c}),\psi_{s}), obtaining the full configuration (qc,qs)=ϕ⁡(qc,ψs)(q_{c},q_{s})=\phi(q_{c},\psi_{s}). Thus, ϕ\phi parameterizes the constraint manifold, and we wish to compute the derivatives

Complete original mathematical display S4.Ex1
Original selectable mathematical display
Equation
∂(qc,qs)∂y=D​ϕ​(qc,ψs)​(∂qc∂y,∂ψ∂y)T.\tfrac{\partial(q_{c},q_{s})}{\partial y}=D\phi(q_{c},\psi_{s})\left(\tfrac{\partial q_{c}}{\partial y},\tfrac{\partial\psi}{\partial y}\right)^{T}.
Original mathematical definitions, assumptions and experimental protocol

First, the Jacobian of ϕ\phi has the organized block structure

Complete original mathematical display S4.EGx1
Original selectable mathematical display
Equation
D​ϕ\displaystyle\hskip-5.0ptD\phi =∂(qc,qs)∂(qc,ψs)=[∂qc∂qc∂qc∂ψs∂qs∂(qc,ψs)]\displaystyle=\frac{\partial(q_{c},q_{s})}{\partial(q_{c},\psi_{s})}=\left[\begin{array}[]{c}\frac{\partial q_{c}}{\partial q_{c}}\quad\frac{\partial q_{c}}{\partial\psi_{s}}\\ \hline\cr\vphantom{\tfrac{)}{}}\frac{\partial q_{s}}{\partial(q_{c},\psi_{s})}\end{array}\right]
=[I7×707×1∂qs∂(XEsW,ψs)​∂(XEsW,ψs)∂(qc,ψs),]\displaystyle=\left[\begin{array}[]{c}\displaystyle I_{7\times 7}\quad 0_{7\times 1}\\ \hline\cr\frac{\partial q_{s}}{\partial({}^{W}\!X^{E_{s}},\psi_{s})}\frac{\partial({}^{W}\!X^{E_{s}},\psi_{s})}{\partial(q_{c},\psi_{s})},\end{array}\right]
=[I7×707×1(D​FKA⁡(qs))−1​[XEsEc​D​FKc⁡(qc)06×101×711×1]],\displaystyle=\left[\begin{array}[]{c}I_{7\times 7}\qquad\qquad\qquad 0_{7\times 1}\\[3.00003pt] \hline\cr\vskip 3.00003pt\cr\displaystyle(D\FK_{A}(q_{s}))^{-1}\begin{bmatrix}{}^{E_{c}}\!X^{E_{s}}D\FK_{c}(q_{c})&0_{6\times 1}\\ 0_{1\times 7}&1_{1\times 1}\end{bmatrix}\end{array}\right],
Original mathematical definitions, assumptions and experimental protocol

Next, we compute the Jacobian-vector product ∂(qc,qs)∂y=D​ϕ​∂(qc,ψs)∂y\frac{\partial(q_{c},q_{s})}{\partial y}=D\phi\frac{\partial(q_{c},\psi_{s})}{\partial y}. We could compute D​ϕD\phi directly by inverting D​FKA​(qs)D\FK_{A}(q_{s}), but symbolic manipulation reveals a more efficient strategy: finding ∂qs∂y\frac{\partial q_{s}}{\partial y} by solving the linear system

Complete original mathematical display S4.EGx2
Original selectable mathematical display
Equation
D​FKA⁡(qs)​∂qs∂y\displaystyle\hskip-5.0ptD\FK_{A}(q_{s})\frac{\partial q_{s}}{\partial y} =[XEsEc​D​FKc⁡(qc)06×101×711×1]​[∂qc∂y∂ψs∂y].\displaystyle=\begin{bmatrix}{}^{E_{c}}\!X^{E_{s}}D\FK_{c}(q_{c})&0_{6\times 1}\\ 0_{1\times 7}&1_{1\times 1}\end{bmatrix}\begin{bmatrix}\frac{\partial q_{c}}{\partial y}\vphantom{\tfrac{}{)}}\\ \vphantom{\tfrac{)}{}}\frac{\partial\psi_{s}}{\partial y}\end{bmatrix}.

IV-B Extending the Domain of IK

Equation7 defines an ideal least-squares extension, and Equation8 differentiates a smooth local optimum using its residual-weighted Hessian. This sensitivity formula needs a locally well-defined optimum and nonsingular local Hessian; it is not automatically valid at nonunique projections or branch transitions. The implementation instead uses efficient clipping, geometric subproblem approximations or bisection toward a nominal reachable target. The six gradient strategies below therefore approximate the extension and can disagree with the actual projection derivative.

Original mathematical definitions, assumptions and experimental protocol

To examine how to compute the gradients in these conditions, we interpret the various methods for extending the domain of IK\IK as approximations of

Complete original mathematical display S4.E7
Original selectable mathematical display
Equation 7
IK^​(XGW,ψ)=argminq⁡||FKA⁡(q)−(XGW,ψ)||22.\widehat{\IK}({}^{W}\!X^{G},\psi)=\operatorname{argmin}_{q}\left|\left|\FK_{A}(q)-({}^{W}\!X^{G},\psi)\right|\right|^{2}_{2}. (7)
Original mathematical definitions, assumptions and experimental protocol

For reachable XGW{}^{W}\!X^{G}, IK^​(XGW,ψ)=IK⁡(XGW,ψ)\widehat{\IK}({}^{W}\!X^{G},\psi)=\IK({}^{W}\!X^{G},\psi). Otherwise, IK^\widehat{\IK} returns a configuration that achieves the closest end-effector pose in a least-squares sense.

Original mathematical definitions, assumptions and experimental protocol

We can compute D​IK^D\widehat{\IK} via sensitivity analysis. Let q∗=IK^​(XGW,ψ)q^{*}=\widehat{\IK}({}^{W}\!X^{G},\psi) be the optimum, JA=D​FKA⁡(q∗)J_{A}=D\FK_{A}(q^{*}) the augmented Jacobian, ri=FKA⁡(q∗)−(XGW,ψ)r_{i}=\FK_{A}(q^{*})-({}^{W}\!X^{G},\psi) the augmented task-space residual, and Hi=Dq​[JA]iH_{i}=D_{q}[J_{A}]_{i} the Hessian slice corresponding to the iith row of JAJ_{A}. Then

Complete original mathematical display S4.E8
Original selectable mathematical display
Equation 8
D​IK^​(XGW,ψ)=(JAT​JA+∑i=1nri​Hi)−1​JAT.D\widehat{\IK}({}^{W}\!X^{G},\psi)=\Big(J_{A}^{T}J_{A}+\sum_{i=1}^{n}r_{i}H_{i}\Big)^{-1}J_{A}^{T}. (8)
Original mathematical definitions, assumptions and experimental protocol

Observe that if XGW{}^{W}\!X^{G} is reachable, the residual ri=0r_{i}=0, so D​IK^​(XGW,ψ)=(JAT​JA)−1​JAT=JA†D\widehat{\IK}({}^{W}\!X^{G},\psi)=\left(J_{A}^{T}J_{A}\right)^{-1}J_{A}^{T}=J_{A}^{\dagger}. Thus, if JAJ_{A} is full rank, we recover the IFT formulation. Also, this matrix inversion (and those in the following approximations) can be avoided with linear system solves, analogous to (6).

IV-B 1 Zero Gradients

Zero gradients avoid numerical explosions but discard useful information about how an infeasible target should move toward reachability. This reproduces a prior fallback and performs poorly for the counterexample-search task under direct reachability, though it remains a comparison baseline.

Complete original mathematical display S4.E9
Original selectable mathematical display
Equation 9
D​IK^​(XGW,ψ)≈0.D\widehat{\IK}({}^{W}\!X^{G},\psi)\approx 0. (9)

IV-B 2 Pseudoinverse

The pseudoinverse keeps sensitivity in nonzero singular directions while removing null directions. It omits the residual-Hessian term of the exact local least-squares derivative. Singular-value thresholds and numerical conditioning still matter; a pseudoinverse does not make the original inverse-function theorem apply at a rank-deficient point.

Original mathematical definitions, assumptions and experimental protocol

If JAJ_{A} is singular, this should still preserve gradient flow in the non-singular directions while avoiding a numerical blowup.

Complete original mathematical display S4.E10
Original selectable mathematical display
Equation 10
D​IK^​(XGW,ψ)≈JA†.D\widehat{\IK}({}^{W}\!X^{G},\psi)\approx J_{A}^{\dagger}. (10)

IV-B 3 Levenberg-Marquardt (LM)

Levenberg–Marquardt adds isotropic damping to the normal equations. The paper compares a constant value with damping activated by the smallest singular value. This stabilizes a surrogate inverse while biasing the derivative. All threshold formulas and conditions remain in the original mathematical context below.

Original mathematical definitions, assumptions and experimental protocol

The classic damped least-squares [39]. For λ\lambda, we consider both constant and singular value thresholding (SVT), where λ=λmax​(1−(σmin​(JA)ϵ)2)\lambda=\lambda_{\max}(1-(\frac{\sigma_{\min}(J_{A})}{\epsilon})^{2}) if σmin​(JA)≤ϵ\sigma_{\min}(J_{A})\leq\epsilon, and λ=0\lambda=0 otherwise, to only apply damping if JAJ_{A} is ill-conditioned.

Complete original mathematical display S4.E11
Original selectable mathematical display
Equation 11
D​IK^​(XGW,ψ)≈(JAT​JA+λ​I)−1​JAT.D\widehat{\IK}({}^{W}\!X^{G},\psi)\approx\left(J_{A}^{T}J_{A}+\lambda I\right)^{-1}J_{A}^{T}. (11)

IV-B 4 Residual Damping

Residual damping increases regularization as the pose/self-motion mismatch grows. It uses information unavailable to zero or undamped pseudoinverse gradients, and gives strong downstream results in this study. Its weighting depends on how translation, rotation and redundancy residuals are represented and scaled.

Complete original mathematical display S4.E12
Original selectable mathematical display
Equation 12
D​IK^​(XGW,ψ)≈(JAT​JA+λ​||r||2​I)−1​JAT.D\widehat{\IK}({}^{W}\!X^{G},\psi)\approx(J_{A}^{T}J_{A}+\lambda\left|\left|r\right|\right|^{2}I)^{-1}J_{A}^{T}. (12)

IV-B 5 Anisotropic Damping

Anisotropic damping uses the Jacobian’s singular directions and residual alignment to set direction-specific regularization. It is more structured than isotropic damping, but not uniformly faster: Table I’s direct-reachability Trajopt time 2.52 s exceeds several simpler alternatives. The original SVD definitions and damping expression remain intact.

Original mathematical definitions, assumptions and experimental protocol

A damping factor is introduced along each singular direction that scales with the magnitude of the residual in that direction. Let JA=U​Σ​VTJ_{A}=U\Sigma V^{T} be the SVD, Λj=λ⁡(||r||2+3​(ujT​r)2)\Lambda_{j}=\lambda(\left|\left|r\right|\right|^{2}+3(u_{j}^{T}r)^{2}), Λ=diag⁡(Λj)\Lambda=\operatorname{diag}(\Lambda_{j}). Then

Complete original mathematical display S4.E13
Original selectable mathematical display
Equation 13
D​IK^​(XGW,ψ)≈(JAT​JA+V​Λ​VT)−1​JAT.D\widehat{\IK}({}^{W}\!X^{G},\psi)\approx\left(J_{A}^{T}J_{A}+V\Lambda V^{T}\right)^{-1}J_{A}^{T}. (13)

IV-B 6 Full Newton

The full-Newton variant adds residual-weighted kinematic Hessian slices plus damping. The implementation includes the six pose components; omitted self-motion Hessians vanish when self-motion is literally a joint angle, but need not vanish for other parameterizations. It is therefore not automatically the full exact sensitivity for every redundancy coordinate choice, and its added computation does not uniformly improve runtime.

Original mathematical definitions, assumptions and experimental protocol

Newton’s method is well-understood for least-squares problems [40, §10]. We only use the first 66 components of the augmented kinematic Hessian, corresponding to the end-effector pose, as these can be obtained in closed-form [41]. When self-motion is parameterized by the angle of one or more joints, that Hessian component vanishes; otherwise we would require second-order autodiff.

Complete original mathematical display S4.E14
Original selectable mathematical display
Equation 14
D​IK^​(XGW,ψ)≈(JAT​JA+∑i=16ri​Hi+λ​I)−1​JAT.D\widehat{\IK}({}^{W}\!X^{G},\psi)\approx\Big(J_{A}^{T}J_{A}+\sum_{i=1}^{6}r_{i}H_{i}+\lambda I\Big)^{-1}J_{A}^{T}. (14)
Original mathematical definitions, assumptions and experimental protocol

We emphasize that we do not compute the least-squares solution in practice, but instead rely on efficient approximations: arccos\arccos clipping [10], geometric subproblem least-squares [20, 21], or even a simple bisection search to a canonical reachable target (used for our RB-Y1 experiment).

IV-C New Reachability Constraints

The reachability argument assumes that an unreachable target’s extended IK solution lies on the actual reachable boundary, where ordinary FK loses rank. Equation15 uses a finite, regularized log determinant and a tuned threshold to discourage that boundary; Equation16 provides its derivative. This practical surrogate depends on ε,τ, Jacobian scaling and the chosen extension. A finite regularized determinant bound is not an unconditional minimum-singular-value certificate: large other singular values can compensate for a small or zero one. Nor does the argument apply to an arbitrary approximate solver returning an interior point for an unreachable target. These are scope conditions, not evidence that the reported robot runs accepted infeasible targets.

Original mathematical definitions, assumptions and experimental protocol

The boundary of the reachable workspace is composed of critical values, i.e., a configuration in the preimage of the boundary must have low-rank Jacobian [23, §3.3]. IK^\widehat{\IK} projects onto the boundary of the reachable set, a property maintained by all three approximations used in our experiments. Thus, enforcing non-singularity of the kinematic Jacobian is sufficient for enforcing reachability. Let q∗=IK^​(XGW,ψ)q^{*}=\widehat{\IK}({}^{W}\!X^{G},\psi) and J=D​FK⁡(q∗)J=D\FK(q^{*}) (the ordinary kinematic Jacobian), then our boundary reachability constraint is

Complete original mathematical display S4.E15
Original selectable mathematical display
Equation 15
b:=−logdet(JJT+ϵI)≤τ,b:=-\log\det(JJ^{T}+\epsilon I)\leq\tau, (15)
Original mathematical definitions, assumptions and experimental protocol

for hyperparameters ϵ\epsilon and τ\tau, which must be chosen on a per-problem basis. We use this log-determinant formulation for its well-behaved gradients: the constraint gradient ∂b∂(XGW,ψ)\frac{\partial b}{\partial({}^{W}\!X^{G},\psi)} can be compiled from the following partial derivatives

Complete original mathematical display S4.E16
Original selectable mathematical display
Equation 16
∂b∂qi∗=−2​tr⁡(JT​(J​JT+ϵ​I)−1​∂J∂qi∗),\frac{\partial b}{\partial q^{*}_{i}}=-2\trace\Big(J^{T}\big(JJ^{T}+\epsilon I\big)^{-1}\frac{\partial J}{\partial q_{i}^{*}}\Big), (16)
Original mathematical definitions, assumptions and experimental protocol

together with the chain rule and D​IK^D\widehat{\IK} obtained from any of the methods in Subsection IV-B. Computing (16) requires the kinematic Hessian, a more expensive computation than direct reachability, which only needs the kinematic Jacobian. But in practice, boundary reachability outperforms direct reachability, thanks in part to its better optimization properties.

V Experiments

The experiments separate derivative correctness on reachable samples, downstream runtime with different reachability treatments, generic-solver grasp optimization, and hardware execution. Each addresses a different question. Agreement with autodiff inside a regular chart does not validate exact out-of-domain gradients, and a runtime benefit in one task need not carry over to another optimizer or derivative-vector size.

Complete original Fig. 2 : Error (top) and runtime (bottom) of the IFT gradients, in comparison to autodiff, tested on 10 000 randomly-sampled reachable configurations.
Axes are log-scale, shading represents the 5-95% range, and outliers with higher error are indicated in red.
Fig. 2: Error (top) and runtime (bottom) of the IFT gradients, in comparison to autodiff, tested on 10 000 randomly-sampled reachable configurations. Axes are log-scale, shading represents the 5-95% range, and outliers with higher error are indicated in red.
Complete original Fig. 3 : A constraint-satisfying motion for a bimanual KUKA iiwa setup [ 10 ] , planned using trajectory optimization with our proposed IFT gradients.
Throughout the motion, the EEs must maintain a constant relative transform.
Fig. 3: A constraint-satisfying motion for a bimanual KUKA iiwa setup [10], planned using trajectory optimization with our proposed IFT gradients. Throughout the motion, the EEs must maintain a constant relative transform.
Complete original TABLE I : Downstream runtimes (seconds) by gradient strategy.
TABLE I: Downstream runtimes (seconds) by gradient strategy.
Accessible original table, all values and grouped headers
Gradient Method IrisNp2 Trajopt TOPPRA
Autodiff Baselines
Autodiff, Direct Reachability 1.14 1.54 0.59
Autodiff, Probing Reachability 1.03 0.98 0.60
Autodiff, Boundary Reachability 1.08 1.24 0.58
Proposed: IFT, Direct Reachability
IFT, Zero Gradients (9) 4.11 1.35 0.75
IFT, Pseudoinverse (10) 4.12 1.31 0.81
IFT, LM Constant (11) 3.12 1.42 0.80
IFT, LM SVT (11) 2.22 1.49 0.80
IFT, Residual Damping (12) 1.58 1.76 0.81
IFT, Anisotropic Damping (13) 1.72 2.52 0.81
IFT, Full Newton (14) 1.71 1.43 0.82
Proposed: IFT, Boundary Reachability
IFT, Zero Gradients (9) 1.64 1.20 0.75
IFT, Pseudoinverse (10) 1.75 1.23 0.80
IFT, LM Constant (11) 2.51 1.31 0.80
IFT, LM SVT (11) 1.29 1.21 0.80
IFT, Residual Damping (12) 1.15 1.24 0.81
IFT, Anisotropic Damping (13) 1.15 1.23 0.81
IFT, Full Newton (14) 1.17 1.20 0.82

V-A Comparison with Forward-Mode Autodiff

On 10,000 sampled reachable configurations, median derivative error is below 10⁻¹³ and the 95th percentile below 10⁻¹²; Figure2 also retains the higher-error outliers. The trial partials are random synthetic derivative directions, not physical trajectories. Standard forward autodiff is faster through partial size 2⁶; IFT gains for larger vectors by amortizing Jacobian construction, while both curves still have solve costs. Downstream Table I averages IrisNp2 over 17 seeds,Trajopt over 10 RRT-initialized solves,and timing over three trajectory families. Boundary/residual IFT gives IrisNp2 1.15 s versus 1.08 s for autodiff with the same constraint, but TOPPRA 0.81 s versus 0.58 s is roughly 40% slower. Direct-reachability IFT can be much slower, and boundary is not best for every metric. These results support compatibility and selected near-baseline timings, not a blanket speedup.

Original mathematical definitions, assumptions and experimental protocol

We evaluate the derivatives for the bimanual parameterization in isolation with low-level calculations, and we examine high-level performance on downstream tasks. Let q~\tilde{q} denote a point in the parameterized space and qq a point in C-space.

Original mathematical definitions, assumptions and experimental protocol

First, we examine speed and accuracy of the derivative calculations themselves. We sample reachable configurations {q~i}i=110 000\left\{\tilde{q}_{i}\right\}_{i=1}^{10\,000} (with rejection sampling). For each configuration qiq_{i}, for a range of decision variable sizes 2j2^{j}, we sample vectors of partial derivatives ∂q~i∂yi​j∈ℝ2j\frac{\partial\tilde{q}_{i}}{\partial y_{ij}}\in\mathbb{R}^{2^{j}}. (Note that in these experiments, the partials do not correspond to any physical quantities; we are purely evaluating the speed and accuracy at the autodiff level.) For each (q~i,∂q~i∂yi​j)(\tilde{q}_{i},\frac{\partial\tilde{q}_{i}}{\partial y_{ij}}), we compute ∂qi∂yi​j\frac{\partial q_{i}}{\partial y_{ij}} via forward-mode autodiff and our IFT technique. The median error for each partial size is less than 10−1310^{-13}, and the 95th percentile is less than 10−1210^{-12}. Autodiff is fastest up to partial size 262^{6}, but for larger partial sizes, IFT is faster, due to constant cost of building the augmented Jacobian D​FKA​(q)D\FK_{A}(q). Full numerical results are shown in Fig. 2.

V-B Using Generic IK Solvers

EAIK and IKFast demonstrate using generated solvers without rewriting them for autodiff. They still require branch/redundancy management and a usable extension outside the reachable domain. The selected examples establish practical applicability to these mechanisms and tasks, while cuspidal robots and unsorted polynomial IK branches remain open cases.

V-B 1 Using a Solution Generated by EAIK

A UR5e selects mug grasps with joint-centering cost, ±π joint limits, collision avoidance and a constrained gripper centerline. One hundred feasible sampled problem instances each get ten shared initializations; those starts also choose the IK branch. Table II reports single-start success 46.5% baseline versus 61.4/63.2% direct/boundary IFT, and three-start success 86% versus 93/90%. Repeated starts on the same instance are not independent new scenes. IFT mean runtimes 0.719/0.776 s are about 2.0/2.17× baseline 0.357 s, although medians are similar. Cost comparisons in parentheses use mutual successes: direct IFT mean 12.57 is slightly lower than baseline 12.81, so higher cost is not universal. Approximate gradients may complicate line-search convergence; the proposed trust-region alternative is not tested here.

Complete original Fig. 4 : Above: three valid grasps for the EAIK grasp selection experiment setup, visualizing the variety of grasps encompassed by our optimization formulation.
Below: the full setup, with added shelf obstacles.
Fig. 4: Above: three valid grasps for the EAIK grasp selection experiment setup, visualizing the variety of grasps encompassed by our optimization formulation. Below: the full setup, with added shelf obstacles.
Complete original mathematical display S5.E17
Original selectable mathematical display
Equations 17a–17d
minq∈ℝd\displaystyle\min_{q\in\mathbb{R}^{d}}\hskip 5.0pt ||q||22\displaystyle\left|\left|q\right|\right|_{2}^{2} (17a)
s.t.\displaystyle\mathrm{s.t.}\hskip 5.0pt plb≤pGM​(q)≤pub\displaystyle p_{\lb}\leq{}^{M}\!p^{G}(q)\leq p_{\ub} (17b)
qlb≤q≤qub,\displaystyle q_{\lb}\leq q\leq q_{\ub}, (17c)
q​ is collision free,\displaystyle q\textrm{{ is collision free}}, (17d)
Original mathematical definitions, assumptions and experimental protocol

where MM is the body frame of the manipuland and pGM​(q){}^{M}\!p^{G}(q) denotes the position of the gripper in the manipuland frame. We set the joint limits to ±π\pm\pi. (17b) enforces that a point between the fingers lie along the centerline of the mug, effectively a nonlinear equality constraint. This can be rewritten as an optimization problem in minimal coordinates space via the IK change-of-variables:

Complete original mathematical display S5.E18
Original selectable mathematical display
Equations 18a–18e
min(pGM,oGM)∈SE⁡(3)\displaystyle\min_{({}^{M}\!p^{G},{}^{M}\!o^{G})\in\SE(3)}\hskip 5.0pt ‖IK^​(pGM,oGM)‖22\displaystyle\|\widehat{\IK}({}^{M}\!p^{G},{}^{M}\!o^{G})\|_{2}^{2} (18a)
s.t.\displaystyle\mathrm{s.t.}\hskip 5.0pt plb≤pGM≤pub\displaystyle p_{\lb}\leq{}^{M}\!p^{G}\leq p_{\ub} (18b)
qlb≤IK^​(pGM,oGM)≤qub,\displaystyle q_{\lb}\leq\widehat{\IK}({}^{M}\!p^{G},{}^{M}\!o^{G})\leq q_{\ub}, (18c)
IK^​(pGM,oGM)​ is collision free,\displaystyle\widehat{\IK}({}^{M}\!p^{G},{}^{M}\!o^{G})\textrm{{ is collision free}}, (18d)
(pGM,oGM)​ is reachable,\displaystyle({}^{M}\!p^{G},{}^{M}\!o^{G})\textrm{{ is reachable}}, (18e)
Original mathematical definitions, assumptions and experimental protocol

where we have separated the pose of the gripper into the position pGM{}^{M}\!p^{G} and orientation oGM{}^{M}\!o^{G} for notational convenience.

Original mathematical definitions, assumptions and experimental protocol

We compare the ordinary configuration-space formulation (17) with the end-effector parameterized formulation (18), using both the direct reachability and boundary reachability constraints, with SNOPT [19] as the optimizer. For direct reachability, we did not square the norm as in (2); this decision empirically led to better performance on this problem instance. We use IFT residual damping to compute D​IK^D\widehat{\IK}.

Complete original TABLE II : UR-5e grasp selection IK experimental results.
The number in parentheses is restricted to three-way mutual successes.
TABLE II: UR-5e grasp selection IK experimental results. The number in parentheses is restricted to three-way mutual successes.
Accessible original table, all values and grouped headers
Formulation Success Rate (Multistart) Mean Cost Median Cost Mean Runtime Median Runtime
1 init 3 init
(17), C-Space Baseline 46.5% 86% 12.68 (12.81) 10.94 (11.11) 0.357 (0.365) 0.105 (0.096)
(18), IFT, Direct 61.4% 93% 13.30 (12.57) 12.65 (11.95) 0.719 (0.720) 0.100 (0.083)
(18), IFT, Boundary 63.2% 90% 13.53 (13.03) 12.83 (12.66) 0.776 (0.825) 0.110 (0.095)

V-B 2 Using a Solution Generated by IKFast

IKFast parameterizes both RB-Y1 arms, with base pose fixed, torso variables and arm self-motion coordinates. Twenty floor positions form a 4×5 grid at 3 cm spacing for the same box/table setup. Six sequential motion phases combine grasp selection, straight-line checks/BiRRT,shortcutting,Trajopt and TOPPRA, with static center-of-mass support constraints. The extension uses bisection toward a nominal reachable target plus residual damping. Mean planning time is 56.1 s (max 135),distinct from 54.1 s execution. Measured relative-constraint violation averages 0.555 mm/1.597 mrad,with maxima 2.235 mm/6.809 mrad; floating-point exactness describes the planned kinematics, not actual hardware tracking. The official project reports 20/20 trials and links the recording. This is limited same-object/grid evidence, not a general contact/dynamics or safety guarantee.

Original mathematical definitions, assumptions and experimental protocol

In our second experiment, we use IKFast to derive a solution for the arms of a Rainbow RB-Y1 robot. We then construct a parameterization that enables it to move objects with both hands, while maintaining a constant relative end-effector transform. In particular, given analytic IK solutions IKleft\IK_{\mathrm{left}} and IKright\IK_{\mathrm{right}} for the left and right arms respectively, our parameterization is

Complete original mathematical display S5.Ex1
Original selectable mathematical display
Equation
ϕ:(qbase,qtorso,XGleftW,ψleft,XGrightW,ψright)↦qfull,\phi:(q_{\mathrm{base}},q_{\mathrm{torso}},{}^{W}\!X_{\mathrm{left}}^{G},\psi_{\mathrm{left}},{}^{W}\!X_{\mathrm{right}}^{G},\psi_{\mathrm{right}})\mapsto q_{\mathrm{full}},
Original mathematical definitions, assumptions and experimental protocol

where qbase∈ℝ3q_{\mathrm{base}}\in\mathbb{R}^{3} and qtorso∈ℝ6q_{\mathrm{torso}}\in\mathbb{R}^{6} are the joint angles of the RB-Y1’s base and torso respectively, ψleft,ψright∈ℝ\psi_{\mathrm{left}},\psi_{\mathrm{right}}\in\mathbb{R} are the self-motion parameters of the left and right arms, and WW is the world frame. qbaseq_{\mathrm{base}} is kept fixed in this experiment.

Original mathematical definitions, assumptions and experimental protocol

The robot’s task is to pick up a box and place it on a nearby table from a set of starting positions arranged in a 4×54\times 5 grid (points spaced 3cm apart) on the floor in front of the robot’s base, for a total of 20 trials. Each pick-and-place trajectory is composed of six parts, planned sequentially:

Complete original TABLE III : RB-Y1 box pickup experimental results.
TABLE III: RB-Y1 box pickup experimental results.
Accessible original table, all values and grouped headers
Metric Mean Max
Planning Statistics
Total Runtime (s) 56.1 135.0
Optimization IK Runtime (s) 19.4 84.0
Trajectory Optimization Runtime (s) 5.6 29.6
Hardware Statistics
Trajectory Duration (s) 54.1 63.6
Measured Constraint Violation (mm) 0.555 2.235
Measured Constraint Violation (mrad) 1.597 6.809

VI Discussion

The useful result is an implementation-independent local derivative interface for suitably continuous IK charts, supplemented by effective approximate gradients and problem-tuned reachability treatment. The paper explicitly leaves branch tracking for unsorted solutions and cuspidal manipulators open. Fixed-chart planning can reason beyond purely local differential IK, but still uses nonlinear optimization and initial guesses; it does not guarantee global optimality or cover every component of the constraint set.

Acknowledgement

The source credits Aditya Agarwal and Lucy Cai for RB-Y1 communication infrastructure and Shrutheesh Iyer for kinematics insights. Equal-contribution marks for Thomas Cohn and Seiji Shaw and the paper’s institutional/support information remain associated with the original source.

Original diagrams and demonstration recordings

Original project images and the boundary-reachability animation are preserved locally with the source movie and a derived preview poster. The original overview and RB-Y1 hardware recording remain linked on YouTube, including the project’s twenty-trial montage. The animation illustrates the constraint value; it is not an additional hardware success experiment. These recordings do not add independent trials to the paper’s twenty runs.

Swept volume of an RB-Y1 humanoid carrying a box from the floor to a table.
Swept volume of an RB-Y1 humanoid carrying a box from the floor to a table.
Swept volume of two KUKA iiwa arms carrying an object between them.
Swept volume of two KUKA iiwa arms carrying an object between them.
Three semi-transparent UR5e arms grasping the same mug from different configurations.
Three semi-transparent UR5e arms grasping the same mug from different configurations.
A KUKA iiwa arm with a plot of the boundary reachability constraint value over time.
Log-log plot of gradient error versus partial derivative vector size.
Log-log plot of gradient error versus partial derivative vector size.
Log-log plot of gradient computation runtime versus partial derivative vector size.
Log-log plot of gradient computation runtime versus partial derivative vector size.
The full grasp selection scene with a UR5e, a mug, tables, and shelf obstacles.
The full grasp selection scene with a UR5e, a mug, tables, and shelf obstacles.
Concept demonstration

Open image ↗