Extract motion primitives from optimization

[whoenig]

Related to #29: if we iteratively update delta, we can also extract valid motion primitives from the optimization. This will likely only have a minor impact on performance, though.