Contact-Aware Motion Planning Among Movable Objects

TL;DR

The paper addresses the limitations of collision-avoidance-only planning by enabling intentional contact with movable objects. It introduces CAMP, a framework that encodes robot-object contact as complementarity constraints within an optimization-based trajectory planning problem and solves it with the augmented Lagrangian method. The approach combines a front-end multi-agent path searching (NAMO/RAMO) with a back-end trajectory optimization that yields feasible, energy-efficient, spatial-temporal trajectories. Results from simulations and real-world experiments show CAMP expands the reachable space, improves task success rates, and adapts to varying task objectives, with code released to the community.

Abstract

Most existing methods for motion planning of mobile robots involve generating collision-free trajectories. However, these methods focusing solely on contact avoidance may limit the robots' locomotion and can not be applied to tasks where contact is inevitable or intentional. To address these issues, we propose a novel contact-aware motion planning (CAMP) paradigm for robotic systems. Our approach incorporates contact between robots and movable objects as complementarity constraints in optimization-based trajectory planning. By leveraging augmented Lagrangian methods (ALMs), we efficiently solve the optimization problem with complementarity constraints, producing spatial-temporal optimal trajectories of the robots. Simulations demonstrate that, compared to the state-of-the-art method, our proposed CAMP method expands the reachable space of mobile robots, resulting in a significant improvement in the success rate of two types of fundamental tasks: navigation among movable objects (NAMO) and rearrangement of movable objects (RAMO). Real-world experiments show that the trajectories generated by our proposed method are feasible and quickly deployed in different tasks.

Figures (6)

• Figure 1: Compare to contact-avoidance method, contact-aware motion planning allows a mobile robot to push movable objects actively in crowded environments.
• Figure 2: The overview of the CAMP framework. The figure illustrates the workflow of CAMP. The perception module of the robot provides scene understanding of the CAMP framework. In the front-end of CAMP, the appropriate searching method is selected based on specific tasks. The method calculates a sequence of states and appropriate time allocations. The back-end optimization, using the ALM as the backbone, first solves an unconstrained optimization problem and then computes energy-optimal continuous polynomials based on the local optimal decision variables. The dual variables and penalty parameters are updated based on violating feasibility constraints. These steps are repeated until feasible and optimal trajectories are obtained.
• Figure 3: The figure illustrates front-end computations in CAMP for NAMO and RAMO tasks, showing the search method finding feasible paths for the robot in the grid map and segmented path finding for RAMO. Red denotes the robot's start and target points, while green represents RAMO's start and target points. Light red cells indicate the robot's feasible path and light yellow cells represent feasible paths for movable objects.
• Figure 4: The figure of the feasibility constraints. The two left subplots show the complementarity constraints for non-penetration between the robot and objects. The two right subplots illustrate the constraints on the contact force or velocity direction for two simple geometric contours in RAMO tasks.
• Figure 5: The omnidirectional mobile robot used in our real world experiments.

Theorems & Definitions (2)

• Remark 1
• Remark 2