From 938b0f8a86f2979d7b506a03ecfa01ec162b2691 Mon Sep 17 00:00:00 2001 From: Havoc <2993167370@qq.com> Date: Mon, 26 May 2025 02:06:04 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E4=BB=BB=E5=8A=A13=E5=AE=9E?= =?UTF-8?q?=E7=8E=B0=EF=BC=9A=E5=BC=95=E5=85=A5=E6=9C=BA=E5=99=A8=E4=BA=BA?= =?UTF-8?q?=E5=91=BD=E4=BB=A4=E5=AD=97=E5=85=B8=EF=BC=8C=E4=BC=98=E5=8C=96?= =?UTF-8?q?=E6=AD=A5=E6=80=81=E6=96=87=E4=BB=B6=E5=8A=A0=E8=BD=BD=E4=B8=8E?= =?UTF-8?q?=E5=8F=91=E9=80=81=E9=80=BB=E8=BE=91=EF=BC=8C=E6=94=AF=E6=8C=81?= =?UTF-8?q?=E7=94=A8=E6=88=B7=E8=87=AA=E5=AE=9A=E4=B9=89=E6=AD=A5=E6=80=81?= =?UTF-8?q?=E5=88=87=E6=8D=A2=E3=80=82=E5=A2=9E=E5=8A=A0=E5=BC=82=E5=B8=B8?= =?UTF-8?q?=E5=A4=84=E7=90=86=E4=BB=A5=E5=BA=94=E5=AF=B9=E4=B8=AD=E6=96=AD?= =?UTF-8?q?=E6=83=85=E5=86=B5=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- task_3/task_3.py | 164 ++++++++++++++++++++++++++--------------------- 1 file changed, 91 insertions(+), 73 deletions(-) diff --git a/task_3/task_3.py b/task_3/task_3.py index bca6f7f..a531748 100644 --- a/task_3/task_3.py +++ b/task_3/task_3.py @@ -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完成", "完成") \ No newline at end of file + 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 \ No newline at end of file