From 88f72b3722239d3ea5dda6ec9af51b7d4d596e2a Mon Sep 17 00:00:00 2001 From: havoc420ubuntu <2993167370@qq.com> Date: Mon, 26 May 2025 12:29:48 +0000 Subject: [PATCH] refactor(detect_track): enhance edge detection and improve track following logic - Update detect_horizontal_track_edge_v3 function for better edge detection accuracy. - Refine parameters in follow_left_side_track function to improve stability and responsiveness. - Adjust logging for clarity and maintainability. --- task_5/Gait_Def_low_height.toml | 36 ++++++++ task_5/Gait_Params_low_height.toml | 87 ++++++++++++++++++++ task_5/README.md | 39 +++++++++ task_5/Usergait_List.toml | 15 ++++ task_5/main.py | 127 +++++++++++++++++++++++++++++ 5 files changed, 304 insertions(+) create mode 100644 task_5/Gait_Def_low_height.toml create mode 100644 task_5/Gait_Params_low_height.toml create mode 100644 task_5/README.md create mode 100644 task_5/Usergait_List.toml create mode 100644 task_5/main.py diff --git a/task_5/Gait_Def_low_height.toml b/task_5/Gait_Def_low_height.toml new file mode 100644 index 0000000..f5b1de8 --- /dev/null +++ b/task_5/Gait_Def_low_height.toml @@ -0,0 +1,36 @@ +# Gait Definition for Low Height Walking + +[[customized_gait]] +name = "low_height" +id = 0 +gait_type = 0 +foot_support = 15 +duration = 0.5 +des_vel_x = 0.0 +des_vel_y = 0.0 +des_vel_yaw = 0.0 +use_full_cycle = 0 +mu = 0.5 +max_foot_vel = 4.0 +load_force = 0.0 +kp_foot_x = 500.0 +kp_foot_y = 500.0 +kp_foot_z = 500.0 +kd_foot_x = 5.0 +kd_foot_y = 5.0 +kd_foot_z = 5.0 +kp_body_x = 500.0 +kp_body_y = 500.0 +kp_body_z = 500.0 +kd_body_x = 5.0 +kd_body_y = 5.0 +kd_body_z = 5.0 +kp_body_wx = 200.0 +kp_body_wy = 200.0 +kp_body_wz = 200.0 +kd_body_wx = 5.0 +kd_body_wy = 5.0 +kd_body_wz = 5.0 +kb_foot_x = 0.0 +kb_foot_y = 0.0 +kb_foot_z = 0.0 \ No newline at end of file diff --git a/task_5/Gait_Params_low_height.toml b/task_5/Gait_Params_low_height.toml new file mode 100644 index 0000000..73be8d7 --- /dev/null +++ b/task_5/Gait_Params_low_height.toml @@ -0,0 +1,87 @@ +# Gait Params for Low Height Walking + +# 1 - Initial position with low body height +[[step]] +type = "usergait" +body_vel_des = [0.0, 0.0, 0.0] +body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, -0.1] # Z值为-0.1，降低体高 +landing_pos_des = [0.2, 0.15, 0.0, 0.2, -0.15, 0.0, -0.2, -0.15, 0.0, -0.2, 0.15, 0.0] +step_height = [0.0, 0.0, 0.0, 0.0] # 初始姿态不抬腿 +weight = [10.0, 10.0, 10.0, 50.0, 50.0, 10.0] +use_mpc_traj = 0 +mu = 0.40 +landing_gain = 1.0 +gait_id = 110 +duration = 500 # 给足够时间降低体高 + +# 2 - Moving forward (left front and right back legs) +[[step]] +type = "usergait" +body_vel_des = [0.05, 0.0, 0.0] # 低速前进 +body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, -0.1] # 保持低体高 +landing_pos_des = [0.25, 0.15, 0.0, 0.2, -0.15, 0.0, -0.2, -0.15, 0.0, -0.25, 0.15, 0.0] +step_height = [0.03, 0.01, 0.01, 0.03] # 减小抬腿高度 +weight = [10.0, 10.0, 10.0, 50.0, 50.0, 10.0] +use_mpc_traj = 0 +mu = 0.40 +landing_gain = 1.0 +gait_id = 110 +duration = 300 + +# 3 - Moving forward (right front and left back legs) +[[step]] +type = "usergait" +body_vel_des = [0.05, 0.0, 0.0] # 继续低速前进 +body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, -0.1] # 保持低体高 +landing_pos_des = [0.2, 0.15, 0.0, 0.25, -0.15, 0.0, -0.25, -0.15, 0.0, -0.2, 0.15, 0.0] +step_height = [0.01, 0.03, 0.03, 0.01] # 减小抬腿高度 +weight = [10.0, 10.0, 10.0, 50.0, 50.0, 10.0] +use_mpc_traj = 0 +mu = 0.40 +landing_gain = 1.0 +gait_id = 110 +duration = 300 + +# 4 - Moving forward (left front and right back legs) +[[step]] +type = "usergait" +body_vel_des = [0.05, 0.0, 0.0] # 低速前进 +body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, -0.1] # 保持低体高 +landing_pos_des = [0.25, 0.15, 0.0, 0.2, -0.15, 0.0, -0.2, -0.15, 0.0, -0.25, 0.15, 0.0] +step_height = [0.03, 0.01, 0.01, 0.03] # 减小抬腿高度 +weight = [10.0, 10.0, 10.0, 50.0, 50.0, 10.0] +use_mpc_traj = 0 +mu = 0.40 +landing_gain = 1.0 +gait_id = 110 +duration = 300 + +# 5 - Moving forward (right front and left back legs) +[[step]] +type = "usergait" +body_vel_des = [0.05, 0.0, 0.0] # 继续低速前进 +body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, -0.1] # 保持低体高 +landing_pos_des = [0.2, 0.15, 0.0, 0.25, -0.15, 0.0, -0.25, -0.15, 0.0, -0.2, 0.15, 0.0] +step_height = [0.01, 0.03, 0.03, 0.01] # 减小抬腿高度 +weight = [10.0, 10.0, 10.0, 50.0, 50.0, 10.0] +use_mpc_traj = 0 +mu = 0.40 +landing_gain = 1.0 +gait_id = 110 +duration = 300 + +# 6 - Stop and maintain low position +[[step]] +type = "usergait" +body_vel_des = [0.0, 0.0, 0.0] # 停止 +body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, -0.1] # 保持低体高 +landing_pos_des = [0.2, 0.15, 0.0, 0.2, -0.15, 0.0, -0.2, -0.15, 0.0, -0.2, 0.15, 0.0] +step_height = [0.0, 0.0, 0.0, 0.0] # 不抬腿 +weight = [10.0, 10.0, 10.0, 50.0, 50.0, 10.0] +use_mpc_traj = 0 +mu = 0.40 +landing_gain = 1.0 +gait_id = 110 +duration = 500 + +# End of low height gait # \ No newline at end of file diff --git a/task_5/README.md b/task_5/README.md new file mode 100644 index 0000000..0a1e9b6 --- /dev/null +++ b/task_5/README.md @@ -0,0 +1,39 @@ +# 低体高步态实现 + +本项目实现了一个让机器狗以较低体高前进的步态，适合在需要降低重心、稳定行走时使用。 + +## 文件说明 + +- `main.py`: 主程序，负责加载步态参数并发送LCM控制命令 +- `Gait_Def_low_height.toml`: 低体高步态的定义文件 +- `Gait_Params_low_height.toml`: 低体高步态的参数文件 +- `Usergait_List.toml`: 用户步态列表，定义执行步态的命令 + +## 步态特点 + +1. 降低了机器狗的体高（z轴降低0.1米） +2. 减小了步态的抬腿高度，保持稳定性 +3. 以较低的速度前进，确保稳定性 +4. 采用对角步态模式（左前右后腿一组，右前左后腿一组） + +## 使用方法 + +1. 确保已安装所需的依赖包（lcm, toml） +2. 将文件放置在同一目录下 +3. 执行主程序： + +```bash +python3 main.py +``` + +4. 程序会自动加载步态参数并执行低体高步态 +5. 按Ctrl+C可中断步态执行 + +## 参数调整 + +如需调整步态参数，可修改`Gait_Params_low_height.toml`文件： + +- `body_pos_des`的第6个元素控制体高，数值越小体高越低 +- `step_height`控制抬腿高度 +- `body_vel_des`的第1个元素控制前进速度 +- `duration`控制每个步态持续的时间 \ No newline at end of file diff --git a/task_5/Usergait_List.toml b/task_5/Usergait_List.toml new file mode 100644 index 0000000..f470486 --- /dev/null +++ b/task_5/Usergait_List.toml @@ -0,0 +1,15 @@ +# User Gait List for Low Height Walking + +[[step]] +mode = 11 # LOCOMOTION +gait_id = 110 # USERGAIT +vel_des = [0.0, 0.0, 0.0] +rpy_des = [0.0, 0.0, 0.0] +pos_des = [0.0, 0.0, -0.1] # 低体高 +acc_des = [10.0, 10.0, 10.0, 50.0, 50.0, 10.0] +foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] +ctrl_point = [0.0, 0.0, 0.4] +step_height = [0.0, 0.0] +value = 0 +contact = 10 +duration = 5000 # 5秒执行步态 \ No newline at end of file diff --git a/task_5/main.py b/task_5/main.py new file mode 100644 index 0000000..76d8f4b --- /dev/null +++ b/task_5/main.py @@ -0,0 +1,127 @@ +''' +此程序实现了一个让机器狗以较低体高前进的步态 +- 使用LCM通信接口 +- 自定义了体高较低的步态参数 +- 适合在平坦地面上低姿态前进 +''' +import lcm +import sys +import time +import toml +import copy +import math +from robot_control_cmd_lcmt import robot_control_cmd_lcmt +from file_send_lcmt import file_send_lcmt + +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 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: + # 加载低体高步态参数 + steps = toml.load("Gait_Params_low_height.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_low_height_full.toml", 'w') + f.write("# Gait Params for Low Height\n") + f.writelines(toml.dumps(full_steps)) + f.close() + + # 发送步态定义和参数文件 + file_obj_gait_def = open("Gait_Def_low_height.toml", 'r') + file_obj_gait_params = open("Gait_Params_low_height_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']: + 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) + + # 保持心跳信号 + print("低体高步态执行中...") + for i in range(75): # 15秒心跳，维持步态 + lcm_cmd.publish("robot_control_cmd", cmd_msg.encode()) + time.sleep(0.2) + + except KeyboardInterrupt: + # 发生中断时，切换到纯阻尼模式 + cmd_msg.mode = 7 # PureDamper + cmd_msg.gait_id = 0 + cmd_msg.duration = 0 + cmd_msg.life_count += 1 + lcm_cmd.publish("robot_control_cmd", cmd_msg.encode()) + print("步态执行已中断") + pass + sys.exit() + +if __name__ == '__main__': + main() \ No newline at end of file