397 lines
14 KiB
Python
397 lines
14 KiB
Python
import time
|
||
import sys
|
||
import os
|
||
import toml
|
||
import copy
|
||
import math
|
||
import lcm
|
||
|
||
# 添加父目录到路径，以便能够导入utils
|
||
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
||
# 添加当前目录到路径，确保可以找到local文件
|
||
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 file_send_lcmt import file_send_lcmt
|
||
|
||
# 创建本模块特定的日志记录器
|
||
logger = get_logger("低头匍匐步态")
|
||
|
||
observe = True
|
||
|
||
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 low_crawl_gait(ctrl, msg, step_count=8, observe=True):
|
||
"""
|
||
执行低头匍匐前进步态
|
||
|
||
参数:
|
||
ctrl: Robot_Ctrl对象，包含机器人控制接口
|
||
msg: 控制消息对象
|
||
step_count: 执行的步数，默认8步（两个完整步态周期）
|
||
observe: 是否输出调试信息
|
||
|
||
返回:
|
||
bool: 是否成功执行步态
|
||
"""
|
||
if observe:
|
||
section('开始执行低头匍匐前进步态', "步态控制")
|
||
info(f"计划执行 {step_count} 步的匍匐步态", "步态")
|
||
|
||
# 切换步态
|
||
usergait_msg = file_send_lcmt()
|
||
|
||
try:
|
||
# 加载步态参数
|
||
gait_def_path = "./task_2/Gait_Def_crawl.toml"
|
||
gait_params_path = "./task_2/Gait_Params_crawl.toml"
|
||
|
||
if observe:
|
||
info(f"加载步态定义文件: {gait_def_path}", "加载")
|
||
info(f"加载步态参数文件: {gait_params_path}", "加载")
|
||
|
||
# 检查文件是否存在
|
||
if not os.path.exists(gait_def_path):
|
||
error(f"步态定义文件不存在: {gait_def_path}", "错误")
|
||
return False
|
||
|
||
if not os.path.exists(gait_params_path):
|
||
error(f"步态参数文件不存在: {gait_params_path}", "错误")
|
||
return False
|
||
|
||
# 加载步态参数
|
||
steps = toml.load(gait_params_path)
|
||
full_steps = {'step':[robot_cmd]}
|
||
k = 0
|
||
|
||
if observe:
|
||
info(f"成功加载 {len(steps['step'])} 个步态参数", "加载")
|
||
|
||
# 处理每个步态参数
|
||
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] # roll, pitch, yaw
|
||
cmd['pos_des'] = i['body_pos_des'][3:6] # x, y, z
|
||
cmd['foot_pose'][0:2] = i['landing_pos_des'][0:2] # FR
|
||
cmd['foot_pose'][2:4] = i['landing_pos_des'][3:5] # FL
|
||
cmd['foot_pose'][4:6] = i['landing_pos_des'][6:8] # RR
|
||
cmd['ctrl_point'][0:2] = i['landing_pos_des'][9:11] # RL
|
||
|
||
# 步高编码（前两腿和后两腿）
|
||
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
|
||
|
||
# 生成完整的步态参数文件
|
||
full_params_path = "./task_2/Gait_Params_crawl_full.toml"
|
||
with open(full_params_path, 'w') as f:
|
||
f.write("# Gait Params for Low Crawl - Generated\n")
|
||
f.writelines(toml.dumps(full_steps))
|
||
|
||
if observe:
|
||
info(f"生成完整步态参数文件: {full_params_path}", "生成")
|
||
|
||
# 发送步态定义文件
|
||
with open(gait_def_path, 'r') as file_obj_gait_def:
|
||
usergait_msg.data = file_obj_gait_def.read()
|
||
ctrl.lc_s.publish("user_gait_file", usergait_msg.encode())
|
||
if observe:
|
||
debug("发送步态定义文件到机器人", "发送")
|
||
time.sleep(0.5)
|
||
|
||
# 发送步态参数文件
|
||
with open(full_params_path, 'r') as file_obj_gait_params:
|
||
usergait_msg.data = file_obj_gait_params.read()
|
||
ctrl.lc_s.publish("user_gait_file", usergait_msg.encode())
|
||
if observe:
|
||
debug("发送步态参数文件到机器人", "发送")
|
||
time.sleep(0.1)
|
||
|
||
# 开始执行步态
|
||
msg.mode = 62 # 自定义步态模式
|
||
msg.value = 0
|
||
msg.contact = 15
|
||
msg.gait_id = 110 # USERGAIT ID
|
||
msg.duration = 1000
|
||
msg.life_count += 1
|
||
|
||
if observe:
|
||
info("开始执行低头匍匐步态...", "执行")
|
||
|
||
# 执行指定步数的步态
|
||
for i in range(step_count):
|
||
if observe:
|
||
debug(f"执行第 {i+1}/{step_count} 步", "步态")
|
||
|
||
ctrl.Send_cmd(msg)
|
||
time.sleep(1) # 每步间隔1秒
|
||
|
||
# 检查机器人状态（可选）
|
||
if hasattr(ctrl, 'odo_msg'):
|
||
current_pos = ctrl.odo_msg.xyz
|
||
current_rpy = ctrl.odo_msg.rpy
|
||
if observe and i % 2 == 0: # 每两步输出一次状态
|
||
debug(f"当前位置: x={current_pos[0]:.3f}, y={current_pos[1]:.3f}, z={current_pos[2]:.3f}", "状态")
|
||
debug(f"当前姿态: roll={math.degrees(current_rpy[0]):.1f}°, pitch={math.degrees(current_rpy[1]):.1f}°, yaw={math.degrees(current_rpy[2]):.1f}°", "状态")
|
||
|
||
if observe:
|
||
success(f"成功完成 {step_count} 步低头匍匐步态", "完成")
|
||
|
||
return True
|
||
|
||
except KeyboardInterrupt:
|
||
warning("用户中断，停止步态执行", "中断")
|
||
# 安全停止
|
||
msg.mode = 7 # PureDamper模式
|
||
msg.gait_id = 0
|
||
msg.duration = 0
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
return False
|
||
|
||
except Exception as e:
|
||
error(f"执行低头匍匐步态时发生错误: {str(e)}", "错误")
|
||
# 安全停止
|
||
try:
|
||
msg.mode = 7 # PureDamper模式
|
||
msg.gait_id = 0
|
||
msg.duration = 0
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
except:
|
||
pass
|
||
return False
|
||
|
||
def transition_to_crawl_position(ctrl, msg, observe=True):
|
||
"""
|
||
过渡到匍匐准备姿态
|
||
|
||
参数:
|
||
ctrl: Robot_Ctrl对象
|
||
msg: 控制消息对象
|
||
observe: 是否输出调试信息
|
||
|
||
返回:
|
||
bool: 是否成功过渡到匍匐姿态
|
||
"""
|
||
if observe:
|
||
section('过渡到低头匍匐准备姿态', "姿态调整")
|
||
|
||
try:
|
||
# 第一阶段：降低身体高度
|
||
if observe:
|
||
info("第一阶段：降低身体高度到匍匐位置", "调整")
|
||
|
||
msg.mode = 11 # Locomotion模式
|
||
msg.gait_id = 26 # 自变频步态
|
||
msg.vel_des = [0.0, 0.0, 0.0] # 停止移动
|
||
msg.rpy_des = [0.0, 0.15, 0.0] # 稍微抬头，为后续低头做准备
|
||
msg.pos_des = [0.0, 0.0, -0.03] # 身体降低3cm
|
||
msg.step_height = [0.04, 0.04] # 较低的步高
|
||
msg.duration = 0
|
||
msg.life_count += 1
|
||
|
||
ctrl.Send_cmd(msg)
|
||
time.sleep(2.0) # 等待姿态调整
|
||
|
||
# 第二阶段：调整到低头姿态
|
||
if observe:
|
||
info("第二阶段：调整到低头匍匐姿态", "调整")
|
||
|
||
msg.rpy_des = [0.0, 0.25, 0.0] # 低头姿态，pitch角约14度
|
||
msg.pos_des = [0.0, 0.0, -0.05] # 身体再降低2cm
|
||
msg.life_count += 1
|
||
|
||
ctrl.Send_cmd(msg)
|
||
time.sleep(2.0)
|
||
|
||
# 第三阶段：稳定在匍匐姿态
|
||
if observe:
|
||
info("第三阶段：稳定在低头匍匐姿态", "调整")
|
||
|
||
msg.vel_des = [0.0, 0.0, 0.0] # 确保静止
|
||
msg.life_count += 1
|
||
|
||
ctrl.Send_cmd(msg)
|
||
time.sleep(1.0)
|
||
|
||
if observe:
|
||
success("成功过渡到低头匍匐准备姿态", "完成")
|
||
|
||
return True
|
||
|
||
except Exception as e:
|
||
error(f"过渡到匍匐姿态时发生错误: {str(e)}", "错误")
|
||
return False
|
||
|
||
def recover_from_crawl_position(ctrl, msg, observe=True):
|
||
"""
|
||
从匍匐姿态恢复到正常站立姿态
|
||
|
||
参数:
|
||
ctrl: Robot_Ctrl对象
|
||
msg: 控制消息对象
|
||
observe: 是否输出调试信息
|
||
|
||
返回:
|
||
bool: 是否成功恢复到正常姿态
|
||
"""
|
||
if observe:
|
||
section('从低头匍匐姿态恢复到正常站立', "姿态恢复")
|
||
|
||
try:
|
||
# 第一阶段：停止当前步态
|
||
if observe:
|
||
info("第一阶段：停止当前步态", "停止")
|
||
|
||
msg.mode = 7 # PureDamper模式，安全停止
|
||
msg.gait_id = 0
|
||
msg.duration = 0
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
time.sleep(1.0)
|
||
|
||
# 第二阶段：逐渐抬高身体
|
||
if observe:
|
||
info("第二阶段：逐渐抬高身体", "恢复")
|
||
|
||
msg.mode = 11 # Locomotion模式
|
||
msg.gait_id = 26 # 自变频步态
|
||
msg.vel_des = [0.0, 0.0, 0.0]
|
||
msg.rpy_des = [0.0, 0.1, 0.0] # 减少低头角度
|
||
msg.pos_des = [0.0, 0.0, -0.02] # 稍微抬高
|
||
msg.step_height = [0.06, 0.06]
|
||
msg.duration = 0
|
||
msg.life_count += 1
|
||
|
||
ctrl.Send_cmd(msg)
|
||
time.sleep(2.0)
|
||
|
||
# 第三阶段：恢复到正常姿态
|
||
if observe:
|
||
info("第三阶段：恢复到正常站立姿态", "恢复")
|
||
|
||
msg.rpy_des = [0.0, 0.0, 0.0] # 恢复正常头部姿态
|
||
msg.pos_des = [0.0, 0.0, 0.0] # 恢复正常身体高度
|
||
msg.step_height = [0.08, 0.08] # 正常步高
|
||
msg.life_count += 1
|
||
|
||
ctrl.Send_cmd(msg)
|
||
time.sleep(2.0)
|
||
|
||
# 最终稳定
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
time.sleep(1.0)
|
||
|
||
if observe:
|
||
success("成功恢复到正常站立姿态", "完成")
|
||
|
||
return True
|
||
|
||
except Exception as e:
|
||
error(f"恢复姿态时发生错误: {str(e)}", "错误")
|
||
return False
|
||
|
||
def run_low_crawl_demo(ctrl, msg, distance=2.0, observe=True):
|
||
"""
|
||
运行完整的低头匍匐前进演示
|
||
|
||
参数:
|
||
ctrl: Robot_Ctrl对象
|
||
msg: 控制消息对象
|
||
distance: 期望的前进距离（米）
|
||
observe: 是否输出调试信息
|
||
|
||
返回:
|
||
bool: 是否成功完成演示
|
||
"""
|
||
if observe:
|
||
section('开始低头匍匐前进演示', "演示")
|
||
info(f"目标前进距离: {distance} 米", "目标")
|
||
|
||
try:
|
||
# 记录起始位置
|
||
start_position = None
|
||
if hasattr(ctrl, 'odo_msg'):
|
||
start_position = list(ctrl.odo_msg.xyz)
|
||
if observe:
|
||
info(f"起始位置: x={start_position[0]:.3f}, y={start_position[1]:.3f}, z={start_position[2]:.3f}", "位置")
|
||
|
||
# 步骤1: 过渡到匍匐姿态
|
||
success_transition = transition_to_crawl_position(ctrl, msg, observe)
|
||
if not success_transition:
|
||
error("过渡到匍匐姿态失败", "错误")
|
||
return False
|
||
|
||
# 步骤2: 估算需要的步数（每步大约前进0.1-0.15米）
|
||
estimated_steps = max(4, int(distance / 0.12)) # 每步约12cm
|
||
if observe:
|
||
info(f"估算需要执行 {estimated_steps} 步来完成 {distance} 米的移动", "计划")
|
||
|
||
# 步骤3: 执行匍匐步态
|
||
success_crawl = low_crawl_gait(ctrl, msg, step_count=estimated_steps, observe=observe)
|
||
if not success_crawl:
|
||
error("执行匍匐步态失败", "错误")
|
||
# 尝试恢复姿态
|
||
recover_from_crawl_position(ctrl, msg, observe)
|
||
return False
|
||
|
||
# 步骤4: 恢复到正常姿态
|
||
success_recovery = recover_from_crawl_position(ctrl, msg, observe)
|
||
if not success_recovery:
|
||
error("恢复正常姿态失败", "错误")
|
||
return False
|
||
|
||
# 计算实际移动距离
|
||
if start_position and hasattr(ctrl, 'odo_msg'):
|
||
end_position = list(ctrl.odo_msg.xyz)
|
||
actual_distance = math.sqrt(
|
||
(end_position[0] - start_position[0])**2 +
|
||
(end_position[1] - start_position[1])**2
|
||
)
|
||
if observe:
|
||
success(f"演示完成！实际前进距离: {actual_distance:.3f} 米", "完成")
|
||
info(f"结束位置: x={end_position[0]:.3f}, y={end_position[1]:.3f}, z={end_position[2]:.3f}", "位置")
|
||
else:
|
||
if observe:
|
||
success("低头匍匐前进演示完成！", "完成")
|
||
|
||
return True
|
||
|
||
except Exception as e:
|
||
error(f"执行匍匐演示时发生错误: {str(e)}", "错误")
|
||
# 尝试安全恢复
|
||
try:
|
||
recover_from_crawl_position(ctrl, msg, observe)
|
||
except:
|
||
pass
|
||
return False
|
||
|