Robot Home motion does not avoid collisions

[shaun-edwards]

The robot home motion does not avoid collisions with the CNC collisions models. When the robot is stopped inside of the CNC and the state machine is restarted, the robot homes itself. The homing motion moves right through the CNC. Steps to reproduce:

1. Using a real robot execute the demo until the robot is inside the CNC
2. Hit control C to bring down the demo.
3. Bring the demo back up.
4. The robot will try to home as the first motion.