PaperlayerView exact revision on arXiv

Source-linked AI summary

A General, Fast, and Robust Implementation of the Time-Optimal Path Parameterization Algorithm

Quang-Cuong Pham

arXiv:1312.6533v2cs.RO

TL;DR

TOPP lacks general and efficient trajectory-planning solutions under kinodynamic constraints, despite the importance of time-optimal parameterization. This paper completes the treatment of dynamic singularities, implements the numerical-integration algorithm openly in C++/Python, and reports robust, fast performance, including roughly an order-of-magnitude advantage over the fastest available convex-optimization implementation.

  • Problem

    General and efficient solutions for time-optimal trajectory planning under kinodynamic constraints remain limited, while dynamic singularities are a main cause of failure in existing TOPP implementations.

  • Method

    The paper completely characterizes and treats dynamic singularities, then provides an open-source C++/Python implementation based on the theoretically faster numerical-integration approach.

  • Results

    The implementation is reported as robust and fast, running about one order of magnitude faster than the fastest currently available implementation of the convex-optimization approach.

  • Takeaways & Limitations

    The open-source implementation is designed to facilitate integration of new system dynamics and constraints for robotics researchers using kinodynamic motion planning.

Abstract

from arXiv · show

Finding the Time-Optimal Parameterization of a given Path (TOPP) subject to kinodynamic constraints is an essential component in many robotic theories and applications. The objective of this article is to provide a general, fast and robust implementation of this component. For this, we give a complete solution to the issue of dynamic singularities, which are the main cause of failure in existing implementations. We then present an open-source implementation of the algorithm in C++/Python and demonstrate its robustness and speed in various robotics settings.

I. INTRODUCTION

TOPP time-parameterizes a fixed geometric path under kinodynamic constraints, supporting direct tracking and decomposition-based motion planning. The article develops a general implementation using numerical integration and addresses dynamic singularities that undermine existing implementations.

  • Motivation: TOPP is useful for predefined-path tasks and for decomposing trajectory planning into path generation followed by optimal time-parameterization.The paper cites painting and welding as examples of predefined-path applications.
  • Existing approaches: Three method families are described: dynamic programming, convex optimization, and numerical integration based on successive maximum- and minimum-acceleration integrations.Numerical integration exploits the bang-bang structure and is theoretically faster than the other two families.
  • Motivation: TOPP minimizes traversal time by choosing a time law for a given geometric path under kinodynamic constraints.Kinodynamic constraints include bounds involving higher-order time derivatives, such as velocity, acceleration, torque, balance, and friction.
  • Contribution: The article targets a general, fast, and robust TOPP implementation using numerical integration while addressing dynamic singularities.The authors identify these singularities as a major source of failure in existing numerical-integration implementations.
  • General formulation: The path is represented as a piecewise-smooth configuration-space curve, and optimization selects an increasing scalar reparameterization s(t).The resulting trajectory is q(s(t)).
  • General formulation: The formulation reduces implementation inputs to vectors a, b, and c evaluated along the path, without requiring full matrices or tensors.For torque bounds, inverse dynamics can compute the needed products directly.

B. Review of the numerical integration approach

The numerical integration approach represents feasible motion through acceleration fields in the (s, ˙s) plane and constructs bang-bang profiles constrained by the maximum velocity curve. Its main difficulty is correctly identifying and treating dynamic singularities, which are distinct from ordinary zero-inertia points.

  • Numerical integration: Each constraint yields acceleration bounds α(s, ˙s) and β(s, ˙s), while feasible velocity profiles must remain below the maximum velocity curve.The α and β fields correspond to minimum- and maximum-acceleration profiles.
  • Numerical integration: The time-minimal profile alternates between β and α fields while staying below the MVC, using forward and backward integrations joined at switch points.The construction starts from the beginning and ending velocities and concatenates intersecting profiles.
  • Switch points: α→β switch points occur when the MVC is discontinuous, continuous but nondifferentiable, or differentiable with a tangent aligned to the α or β field.These are respectively discontinuous, singular, and tangent switch points.
  • Dynamic singularities: Zero-inertia points are not all dynamic singularities, because only some produce an undifferentiable MVC.Earlier practice of equating every zero-inertia point with a singular switch point is therefore incorrect.
  • Dynamic singularities: Dynamic singularities occur frequently in real-world TOPP instances and cause most failures of existing implementations because α and β diverge near them.They are reported as more frequent than discontinuous and tangent switch points together.

