240 lines
9.7 KiB
Python
240 lines
9.7 KiB
Python
#!/usr/bin/env python3
|
||
import time
|
||
|
||
from kuavo_humanoid_sdk import (KuavoRobot, KuavoRobotState, KuavoRobotTools,
|
||
KuavoRobotVision, KuavoSDK)
|
||
from kuavo_humanoid_sdk.common.logger import SDKLogger
|
||
from kuavo_humanoid_sdk.interfaces.data_types import (AprilTagData,
|
||
PoseQuaternion)
|
||
from kuavo_humanoid_sdk.kuavo_strategy.grasp_box.grasp_box_strategy import (
|
||
BoxInfo, KuavoGraspBox)
|
||
|
||
"""
|
||
安全须知:
|
||
请注意
|
||
实物机器人当中因为搬箱子需要用到末端力控制, 请把 arm_transport_target_up 当中的sim_mode设置为False
|
||
仿真机器人当中因为物理性质差异，所以不需要用到末端力控制 请把 arm_transport_target_up 当中的sim_mode设置为True
|
||
|
||
长春一汽绿色箱子质量为1.5kg, 请根据实际情况修改箱子质量
|
||
"""
|
||
def main(real: bool = True):
|
||
start_time = time.time() # 记录开始时间
|
||
# Initialize SDK
|
||
if not KuavoSDK().Init():
|
||
print("Init KuavoSDK failed, exit!")
|
||
exit(1)
|
||
|
||
SDKLogger.info("初始化机器人...")
|
||
|
||
# 初始化机器人及相关组件
|
||
robot = KuavoRobot()
|
||
robot_state = KuavoRobotState()
|
||
robot_tools = KuavoRobotTools()
|
||
robot_vision = KuavoRobotVision()
|
||
|
||
# 初始化箱子抓取策略
|
||
grasp_strategy = KuavoGraspBox(robot, robot_state, robot_tools, robot_vision)
|
||
|
||
# 粘贴在箱子上的 AprilTag 信息: 需要 ID，尺寸和基于odom坐标系下的大致位姿
|
||
box_tag = AprilTagData(
|
||
id=[1], # AprilTag ID
|
||
size=[0.1], # AprilTag 标签尺寸
|
||
pose=[PoseQuaternion(
|
||
# TODO: 需要根据实际情况调整
|
||
position=(0.0, -1.5, 0.8), # 基于odom坐标系下的大致位置, 查找时会对齐到这个方向
|
||
orientation=(0.0, 0.0, 0.0, 1.0) # 四元数方向
|
||
)]
|
||
)
|
||
|
||
# 粘贴在放置位置的 AprilTag 信息: 需要 ID，尺寸和基于odom坐标系下的大致位姿
|
||
placement_tag = AprilTagData(
|
||
id=[0], # AprilTag ID
|
||
size=[0.1], # AprilTag 标签尺寸
|
||
pose=[PoseQuaternion(
|
||
# TODO: 需要根据实际情况调整
|
||
position=(0.0, 1.5, 1.5), # 基于odom坐标系下的大致位置, 查找时会对齐到这个方向
|
||
orientation=(0.0, 0.0, 1.0, 0.0) # 四元数方向 - 旋转180度
|
||
)]
|
||
)
|
||
|
||
# 创建箱子信息对象
|
||
box_size = (0.3, 0.4, 0.22) # xyz( 宽, 长, 高)
|
||
box_mass = 1.5
|
||
|
||
time.sleep(1)
|
||
|
||
# 执行完整抓取策略
|
||
try:
|
||
SDKLogger.info("========== 开始执行箱子抓取策略 ==========")
|
||
|
||
# !!! Important: 关闭 basePitch 限制, 否则无法进行大幅度的前后倾动作 会导致摔倒
|
||
SDKLogger.info("⚠️ 重要提示: basePitch 限制需要关闭!!!")
|
||
SDKLogger.info("⚠️ 重要提示: basePitch 限制需要关闭!!!")
|
||
SDKLogger.info("⚠️ 重要提示: basePitch 限制需要关闭!!!")
|
||
grasp_strategy.robot.enable_base_pitch_limit(False) # 关闭 basePitch 限制
|
||
# Retry up to 3 times to check pitch limit status
|
||
for i in range(3):
|
||
if not robot_state.pitch_limit_enabled():
|
||
SDKLogger.info("✅ 已关闭 basePitch 限制")
|
||
break
|
||
else:
|
||
if i < 2: # Don't log on last attempt
|
||
SDKLogger.info(f"⚠️ 第{i+1}次检查 basePitch 限制状态...")
|
||
time.sleep(0.5) # Brief pause between retries
|
||
else: # Loop completed without break
|
||
SDKLogger.info("❌ 关闭 basePitch 限制失败")
|
||
return
|
||
# -----------------------------------------------------------
|
||
|
||
# 步骤1：使用头部寻找目标
|
||
SDKLogger.info("1. 寻找目标箱子...")
|
||
grasp_strategy.robot.disable_head_tracking()
|
||
SDKLogger.info("✅ 已关闭头部追踪")
|
||
|
||
find_success = grasp_strategy.head_find_target(
|
||
box_tag,
|
||
max_search_time=15.0,
|
||
search_pattern="rotate_body" # rotate_head
|
||
)
|
||
|
||
if not find_success:
|
||
SDKLogger.info("❌ 寻找目标失败，无法继续执行")
|
||
return
|
||
|
||
SDKLogger.info("✅ 已找到目标箱子")
|
||
time.sleep(1) # 短暂暂停
|
||
box_tag_data = robot_vision.get_data_by_id_from_odom(box_tag.id[0]) # 拿到箱子tag数据 odom系
|
||
if not box_tag_data:
|
||
SDKLogger.info(f"❌ 未识别到 AprilTag ID 为{box_tag.id[0]} 的目标箱子")
|
||
return
|
||
|
||
SDKLogger.info(f"box_tag_data: {box_tag_data}")
|
||
|
||
box_info = BoxInfo.from_apriltag(
|
||
box_tag_data,
|
||
xyz_offset=(box_size[0]/2, 0.0, -0.00), # tag 粘贴在箱子正面，为了得到箱子中心点，因此偏移量为箱子宽度的一半
|
||
size=box_size, # 箱子尺寸，单位：米
|
||
mass=box_mass # 箱子重量，单位：千克
|
||
)
|
||
|
||
SDKLogger.info(f"box_info: {box_info}")
|
||
|
||
# 步骤2：走路接近目标
|
||
# for i in range(1):
|
||
SDKLogger.info("2. 走路接近目标...")
|
||
approach_success = grasp_strategy.walk_approach_target(
|
||
box_tag.id[0],
|
||
target_distance=0.4, # 与目标箱子保持0.7米的距离
|
||
approach_speed=0.2 # 接近速度0.2米/秒
|
||
)
|
||
|
||
if not approach_success:
|
||
SDKLogger.info("❌ 接近目标失败，无法继续执行")
|
||
return
|
||
|
||
SDKLogger.info("✅ 已成功接近目标")
|
||
time.sleep(1) # 短暂暂停
|
||
|
||
# 步骤3：手臂移动到抓取位置
|
||
SDKLogger.info("3. 手臂移动到抓取位置...")
|
||
move_success = grasp_strategy.arm_move_to_target(
|
||
box_info,
|
||
arm_mode="manipulation_mpc"
|
||
)
|
||
|
||
if not move_success:
|
||
SDKLogger.info("❌ 手臂移动失败，无法继续执行")
|
||
return
|
||
|
||
SDKLogger.info("✅ 手臂已到达抓取位置")
|
||
# grasp_strategy.robot.arm_reset()
|
||
time.sleep(1.0) # 短暂暂停
|
||
|
||
|
||
# 步骤4：提起箱子
|
||
SDKLogger.info("4. 提起箱子...")
|
||
transport_up_success = grasp_strategy.arm_transport_target_up(
|
||
box_info,
|
||
arm_mode="manipulation_mpc",
|
||
sim_mode=not real
|
||
)
|
||
|
||
if not transport_up_success:
|
||
SDKLogger.info("❌ 提起箱子失败")
|
||
return
|
||
time.sleep(1.0) # 短暂暂停
|
||
|
||
SDKLogger.info("✅ 成功提起箱子")
|
||
# grasp_strategy.robot.arm_reset()
|
||
time.sleep(1.0) # 展示一下成功提起的状态
|
||
|
||
# return # FIXME:测试提起箱子成功
|
||
|
||
# 步骤5：关闭头部追踪
|
||
SDKLogger.info("5. 关闭头部追踪...")
|
||
grasp_strategy.robot.disable_head_tracking()
|
||
SDKLogger.info("✅ 已关闭头部追踪")
|
||
time.sleep(1.0) # 短暂暂停
|
||
if not grasp_strategy.head_find_target(
|
||
placement_tag,
|
||
max_search_time=15.0,
|
||
search_pattern="rotate_body" # rotate_head
|
||
):
|
||
SDKLogger.info("❌ 寻找目标失败，无法继续执行")
|
||
return
|
||
placement_tag_data = robot_vision.get_data_by_id_from_odom(placement_tag.id[0])
|
||
if placement_tag_data is None:
|
||
SDKLogger.info(f"❌ 未识别到 AprilTag ID 为{placement_tag.id[0]} 的目标箱子")
|
||
return
|
||
|
||
# 步骤6：移动到放置位置
|
||
SDKLogger.info("6. 移动到放置位置...")
|
||
move_success = grasp_strategy.walk_approach_target(
|
||
placement_tag.id[0],
|
||
target_distance=0.4,
|
||
approach_speed=0.2,
|
||
)
|
||
if not move_success:
|
||
SDKLogger.info("❌ 移动到放置位置失败")
|
||
return
|
||
time.sleep(1.0) # 短暂暂停
|
||
|
||
# 步骤7：放下箱子
|
||
placement_box_info = BoxInfo.from_apriltag(
|
||
placement_tag_data,
|
||
xyz_offset=(box_size[0]/2, 0.0, -0.5), # tag 粘贴在货架上，需要把箱子放下距离货架的高度 -0.5m
|
||
size=box_size, # 箱子尺寸(长、宽、高)，单位：米
|
||
mass=box_mass # 箱子重量，单位：千克
|
||
)
|
||
SDKLogger.info("7. 放下箱子...")
|
||
transport_down_success = grasp_strategy.arm_transport_target_down(
|
||
placement_box_info,
|
||
arm_mode="manipulation_mpc"
|
||
)
|
||
|
||
if not transport_down_success:
|
||
SDKLogger.info("❌ 放下箱子失败")
|
||
return
|
||
|
||
SDKLogger.info("✅ 成功放下箱子")
|
||
time.sleep(1.0) # 短暂暂停
|
||
|
||
# 步骤8: 回到初始位置
|
||
SDKLogger.info("8. 回到初始位置...")
|
||
grasp_strategy.robot.control_command_pose_world(0, 0, 0, 0)
|
||
time.sleep(10) # 等待10s转身完成
|
||
total_time = time.time() - start_time # 计算总时间
|
||
SDKLogger.info(f"========== 搬箱子任务完成，总耗时: {total_time:.2f}秒 ==========")
|
||
except Exception as e:
|
||
SDKLogger.info(f"执行过程中发生错误: {e}")
|
||
finally:
|
||
# 确保机器人回到安全状态
|
||
SDKLogger.info("将机器人恢复到安全姿态...")
|
||
# 这里可以添加使机器人回到默认姿态的代码
|
||
|
||
if __name__ == "__main__":
|
||
import argparse
|
||
parser = argparse.ArgumentParser()
|
||
parser.add_argument('--sim', action='store_true', default=False)
|
||
args, unknown = parser.parse_known_args()
|
||
main(not args.sim) |