PaperlayerView exact revision on arXiv

Source-linked AI summary

Safe Multi-Robot Collaborative Transport Using Density Functions

Jagannath Prasad Sahoo, Sriram S. K. S. Narayanan, Umesh Vaidya

arXiv:2610.08441v1cs.RO

TL;DR

Collaborative pushing requires coordinated forces while robots avoid collisions, and existing approaches do not provide formal execution-time collision-safety guarantees for the robots and shared object. The paper proposes hierarchical density-constrained MPC, with centralized object-level force optimization and distributed robot-level tracking. In simulation, it attained goals with positive recorded clearances, reduced completion time against matched CBF MPC, and RRT*-based tracking did not complete either task.

  • Problem

    Collaborative pushing couples object translation and rotation with collision avoidance, while prior approaches do not provide formal collision-safety guarantees for robots and the shared object during execution.

    Source: §I · p. 1

  • Method

    A centralized density-constrained object-level MPC optimizes push forces while distributed robot-level whole-body MPCs track moving contacts and enforce obstacle and inter-agent safety constraints.

    Source: §I · pp. 1–2

  • Results

    Compared with matched CBF MPC, density MPC reduced completion time by 22.5% and 6.3% in the two scenarios, while RRT*-based tracking MPC failed to complete either task.

    Source: §VI · p. 8

  • Takeaways & Limitations

    MuJoCo experiments demonstrated goal attainment with positive recorded object-footprint clearances for the evaluated collaborative pushing tasks.

    Source: §VI · p. 8

    Validation is limited to simulated box and T-shape pushing with two and three robots, fixed contacts, and planar safety envelopes.

    Source: §V · p. 8

Abstract

from arXiv · show

This paper presents a hierarchical density-based model predictive control framework for safe collaborative manipulation by multiple quadrupedal robots. The framework enables a team of robots to push a shared object to a desired pose using only the initial and goal poses, without requiring a precomputed reference trajectory. A centralized box-level MPC optimizes contact forces while enforcing a control-density constraint for goal convergence and obstacle avoidance. Each robot then solves its own distributed robot-level whole-body MPC, under a stated shared-information assumption, to track its moving contact location while accounting for static obstacles and the time-varying positions of neighboring robots. The approach is evaluated in MuJoCo using whole-body contact dynamics for two and three Unitree Go2 quadrupeds collaboratively pushing rigid objects through narrow passages. Comparisons with matched Control Barrier Function and RRT* based tracking baselines demonstrate the effectiveness of the proposed density-based formulation for push-only, force- and torque-coupled manipulation tasks. Implementation videos are available at https://jaggu2606.github.io/go2-density-mpc-pushing/

I. Introduction

Collaborative pushing couples object translation and rotation with robot–robot, object, and obstacle collision avoidance, while prior approaches do not provide formal execution-time collision-safety guarantees. The paper proposes hierarchical density-constrained MPC for goal-directed pushing and evaluates it against CBF-MPC and RRT*-based tracking baselines.

I. Introduction

  • Prior collaborative-pushing approaches demonstrate coordination but do not provide formal collision-safety guarantees for robots and the shared object during execution.Existing barrier and density methods do not address the coupled push-force and robot-motion problem described here.Source: §I · p. 1
  • The framework couples centralized object-level push-force optimization with distributed robot-level tracking of moving contact locations, without requiring a prescribed object trajectory.A discrete-time density-based constraint is integrated into both MPC layers for obstacle avoidance and inter-agent separation.Source: §I · pp. 1–2
  • MuJoCo evaluations with two and three Unitree Go2 robots compare the framework with matched CBF-MPC and RRT*-based tracking baselines.Source: §I · p. 2

A. Notation and Discrete-Time Dynamics

  • The discrete-time density analysis assumes each fixed-input state map is invertible, while the receding-horizon controllers generally generate time-varying inputs.Invertibility is required to pull a density back through the state map.Source: A. Notation and Discrete-Time Dynamics · p. 2

B. Perron–Frobenius Operator

The Perron–Frobenius operator describes how a state-space density changes under the dynamics. The paper uses this density-transport formulation alongside obstacle-specific safety functions in its predictive control conditions.

B. Perron–Frobenius Operator

  • Under an invertible differentiable state map, the Perron–Frobenius operator propagates a state-space density, with the absolute Jacobian determinant accounting for local volume changes.Source: B. Perron–Frobenius Operator · p. 2
  • In the MPC realization, the density condition is applied stage by stage at the input realized at each prediction step and re-imposed at every solve.This constrains the time-varying closed-loop trajectory rather than a hypothetical constant-input trajectory.Source: B. Perron–Frobenius Operator · p. 2