C. Characterizing dynamic singularities

The singularity characterization distinguishes whether a zero-inertia constraint actually makes the MVC nondifferentiable. The test compares the MVC with and without the triggering constraint after checking the sign of its coefficient b_k.

  • Characterization: For a zero-inertia point triggered by constraint k, the analysis assumes a_k changes from negative to positive across s* and examines b_k(s*).The opposite sign transition can be handled similarly by changing signs.
  • Characterization: If b_k(s*) > 0, the candidate velocity s˙* is compared with s˙†, the MVC value obtained after removing constraint k.The proposed procedure computes both values at s*.
  • Characterization: When s˙† < s˙*, constraint k does not trigger a dynamic singularity.When s˙† > s˙*, the MVC is undifferentiable and s* is a dynamic singularity.
  • Characterization: If b_k(s*) < 0, constraint k does not trigger a dynamic singularity.This is one branch of the proposed classification.

1) Previous treatments:

Previous treatments did not provide a principled, robust initialization at dynamic singularities, where α and β diverge and small acceleration errors can destabilize integration. The figure distinguishes singular from nonsingular zero-inertia points and illustrates the problematic profile behavior.

  • Previous treatments:: At dynamic singularities, α and β are not naturally defined because the triggering coefficient a_k(s*) equals zero.Existing treatments either omitted initialization guidance or proposed selecting an arbitrary acceleration.
  • Previous treatments:: Fig. 2 compares α- and β-profiles near zero-inertia points, showing one singular switch point and one nonsingular point.The red β-profile has a higher slope than the green α-profile except on the MVC, where their slopes are equal.
  • Previous treatments:: The proposed α− definition is arbitrary because its limiting value depends on the approach direction near the singularity.The α-profiles diverge when approaching the singular switch point from the left, preventing a continuous extension there.
  • Previous treatments:: Incorrect α and β values at a singularity can produce strong oscillations and cause the algorithm to terminate incorrectly.The velocity profile may cross the MVC or the line ˙s = 0; this is identified as a main failure reason in existing implementations.

2) Proposed new treatment:

The new treatment determines whether a zero-inertia point is dynamically singular, computes the appropriate neutral-line slope when it is, and initializes integration along that constant field. After a small number of steps, integration resumes with the smooth α and β fields.

  • Proposed new treatment:: At a dynamic singularity, the Taylor-expanded constraint equations yield the same λ value from the left and right sides.This establishes a consistent slope for initializing both backward and forward integrations.
  • Proposed new treatment:: The optimal backward and forward acceleration at the singular point is defined by equation (10), whose λ-slope forms a neutral line through the switch point.Fig. 2A illustrates this dotted neutral line at (s*, ˙s*).
  • Proposed new treatment:: The algorithm classifies a zero-inertia point using b_k(s*) and, when b_k(s*) > 0, compares the MVC velocity with the MVC after removing constraint k.If ˙s* > ˙s†, the constraint does not trigger a singularity; if ˙s* < ˙s†, the point is dynamically singular.
  • Proposed new treatment:: The algorithm integrates the constant field (˙s*, λ˙s*) backward and forward for a small number of time steps before continuing with α and β.After moving away from s*, the α and β fields become smooth, so numerical integration proceeds without this singularity problem.

3) About Kunz and Stilman’s conjecture:

