PaperlayerView exact revision on arXiv

Source-linked AI summary

Fast Coordinated Bimanual Motion Planning With Hard Constraints

Borna Paro, Luka Petrović, Ivan Marković

arXiv:2608.20946v1cs.RO

TL;DR

Rigid-object bimanual planning must preserve a fixed relative end-effector transformation on a measure-zero feasible manifold, which challenges conventional planners. The paper uses leader-follower IK to enforce the constraint continuously, achieving 19.4× faster planning than prior work and transferring trajectories directly to hardware.

  • Problem

    Rigid-object bimanual manipulation requires fixed relative end-effector transformations, creating a measure-zero feasible manifold that challenges conventional motion planners.

  • Method

    A leader-follower pipeline treats the leader configuration as free and solves the follower configuration by IK, with constraint-aware interpolation maintaining validity between states.

  • Results

    19.4× faster planning than prior work was achieved on the KUKA iiwa benchmark, while real-world Kinova Gen3 trajectories remained continuously constrained and directly executable.

  • Takeaways & Limitations

    The robot-agnostic pipeline transfers constrained bimanual trajectories from simulation to physical hardware without posthoc correction.

  • Takeaways & Limitations

    Large C-space discontinuities can still cause post-processing failures, especially in cluttered environments.

Abstract

from arXiv · show

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.4x 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.

I. INTRODUCTION

Rigidly grasped bimanual manipulation requires the relative end-effector transformation to remain fixed, creating a difficult measure-zero constraint-manifold planning problem. The proposed leader-follower pipeline enforces this constraint continuously without offline precomputation and is evaluated in simulation and real-world demonstrations.

  • Motivation: Rigidly grasped arms must preserve their relative end-effector transformation, forming a nonlinear equality constraint whose feasible configurations occupy a measure-zero subset of C-space.Random samples almost surely violate the constraint, while trajectory optimization may struggle with its nonconvexity.
  • Motivation: Maintaining the rigid coupling is physically necessary for tasks such as upright tray transport and carrying elongated objects without spilling, toppling, slipping, or deforming them.These tasks illustrate why continuous constraint satisfaction matters in practical manipulation.
  • Approach: The proposed pipeline treats the leader configuration as free and determines the follower configuration through inverse kinematics to satisfy the hard constraint at every trajectory point.It requires no offline precomputation or analytic IK solution, supporting arbitrary manipulators and unseen environments.
  • Approach: The robot-agnostic system combines constrained RRT-Connect planning, continuous constraint-aware interpolation, and reuse of intermediate IK solutions during motion validity checks.The stated evaluation spans KUKA iiwa, Kinova Gen3, and UR5 platforms across simulated settings and real-world demonstrations.
  • Evaluation: The method is reported to achieve planning times significantly faster than comparable methods while continuously satisfying the hard relative transformation constraint.The supplied introduction identifies speed and continuous satisfaction as central evaluation targets.

B. Hard Constraints vs. Relaxation

Rigid bimanual manipulation is formulated as planning on a collision-free configuration-space subset that also satisfies a hard end-effector transformation constraint. The formulation exploits the two-arm system’s closed-chain structure rather than relaxing or numerically projecting the constraint.

  • Hard Constraints vs. Relaxation: Relaxation-based methods replace F(q) = 0 with ∥F(q)∥< ε, enlarging the feasible set but no longer enforcing the exact equality constraint.This makes standard sampling-based planning applicable without modification.
  • Hard Constraints vs. Relaxation: Projection-based methods retract sampled configurations onto the constraint manifold using iterative Jacobian-based procedures, which can be computationally expensive without analytic Jacobians.Their cost comes from repeated constraint-Jacobian evaluations and iterative retraction.
  • Closed Kinematic Chains: Rigid bimanual grasping creates a closed kinematic chain whose loop-closure constraint reduces the effective dimensionality of the planning space.The leader-follower parameterization is an active/passive decomposition that closes the remaining chain through IK.
  • Problem Formulation: The system configuration combines leader and follower arm configurations in a 2n-dimensional space, with each arm having n = {6, 7} degrees of freedom.Forward kinematics map both configurations to end-effector poses in a common world frame.
  • Problem Formulation: The hard constraint requires the follower pose to equal the leader pose multiplied by a fixed transformation T at every time t ∈[0, 1].Planning seeks a continuous, collision-free path satisfying this relation while permitting construction of an executable trajectory.

1) The constraint manifold:

