策略模块搬箱子案例
⚠️⚠️⚠️ 注意： 综合案例为参考案例，复杂度较高，使用该案例需要具备相关领域基础知识
⚠️⚠️⚠️ 注意： 综合案例非标准交付功能，仅提供参考
(一)案例介绍
案例功能
基于二维码识别实现箱体与货架的精准定位
完成箱体的抓取、运输及摆放全流程
案例流程逻辑
机器人自主移动至预设箱体位置
机器人头部旋转扫描箱体二维码 → 精确定位 → 接近作业位置
控制机械臂位控与力控结合抓取箱体
自主移动至预设货架位置
头部旋转扫描货架二维码 → 精确定位 → 接近放置位置
完成箱体放置 → 返回起始点
注意： 经测试,使用夹板搬箱子的稳定性更好,因此建议用户将末端执行器更换为夹板
更换夹板说明
夹板物料准备
用户需要自行联系3D打印厂商打印夹板, 推荐使用金属加工, 效果更好
- gitee仓库模型文件位置:
https://gitee.com/leju-robot/kuavo-ros-opensource/tree/master/docs/models 3D打印文件位置:~/kuavo-ros-opensource/docs/models/手掌板.STEP2D加工图纸位置:~/kuavo-ros-opensource/docs/models/手掌板.pdf
- gitee仓库模型文件位置:
关于材质和相关加工要求, 请参考
2D加工图纸上的内容
灵巧手更换为夹板
若您的机器的末端执行器是灵巧手,可参考按如下步骤将灵巧手替换为夹板
这里以右手灵巧手更换夹板为例,左手按相同步骤操作即可
拧掉红圈中的螺丝(背面还有一个,共三个),将如图所示外壳拆下
拧掉红圈中的螺丝(背面还有两个,共四个),将灵巧手拆下(注意要扶住灵巧手防止掉落)
将灵巧手上的接线拔掉,按住箭头处拔掉即可
将夹板从下方插入图示结构件中, 并拧紧螺丝(背面还有两个,共四个),安装好夹板(注意夹板安装方向)
装上第一步拆下的手臂外壳,最终效果如图所示
自研夹爪更换为夹板
若您的机器的末端执行器是乐聚自研夹爪, 请先打印替换支架并进行替换
替换支架材料文件位置:
- gitee仓库模型文件位置:
https://gitee.com/leju-robot/kuavo-ros-opensource/tree/master/docs/models 3D打印文件位置:~/kuavo-ros-opensource/docs/models/手掌连接主支架.STEP2D加工图纸位置:~/kuavo-ros-opensource/docs/models/手掌连接主支架.pdf
- gitee仓库模型文件位置:
关于材质和相关加工要求, 请参考
2D加工图纸上的内容
替换步骤(这里以左臂夹爪为例):
拧掉红圈中的螺丝(两张图片,共五个)
使用一字螺丝刀等工具,在箭头处一点点撬开结构件
撬开结构件后, 拧掉红圈中的螺丝(共四个)
按压箭头处将夹爪通信线取下,即可卸下夹爪
卸下夹爪后, 装上替换支架, 其余步骤和灵巧手更换为夹板的步骤相同
案例物料说明
箱子需要用户自行购买, 推荐使用如图所示的塑料折叠箱(箱子尺寸: 外400 300 230mm)
货架需要用户自行购买, 推荐使用如图所示的货架(需要至少三层, 货架尺寸: 1500 400 2000mm)
搬箱子场景搭建物料清单
视频展示
(二)代码仓库配置
注意： 代码仓库配置只需要执行一次, 后续修改参数后直接重新运行程序即可
上位机代码编译
- 编译相关功能包:
cd ~/kuavo_ros_application
git checkout opensource/navigation_demo
catkin build apriltag_ros
catkin build kuavo_camera dynamic_biped kuavo_tf2_web_republisher
下位机代码编译
- 下载依赖环境
# 更新软件源
sudo apt-get update
# grab_box功能包相关依赖
sudo apt-get install ros-noetic-behaviortree-cpp-v3 ros-noetic-roslint ros-noetic-eigen-conversions -y
sudo apt-get install libgeographic-dev ros-noetic-geographic* -y
- 编译相关功能包:
cd ~/kuavo-ros-opensource
git checkout opensource/kuavo4pro_course_code
sudo su
catkin clean -y
catkin config -DCMAKE_ASM_COMPILER=/usr/bin/as -DCMAKE_BUILD_TYPE=Release
source installed/setup.bash
catkin build humanoid_controllers kuavo_msgs gazebo_sim ar_control grab_box
下位机kuavo-humanoid-sdk安装
cd ~/kuavo-ros-opensource/src/kuavo_humanoid_sdk/
./install.sh
报错解决
若前面几部配置正常无报错, 可跳过此部分
若执行
git checkout时报错Please commit your changes or stash them before you switch branches.- 原因:本地编译过其他分支的代码或存在自行修改的代码
- 解决:手动备份自己需要的代码,再执行
git reset --hard,就可以运行git checkout切换分支了
若执行
git checkout时报错Permission Denied- 原因:本地在sudo用户下执行过git操作
- 解决:下位机执行
sudo chmod 777 /home/lab/kuavo-ros-opensource -R,就可以运行git checkout切换分支了
若执行
sudo apt update时报错- 需要手动更新ros源,按如下步骤操作
# 删除现有 ROS 密钥
sudo apt-key del F42ED6FBAB17C654
# 重新导入 ROS 官方密钥
sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-keys F42ED6FBAB17C654
# 更新密钥环
sudo apt update- 之后再执行
sudo apt update即可
(三) 预设参数调整
⚠️⚠️⚠️ 注意： 调整参数后建议先在gazebo仿真中进行测试, 确认效果无误后再在实物上运行
文件位置: src/kuavo_humanoid_sdk/examples/strategies/grasp_box_example.py
距离配置
- 抓取/放置时机器人与 AprilTag 的停靠距离（不能为负数）:
GRASP_DISTANCE = 0.28 # 抓取时与AprilTag的距离(米)
PLACE_DISTANCE = 0.53 # 放置时与AprilTag的距离(米)
中间位置配置
- 放置位置的 AprilTag 在机器人后方时，需要先走到中间位置再转向识别:
MID_POSITION = (-0.5, 0.0) # 中间位置坐标 (odom坐标系)
箱子与货架二维码配置
粘贴在箱子上的 AprilTag 信息：AprilTag ID、标签尺寸(单位是米)和基于odom坐标系下的大致位姿
- 用户使用时需将 AprilTag ID 和标签尺寸修改为与实际一致
target_april_tag = AprilTagData(
id=[1], # AprilTag ID
size=[0.1], # AprilTag 标签尺寸
pose=[PoseQuaternion(
# TODO: 需要根据实际情况调整
position=(1.0, 0.0, 0.8), # 基于odom坐标系下的大致位置, 查找时会对齐到这个方向
orientation=(0.0, 0.0, 0.0, 1.0) # 四元数方向
)]
)粘贴在放置位置(货架)的 AprilTag 信息：AprilTag ID、标签尺寸(单位是米)和基于odom坐标系下的大致位姿
- 用户使用时需将 AprilTag ID 和标签尺寸修改为与实际一致
placement_april_tag = AprilTagData(
id=[2], # 放置位置的AprilTag ID
size=[0.1], # AprilTag 标签尺寸
pose=[PoseQuaternion(
# TODO: 需要根据实际情况调整
position=(-1.0, 0.0, 0.8), # 放置位置的大致坐标
orientation=(0.0, 0.0, 1.0, 0.0) # 四元数方向 - 旋转180度
)]
)
箱子信息设定
- 描述箱子信息：宽, 长, 高(单位是米) 和 箱子重量 (单位是kg)
- 这里以示例中的箱子参数为例, 用户使用时需根据实际情况填写
box_size = (0.3, 0.4, 0.22) # xyz(宽, 长, 高) 单位是米
box_mass = 1.5 # 箱子重量 (单位是kg)
抓取点偏移配置
xyz_offset为相对于 AprilTag 中心点的偏移量，用于将坐标从标签平面偏移到箱子中心:
box_info = BoxInfo.from_apriltag(
box_tag_data,
xyz_offset=(box_size[0]/2 + 0.03, 0.0, -0.06), # tag粘贴在箱子正面，偏移到箱子中心
size=box_size,
mass=box_mass
)
预设手臂姿态（可以通过手臂软件调整）
- 手臂在抓取前按顺序经过的预设关节角度（角度制），每组14个关节：左臂7个 + 右臂7个:
ARM_POSES = {
# 安全收起姿态
"side": [
20, 90.8, 0.0, -30, 0.0, 0.0, 0.0, # 左臂
20, -90.8, 0.0, -30, -0.0, 0.0, 0.0 # 右臂
],
"safe": [
7.6, 68.8, 0.0, -90.8, 77.1, 0.0, 0.0, # 左臂
7.6, -68.8, 0.0, -90.8, -77.1, 0.0, 0.0 # 右臂
],
# 手臂抬高准备姿态（避免撞到桌子）
"prepare_high": [
7.6, 68.8, -25.0, -90.8, 77.1, 0.0, 0.0, # 左臂
7.6, -68.8, 25.0, -90.8, -77.1, 0.0, 0.0 # 右臂
],
}
备用放置位置
- 当无法识别放置位置 AprilTag 时，使用固定坐标作为备用:
placement_pose = KuavoPose(
position=(0.5, 0.0, 0.8), # 目标放置位置坐标（机器人正前方0.5m）
orientation=(0.0, 0.0, 0.0, 1.0) # 目标放置方向（四元数）yaw=0°
)
(四) 实物运行
⚠️⚠️⚠️ 注意： 确保已通过零点工装完成机器人关节标定,参考:机器人关节标定
⚠️⚠️⚠️ 注意： 确保机器人头部零点和手臂零点正确
⚠️⚠️⚠️ 注意： 机器人站立后,建议先执行原地踏步,观察机器人状态,确保机器人状态正常再进行后续操作
⚠️⚠️⚠️ 注意： 当前版本机器人抓取箱子时不会下蹲，箱子所在平台高度需在 0.7m～0.9m 范围内，超出此范围手臂可能无法到达抓取位置
环境配置
- 上位机需要先修改
~/kuavo_ros_application/src/ros_vision/detection_apriltag/apriltag_ros/config/tags.yaml文件, 将 tag 的 size 尺寸修改为实际大小，比如 0.1 米
standalone_tags:
[
{id: 0, size: 0.1, name: 'tag_0'},
{id: 1, size: 0.1, name: 'tag_1'},
{id: 2, size: 0.1, name: 'tag_2'},
{id: 3, size: 0.1, name: 'tag_3'},
{id: 4, size: 0.1, name: 'tag_4'},
{id: 5, size: 0.1, name: 'tag_5'},
{id: 6, size: 0.1, name: 'tag_6'},
{id: 7, size: 0.1, name: 'tag_7'},
{id: 8, size: 0.1, name: 'tag_8'},
{id: 9, size: 0.1, name: 'tag_9'}
]
- 机器人会以站立时的位置作为其坐标系原点,箱子在坐标系原点的正前方(x正方向),货架在坐标系原点的正后方(x负方向)
启动场景说明
机器人会以站立时的位置作为其坐标系原点，面朝x轴正方向站立
箱子在机器人正前方(x正方向)，货架（放置位置）在机器人正后方(x负方向)
机器人站立时，需保证站立位置在箱子与货架之间的连线上，正前方朝向箱子
摆放箱子和货架的详细位置参数
以机器人站立位置为参照，箱子需在机器人正前方约150cm处，箱子需放置于高度 0.7m～0.9m 的平台上（当前版本机器人不会下蹲，超出此范围手臂无法到达），箱子上二维码中心点距地高度约为80cm
以机器人站立位置为参照，货架需在机器人正后方约150cm处，放置层高度约为70cm，货架上二维码中心点距地高度约为180cm
若用户的使用场景与案例中完全一致, 可先按照实物运行参数调整预先调整好参数, 再在实物上运行
上位机运行
- 上位机打开终端,运行
cd ~/kuavo_ros_application
source devel/setup.bash
# 根据您的机器人版本运行不同的命令
# 旧版4代, 4Pro
roslaunch dynamic_biped load_robot_head.launch
# 标准版, 进阶版, 展厅版, 展厅算力版
roslaunch dynamic_biped load_robot_head.launch use_orbbec:=true
# Max版
roslaunch dynamic_biped load_robot_head.launch use_orbbec:=true enable_wrist_camera:=true
- 另启一个新终端,运行
cd ~/kuavo_ros_application
source devel/setup.bash
roslaunch kuavo_tf2_web_republisher start_websocket_server.launch
下位机运行
注意：
⚠️⚠️⚠️ 确保已阅读(三) 预设参数调整部分, 并完成相应配置内容的检查
⚠️⚠️⚠️ 受imu飘移及机器人机况影响, 若需要连续搬运, 建议搬运三次后就关掉程序让机器人重新站立, 这样在机况良好的情况下可以达到90%以上的搬箱子成功率
- 下位机打开终端一,让机器人站立(推荐使用tmux终端, 防止网络问题导致终端断开)
cd ~/kuavo-ros-opensource
sudo su
source devel/setup.bash
roslaunch humanoid_controllers load_kuavo_real.launch joystick_type:=bt2pro
- 下位机打开终端二,启动Tag Tracker节点
cd ~/kuavo-ros-opensource
sudo su
source devel/setup.bash
roslaunch ar_control robot_strategies.launch real:=true
- 下位机打开终端三,运行搬箱子示例:
命令行参数说明：
| 参数 | 说明 |
|---|---|
--sim | 仿真模式（实物运行不需要此参数） |
--velocity-control | 使用速度控制模式（运动更平滑，推荐使用） |
--wait-for-bottle | 等待抓水瓶任务完成信号后再开始（用于连续任务） |
cd ~/kuavo-ros-opensource
sudo su
source devel/setup.bash
cd src/kuavo_humanoid_sdk/examples/strategies
# 基本运行（默认单步控制模式）
python3 grasp_box_example.py
# 使用速度控制模式（推荐，运动更平滑）
python3 grasp_box_example.py --velocity-control
实物运行参数调整
因为每台机的机况不同, 箱子也可能不同, 因此用户可能需要根据实际情况调整抓取或放置的参数
调整抓取/放置距离（
GRASP_DISTANCE/PLACE_DISTANCE）- 若机器人停靠位置与箱子或货架的距离不合适，调整文件顶部的距离配置：
GRASP_DISTANCE = 0.28 # 抓取时与AprilTag的距离(米)
PLACE_DISTANCE = 0.53 # 放置时与AprilTag的距离(米)调整中间位置（
MID_POSITION）- 若机器人搬运路径上需要经过不同的中间点，调整：
MID_POSITION = (-0.5, 0.0) # 中间位置坐标 (odom坐标系)若箱子抓取点偏前(机器人的x正方向)
- 需要调整
xyz_offset的第一个参数：
# 调整前
box_info = BoxInfo.from_apriltag(
box_tag_data,
xyz_offset=(box_size[0]/2 + 0.03, 0.0, -0.06),
size=box_size,
mass=box_mass
)
# 调整示例：x方向减小0.05m
box_info = BoxInfo.from_apriltag(
box_tag_data,
xyz_offset=(box_size[0]/2 + 0.03 - 0.05, 0.0, -0.06),
size=box_size,
mass=box_mass
)- 需要调整
调整手臂预设姿态
- 若需要调整手臂经过的预设点，修改
ARM_POSES字典中对应姿态的关节角度（角度制）：
ARM_POSES = {
"safe": [
7.6, 68.8, 0.0, -90.8, 77.1, 0.0, 0.0, # 左臂7个关节角度
7.6, -68.8, 0.0, -90.8, -77.1, 0.0, 0.0 # 右臂7个关节角度
],
}- 若需要调整手臂经过的预设点，修改
故障排查
| 问题 | 解决方案 |
|---|---|
| 找不到AprilTag | 检查标签ID配置（箱子ID=1，放置位置ID=2），检查尺寸配置和光照条件 |
| 抓取失败 | 调整 GRASP_DISTANCE 参数或 xyz_offset 偏移量 |
| 放置失败 | 调整 PLACE_DISTANCE 参数 |
| 行走不稳定 | 使用 --velocity-control 参数，或检查箱子重量和手臂姿态配置 |
| 无法识别放置位置 | 检查 MID_POSITION 中间位置设置，确保机器人能转向识别AprilTag |
| WebSocket连接失败 | 确认 kuavo_tf2_web_republisher 服务已启动 |
| basePitch限制警告 | 程序会自动关闭限制，若仍有问题请检查机器人状态 |