changed Vector6ds to ArmVector and LegVector to work with updated Hubo_Control