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.
This commit is contained in:
parent
e9ff362b01
commit
88f72b3722
36
task_5/Gait_Def_low_height.toml
Normal file
36
task_5/Gait_Def_low_height.toml
Normal file
@ -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
|
||||
87
task_5/Gait_Params_low_height.toml
Normal file
87
task_5/Gait_Params_low_height.toml
Normal file
@ -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 #
|
||||
39
task_5/README.md
Normal file
39
task_5/README.md
Normal file
@ -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`控制每个步态持续的时间
|
||||
15
task_5/Usergait_List.toml
Normal file
15
task_5/Usergait_List.toml
Normal file
@ -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秒执行步态
|
||||
127
task_5/main.py
Normal file
127
task_5/main.py
Normal file
@ -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()
|
||||
Loading…
x
Reference in New Issue
Block a user