feat(task_5): 添加前进运动和姿态控制

- 设置俯身姿态并开始前进运动
- 更新运动参数，包括速度、步态和步高
- 恢复到正常站立高度

task_5/task_5.py

@@ -21,9 +21,31 @@ def run_task_5(ctrl, msg):
     # ctrl.Send_cmd(msg)
     # ctrl.Wait_finish(11, msg.gait_id)
 
+    # 设置俯身姿态
+    msg.mode = 3  # 姿态控制模式
+    msg.gait_id = 0
+    msg.pos_des = [0, 0, 0.15]  # 设置较低的姿态高度
+    msg.life_count += 1
+    ctrl.Send_cmd(msg)
+    ctrl.Wait_finish(3, msg.gait_id)
+    
+    # 等待姿态稳定
+    time.sleep(1.0)
+    
+    # 开始前进运动
+    msg.mode = 11  # 运动控制模式
+    msg.gait_id = 3  # 使用 trot 步态
+    msg.vel_des = [0.5, 0, 0]  # 设置前进速度，x方向0.5m/s
+    msg.duration = 2000  # 运动持续2秒
+    msg.step_height = [0.03, 0.03]  # 设置步高
+    msg.life_count += 1
+    ctrl.Send_cmd(msg)
+    ctrl.Wait_finish(11, msg.gait_id)
+    
+    # 恢复站立姿态
     msg.mode = 3
     msg.gait_id = 0
-    msg.pos_des = [0, 0, 0.15]
+    msg.pos_des = [0, 0, 0.25]  # 恢复到正常站立高度
     msg.life_count += 1
     ctrl.Send_cmd(msg)
     ctrl.Wait_finish(3, msg.gait_id)