C. Safe Set

  • Obstacle-specific safety functions define individual safe sets, and object- and robot-level safety functions are constructed from their respective safety requirements.Source: C. Safe Set · p. 2

D. Control Density Functions

The paper combines goal error and obstacle avoidance in a control density, then models how bounded push forces and fixed contact offsets affect the shared box. Assigned contact locations move with the box during a maneuver.

D. Control Density Functions

  • The control density combines a goal-error measure with a nonnegative bump function that vanishes on unsafe states, yielding almost-everywhere safety and convergence under stated conditions.The framework allows exceptions only for a set of initial conditions of zero Lebesgue measure.Source: D. Control Density Functions · p. 3

E. Box and Robot Dynamics

  • Each robot applies a nonnegative, bounded push force, while the box model accounts for ground friction and force-induced translation and rotation.The torque contribution depends on each robot’s fixed lateral contact offset from the box centerline.Source: E. Box and Robot Dynamics · p. 3
  • A robot’s assigned contact location is determined by the current box state and moves with the box because its lateral offset is fixed.Source: E. Box and Robot Dynamics · p. 3

F. Problem Statement

The problem is to transport a shared object to a desired pose while maintaining object- and robot-level safety and tracking moving contact locations. The proposed hierarchical controller combines object-level contact-force regulation with robot-level tracking and avoidance, and the density construction underlies both layers.

F. Problem Statement

  • The problem specifies collaborative transport from initial states and environment and contact geometry to a desired object pose, without prescribing a trajectory.The design variables include object-level contact forces and robot-level feedback laws.Source: F. Problem Statement · p. 3
  • The controller uses object-level density MPC for contact-force regulation and robot-level density MPC for moving-contact tracking, obstacle avoidance, and inter-agent avoidance.The stated objectives also include object convergence, safe object and robot states, and admissible contact forces and robot inputs.Source: F. Problem Statement · p. 3
  • Robot feedback may depend on every teammate’s state, with the stated information assumption providing object and robot states, force references, and predicted contact targets without delay or estimation error.The architecture solves one object-level problem centrally and each robot-level problem independently; future work includes consensus-based local state estimation and robust density constraints for uncertainty.Source: F. Problem Statement · p. 3

III. Construction of Density Functions

  • The density construction represents unsafe regions with implicit functions, converts them into smooth bump functions, and combines them with goal-error functions to form a control density.Source: §III · p. 3

A. Unsafe Sets and Smooth Bump Functions

The section constructs smooth density functions from obstacle-related unsafe and sensing regions, then introduces a discrete-time safety condition for the proposed density-MPC framework.

A. Unsafe Sets and Smooth Bump Functions

B. Object-Level Density

The object-level density combines goal-directed motion with obstacle avoidance using the complete object footprint. The robot-level construction targets moving contact locations and accounts for environmental obstacles and neighboring robots’ changing safety envelopes.

B. Object-Level Density

  • Object-level obstacle geometry uses the complete object footprint, and its aggregate bump function is constructed from footprint-aware obstacle representations.Source: B. Object-Level Density · p. 4
  • The object-level density combines goal-directed motion with obstacle avoidance, with a positive weight on orientation error relative to the desired pose.Source: B. Object-Level Density · p. 4
  • Figure 2 pairs density and navigation-field panels for circular and rectangular regions; the rectangular construction approximates an axis-aligned cuboid, with larger even p giving a closer approximation.Source: Figure 2 · p. 4, B. Object-Level Density · p. 4

C. Robot-Level Multi-Agent Density

  • Robot-level pairwise safety uses a desired separation between robots, and more general pairwise functions can represent ellipsoidal, rectangular, or configuration-dependent robots.Source: C. Robot-Level Multi-Agent Density · p. 4
  • The robot-level density targets each robot’s moving contact location, while its bump function includes environmental obstacles and time-varying neighboring-robot safety envelopes updated online.For planar navigation and inter-agent collision checking, each neighboring robot is represented by a circular safety envelope.Source: C. Robot-Level Multi-Agent Density · pp. 4–5

IV. Safety Using Discrete-Time Density Constraints

The section derives a discrete-time density-safety condition from a continuous-time divergence condition and uses it stage-wise in both MPC layers. The condition is re-imposed as the controllers resolve under receding-horizon control.

