Anytime Planning for End-Effector Trajectory Tracking

TL;DR

This work addresses end-effector trajectory tracking for redundant manipulators by proposing a guided anytime framework that bias-samples inverse kinematics toward guide paths that approximately follow the reference trajectory. The method maintains a layered graph and progressively densifies it through six stages, enabling rapid initial solutions and continuous improvement with limited computation. It formalizes the problem with a layered graph $G=(V,E)$ and provides concrete procedures for sparse and dense edge construction, guide-path search, and targeted sampling, showing compatibility with multiple IK solvers and search strategies. Across three experiments on Stampede and IKLink, the framework yields faster initial solutions and equal or better motion quality within the same time budget, and it extends semi-constrained tracking to tolerate end-effector deviations while reducing reconfigurations.

Abstract

End-effector trajectory tracking algorithms find joint motions that drive robot manipulators to track reference trajectories. In practical scenarios, anytime algorithms are preferred for their ability to quickly generate initial motions and continuously refine them over time. In this paper, we present an algorithmic framework that adapts common graph-based trajectory tracking algorithms to be anytime and enhances their efficiency and effectiveness. Our key insight is to identify guide paths that approximately track the reference trajectory and strategically bias sampling toward the guide paths. We demonstrate the effectiveness of the proposed framework by restructuring two existing graph-based trajectory tracking algorithms and evaluating the updated algorithms in three experiments.

Figures (3)

• Figure 1: Three algorithmic frameworks described and evaluated in this paper
• Figure 2: An illustration of the guided anytime algorithmic framework. In the 1st iteration, a limited set of IK solutions are sampled for specific waypoints. The IK solutions construct a graph with sparse edges (dashed lines) and a guide path (red) is identified by searching in the graph. Additional IK solutions are sampled with a bias toward the guide path. Dense edges (solid lines) are added to connect vertices in adjacent layers and an initial solution (red) is identified by searching through the dense edges. In the 2nd iteration, additional sparse edges are added to connect the newly added vertices, and a new guide path (red) is found by searching through both sparse and dense edges. More vertices are sampled, biasing toward the new guide path. After additional dense edges are added, a new solution (red) is identified in the new graph. In subsequent iterations, the algorithm continues to densify the graph and refine the solutions.
• Figure 3: Results of our three experiments, each on two testbeds. For random trajectory tracking (top row), the results are averaged over 10 randomly generated trajectories. For specific tasks (bottom row), the results are averaged over 10 repetitions of the same trajectory, with colored regions indicating the range between the maximum and minimum values within these repetitions. In the line charts, the initial point of a line encodes the average computation time to get initial solutions. Left column: our framework enabled Stampede to find initial effective solutions faster than the baselines. Middle column: IKLink with our framework converged faster than with the naïve framework. Right column: for trajectories with tolerances, our framework enabled IKLink to find motion with fewer reconfigurations than the baseline frameworks. The robot visualizations were generated using Motion Comparator wang2024motion.