新增go_to_x_v2函数以支持机器人移动到指定x坐标，更新task_3.py以引入新函数并调整稳定性检测参数，优化代码逻辑和文件处理。
This commit is contained in:
parent
92b2d40826
commit
918f5f20c5
@ -637,3 +637,16 @@ def go_to_y_v2(ctrl, msg, target_y, speed=0.5, precision=True, observe=False):
|
||||
"""
|
||||
target_x = ctrl.odo_msg.xyz[0]
|
||||
return go_to_xy_v2(ctrl, msg, target_x, target_y, speed, precision, observe)
|
||||
|
||||
def go_to_x_v2(ctrl, msg, target_x, speed=0.5, precision=True, observe=False):
|
||||
"""
|
||||
控制机器人移动到指定的x坐标位置，使用直接x速度控制
|
||||
|
||||
参数:
|
||||
ctrl: Robot_Ctrl 对象，包含里程计信息
|
||||
msg: robot_control_cmd_lcmt 对象，用于发送命令
|
||||
target_y: 目标Y坐标(米)
|
||||
speed: 行走速度(米/秒)，范围0.1~1.0，默认为0.5
|
||||
"""
|
||||
target_y = ctrl.odo_msg.xyz[1]
|
||||
return go_to_xy_v2(ctrl, msg, target_x, target_y, speed, precision, observe)
|
||||
|
||||
4981
task_3/Gait_Params_down_full.toml
Normal file
4981
task_3/Gait_Params_down_full.toml
Normal file
File diff suppressed because it is too large
Load Diff
@ -14,6 +14,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, turn_degree_v2
|
||||
from base_move.go_straight import go_straight
|
||||
from base_move.go_to_xy import go_to_x_v2
|
||||
from file_send_lcmt import file_send_lcmt
|
||||
|
||||
# 创建本模块特定的日志记录器
|
||||
@ -96,18 +97,14 @@ def run_task_3(ctrl, msg):
|
||||
|
||||
# 参数设置
|
||||
stable_count = 0 # 用于计数z轴稳定的次数
|
||||
stable_threshold = 10 # 连续15次检测z轴不再增加则认为已经停止
|
||||
stable_threshold = 8 # 连续15次检测z轴不再增加则认为已经停止
|
||||
z_speed_threshold = 0.01 # z轴速度阈值，小于这个值认为已经停止爬升
|
||||
climb_speed_threshold = 0.05 # 检测到开始爬坡的速度阈值
|
||||
max_iterations = 600 # 最大循环次数，作为安全保障
|
||||
min_iterations = 200 # 最小循环次数，作为安全保障
|
||||
start_height = ctrl.odo_msg.xyz[2] # 记录起始高度
|
||||
|
||||
max_iterations = 250 # 最大循环次数，作为安全保障
|
||||
min_iterations = 150 # 最小循环次数，作为安全保障
|
||||
|
||||
# 阶段控制
|
||||
climbing_detected = False # 是否检测到正在爬坡
|
||||
|
||||
info(f"开始监测里程计Z轴速度，初始高度: {start_height}", "监测")
|
||||
|
||||
for i in range(max_iterations):
|
||||
# 发送控制命令维持心跳
|
||||
ctrl.Send_cmd(msg)
|
||||
@ -129,11 +126,10 @@ def run_task_3(ctrl, msg):
|
||||
# 只有在检测到爬坡后，才开始监控Z轴是否停止增加
|
||||
if i > min_iterations and climbing_detected:
|
||||
# 如果Z轴速度接近于0或者为负，表示已经停止爬升或开始下降
|
||||
if abs(vz) < z_speed_threshold or vz < 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}", "监测")
|
||||
info(f"Z轴速度趋近于0，停止循环。当前速度: {vz:.3f}", "监测")
|
||||
break
|
||||
else:
|
||||
# 如果Z轴仍有明显上升速度，重置稳定计数
|
||||
@ -148,21 +144,20 @@ def run_task_3(ctrl, msg):
|
||||
ctrl.Send_cmd(msg)
|
||||
pass
|
||||
|
||||
section('任务3-2：直线行走', "开始")
|
||||
msg.mode = 11 # Locomotion模式
|
||||
msg.gait_id = 26 # 自变频步态
|
||||
msg.duration = 0 # wait next cmd
|
||||
msg.step_height = [0.06, 0.06] # 抬腿高度
|
||||
msg.vel_des = [0, 0.2, 0] # [前进速度, 侧向速度, 角速度]
|
||||
msg.life_count += 1
|
||||
ctrl.Send_cmd(msg)
|
||||
time.sleep(0.3)
|
||||
|
||||
return
|
||||
section('任务3-2：x = 2', "开始")
|
||||
# msg.mode = 11 # Locomotion模式 # DEBUG
|
||||
# msg.gait_id = 26 # 自变频步态
|
||||
# msg.duration = 0 # wait next cmd
|
||||
# msg.step_height = [0.06, 0.06] # 抬腿高度
|
||||
# msg.vel_des = [0, 0.2, 0] # [前进速度, 侧向速度, 角速度]
|
||||
# msg.life_count += 1
|
||||
# ctrl.Send_cmd(msg)
|
||||
time.sleep(1)
|
||||
# go_to_x_v2(ctrl, msg, 2, speed=0.5, precision=True, observe=True)
|
||||
|
||||
section('任务3-3：down', "完成")
|
||||
try:
|
||||
steps = toml.load("./task_3/Gait_Params_up.toml")
|
||||
steps = toml.load("./task_3/Gait_Params_down.toml")
|
||||
full_steps = {'step':[robot_cmd]}
|
||||
k = 0
|
||||
for i in steps['step']:
|
||||
@ -189,14 +184,14 @@ def run_task_3(ctrl, msg):
|
||||
else:
|
||||
full_steps['step'].append(cmd)
|
||||
k=k+1
|
||||
f = open("./task_3/Gait_Params_up_full.toml", 'w')
|
||||
f = open("./task_3/Gait_Params_down_full.toml", 'w')
|
||||
f.write("# Gait Params\n")
|
||||
f.writelines(toml.dumps(full_steps))
|
||||
f.close()
|
||||
|
||||
# pre
|
||||
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')
|
||||
file_obj_gait_params = open("./task_3/Gait_Params_down_full.toml",'r')
|
||||
usergait_msg.data = file_obj_gait_def.read()
|
||||
lcm_usergait.publish("user_gait_file",usergait_msg.encode())
|
||||
time.sleep(0.5)
|
||||
@ -206,12 +201,6 @@ def run_task_3(ctrl, msg):
|
||||
file_obj_gait_def.close()
|
||||
file_obj_gait_params.close()
|
||||
|
||||
file_obj_gait_params = open("./task_3/Gait_Params_up_full.toml",'r')
|
||||
usergait_msg.data = file_obj_gait_params.read()
|
||||
lcm_usergait.publish("user_gait_file", usergait_msg.encode())
|
||||
time.sleep(0.5)
|
||||
file_obj_gait_params.close()
|
||||
|
||||
msg.mode = 62
|
||||
msg.value = 0
|
||||
msg.contact = 15
|
||||
@ -221,11 +210,11 @@ def run_task_3(ctrl, msg):
|
||||
|
||||
# 参数设置
|
||||
stable_count = 0 # 用于计数z轴稳定的次数
|
||||
stable_threshold = 8 # 连续10次检测z轴速度接近零则认为已经到达平地
|
||||
z_speed_threshold = 0.01 # z轴速度阈值，小于这个值认为已经停止下降
|
||||
stable_threshold = 10 # 连续10次检测z轴速度接近零则认为已经到达平地
|
||||
z_speed_threshold = 0.005 # z轴速度阈值，小于这个值认为已经停止下降
|
||||
descent_speed_threshold = -0.05 # 检测到开始下坡的速度阈值（负值表示下降）
|
||||
max_iterations = 600 # 最大循环次数，作为安全保障
|
||||
min_iterations = 100 # 最小循环次数，确保有足够的时间开始动作
|
||||
max_iterations = 250 # 最大循环次数，作为安全保障
|
||||
min_iterations = 150 # 最小循环次数，确保有足够的时间开始动作
|
||||
start_height = ctrl.odo_msg.xyz[2] # 记录起始高度
|
||||
|
||||
# 阶段控制
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user