Task-Space Constraints

Planning with Kinematic Constraints

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}\]

Our Idea: Plan with a Parametrization

• Constraint manifold \(\mathcal M=\{q\in\mathbb R^n:h(q)=0\}\)
• Parametrization \(\xi:\mathcal U\to\mathbb R^n\) (with \(\mathcal U\subseteq\mathbb R^m\))
• Require:
  • \(\xi\) is smooth and injective
  • \(\xi(\mathcal U)\subseteq\mathcal M\), i.e., \(\forall \tilde q\in\mathcal U\), \(\xi(\tilde q)\in\mathcal M\)

Building Parametrizations with Analytic IK

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.

Optimization IK (Standard Form)

\[\begin{array}{rl}\min_q & f(q)\\ \operatorname{subject\; to} & {}^WX^G=\operatorname{FK}(q),\\ & g_i(q)\ge0,\;\forall i\in I,\\ & h_j(q)=0,\;\forall j\in J.\end{array}\]

• \(f\) is our objective.
• \(g_i\) are generic inequality constraints.
• \(h_j\) are generic equality constraints.

Reformulated IK

\[\begin{array}{rl}\min_{X,\psi,\kappa} & f(q)\\\operatorname{subject\; to} & X\in\operatorname{SE}(3),\psi\in\Psi,\kappa\in\mathscr K\\ & q:=\operatorname{IK}(X,\psi,\kappa),\\ & \textrm{$q$ satisfies joint limits},\\  & g_i(q)\ge0,\;\forall i\in I,\\ & h_j(q)=0,\;\forall j\in J.\end{array}\]

• \(q\) is not a variable, but is substituted
• "\(\textrm{$q$ satisfies joint limits}\)" is now a nonlinear inequality constraint
• But wait! What about the domain limits of \(\operatorname{IK}\)?

First Try (From our 2023 Paper)

The analytic IK function will have domain limited functions

• \(\arccos(\cdot):[-1,1]\to\mathbb{R}\)
• \(\sqrt{\cdot}:[0,\infty)\to\mathbb{R}\)

Reachability Constraint

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.

Probing Functions (with Julia)

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}\]

Example

Suppose we must compute \(\arccos(f(X,\psi,\kappa))\) within \(\operatorname{IK}\)...

• \(\mathcal{D}_1(X,\psi,\kappa)=1-f(X,\psi,\kappa)\)
• \(\mathcal{D}_2(X,\psi,\kappa)=1+f(X,\psi,\kappa)\)

If \(\mathcal D_1(X,\psi,\kappa)\ge 0\) and \(\mathcal D_2(X,\psi,\kappa)\ge 0\), then \(-1\le f(X,\psi,\kappa)\le 1\).

(Thus, \(\arccos(f(X,\psi,\kappa))\) is well-defined.)

Reachability Constraint

Comparison of new-style reachability constraint

\[\mathcal D_k(f(X,\psi,\kappa)\ge 0,\;\forall k\]

to old-style reachability constraint

\[\operatorname{FK}(\operatorname{IK}(X,\psi,\kappa))=X\]

• Both have positive-measure feasible set
• New-style constraint is only active on the boundary of the feasible set!
• Empirically: much better results for IK and planning!

Black-Box IK Functions

• We want to allow people to use any IK function they have
• Main target: codegen tools like IKFast
• No inspection of the code is a problem
  • We can't use new-style constraint
  • Even the old style-constraint may not work!

Idea 1: Maintain Feasibility

• Given an initial guess which is feasible, we can use line search within optimization to maintain feasibility
• Can apply this to a subset of constraints
  • For example, start out reachable but not collision-free
• We can make this work within IPOPT (already used for humanoid stability IK problems)

Idea 2: Learned Reachability

• Construct a description of the reachable set and use that to define the constraint
• For example, one can learn a "differentiable reachability map" (Murooka 2025)
  • \(f(X)\) such that \(f(X)\ge 0\) if \(X\) is reachable and \(f(X)<0\) otherwise.

Idea 3: Boundary Singularities

• Points on the workspace boundary are singularities
• Singular configurations have a rank-deficient kinematic Jacobian
• We can force full-rank, i.e., with \(\det(JJ^T)>\epsilon\)
  • How to choose \(\epsilon\)?
  • How to write this in end-effector space?

• Could use with a log-barrier cost to maintain feasibility