zhaodong1986 / large_scale_traj_optimizer

Fast (Fastest?) Minimum Snap Trajectory Generation with Analytical Gradient

Large-Scale Trajectory Optimizer

This is probably the fastest minimum jerk or minimum snap trajectory generator you can find.

It also provides analytical gradient of energy with respect to time allocations and waypoints.

1. About

This is a highly-algorithmically-optimized implementation for minimum jerk/snap trajectories with exact gradient w.r.t. time allocation and waypoints. All involved computations enjoy linear time and space complexity. It is based on completely analytical results of our paper. Only two header files are all you need to use our module as a super efficient differentiable black box.

ICRA Paper: Generating Large-Scale Trajectories Efficiently using Double Descriptions of Polynomials

(Video and Arxiv are avaiable.)

If this repo helps you, please cite our paper:

@inproceedings{wang2021glst,
    title={Generating Large-Scale Trajectories Efficiently using Double Descriptions of Polynomials},
    author={Wang, Zhepei and Ye, Hongkai and Xu, Chao and Gao, Fei},
    address={Xi'an, China},
    booktitle={IEEE International Conference on Robotics and Automation},
    pages={7436-7442},
    year={2021},
    organization={IEEE}
}

Author: Zhepei Wang and Fei Gao from the ZJU Fast Lab.

2. How to Use

a. Initialize an instance of "JerkOpt/SnapOpt"

b. Call "reset" to specify the piece number and initial/terminal conditions. The i-th column of "headState/tailState" is a 3-dimensional specified (i-1)-order derivative. For example, the initial/final position is always stored in the first column.

c. Call "generate" to compute a minimum jerk/snap trajectory that passes intermediate waypoints with specified time allocations. The i-th column of "inPs" is a 3-dimensional vector of the i-th intermediate waypoint. For example, there are 4 columns of waypoints and 5 time durations for a 5-piece trajectory.

d. Call "getObjective" to get energy cost.

e. Call "getGradT" and "getGradInnerP" to obtain gradients for the specified time allocation and intermediate waypoints.

f. If needed, repeat c, d, and e to optimize time allocation and intermediate waypoints using the objective and the gradient.

g. Call "getTraj" to obtain the trajectory.

Note: Part c must be conducted after a and b. Part d, e, f and g must be conducted after c.

3. Examples

Example 1 gives the computation speed of our implementation. Please use catkin_make of ROS to build the source code of this example.

Example 2 is only described in the the related paper as a high-performance large-scale trajectory optimizer. By following the paper, you can use our differentiable black box to directly optimize both the waypoints and times of a trajectory. It can achieve almost the same trajectory quality as the global trajectory optimizer in Teach-Repeat-Replan while using significantly less computation time.

4. Performance

We compare our original implementation with four existing works. The original performance is shown as follows. The performance of the up-to-date master branch is SIGNIFICANTLY HIGHER than that is shown in the figure.

_{Burke et al., “Generating minimum-snap quadrotor trajectories really fast,” IROS 2020.
Bry et al., “Aggressive flight of fixed-wing and quadrotor aircraft in dense indoor environments,” IJRR 2015.
Mellinger et al., “Minimum snap trajectory generation and control for quadrotors,” ICRA 2011.}