The constraint manifold is represented as the zero set of a six-dimensional residual encoding the follower pose error relative to the transformed leader pose. Under a full-rank Jacobian condition, it is a smooth lower-dimensional manifold and therefore measure-zero in the ambient configuration space.

  • 1) The constraint manifold:: Equation (2) is converted into a residual map F: C →R6 whose zero value is equivalent to the follower end-effector matching its constrained target pose.The residual uses exponential coordinates to represent the SE(3) pose discrepancy.
  • 1) The constraint manifold:: The manifold M is the set of configurations satisfying FKF(qF ) = FKL(qL) T, while valid states additionally exclude collisions.Thus, Xvalid is the intersection of the collision-free space and the constraint manifold.
  • 1) The constraint manifold:: Because T is constant, the two end-effector spatial twists coincide along constrained motion.The associated constraint Jacobian is formed from the leader and follower spatial Jacobians.
  • 1) The constraint manifold:: When the follower Jacobian has full row rank, the constraint Jacobian has full rank and M is a smooth embedded submanifold.The regular value theorem supplies this geometric characterization.
  • 1) The constraint manifold:: The manifold has dimension 8 for 7-DoF arms and 6 for 6-DoF arms, making it measure-zero in the combined configuration space.Uniformly sampled configurations therefore violate the constraint almost surely.

B. Constraint-Manifold Parameterization

The method parameterizes the constraint manifold directly by treating the leader configuration as coordinates and recovering the follower through inverse kinematics. This chart-based construction lets planning operate directly on valid bimanual configurations without projection.

  • The follower end-effector pose is computed from the leader pose and the desired transformation, then recovered through inverse kinematics.
  • The seed configuration biases the follower IK solver toward solutions near a reference state.
  • Equations (7)–(8) define a parameterization map that lifts each leader configuration to a constraint-satisfying bimanual state.
  • Using leader joint angles as manifold coordinates allows the planner to search directly on the constraint manifold.
  • The map uses a single manipulator IK call rather than atlas construction or iterative Jacobian-based projection, and remains agnostic to the IK implementation.

2) Role of the seed:

The follower IK solution may contain multiple branches or a redundant self-motion manifold, so the seed selects the solution branch and helps preserve continuity between states. Consequently, every generated state satisfies the hard constraint up to solver tolerance.

  • For a fixed leader configuration, follower IK yields up to eight branches for 6-DoF arms or a one-dimensional self-motion manifold for redundant 7-DoF arms.
  • The seed selects an IK branch and, for redundant manipulators, determines the redundancy resolution.
  • Seeding each solve with the previous follower state keeps the solution on one branch and minimizes discontinuities between adjacent configurations.
  • Every inserted state satisfies F(q) = 0 up to the IK solver tolerance because the follower pose is solved to match the required pose exactly.
  • The hard constraint therefore holds at every state inserted into the tree, unlike methods that interpolate freely between constrained samples.

C. Follower Sensitivity and C-Space Discontinuities

Leader-to-follower sensitivity can create large joint-space changes near follower singularities, producing discontinuities that complicate planning. The pipeline addresses this with adaptive interpolation, singularity rejection, follower-weighted distances, and multi-branch start and goal generation.

  • Near a follower kinematic singularity, a small leader change can require a disproportionately large follower joint-space motion, creating C-space discontinuities.
  • The planner scales interpolation steps with state distance, rejects samples near singularities, and up-weights follower joints in its nearest-neighbor metric.
  • The wrapped weighted angular metric respects continuous-joint circular topology while emphasizing follower-joint motion.
  • Start and goal poses are lifted through multiple leader IK branches or redundancy samples to generate several valid manifold entry points.
  • Generating multiple entry points increases the chance that start and goal lie in a common connected component and that a connectable pair is found rapidly.
  • The sampler supports leader-based C-space sampling and Cartesian pose sampling, with the latter also allowing an absolute orientation constraint.

F. Constraint-Aware State Interpolation

The planner connects valid states using constraint-aware predictor–corrector interpolation on the constraint manifold. It adapts step size to distance, warm-starts follower IK, and reuses intermediate states to improve search efficiency.

  • The interpolation scheme traces a curve on the constraint manifold instead of using constraint-violating linear or spline interpolation.
  • The interval is discretized into K steps proportional to state distance, keeping steps small where the parameterization is stiff.
  • At each step, a predictor advances the leader linearly and a corrector recovers the follower configuration through warm-started IK.
  • Each corrected state lies on the manifold by construction, so the resulting discretized path segment satisfies the hard constraint.
  • Intermediate states generated during motion validation are retained in the tree, densifying constraint-satisfying regions without additional IK cost.
  • Cartesian interpolation can simultaneously enforce the relative transformation constraint and a desired absolute end-effector orientation.

G. Path Planning Algorithm

