Added gripper control and staged head control - fixed edit_mode selector