This repository contains a Python implementation of a real-time trajectory smoothing algorithm based on the paper:
"Fast Smoothing of Manipulator Trajectories using Optimal Bounded-Acceleration Shortcuts", ICRA 2010.
It efficiently generates shortcut trajectories that satisfy velocity and acceleration constraints.
📌 Note: This implementation is a reproduction of the original paper. You can find the original implementation here.
The following example illustrates how the trajectory smoothing algorithm refines a raw RRT-generated path.
| Original RRT Path | Smoothed Path |
|---|---|
 |
 |
To better visualize the process of smoothing, see the GIF below:
Smoother integrates the trajectory smoothing algorithm by applying optimal bounded-acceleration shortcuts to smooth a given path. Its main idea is to continuously select two random points along the trajectory, using their positions and velocities as boundary conditions. Under the given velocity and acceleration constraints, it computes the optimal trajectory for that segment as a shortcut to replace the original path. The optimal solution for this problem is the trapezoidal velocity profile. The method computes the optimal trajectory by employing four motion modes: "P+P-", "P-P+", "P+L+P-", and "P-L-P+", as detailed in the original paper.
-
Time-Optimal Straight-Line Trajectories
This corresponds to Section IV.A of the paper. It computes a time-optimal straight-line trajectory between two states, as opposed to Univariate Time-Optimal Interpolants which may produce curves. Generating a straight-line trajectory allows it to be directly used by geometric planning algorithms that rely on straight-line steering methods. -
Time-Optimal, Bounded-Acceleration, Bounded-Velocity Interpolants
This corresponds to Section IV.B of the paper. This utilizes Univariate Time-Optimal Interpolants and Minimum-Acceleration Interpolants to compute a time-optimal trajectory under given boundary conditions with velocity and acceleration constraints in the multi-dimensional case. It first calculates the shortest time for each dimension, taking the longest of these times as the trajectory time. Then, based on this time, it calculates the minimum acceleration required for each dimension. Finally, the function checks whether the computed trajectory is collision-free and within bounds. -
Univariate Time-Optimal Interpolants
This corresponds to Section IV.C of the paper. It calculates the shortest time required for a segment given the boundary conditions and velocity/acceleration constraints. This problem always has a solution. The file also provides several examples. -
Minimum-Acceleration Interpolants
This corresponds to Section IV.D of the paper. It computes the minimum acceleration needed for a segment given the boundary conditions along with velocity and time constraints. Note that this problem may not always have a solution. An explaination has been included in the paper "Probabilistically Complete Kinodynamic Planning for Robot Manipulators with Acceleration Limits". The file similarly includes some examples. -
get motion state
This returns the motion state given a time t, whether it is within a specific trajectory segment (local) or across the entire trajectory (global). -
consistency_validatation
This validates the consistency between Univariate Time-Optimal Interpolants and Minimum-Acceleration Interpolants.
This implementation carefully handles numerical precision issues to ensure robust performance.
🌟If you find this useful, please star it—thank you! If you have any questions, feel free to open an issue.