The planner operates directly in the constrained bimanual C-space and then post-processes paths into smooth, executable trajectories. Experiments cover multiple bimanual platforms and simulated and physical settings.

  • Path Planning: Every tree state satisfies the transformation constraint by construction during RRT-Connect planning in the bimanual C-space.The method uses a follower-weighted metric and constraint-aware interpolation for nearest-neighbor queries and edge validation.
  • Path Post-Processing: Raw paths are shortcut by replacing subsequences with direct constraint-satisfying connections, followed by re-interpolation between remaining waypoints.
  • Path Post-Processing: Discontinuities between adjacent IK solutions are corrected by alternating which arm serves as the leader and selecting a nearby solution.
  • Trajectory Generation: TOPP-RA generates time-parameterized trajectories subject to joint velocity and acceleration limits, while trajectories failing the final constraint check are rejected.
  • Evaluation Platforms: Experiments evaluate two KUKA iiwa, two Kinova Gen3, and two UR5 arms across simulation environments and Kinova physical hardware.The KUKA setup supports direct comparison with prior work, while Kinova experiments include simulation and real-world trials.

A. Comparison Against Prior Work on the KUKA iiwa

The paper compares its constrained planner with prior methods on the KUKA iiwa benchmark and extends evaluation across platforms, environments, and transformation constraints. The benchmark measures path length and online planning time, while broader experiments assess pipeline behavior under increasing obstacle density.

  • Benchmark: The benchmark transports an object among shelves while avoiding collisions under a fixed rigid transformation between end effectors.Trials use three start-goal configurations, including Top to Middle and Middle to another configuration described in the benchmark setup.
  • Prior Work: IK-BiRRT is the most direct comparison because both methods use bidirectional RRT, whereas IK-GCS and IK-PRM require substantial offline precomputation.The reported offline construction times are approximately 5.1 hours for IK-GCS and over 44 minutes for IK-PRM.
  • Metrics: Table I reports C-space path length and online planning time, separating methods that require offline computation from fully online methods.
  • Broader Evaluation: The broader simulation study uses Kinova Gen3 and UR5 platforms across three obstacle densities and three rigid transformation constraints.The constraints represent parallel-arm transport, facing-arm pick-and-place, and elongated-object transport scenarios.
  • Baseline: A naive baseline plans only in the leader-arm C-space and imposes the follower constraint afterward, providing a comparison that does not account for the transformation during planning.The baseline is used because the directly comparable IK-BiRRT implementation is not publicly available.

2) Results:

The constrained planner substantially outperforms the naive baseline across tested environments and transformation constraints, while real-world Kinova experiments demonstrate successful constrained tray transport. Path simplification trades additional processing time for shorter paths but is optional for constraint satisfaction.

  • Failure Analysis: The planner’s advantage comes from expanding only states with valid follower configurations that are collision-free and correctly parameterized under the transformation.The baseline can fail because post-processing produces discontinuous IK solutions or follower-arm collisions.
  • Quantitative Results: The proposed method maintains substantially higher pipeline success rates than the naive baseline across environments and transformation constraints.The baseline degrades sharply with obstacle density and frequently produces zero successful trajectories in obstacle environments.
  • Metrics: Table II reports pipeline success rate, planning time, full-configuration-space path length, and end-to-end pipeline time for both platforms.
  • Real-World Validation: The Kinova Gen3 tray experiment successfully transported a tray carrying water and an upright marker while maintaining the relative transformation and sufficient trajectory smoothness.The real-world experiment also required a global orientation constraint in addition to the relative transform constraint.

2) Elongated Object Transport (T3):

The T3 experiment evaluates collaborative transport of elongated objects using a fixed relative end-effector transformation. Real-world execution transferred planned trajectories directly to hardware while maintaining the hard constraint continuously.

  • Elongated Object Transport (T3):: T3 uses a shared-orientation constraint with the end effectors offset along a specified axis for elongated-object transport.The setup targets objects that cannot be carried by a single arm because of their dimensions or weight.
  • Elongated Object Transport (T3):: The real-world experiment visualizes the transport sequence from blue to green to red.The caption directs readers to supplementary videos for the motion.
  • Elongated Object Transport (T3):: Successful execution showed that the planner generalizes to elongated-object geometries and constraint configurations relevant to warehouse and industrial manipulation.The trajectory carried the object from its initial grasped pose to a designated goal pose.
  • Elongated Object Transport (T3):: Real-world experiments executed the planned trajectories directly on physical hardware without posthoc correction while maintaining the hard constraint continuously.The planned paths were smooth enough to prevent undesirable dynamic effects during execution.
  • Elongated Object Transport (T3):: In asymmetric settings, leader selection can affect planning efficiency because the leader is sampled freely while follower collision checks require inverse kinematics.The authors recommend assigning the more cluttered arm as leader; their experiments used symmetric platforms and the right arm as leader.
  • Elongated Object Transport (T3):: Numerical inverse kinematics is the primary computational bottleneck during constraint-aware interpolation.The method retains numerical IK for generality across manipulators without closed-form solutions, while C-space discontinuity resolution remains an open robustness issue.
Loading 2608.20946v1…