Fix race condition on creating robot physics body
Correct accessors for position/heading of Robots
Use a shared physics lock

This prevents a nasty race condition with creating constraints for
grabbed tokens while running the physics step.