The paper proves Kunz and Stilman’s conjecture for dynamic singularities in the circular-path setting and evaluates the resulting implementation across robot systems and constraint types.

  • 3) About Kunz and Stilman’s conjecture:: At a zero-inertia point that is also a dynamic singularity, the paper derives λ = 0, proving Kunz and Stilman’s conjecture.The derivation uses the correspondence between the parameters in and the present formulation.
  • A. Open-source implementation: The implementation supports velocity, acceleration, torque, ZMP, multi-contact friction constraints, and combinations thereof through C++ and Python interfaces.OpenRAVE handles dynamics computations, and the formulation is designed to accommodate additional dynamics and constraints.
  • A. Open-source implementation: In the HRP2 multi-contact task, TOPP handled 131 inequality constraints for a 1 s trajectory with grid size N = 100.The constraints included joint velocity and torque bounds plus friction-cone constraints on the left foot and right hand.
  • A. Open-source implementation: The HRP2 profile followed α- and β-profiles in some regions and slid along direct velocity limits in others, while foot and hand normal-force constraints saturated at different times.The figure combines torque- and friction-imposed velocity limits with direct velocity constraints and reaction-force profiles.
  • B. Comparison with previous treatments of dynamic singularities: Across 1000 random trajectories in each of two 7-dof settings, Table II reports implementation failures when profiles do not cover the whole segment.The supplied passage specifies the experimental settings and failure definition but not the table’s numerical failure counts.

C. Comparison with the convex optimization approach

The paper compares its numerical-integration implementation with convex-optimization implementations using both informal literature results and a controlled C++ benchmark. The controlled comparison reports a substantial speed advantage while noting a competing pruning optimization in MINTOS.

  • C. Comparison with the convex optimization approach: The informal torque-constraint comparison contrasts the C++ implementation with MATLAB-based implementations whose convex solvers rely on external precompiled packages.The reported comparison is explicitly described as informal because the software stacks differ.
  • C. Comparison with the convex optimization approach: The formal benchmark used the same computer and programming language to compare the implementation with MINTOS on 1000 random 7-dimensional trajectories.The trajectories used velocity and acceleration bounds matching the earlier experiment, while robot dynamics computation times were excluded.
  • C. Comparison with the convex optimization approach: 7 to 15 times faster than MINTOS was the reported average total execution-time advantage of the proposed implementation.The comparison covered 1000 random trajectories under velocity and acceleration bounds.
  • C. Comparison with the convex optimization approach: MINTOS includes a constraints-pruning step that significantly speeds execution relative to earlier convex-optimization implementations.The authors state that they were investigating how to integrate this idea into numerical integration.
  • IV. CONCLUSION: The conclusion characterizes the open-source implementation as robust and about one order of magnitude faster than the fastest currently available convex-optimization implementation.Its formulation is intended to facilitate integration of new system dynamics and constraints.

A. Characterizing dynamic singularities

The section characterizes when a zero-inertia point becomes a dynamic singularity by analyzing the signs of constraint coefficients and the relation between modified acceleration bounds. It shows that some zero-inertia points are regular, while others make the MVC undifferentiable.

  • If c_k(s*) > 0, the MVC becomes zero immediately to the left of s*, so the path is not traversable.This follows because α(s, 0) diverges while β remains bounded near the point.
  • When c_k(s*) < 0 and b_k(s*) > 0, the smallest velocity ṡ† satisfying α̃(s*, ṡ) = β̃(s*, ṡ) separates two cases.The construction sets ṡ† = +∞ when no such velocity exists.
  • If ṡ† < ṡ*, the MVC is determined by α̃ and β̃ near (s*, ṡ†), remains continuous and differentiable, and constraint k causes no singularity.The constraint does not contribute to α or β in the relevant neighborhoods and velocity range.
  • If ṡ† > ṡ*, the MVC remains continuous but is generally undifferentiable at s*, making s* a dynamic singularity.On opposite sides of s*, the MVC follows different limiting profiles, producing the nondifferentiability.
  • For c_k(s*) < 0 and b_k(s*) < 0, constraint k does not trigger a dynamic singularity at s*.The constraint contributes to α on the left and β on the right, while modified bounds remain continuous and differentiable nearby.
Loading 1312.6533v2…