Fast Coordinated Bimanual Motion Planning
With Hard Constraints

Borna Paro, Luka Petrović, Ivan Marković

University of Zagreb, Faculty of Electrical Engineering and Computing
Laboratory for Autonomous Systems and Mobile Robotics (LAMOR)

Paper arXiv

Two Kinova Gen3 arms cooperatively transport a tray carrying a glass of water and an upright marker, keeping the relative grasp and a global upright orientation so that nothing spills or topples, executed directly from the planned trajectory.

Abstract

Bimanual manipulation enables complex tasks but introduces added complexity from the high number of degrees of freedom involved. When handling rigid objects, the relative transformation between the two end effectors must remain fixed throughout the motion, manifesting as a nonlinear equality constraint that confines the feasible configuration space to a measure-zero manifold and challenges conventional motion planners.

We propose a fast bimanual motion planning pipeline that enforces this hard transformation constraint continuously along the entire path, using a leader-follower parameterization: the leader's configuration is treated as a free variable, while the follower's is determined via inverse kinematics to satisfy the constraint. We extensively evaluate the method in simulation across diverse environments, constraints and bimanual platforms, achieving 19.4× faster planning than prior work while guaranteeing continuous constraint satisfaction. Real-world experiments on a bimanual Kinova Gen3 setup, involving tray transport and elongated-object manipulation, validate direct transfer of planned trajectories to physical hardware.

19.4×

faster planning than the comparable prior method (IK-BiRRT)

3 platforms

KUKA iiwa, Kinova Gen3 (7-DoF) & UR5 (6-DoF), 12 to 14 DoF

<10⁻¹⁷ cm

peak constraint violation, hard constraint guaranteed by construction

0 s

offline precomputation, deploy immediately in unseen scenes

Method & Contributions

When two arms rigidly grasp a shared object, the transformation between their end effectors must stay fixed, a nonlinear equality constraint whose solution set is a measure-zero manifold inside the joint configuration space. Randomly sampled configurations almost surely violate it, and trajectory optimizers struggle with its nonconvexity. Instead of sampling in the full space and projecting back onto the manifold, we parameterize the manifold directly: we sample only the leader arm and recover the follower with a single inverse-kinematics call so the constraint holds exactly, at every state the planner ever inserts into the tree, not merely at waypoints.

Overview of the proposed constrained bimanual planning pipeline: multi-configuration start/goal seeding, parallel RRT-Connect on the manifold chart, constraint-aware interpolation that reuses intermediate IK solutions, discontinuity correction via leader-follower role swapping, and time-optimal retiming with TOPP-RA.

A robot-agnostic pipeline that returns a smooth, executable bimanual trajectory from only the start/goal end-effector poses and the desired transformation constraint.
A fast constrained sampling-based planner (parallel RRT-Connect) that satisfies a hard relative transformation constraint continuously along the entire path.
A constraint-aware interpolation scheme that guarantees constraint satisfaction between any two states and reuses intermediate IK solutions to densify the tree at no extra cost.
Extensive evaluation on KUKA iiwa, Kinova Gen3 and UR5 across multiple environments and constraints, plus real-world Kinova Gen3 demonstrations.

Comparison Against Prior Work (KUKA iiwa)

Click a trajectory to open the interactive viewer: drag to orbit, scroll to zoom, and use the Open Controls panel to play, pause, reset, or scrub the motion.

The prior work offers several planners. IK-PRM and IK-GCS achieve fast online queries, but only after heavy offline precomputation (roughly 44 minutes and 5.1 hours, respectively) that any change to the scene invalidates; only IK-BiRRT, like our method, needs none, making it the directly comparable baseline. Against it, our method plans 19.4× faster on average and produces shorter paths in two of the three configurations, all with no precomputation. Per-configuration path lengths and planning times are shown below.

Method	Offline precomp.	Path length ℓ			Planning time [s]
Method	Offline precomp.	T→M	M→B	B→T	T→M	M→B	B→T
IK-PRM	~44 min	4.67	8.93	9.21	0.46	0.64	0.61
IK-GCS	~5.1 h	2.09	3.32	5.62	3.41	2.32	3.32
IK-BiRRT	none	9.91	8.69	11.42	49.42	52.53	54.10
Ours	none	8.70	6.55	20.57	2.66	0.45	4.95

Bold marks the best value in each column; underline marks the best among the fully online methods (IK-BiRRT and Ours), the only ones needing no precomputation.

Trajectory Interactive 3D Viewers

Interactive 3D viewers of the planned trajectories for the two bimanual platforms (Kinova Gen3 and UR5), across the three transformation constraints ( T₁ , T₂ , T₃ ) and the two cluttered environments (dense and sparse). Click a thumbnail to open it, then drag to orbit, scroll to zoom, and use the Open Controls panel to play, pause, reset, or scrub the motion along the trajectory.

Real-World Deployment

Planned trajectories transfer directly to two physical Kinova Gen3 arms with no manual correction. The hard transformation constraint is held continuously, and the smoothness from path simplification prevents undesirable dynamic effects during execution.

Elongated-object transport

The two arms grasp an elongated object, too long for a single arm, and carry it to the goal while preserving the coordinated dual-arm grasp.

Citation

If you find our work helpful, please cite us:

@article{paro2026coordinatedbimanual,
  title={Fast Coordinated Bimanual Motion Planning With Hard Constraints},
  author={Paro, Borna and Petrović, Luka and Marković, Ivan},
  journal={arXiv preprint arXiv:2608.20946},
  year={2026},
}