My stuff. maybe a* works better? new motion primitives
Some stuff change
Seems to work