import time

def run_task_5(ctrl, msg):
    # DEBUG
    # msg.mode = 11  # 运动模式
    # msg.gait_id = 26
    # msg.vel_des = [0, 0, 2]  # 期望速度
    # msg.duration = 0  # 零时长表示持续运动，直到接收到新命令
    # msg.step_height = [0.02, 0.02]  # 持续运动时摆动腿的离地高度
    # msg.life_count += 1
    # ctrl.Send_cmd(msg)
    # time.sleep(1.1)  # 持续5秒钟

    # msg.mode = 11
    # msg.gait_id = 3
    # msg.vel_des = [1, 0, 0]
    # msg.duration = 1000
    # msg.step_height = [0.03, 0.03]
    # msg.life_count += 1
    # print(msg.pos_des)
    # 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.25]  # 恢复到正常站立高度
    msg.life_count += 1
    ctrl.Send_cmd(msg)
    ctrl.Wait_finish(3, msg.gait_id)