Fixed shoulder, added shoulder control, changed gripper mode stuff.