Fix bug introduced in f907f463af28b8896869a712f076a48938fc512c

The bug caused the planning example to not find a path because the collision shapes were offset.