This repository contains python code for hands-on numerical, gradient-free, time-optimal trajectory optimization. It implements the VP-STO stochastic trajectory optimizer. The code is intended for offline planning purposes.
Features:
- Implement your objective as a function of positions, velocities, accelerations and total duration of the trajectory, no gradient needed.
- Define initial position, initial velocity and final velocity, the solution is internally constrained to those (no extra cost term needed).
- Define velocity and acceleration limits, the solution is internally constrained to those (no extra cost term needed).
- Increase chances to avoid local minima through the exploration characteristics of VP-STO due to CMA-ES as underlying optimization technique.
- Choose to constrain the final position, e.g. the target is known but the trajectory connecting initial and final position should be collision-free (no extra cost term needed). Keep it subject to optimization if final position is implicitely described by the cost function (e.g. joint-space planning with task-space target).
- Use a low resolution for fast optimization, and a high resolution for the final solution due to the time-continuous trajectory representation.
- Linear scaling of the computational complexity with the number of DoFs: Easy-to-adopt for planning joint-space trajectories of robot arms, e.g. Franka Emika robot.
The process of optimizing for collision-free, time-optimal trajectories in cluttered environments (check out the example code for reproducing the left environment).
VP-STO used in a Model-predictive Control task for navigating in dynamic environments (check out the example code for reproducing the results).
The optimization algorithm depends on numpy, threaded and pycma. Those can be installed by
pip install numpy
pip install threaded
pip install git+https://github.com/CMA-ES/pycma.git@master
Optional: The example notebooks additionally depend on matplotlib and shapely. Those can be installed by
pip install matplotlib
pip install shapely
Clone the repository to your machine and install locally using:
git clone https://github.com/JuJankowski/vp-sto
cd vp-sto
pip install .
Note: If you want to install vp-sto in a conda environment, make sure to use the pip binary that is installed through conda.
The examples folder contains compact python notebooks with implementations for simple toy examples. They demonstrate the easy-to-use optimization interface and example implementations of typical discontinuous cost terms (e.g. collision avoidance). For a first overview, it is recommended to run the examples.