returning to initial disabled state at the end of the prepare_to_tuck() (head move) function had opposite logic, causing the robot to disable if it was already at untucked pose to start.
Also fixed, I had accidentally passed a reference instead of a copy to the "flipped" state for suppressing collision, which would cause the collision suppression to stop as soon as the arm was over the top (but still in the head force field).
returning to initial disabled state at the end of the prepare_to_tuck() (head move) function had opposite logic, causing the robot to disable if it was already at untucked pose to start.
Also fixed, I had accidentally passed a reference instead of a copy to the "flipped" state for suppressing collision, which would cause the collision suppression to stop as soon as the arm was over the top (but still in the head force field).