更新任务3实现：引入机器人命令字典，优化步态文件加载与发送逻辑，支持用户自定义步态切换。增加异常处理以应对中断情况。
This commit is contained in:
parent
5ab480d507
commit
938b0f8a86
158
task_3/task_3.py
158
task_3/task_3.py
@ -2,6 +2,8 @@ import time
|
|||||||
import sys
|
import sys
|
||||||
import os
|
import os
|
||||||
import toml
|
import toml
|
||||||
|
import copy
|
||||||
|
import math
|
||||||
|
|
||||||
# 添加父目录到路径，以便能够导入utils
|
# 添加父目录到路径，以便能够导入utils
|
||||||
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
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 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.turn_degree import turn_degree
|
||||||
from base_move.go_straight import go_straight
|
|
||||||
from file_send_lcmt import file_send_lcmt
|
from file_send_lcmt import file_send_lcmt
|
||||||
|
|
||||||
# 创建本模块特定的日志记录器
|
# 创建本模块特定的日志记录器
|
||||||
@ -18,6 +19,18 @@ logger = get_logger("任务3")
|
|||||||
|
|
||||||
observe = True
|
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):
|
def load_gait_file(file_path):
|
||||||
"""加载步态文件"""
|
"""加载步态文件"""
|
||||||
try:
|
try:
|
||||||
@ -30,76 +43,81 @@ def load_gait_file(file_path):
|
|||||||
def run_task_3(ctrl, msg):
|
def run_task_3(ctrl, msg):
|
||||||
section('任务3：步态切换', "启动")
|
section('任务3：步态切换', "启动")
|
||||||
info('开始执行任务3...', "启动")
|
info('开始执行任务3...', "启动")
|
||||||
|
usergait_msg = file_send_lcmt()
|
||||||
|
lcm_usergait = ctrl.lc_s
|
||||||
|
|
||||||
# 创建一个LCM通信实例用于发送文件
|
try:
|
||||||
lc_file = ctrl.lc_s # 复用已有的LCM发送通道
|
steps = toml.load("Gait_Params_up.toml")
|
||||||
|
full_steps = {'step':[robot_cmd]}
|
||||||
section('任务3-1：切换到直立步态', "步态")
|
k =0
|
||||||
# 加载直立步态文件
|
for i in steps['step']:
|
||||||
gait_up_def = load_gait_file(os.path.join(os.path.dirname(__file__), "Gait_Def_up.toml"))
|
cmd = copy.deepcopy(robot_cmd)
|
||||||
gait_up_params = load_gait_file(os.path.join(os.path.dirname(__file__), "Gait_Params_up.toml"))
|
cmd['duration'] = i['duration']
|
||||||
|
if i['type'] == 'usergait':
|
||||||
if gait_up_def and gait_up_params:
|
cmd['mode'] = 11 # LOCOMOTION
|
||||||
# 发送步态定义文件
|
cmd['gait_id'] = 110 # USERGAIT
|
||||||
file_msg = file_send_lcmt()
|
cmd['vel_des'] = i['body_vel_des']
|
||||||
file_msg.data = gait_up_def
|
cmd['rpy_des'] = i['body_pos_des'][0:3]
|
||||||
lc_file.publish("Gait_Def", file_msg.encode())
|
cmd['pos_des'] = i['body_pos_des'][3:6]
|
||||||
time.sleep(0.5) # 等待处理
|
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]
|
||||||
file_msg.data = gait_up_params
|
cmd['ctrl_point'][0:2] = i['landing_pos_des'][9:11]
|
||||||
lc_file.publish("Gait_Params", file_msg.encode())
|
cmd['step_height'][0] = math.ceil(i['step_height'][0] * 1e3) + math.ceil(i['step_height'][1] * 1e3) * 1e3
|
||||||
time.sleep(0.5) # 等待处理
|
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']
|
||||||
msg.mode = 62 # 用户自定义步态模式
|
cmd['contact'] = math.floor(i['landing_gain'] * 1e1)
|
||||||
msg.gait_id = 110 # 根据Usergait_List.toml定义
|
cmd['ctrl_point'][2] = i['mu']
|
||||||
ctrl.Send_cmd(msg)
|
if k == 0:
|
||||||
|
full_steps['step'] = [cmd]
|
||||||
# 等待步态切换完成
|
|
||||||
success("直立步态切换完成", "步态")
|
|
||||||
time.sleep(2) # 给机器人一些时间适应新步态
|
|
||||||
|
|
||||||
# 使用新步态进行简单移动
|
|
||||||
go_straight(ctrl, msg, distance=0.5, speed=0.3, observe=observe)
|
|
||||||
else:
|
else:
|
||||||
error("直立步态文件加载失败，无法执行任务3-1", "错误")
|
full_steps['step'].append(cmd)
|
||||||
return
|
k=k+1
|
||||||
|
f = open("Gait_Params_up_full.toml", 'w')
|
||||||
|
f.write("# Gait Params\n")
|
||||||
|
f.writelines(toml.dumps(full_steps))
|
||||||
|
f.close()
|
||||||
|
|
||||||
section('任务3-2：切换到后退步态', "步态")
|
file_obj_gait_def = open("Gait_Def_up.toml",'r')
|
||||||
# 加载后退步态文件
|
file_obj_gait_params = open("Gait_Params_up_full.toml",'r')
|
||||||
gait_moonwalk_def = load_gait_file(os.path.join(os.path.dirname(__file__), "Gait_Def_moonwalk.toml"))
|
usergait_msg.data = file_obj_gait_def.read()
|
||||||
gait_moonwalk_params = load_gait_file(os.path.join(os.path.dirname(__file__), "Gait_Params_moonwalk.toml"))
|
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()
|
||||||
|
|
||||||
if gait_moonwalk_def and gait_moonwalk_params:
|
user_gait_list = open("Usergait_List.toml",'r')
|
||||||
# 发送步态定义文件
|
steps = toml.load(user_gait_list)
|
||||||
file_msg = file_send_lcmt()
|
for step in steps['step']:
|
||||||
file_msg.data = gait_moonwalk_def
|
msg.mode = step['mode']
|
||||||
lc_file.publish("Gait_Def", file_msg.encode())
|
msg.value = step['value']
|
||||||
time.sleep(0.5) # 等待处理
|
msg.contact = step['contact']
|
||||||
|
msg.gait_id = step['gait_id']
|
||||||
# 发送步态参数文件
|
msg.duration = step['duration']
|
||||||
file_msg.data = gait_moonwalk_params
|
msg.life_count += 1
|
||||||
lc_file.publish("Gait_Params", file_msg.encode())
|
for i in range(3):
|
||||||
time.sleep(0.5) # 等待处理
|
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.mode = 62 # 用户自定义步态模式
|
msg.acc_des[i] = step['acc_des'][i]
|
||||||
msg.gait_id = 110 # 根据Usergait_List.toml定义
|
msg.acc_des[i+3] = step['acc_des'][i+3]
|
||||||
ctrl.Send_cmd(msg)
|
msg.foot_pose[i] = step['foot_pose'][i]
|
||||||
|
msg.ctrl_point[i] = step['ctrl_point'][i]
|
||||||
# 等待步态切换完成
|
for i in range(2):
|
||||||
success("后退步态切换完成", "步态")
|
msg.step_height[i] = step['step_height'][i]
|
||||||
time.sleep(2) # 给机器人一些时间适应新步态
|
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
|
||||||
go_straight(ctrl, msg, distance=-0.5, speed=0.3, observe=observe)
|
ctrl.Send_msg()
|
||||||
else:
|
time.sleep( 0.2 )
|
||||||
error("后退步态文件加载失败，无法执行任务3-2", "错误")
|
except KeyboardInterrupt:
|
||||||
return
|
msg.mode = 7 #PureDamper before KeyboardInterrupt:
|
||||||
|
msg.gait_id = 0
|
||||||
# 回到默认步态
|
msg.duration = 0
|
||||||
section('任务3-3：恢复默认步态', "步态")
|
msg.life_count += 1
|
||||||
ctrl.base_msg.stand_up() # 使用基础消息模块恢复到默认站立姿态
|
ctrl.Send_msg()
|
||||||
|
pass
|
||||||
success("任务3完成", "完成")
|
|
||||||
Loading…
x
Reference in New Issue
Block a user