switch humanoids14 planners to trim-final for consistency
reduce yaw penalty