Configuration-Space Planning

Task-Space Constraints

Existing Approaches

Trajectory Optimization

Existing Approaches

Sampling-Based Planning

Inverse Kinematics

Analytic Inverse Kinematics

Analytic Inverse Kinematics

System Setup

Problem: FK is Not Injective

Self Motion

Making IK a Bijection

1. Fix a global configuration
2. Treat the redundancy parameter as an argument
3. Restrict the domain and range to avoid singularities, etc.

Topology of Kinematics

• Configuration space \(\mathcal{Q}\)
• End-effector space \(\mathcal{X}\)
• Forward kinematics \(f:\mathcal{Q}\to\mathcal{X}\)
• Regular Point: \(q\in\mathcal{Q}\) s.t. \(Df(q)\) is full rank
• Critical Point: \(q\in\mathcal{Q}\) s.t. \(Df(q)\) is singular
• Regular Value: \(x\in\mathcal{X}\) s.t. \(\forall q\in f^{-1}(x)\), \(Df(q)\) is full rank
• Critical Value: \(x\in\mathcal{X}\) s.t. \(\forall q\in f^{-1}(x)\), \(Df(q)\) is singular
• Coregular Value: \(x\in\mathcal{X}\) s.t. \(\exists q_1,q_2\in f^{-1}(x)\) s.t. \(Df(q_1)\) is full rank and \(Df(q_2)\) is singular

• \(\mathcal{W}\)-Sheet: A connected set of regular and coregular values. Their boundaries are called Critical Value Manifolds.
• \(\mathcal{C}\)-Bundle: A connected set of regular points. Their boundaries are called Coregular Surfaces.

Making IK a Bijection

1. Fix a global configuration
2. Treat the redundancy parameter as an argument
3. Restrict the domain and range to avoid singularities, etc.
   1. End effector must stay within a single \(\mathcal{W}\)-sheet
   2. Joints must stay within a single \(\mathcal{C}\)-bundle

Our Parametrization in Practice

Sampling-Based Planning

Easy! Just draw samples in the parametrized space

C.f. Atlas-BiRRT (from OMPL)

Trajectory Optimization

C.f. Baseline

But We're the Robot Locomotion Group...

Constrained IRIS

Grow an IRIS region in the parametrized space

Multiple sources of hyperplanes

• IK Mapping Domain (\(\arccos(w)\) where \(w\not\in[-1,1]\))
• Subordinate arm joint limit violations
• Reachability violations
• Collisions

Harder optimization landscape -- need many more counterexample searches

IRIS Random Walk

GCS Planning

• All planning done in the parametrized space
• Use arc length in parametrized space as the objective
• Self-motion manifolds?
  • In our experience, don't need GGCS -- just cut the configuration space
  • If the joints or self-motions can wrap around, we can use a flat metric.

Varying the Grasp Distance

• Can treat the entries of the end-effector transform (e.g. grasp distance) as free variables for IRIS
• Fix the transform at plan time

" 'Harder Better

Faster Stronger'

-Daft Punk "

-Tommy Cohn

More Ideas

• Post-processing GCS trajectories with PGD
  • Improve trajectories while maintaining guarantees
• Planning across \(\mathcal{C}\)-bundles and \(\mathcal{W}\)-sheets
  • When can/can't we avoid singularities?
• Manipulating articulated objects in the environment
  • Tools, doors, drawers, rubix cubes?
• Extend to more general kinematic structures
  • Leverage a formulation of IK as an eigenvalue problem