Abstract
Navigating doorways is a fundamental capability for mobile manipulators operating in human environments, requiring coordinated motion between the mobile base and manipulator arm. This paper presents a motion planning framework that generates dynamically feasible and collision-free trajectories for autonomously opening and traversing both push and pull doors. The proposed method formulates the robot and door as a coupled dynamical system within a nonlinear Model Predictive Path Integral (MPPI) optimization framework. Manipulation feasibility is enforced through a penalty-based constraint, avoiding explicit arm kinematic modeling in the planner. Simulations and a hardware experiment demonstrate that the approach successfully plans feasible trajectories for door traversal.
Unified Planning with Nonlinear MPC
Our core contribution is a nonlinear model predictive path integrator (MPPI) formulation that plans for the mobile base and the door simultaneously, treating them as a single coupled dynamical system. This unified approach produces smooth, dynamically consistent motions. By encoding a geometric manipulability metric into the cost function, our planner remains generalizable across different robot arms without needing to model their specific kinematics during high-level planning. Below are examples of the optimized trajectories generated by our planner for both push and pull-style doors.
Planned trajectory for a push-style door.
Planned trajectory for a pull-style door.
Validation in High-Fidelity Simulation
We validated our approach in NVIDIA Isaac Sim using a mobile manipulator composed of a Husky base and a UR10 arm. The robot successfully uses the pre-planned paths to autonomously traverse both push-type and pull-type doors, demonstrating the effectiveness of the generated trajectories and the tracking controller.
Simulation of traversing a push-type door.
Simulation of traversing a pull-type door.
Comparison to Search-Based Methods
Compared to traditional decoupled, search-based algorithms, our optimization-based planner generates significantly shorter and smoother paths. For a push-type door, our planner found a path that was 11.6% shorter. [ Jang et al.] The improvement was even more pronounced for the pull-type door, where our path was 22.6% shorter[ Jang et al.]. This demonstrates a favorable trade-off between planning speed and path quality, achieving near-real-time performance.
Path comparison for a push-style door (Our MPC vs. Search-Based).
Path comparison for a pull-style door (Our MPC vs. Search-Based).
Robotic Platforms
The generalizability of our kinematics-free planner was demonstrated across two different mobile manipulator platforms.
Simulation Platform: Husky A100 base with a 6-DOF UR10 robotic arm.
Hardware Platform: Commercial dual-arm robot with two 6-DOF arms.