change how we handle motion primitives, add snap to joint primitive, now planning