Constrained Bimanual Planning with Analytic Inverse Kinematics

Authors: Thomas Cohn, Seiji Shaw, Max Simchowitz, Russ Tedrake · ICRA 2024

Licences

Open Access
Paper: CC BY 4.0

Everything below draws on the full paper under its CC BY 4.0 licence; its figure is credited to the authors.

TL;DR

Parametrising one arm's joints plus the other arm's redundancy angle through analytic IK turns the two-hand grasp constraint into a space that RRT, PRM, trajectory optimisation and GCS can plan in.

Why it matters

Paths from the parametrisation satisfy the closed-chain constraint at every point by construction, not only at checked waypoints, which matters when both arms hold one rigid object. It also extends convex-set planners such as GCS to configuration spaces with this kind of equality constraint, which they could not handle before; analytic IK exists for many common arms, including the KUKA iiwa, Franka Emika Panda and Barrett WAM.

Problem

When both hands hold one object, the transformation between the end effectors must stay fixed, which appears as a nonlinear equality constraint in configuration space. The feasible set then has measure zero, so sampling-based planners must project samples onto it or approximate it, trajectory optimizers struggle with it, and most existing methods enforce the constraint only at discrete points along a path.

Proposed solution

The planner chooses joint angles for a controlled arm and a redundancy (self-motion) parameter for a subordinate arm; the subordinate arm's joints follow in closed form from analytic IK of the pose the grasp requires, and unreachable poses or joint-limit violations are treated as obstacles. In this lower-dimensional space the valid set has positive measure, so BiRRT, PRM and kinematic trajectory optimisation need only denser collision checking. An extension of IRIS-NP builds convex sets in which every configuration is collision free and kinematically valid, so the Graph of Convex Sets (GCS) planner can be used.

Key insights

Analytic IK, with a fixed global configuration parameter and W-sheet, gives a bijection between an arm's joint angles and its end-effector pose plus redundancy parameters, which removes the equality constraint.
A small step in the parametrised space can be a large motion in the full configuration space, so collision checks must be denser or vary in scale.
The parametrisation is deliberately asymmetric: driving both arms through IK would need 64 global-configuration choices on the iiwa instead of 8 and planning over SO(3), which GCS cannot handle.
Restricting each trajectory to one C-bundle is a theoretical limitation that the authors report was not a major obstacle in their experiments.

Main results

Maximum constraint violation: 6.76 cm for the OMPL atlas planners and 7.00 cm for constrained trajectory optimisation, against within 0.001 cm for all parametrised planners; the trajectory-optimisation baseline's paths also collided with obstacles.
IK-GCS online planning took 3.41 s, 2.32 s and 3.32 s for the three shelf-to-shelf queries, the fastest of all methods; Atlas-BiRRT took 118.66 s to 421.30 s on its successful runs.
Atlas-BiRRT timed out (10-minute limit) in 22 of 30 runs; IK-BiRRT never timed out, and its longest plan took 319.12 s.
IK-GCS configuration-space path lengths were 2.09, 3.32 and 5.62; IK-Trajopt found shorter paths on two queries (3.00 and 4.75) but can get stuck in local minima.
Offline cost: the 19 IRIS regions took 18,361.36 s (966.39 s per region on average) and the IK-PRM 12,124.21 s; Atlas-PRM construction took 326.30 s, 1,878.30 s and 5,405.54 s across three seeds.

Limitations

Each trajectory must stay within a single C-bundle, with the subordinate arm's global configuration parameter and W-sheet fixed (stated as a theoretical limitation).
The parametrisation is asymmetric and can minimise the work of only one arm (stated by the authors).
It requires a closed-form analytic IK solution, which exists only for certain classes of arms.
Building the IRIS regions for GCS is expensive offline (18,361.36 s for 19 regions grown from hand-selected seed points), though it can be parallelised.
The evaluation covers one shelf scene with three start-goal pairs on two KUKA iiwa arms.

Field impact

Finalist for the IEEE ICRA 2024 Best Paper Award in Robot Manipulation; 26 citations. The paper links interactive recordings of all planned trajectories on its project page; it links no code repository.

Abstract

In order for a bimanual robot to manipulate an object that is held by both hands, it must construct motion plans such that the transformation between its end effectors remains fixed. This amounts to complicated nonlinear equality constraints in the configuration space, which are difficult for trajectory optimizers. In addition, the set of feasible configurations becomes a measure zero set, which presents a challenge to sampling-based motion planners. We leverage an analytic solution to the inverse kinematics problem to parametrize the configuration space, resulting in a lower-dimensional representation where the set of valid configurations has positive measure. We describe how to use this parametrization with existing algorithms for motion planning, including sampling-based approaches, trajectory optimizers, and techniques that plan through convex inner-approximations of collision-free space.

Open Source Robotics