Constrained Bimanual Planning with Analytic Inverse Kinematics
Thomas Cohn, Seiji Shaw, Max Simchowitz, Russ Tedrake
September 29 2023
Outline
- Background: Constrained Planning
- Background: Analytic IK
- Reparametrizing the Constraint Manifold
- Planning with the Reparametrization
- Next Steps
Part 1
Constrained Planning
Configuration-Space Planning
Task-Space Constraints
Existing Approaches
Trajectory Optimization
Existing Approaches
Sampling-Based Planning
Part 2
Analytic Inverse Kinematics
Inverse Kinematics
Analytic Inverse Kinematics
Analytic Inverse Kinematics
IKFast
Part 3
Reparametrizing the Constraint Manifold
System Setup
Problem: FK is Not Injective
Self Motion
Making IK a Bijection
- Fix a global configuration
- Treat the redundancy parameter as an argument
- 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
Topology of Kinematics (cont'd)
- \(\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
- Fix a global configuration
- Treat the redundancy parameter as an argument
- Restrict the domain and range to avoid singularities, etc.
- End effector must stay within a single \(\mathcal{W}\)-sheet
- Joints must stay within a single \(\mathcal{C}\)-bundle
Our Parametrization in Practice
Part 4
Planning with the Reparametrization
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...
So let's do GCS
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
Part 5
Next Steps
" '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
Constrained Bimanual Planning with Analytic Inverse Kinematics
Thomas Cohn, Seiji Shaw, Max Simchowitz, Russ Tedrake
September 29 2023
RLG Group Meeting Long Talk 9/29/23
By tcohn
RLG Group Meeting Long Talk 9/29/23
- 77