Constrained Nonlinear Kaczmarz Projection on Intersections of Manifolds for Coordinated Multi-Robot Mobile Manipulation

TL;DR

This paper introduces a novel constrained nonlinear Kaczmarz projection method for solving multiple simultaneous constraints modeled as manifolds, enabling coordinated multi-robot manipulation in complex environments.

Contribution

It presents the constrained nonlinear Kaczmarz (cNKZ) technique for efficiently solving numerous manifold constraints in multi-robot systems, improving over existing methods.

Findings

cNKZ outperforms baseline approaches, solving up to 80 constraints simultaneously.

Achieves up to 92% success rate in cluttered environments.

Successfully demonstrated on hardware with three mobile robots.

Abstract

Cooperative manipulation tasks impose various structure-, task-, and robot-specific constraints on mobile manipulators. However, current methods struggle to model and solve these myriad constraints simultaneously. We propose a twofold solution: first, we model constraints as a family of manifolds amenable to simultaneous solving. Second, we introduce the constrained nonlinear Kaczmarz (cNKZ) projection technique to produce constraint-satisfying solutions. Experiments show that cNKZ dramatically outperforms baseline approaches, which cannot find solutions at all. We integrate cNKZ with a sampling-based motion planning algorithm to generate complex, coordinated motions for 3 to 6 mobile manipulators (18--36 DoF), with cNKZ solving up to 80 nonlinear constraints simultaneously and achieving up to a 92% success rate in cluttered environments. We also demonstrate our approach on hardware using three Turtlebot3 Waffle Pi robots with OpenMANIPULATOR-X arms.