From 7b106c03dc6e7c4d8955c9f4e8d05575d1d351f0 Mon Sep 17 00:00:00 2001 From: havoc420ubuntu <2993167370@qq.com> Date: Mon, 26 May 2025 06:11:09 +0000 Subject: [PATCH] refactor(task_3): update gait processing and enhance control logic - Refactored gait processing in main.py to load parameters from Gait_Params_up.toml and publish user gait files. - Improved control logic in task_3.py to monitor Z-axis speed and detect climbing phases. - Added functionality for straight movement after task completion. - Cleaned up commented-out code and ensured consistent message handling. --- main.py | 22 +++---- task_3/main.py | 72 ++++++++++++++------- task_3/task_3.py | 160 +++++++++++++++++++++++++++++++++++++---------- 3 files changed, 190 insertions(+), 64 deletions(-) diff --git a/main.py b/main.py index 02a0acf..06e3b66 100644 --- a/main.py +++ b/main.py @@ -107,18 +107,18 @@ class Robot_Ctrl(object): def msg_handler_o(self, channel, data): self.odo_msg = localization_lcmt().decode(data) # 如果尚未校准，记录第一帧数据作为校准基准 - if not self.is_calibrated: - self.calibration_offset = self.odo_msg.xyz - self.is_calibrated = True - print(f"校准完成，基准值: {self.calibration_offset}") + # if not self.is_calibrated: + # self.calibration_offset = self.odo_msg.xyz + # self.is_calibrated = True + # print(f"校准完成，基准值: {self.calibration_offset}") # # 将接收到的 odo 数据减去校准基准值 - self.odo_msg.xyz = [ - self.odo_msg.xyz[0] - self.calibration_offset[0], - self.odo_msg.xyz[1] - self.calibration_offset[1], - self.odo_msg.xyz[2] - self.calibration_offset[2] - ] - # if self.odo_msg.timestamp % 50 == 0: - # print(self.odo_msg.rpy) + # self.odo_msg.xyz = [ + # self.odo_msg.xyz[0] - self.calibration_offset[0], + # self.odo_msg.xyz[1] - self.calibration_offset[1], + # self.odo_msg.xyz[2] - self.calibration_offset[2] + # ] + # if self.odo_msg.timestamp % 100 == 0: + # print(self.odo_msg.xyz, self.odo_msg.rpy, self.odo_msg.vxyz, self.odo_msg.omegaBody, self.odo_msg.vBody) def odo_reset(self): self.calibration_offset = self.odo_msg.xyz diff --git a/task_3/main.py b/task_3/main.py index 16e0d4c..fe251df 100755 --- a/task_3/main.py +++ b/task_3/main.py @@ -29,29 +29,59 @@ robot_cmd = { def main(): lcm_cmd = lcm.LCM("udpm://239.255.76.67:7671?ttl=255") + lcm_usergait = lcm.LCM("udpm://239.255.76.67:7671?ttl=255") + usergait_msg = file_send_lcmt() cmd_msg = robot_control_cmd_lcmt() try: - user_gait_list = open("Usergait_List.toml",'r') - steps = toml.load(user_gait_list) - for step in steps['step']: - cmd_msg.mode = step['mode'] - cmd_msg.value = step['value'] - cmd_msg.contact = step['contact'] - cmd_msg.gait_id = step['gait_id'] - cmd_msg.duration = step['duration'] - cmd_msg.life_count += 1 - for i in range(3): - cmd_msg.vel_des[i] = step['vel_des'][i] - cmd_msg.rpy_des[i] = step['rpy_des'][i] - cmd_msg.pos_des[i] = step['pos_des'][i] - cmd_msg.acc_des[i] = step['acc_des'][i] - cmd_msg.acc_des[i+3] = step['acc_des'][i+3] - cmd_msg.foot_pose[i] = step['foot_pose'][i] - cmd_msg.ctrl_point[i] = step['ctrl_point'][i] - for i in range(2): - cmd_msg.step_height[i] = step['step_height'][i] - lcm_cmd.publish("robot_control_cmd",cmd_msg.encode()) - time.sleep( 0.1 ) + 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() + + cmd_msg.mode = 62 + cmd_msg.value = 0 + cmd_msg.contact = 15 + cmd_msg.gait_id = 110 + cmd_msg.duration = 1000 + cmd_msg.life_count += 1 for i in range(325): #15s Heat beat It is used to maintain the heartbeat when life count is not updated lcm_cmd.publish("robot_control_cmd",cmd_msg.encode()) time.sleep( 0.2 ) diff --git a/task_3/task_3.py b/task_3/task_3.py index cc680f1..c660799 100644 --- a/task_3/task_3.py +++ b/task_3/task_3.py @@ -13,6 +13,7 @@ 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 # 创建本模块特定的日志记录器 @@ -45,7 +46,8 @@ def run_task_3(ctrl, msg): section('任务3：步态切换', "启动") info('开始执行任务3...', "启动") - # turn_degree(ctrl, msg, 90, absolute=True) + turn_degree(ctrl, msg, 90, absolute=True) + turn_degree(ctrl, msg, 90, absolute=True) usergait_msg = file_send_lcmt() lcm_usergait = lcm.LCM("udpm://239.255.76.67:7671?ttl=255") @@ -83,45 +85,139 @@ def run_task_3(ctrl, msg): f.writelines(toml.dumps(full_steps)) f.close() - file_obj_gait_def = open("./task_3/Gait_Def_up.toml",'r') file_obj_gait_params = open("./task_3/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() + lcm_usergait.publish("user_gait_file", usergait_msg.encode()) + time.sleep(0.5) file_obj_gait_params.close() - user_gait_list = open("./task_3/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] + msg.mode = 62 + msg.value = 0 + msg.contact = 15 + msg.gait_id = 110 + msg.duration = 1000 + msg.life_count += 1 + + # 参数设置 + stable_count = 0 # 用于计数z轴稳定的次数 + stable_threshold = 10 # 连续5次检测z轴不再增加则认为已经停止 + z_speed_threshold = 0.01 # z轴速度阈值，小于这个值认为已经停止爬升 + climb_speed_threshold = 0.05 # 检测到开始爬坡的速度阈值 + max_iterations = 600 # 最大循环次数，作为安全保障 + start_height = ctrl.odo_msg.xyz[2] # 记录起始高度 + + # 阶段控制 + climbing_detected = False # 是否检测到正在爬坡 + initial_flat_stage = True # 是否在初始平路阶段 + + info(f"开始监测里程计Z轴速度，初始高度: {start_height}", "监测") + + for i in range(max_iterations): + # 发送控制命令维持心跳 ctrl.Send_cmd(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_cmd(msg) - time.sleep( 0.2 ) + + # 每10次迭代打印一次当前信息 + if i % 10 == 0: + # 获取当前Z轴位置和速度 + current_vz = ctrl.odo_msg.vxyz[2] # z轴速度 + info(f"当前Z轴速度={current_vz:.3f}", "监测") + + # 获取z轴速度 + vz = ctrl.odo_msg.vxyz[2] + + # 检测是否开始爬坡阶段 - 使用z轴速度判断 + if not climbing_detected and vz > climb_speed_threshold: + climbing_detected = True + initial_flat_stage = False + info(f"检测到开始爬坡，Z轴速度: {vz:.3f}, 当前高度: {ctrl.odo_msg.xyz[2]:.3f}", "监测") + + # 只有在检测到爬坡后，才开始监控Z轴是否停止增加 + if climbing_detected: + # 如果Z轴速度接近于0或者为负，表示已经停止爬升或开始下降 + if abs(vz) < z_speed_threshold or vz < 0: + stable_count += 1 + if stable_count >= stable_threshold: + current_height = ctrl.odo_msg.xyz[2] + info(f"Z轴速度趋近于0，停止循环。当前速度: {vz:.3f}, 当前高度: {current_height:.3f}", "监测") + break + else: + # 如果Z轴仍有明显上升速度，重置稳定计数 + stable_count = 0 + + 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_cmd(msg) - pass \ No newline at end of file + pass + + section('任务3-2：直线行走', "开始") + go_straight(ctrl, msg, distance=1) + + section('任务3-3：down', "完成") + try: + msg.mode = 62 + msg.value = 0 + msg.contact = 15 + msg.gait_id = 110 + msg.duration = 1000 + msg.life_count += 1 + + # 参数设置 + stable_count = 0 # 用于计数z轴稳定的次数 + stable_threshold = 10 # 连续5次检测z轴不再增加则认为已经停止 + z_speed_threshold = 0.01 # z轴速度阈值，小于这个值认为已经停止爬升 + climb_speed_threshold = 0.05 # 检测到开始爬坡的速度阈值 + max_iterations = 600 # 最大循环次数，作为安全保障 + start_height = ctrl.odo_msg.xyz[2] # 记录起始高度 + + # 阶段控制 + climbing_detected = False # 是否检测到正在爬坡 + initial_flat_stage = True # 是否在初始平路阶段 + + info(f"开始监测里程计Z轴速度，初始高度: {start_height}", "监测") + + for i in range(max_iterations): + # 发送控制命令维持心跳 + ctrl.Send_cmd(msg) + + # 每10次迭代打印一次当前信息 + if i % 10 == 0: + # 获取当前Z轴位置和速度 + current_vz = ctrl.odo_msg.vxyz[2] # z轴速度 + info(f"当前Z轴速度={current_vz:.3f}", "监测") + + # 获取z轴速度 + vz = ctrl.odo_msg.vxyz[2] + + # 检测是否开始爬坡阶段 - 使用z轴速度判断 + if not climbing_detected and vz < -climb_speed_threshold: + climbing_detected = True + initial_flat_stage = False + info(f"检测到开始爬坡，Z轴速度: {vz:.3f}, 当前高度: {ctrl.odo_msg.xyz[2]:.3f}", "监测") + + # 只有在检测到爬坡后，才开始监控Z轴是否停止增加 + if climbing_detected: + # 如果Z轴速度接近于0或者为正，表示已经停止爬升或开始下降 + if abs(vz) < z_speed_threshold or vz > 0: + stable_count += 1 + if stable_count >= stable_threshold: + current_height = ctrl.odo_msg.xyz[2] + info(f"Z轴速度趋近于0，停止循环。当前速度: {vz:.3f}, 当前高度: {current_height:.3f}", "监测") + break + else: + # 如果Z轴仍有明显上升速度，重置稳定计数 + stable_count = 0 + 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_cmd(msg) + pass + \ No newline at end of file