Havoc420mac/mi-task
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

refactor(task_3): update gait processing and enhance control logic

Browse Source 
- 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.
This commit is contained in:
havoc420ubuntu 2025-05-26 06:11:09 +00:00
parent fbc2d5a7cd
commit 7b106c03dc
3 changed files with 190 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
main.py
View File

@ -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

72
task_3/main.py
View File

@ -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 )

160
task_3/task_3.py
View File

@ -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
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