IV. Safety Using Discrete-Time Density Constraints

  • The derivation connects the continuous-time divergence condition to the discrete-time condition through Euler-map and density expansions for sufficiently small sampling time.Source: §IV · p. 5
  • For an Euler-discretized system with a nonsingular one-step map, Theorem 2 gives a discrete-time density-safety condition for sufficiently small sampling time.Source: §IV · p. 5
  • The condition is equivalently expressed using the Perron–Frobenius operator as ρ(y) − [P_uρ](y) > 0, where y = F_u(x).Source: §IV · p. 5
  • The density constraint is evaluated at each prediction stage using the object-level and robot-level densities and imposed in their respective MPC problems.For robots, evaluation uses the predicted contact target and neighboring-robot safety parameters updated at time k.Source: §IV · p. 5
  • The input is held constant only over each finite one-step sampling interval; MPC re-imposes the condition at each stage and whenever the controllers re-solve.Source: §IV · p. 5

A. Object-Level Density MPC

The object-level MPC regulates contact forces under dynamics and density-safety constraints, using the density constraint rather than state tracking to enforce goal convergence. Its predicted object motion determines robot contact targets.

A. Object-Level Density MPC

B. Robot-Level Density MPC

The robot-level MPC tracks assigned moving contact targets using references that also retain nominal locomotion components. Its stage and terminal costs regulate tracking of those targets.

B. Robot-Level Density MPC

Robot-Level Density MPC:

In MuJoCo, the hierarchical controller completed the tested box- and T-shape pushing tasks, and outperformed the matched CBF and RRT* tracking baselines on the reported comparisons. The evaluation also reports computation times and bounds its claims to simulated teams, fixed contacts, and planar safety envelopes.

Robot-Level Density MPC:

  • At each sampling step, each robot updates safety parameters and solves its local MPC; in the three-robot team, only the nearest teammate is constrained, reselected each step.All teammate states remain available under the stated information assumption.Source: Robot-Level Density MPC · p. 6

V. Multi Robot Collaborative Transport

  • Tests used two Go2 robots to push a 0.5×1.2×0.5-m, 5-kg box through a 1.90-m gap and three robots to push a heavier T-shaped object through a 2.50-m gap.Evaluation covered 10 trials with initial-pose perturbations up to ±0.05 m in position and ±3° in orientation.Source: §V · p. 6
  • 10/10 trials succeeded, with completion times of 38.6 ± 0.8 s for box pushing and 28.5 ± 1.0 s for T-shape pushing.Representative trajectories recorded minimum object–obstacle clearances of 0.02 m and 0.04 m, respectively.Source: §V · p. 6, Figure 3 · p. 7, Figure 4 · p. 7
  • Object-level solves took 15.55±2.39 ms for box pushing and 16.98±2.25 ms for T-shape pushing; robot-level solves took 1.54 ± 0.26 and 1.47 ± 0.25 ms for two robots, and 1.56 ± 0.24, 1.50 ± 0.24, and 1.50 ± 0.23 ms for three.The measured times support object-level updates at 20 Hz and robot-level updates at 100 Hz.Source: §V · pp. 6–7
  • Density MPC cuts completion time vs CBF MPC by 22.5% for box pushing and 6.3% for T-shape pushing, while RRT* tracking fails both tasks.Source: Table II · p. 7, Figure 4 · p. 7, §V · p. 7
  • With a half-cycle gait offset, force CV rose from 78.1% to 84.5% and mean step-to-step force change from 13.09 to 13.89 N, while completion time changed little and no failures occurred.The reported tests indicate phase locking modestly improved force smoothness but was not essential for successful pushing in this scenario.Source: §V · p. 7
  • Validation is limited to simulated two- and three-robot teams, fixed contacts, and planar safety envelopes; larger teams, richer environments, uncertainty, and hardware execution remain future work.Source: §V · p. 8

VI. Conclusions

The paper presents hierarchical density-constrained MPC for multi-quadruped pushing without a prescribed object trajectory. Simulations show faster completion than matched CBF MPC and failure of RRT* tracking on both tasks, within a limited validation scope.

VI. Conclusions

  • The framework couples object-level contact-force optimization with robot-level tracking and safety constraints for collaborative pushing without a prescribed object trajectory.Source: §VI · p. 8
  • In the two tested scenarios, density MPC reduced completion time by 22.5% and 6.3% versus CBF MPC, while RRT* tracking failed to complete either task.Source: §VI · p. 8
  • Validation is limited to simulated two- and three-robot teams, fixed contacts, and planar safety envelopes; future work includes scalability, dynamic environments, uncertainty, and hardware validation.Source: §VI · p. 8
Loading 2610.08441v1…