Model Predictive Trajectory Optimization With Dynamically Changing Waypoints for Serial Manipulators

TL;DR

This paper tackles online replanning for serial manipulators with dynamically changing waypoints by introducing waypoint MPC (wMPC), which splits the receding horizon at a waypoint once it becomes reachable, thereby reducing planning horizons toward subsequent goals. The method formulates a cost-to-go toward the waypoint and a constraint-driven horizon split, avoiding the need for a global reference trajectory and incorporating collision avoidance within the same optimization. Simulation and real-world experiments on a KUKA LBR iiwa 14 R820 demonstrate that wMPC achieves competitive path lengths and trajectory durations relative to offline RRT-based planners while enabling fast online replanning and reactive waypoint tracking. The work extends to multiple waypoints and shows robust performance under dynamic changes, highlighting practical applicability for task-planning coupled manipulation in changing environments.

Abstract

Systematically including dynamically changing waypoints as desired discrete actions, for instance, resulting from superordinate task planning, has been challenging for online model predictive trajectory optimization with short planning horizons. This paper presents a novel waypoint model predictive control (wMPC) concept for online replanning tasks. The main idea is to split the planning horizon at the waypoint when it becomes reachable within the current planning horizon and reduce the horizon length towards the waypoints and goal points. This approach keeps the computational load low and provides flexibility in adapting to changing conditions in real time. The presented approach achieves competitive path lengths and trajectory durations compared to (global) offline RRT-type planners in a multi-waypoint scenario. Moreover, the ability of wMPC to dynamically replan tasks online is experimentally demonstrated on a KUKA LBR iiwa 14 R820 robot in a dynamic pick-and-place scenario.

Figures (8)

• Figure 1: The proposed wMPC planner first plans towards the waypoint $\boldsymbol{\mathbf{q}}_{\mathrm{w}}$, avoiding the obstacle $\mathcal{O}$. The planner splits the horizon at $k = N_s$ as soon as the waypoint $\boldsymbol{\mathbf{q}}_{\mathrm{w}}$ is reachable within a tolerance $\varepsilon$. Then, the waypoint is constrained by the planner with $\boldsymbol{\mathbf{q}}_{N_s - 1 | n} \in \mathcal{Q}_{\mathrm{w}}$ to be within the tolerance band around the waypoint $\boldsymbol{\mathbf{q}}_{\mathrm{w}}$, and the remaining samples are used to optimize towards the goal point $\boldsymbol{\mathbf{q}}_{\mathrm{g}}$.
• Figure 2: This figure illustrates when a goal point $\boldsymbol{\mathbf{q}}_{\mathrm{g}}$ counts as reachable within the horizon for $m = 2$. First, if all components $j = 0, \dots, m - 1$ of a point $\boldsymbol{\mathbf{q}}_{i}$ are within the tolerance band $\varepsilon$, then the goal is reachable. In this example, this is only the case for the second component $q_{i - 1, 1}$ and $q_{i, 1}$. However, it is evident for the first component that the connecting line between $q_{i - 1, 0}$ and $q_{i, 0}$ goes through the tolerance band.
• Figure 3: Sequential manipulation task in MuJoCo Todorov2012: The robot starts from an initial configuration in (a) and then moves through a sequence of waypoints to open the cabinet door in (b). Afterwards, the robot must avoid the cylindrical obstacle while approaching and grasping the object in (c). Finally, the robot places the object into the cabinet in (d).
• Figure 4: Robotic grasping scenario with waypoints and dynamic replanning: The robot grasps the cylinder in (a) after passing through a waypoint above it. The cup is approached in (b) through a waypoint to align the approach direction. After moving the cup, the robot adjusts the waypoint and the goal for the new cup position (c) and places the object in (d).
• Figure 5: Cartesian end-effector trajectory for the dynamic replanning experiment: The robot starts at , moves towards the cylinder through a waypoint at , and grasps the cylinder at . Afterward, the robot moves back through and attempts to put the cylinder in the cup at , moving to the appropriate waypoint. However, the cup is moved, and the robot adjusts the trajectory to move through a waypoint at and places the cylinder in the cup at . Finally, the robot returns to the initial pose at .

Theorems & Definitions (3)

• Remark 1
• Remark 2
• Remark 3