Havoc420mac/mi-task
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

更新任务3实现：引入机器人命令字典，优化步态文件加载与发送逻辑，支持用户自定义步态切换。增加异常处理以应对中断情况。

Browse Source
This commit is contained in:
Havoc 2025-05-26 02:06:04 +08:00
parent 5ab480d507
commit 938b0f8a86
1 changed files with 91 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

164
task_3/task_3.py
View File

@ -2,6 +2,8 @@ import time
import sys
import os
import toml
import copy
import math
# 添加父目录到路径，以便能够导入utils
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
@ -10,7 +12,6 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__)))
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
from base_move.turn_degree import turn_degree
from base_move.go_straight import go_straight
from file_send_lcmt import file_send_lcmt
# 创建本模块特定的日志记录器
@ -18,6 +19,18 @@ logger = get_logger("任务3")
observe = True
robot_cmd = {
'mode':0, 'gait_id':0, 'contact':0, 'life_count':0,
'vel_des':[0.0, 0.0, 0.0],
'rpy_des':[0.0, 0.0, 0.0],
'pos_des':[0.0, 0.0, 0.0],
'acc_des':[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
'ctrl_point':[0.0, 0.0, 0.0],
'foot_pose':[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
'step_height':[0.0, 0.0],
'value':0, 'duration':0
}
def load_gait_file(file_path):
"""加载步态文件"""
try:
@ -30,76 +43,81 @@ def load_gait_file(file_path):
def run_task_3(ctrl, msg):
section('任务3：步态切换', "启动")
info('开始执行任务3...', "启动")
usergait_msg = file_send_lcmt()
lcm_usergait = ctrl.lc_s
# 创建一个LCM通信实例用于发送文件
lc_file = ctrl.lc_s # 复用已有的LCM发送通道
section('任务3-1：切换到直立步态', "步态")
# 加载直立步态文件
gait_up_def = load_gait_file(os.path.join(os.path.dirname(__file__), "Gait_Def_up.toml"))
gait_up_params = load_gait_file(os.path.join(os.path.dirname(__file__), "Gait_Params_up.toml"))
if gait_up_def and gait_up_params:
# 发送步态定义文件
file_msg = file_send_lcmt()
file_msg.data = gait_up_def
lc_file.publish("Gait_Def", file_msg.encode())
time.sleep(0.5) # 等待处理
# 发送步态参数文件
file_msg.data = gait_up_params
lc_file.publish("Gait_Params", file_msg.encode())
time.sleep(0.5) # 等待处理
# 执行步态切换
msg.mode = 62 # 用户自定义步态模式
msg.gait_id = 110 # 根据Usergait_List.toml定义
ctrl.Send_cmd(msg)
# 等待步态切换完成
success("直立步态切换完成", "步态")
time.sleep(2) # 给机器人一些时间适应新步态
# 使用新步态进行简单移动
go_straight(ctrl, msg, distance=0.5, speed=0.3, observe=observe)
else:
error("直立步态文件加载失败，无法执行任务3-1", "错误")
return
section('任务3-2：切换到后退步态', "步态")
# 加载后退步态文件
gait_moonwalk_def = load_gait_file(os.path.join(os.path.dirname(__file__), "Gait_Def_moonwalk.toml"))
gait_moonwalk_params = load_gait_file(os.path.join(os.path.dirname(__file__), "Gait_Params_moonwalk.toml"))
if gait_moonwalk_def and gait_moonwalk_params:
# 发送步态定义文件
file_msg = file_send_lcmt()
file_msg.data = gait_moonwalk_def
lc_file.publish("Gait_Def", file_msg.encode())
time.sleep(0.5) # 等待处理
# 发送步态参数文件
file_msg.data = gait_moonwalk_params
lc_file.publish("Gait_Params", file_msg.encode())
time.sleep(0.5) # 等待处理
# 执行步态切换
msg.mode = 62 # 用户自定义步态模式
msg.gait_id = 110 # 根据Usergait_List.toml定义
ctrl.Send_cmd(msg)
# 等待步态切换完成
success("后退步态切换完成", "步态")
time.sleep(2) # 给机器人一些时间适应新步态
# 使用新步态进行简单移动
go_straight(ctrl, msg, distance=-0.5, speed=0.3, observe=observe)
else:
error("后退步态文件加载失败，无法执行任务3-2", "错误")
return
# 回到默认步态
section('任务3-3：恢复默认步态', "步态")
ctrl.base_msg.stand_up() # 使用基础消息模块恢复到默认站立姿态
success("任务3完成", "完成")
try:
steps = toml.load("Gait_Params_up.toml")
full_steps = {'step':[robot_cmd]}
k =0
for i in steps['step']:
cmd = copy.deepcopy(robot_cmd)
cmd['duration'] = i['duration']
if i['type'] == 'usergait':
cmd['mode'] = 11 # LOCOMOTION
cmd['gait_id'] = 110 # USERGAIT
cmd['vel_des'] = i['body_vel_des']
cmd['rpy_des'] = i['body_pos_des'][0:3]
cmd['pos_des'] = i['body_pos_des'][3:6]
cmd['foot_pose'][0:2] = i['landing_pos_des'][0:2]
cmd['foot_pose'][2:4] = i['landing_pos_des'][3:5]
cmd['foot_pose'][4:6] = i['landing_pos_des'][6:8]
cmd['ctrl_point'][0:2] = i['landing_pos_des'][9:11]
cmd['step_height'][0] = math.ceil(i['step_height'][0] * 1e3) + math.ceil(i['step_height'][1] * 1e3) * 1e3
cmd['step_height'][1] = math.ceil(i['step_height'][2] * 1e3) + math.ceil(i['step_height'][3] * 1e3) * 1e3
cmd['acc_des'] = i['weight']
cmd['value'] = i['use_mpc_traj']
cmd['contact'] = math.floor(i['landing_gain'] * 1e1)
cmd['ctrl_point'][2] = i['mu']
if k == 0:
full_steps['step'] = [cmd]
else:
full_steps['step'].append(cmd)
k=k+1
f = open("Gait_Params_up_full.toml", 'w')
f.write("# Gait Params\n")
f.writelines(toml.dumps(full_steps))
f.close()
file_obj_gait_def = open("Gait_Def_up.toml",'r')
file_obj_gait_params = open("Gait_Params_up_full.toml",'r')
usergait_msg.data = file_obj_gait_def.read()
lcm_usergait.publish("user_gait_file",usergait_msg.encode())
time.sleep(0.5)
usergait_msg.data = file_obj_gait_params.read()
lcm_usergait.publish("user_gait_file",usergait_msg.encode())
time.sleep(0.1)
file_obj_gait_def.close()
file_obj_gait_params.close()
user_gait_list = open("Usergait_List.toml",'r')
steps = toml.load(user_gait_list)
for step in steps['step']:
msg.mode = step['mode']
msg.value = step['value']
msg.contact = step['contact']
msg.gait_id = step['gait_id']
msg.duration = step['duration']
msg.life_count += 1
for i in range(3):
msg.vel_des[i] = step['vel_des'][i]
msg.rpy_des[i] = step['rpy_des'][i]
msg.pos_des[i] = step['pos_des'][i]
msg.acc_des[i] = step['acc_des'][i]
msg.acc_des[i+3] = step['acc_des'][i+3]
msg.foot_pose[i] = step['foot_pose'][i]
msg.ctrl_point[i] = step['ctrl_point'][i]
for i in range(2):
msg.step_height[i] = step['step_height'][i]
ctrl.Send_msg()
time.sleep( 0.1 )
for i in range(325): #15s Heat beat It is used to maintain the heartbeat when life count is not updated
ctrl.Send_msg()
time.sleep( 0.2 )
except KeyboardInterrupt:
msg.mode = 7 #PureDamper before KeyboardInterrupt:
msg.gait_id = 0
msg.duration = 0
msg.life_count += 1
ctrl.Send_msg()
pass