Implement yaw rotation during static phase + Try to fix issue when returning to trot then switching back to static again