策略模块搬箱子案例
(一)案例介绍
案例功能
基于二维码识别实现箱体与货架的精准定位
完成箱体的抓取、运输及摆放全流程
案例流程逻辑
机器人自主移动至预设箱体位置
机器人头部旋转扫描箱体二维码 → 精确定位 → 接近作业位置
控制机械臂位控与力控结合抓取箱体
自主转向并移动至预设货架位置
头部旋转扫描货架二维码 → 精确定位 → 接近放置位置
完成箱体放置 → 返回起始点
注意： 经测试,使用夹板搬箱子的稳定性更好,因此建议用户将末端执行器更换为夹板
更换夹板说明
夹板物料准备
用户需要自行联系3D打印厂商打印夹板, 推荐使用金属加工, 效果更好
- 需要先切换到beta分支
3D打印文件位置:~/kuavo-ros-opensource/docs/models/手掌板.STEP2D加工图纸位置:~/kuavo-ros-opensource/docs/models/手掌板.pdf
关于材质和相关加工要求, 请参考
2D加工图纸上的内容
灵巧手更换为夹板
若您的机器的末端执行器是灵巧手,可参考按如下步骤将灵巧手替换为夹板
这里以右手灵巧手更换夹板为例,左手按相同步骤操作即可
拧掉红圈中的螺丝(背面还有一个,共三个),将如图所示外壳拆下
拧掉红圈中的螺丝(背面还有两个,共四个),将灵巧手拆下(注意要扶住灵巧手防止掉落)
将灵巧手上的接线拔掉,按住箭头处拔掉即可
将夹板从下方插入图示结构件中, 并拧紧螺丝(背面还有两个,共四个),安装好夹板(注意夹板安装方向)
装上第一步拆下的手臂外壳,最终效果如图所示
自研夹爪更换为夹板
若您的机器的末端执行器是乐聚自研夹爪, 请先打印替换支架并进行替换
替换支架材料文件位置:
- 需要先切换到beta分支
3D打印文件位置:~/kuavo-ros-opensource/docs/models/手掌连接主支架.STEP2D加工图纸位置:~/kuavo-ros-opensource/docs/models/手掌连接主支架.pdf
关于材质和相关加工要求, 请参考
2D加工图纸上的内容
替换步骤(这里以左臂夹爪为例):
拧掉红圈中的螺丝(两张图片,共五个)
使用一字螺丝刀等工具,在箭头处一点点撬开结构件
撬开结构件后, 拧掉红圈中的螺丝(共四个)
按压箭头处将夹爪通信线取下,即可卸下夹爪
卸下夹爪后, 装上替换支架, 其余步骤和灵巧手更换为夹板的步骤相同
案例物料说明
箱子需要用户自行购买, 推荐使用如图所示的塑料折叠箱(箱子尺寸: 外400 300 230mm)
货架需要用户自行购买, 推荐使用如图所示的货架(需要至少三层, 货架尺寸: 1500 400 2000mm)
搬箱子场景搭建物料清单
视频展示
(二)代码仓库配置
注意： 代码仓库配置只需要执行一次, 后续修改参数后直接重新运行程序即可
上位机代码编译
- 编译相关功能包:
cd kuavo_ros_application
git checkout dev
catkin build apriltag_ros
catkin build kuavo_camera dynamic_biped kuavo_tf2_web_republisher ar_control
下位机代码编译
- 下载依赖环境
# 更新软件源
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 dev
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
sudo su
# 编译kuavo-humanoid-sdk所需功能包
catkin build kuavo_msgs ocs2_msgs motion_capture_ik
# 本地安装kuavo-humanoid-sdk：
source devel/setup.bash
cd src/kuavo_humanoid_sdk
chmod +x install.sh
./install.sh
报错解决
若前面几部配置正常无报错, 可跳过此部分
若执行
git checkout dev时报错Please commit your changes or stash them before you switch branches.- 原因:本地编译过其他分支的代码或存在自行修改的代码
- 解决:手动备份自己需要的代码,再执行
git reset --hard,就可以运行git checkout dev切换分支了
若执行
git checkout dev时报错Permission Denied- 原因:本地在sudo用户下执行过git操作
- 解决:下位机执行
sudo chmod 777 /home/lab/kuavo-ros-opensource -R,就可以运行git checkout dev切换分支了
若执行
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 信息: AprilTag ID，标签尺寸(单位是米)和基于odom坐标系下的大致位姿
- 用户使用时需将AprilTag ID和标签尺寸修改为与实际一致
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 信息: AprilTag ID，标签尺寸(单位是米)和基于odom坐标系下的大致位姿
- 用户使用时需将AprilTag ID和标签尺寸修改为与实际一致
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度
)]
)
箱子信息设定
- 描述箱子信息:宽, 长, 高(单位是米) 和 箱子重量 (单位是kg)
- 这里以示例中的箱子参数为例, 用户使用时需根据实际情况填写
box_size = (0.3, 0.4, 0.22) # xyz( 宽, 长, 高) 单位是米
box_mass = 1.5 # 箱子重量 (单位是kg)
放置点设定
- 放置点基于货架上二维码的位置进行相对调整
- 默认参数为:
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 # 箱子重量，单位：千克
)- 一般情况下需要先设定好高度的偏移量,即
xyz_offset=(box_size[0]/2, 0.0, -0.5)的第三个参数, - 默认参数中的
-0.5即表示 放置点相对货架二维码的位置低0.5米
安全区域调整
根据您的机器人型号,选择需要修改的配置文件
若您的机器人是
旧版4代, 4Pro之一- 修改
src/demo/grab_box/cfg/kuavo_v45/bt_config.yaml中的safe_space参数:
safe_space: [2.0, -4.0, 1.2, -1.2] # [x_+, x_-, y_+, y_-]更改为:
safe_space: [20.0, -20.0, 12, -12] # [x_+, x_-, y_+, y_-]- 修改
若您的机器人是
标准版, 进阶版, 展厅版, 展厅算力版, Max版之一- 修改
src/demo/grab_box/cfg/kuavo_v49/bt_config.yaml中的safe_space参数:
safe_space: [2.0, -4.0, 1.2, -1.2] # [x_+, x_-, y_+, y_-]更改为:
safe_space: [20.0, -20.0, 12, -12] # [x_+, x_-, y_+, y_-]- 修改
(四) 仿真运行
注意： gazebo对GPU有一定要求, 需要用户使用自己电脑在本地docker环境中运行, 以下为在用户电脑本地ubuntu20.04的本地docker环境运行的说明
docker环境
下载已经编译好的镜像：
wget https://kuavo.lejurobot.com/docker_images/kuavo_opensource_mpc_wbc_img_v0.6.1.tar.gz执行以下命令导入容器镜像：
docker load -i kuavo_opensource_mpc_wbc_img_v0.6.1.tar.gz以下命令二选一执行, 推荐使用带GPU版本, 后续文档以带GPU版本为例进行说明
- 运行不带GPU版本:
./docker/run.sh - 运行GPU版本，需要配置好nvidia-container-toolkit和nvidia-runtime等环境变量，可以在带GPU的宿主机上mujoco、gazebo等仿真更流畅
./docker/run_with_gpu.sh
- 运行不带GPU版本:
环境配置
仿真使用时, 需确认机器人版本
ROBOT_VERSION=45修改tag尺寸:在用户电脑本地的
/opt/ros/noetic/share/apriltag_ros/config/tags.yaml文件中将 tag 的 size 尺寸修改为与 gazebo 中tag码的尺寸一致
standalone_tags:
[
{id: 0, size: 0.088, name: 'tag_0'},
{id: 1, size: 0.088, name: 'tag_1'},
{id: 2, size: 0.088, name: 'tag_2'},
{id: 3, size: 0.088, name: 'tag_3'},
{id: 4, size: 0.088, name: 'tag_4'},
{id: 5, size: 0.088, name: 'tag_5'},
{id: 6, size: 0.088, name: 'tag_6'},
{id: 7, size: 0.088, name: 'tag_7'},
{id: 8, size: 0.088, name: 'tag_8'},
{id: 9, size: 0.088, name: 'tag_9'}
]
上位机运行
需要自行配置上位机与自己电脑的主从机通信
若以自己电脑为ROS主机, 需要先运行roscore, 再在上位机执行如下操作
上位机打开终端,运行
cd ~/kuavo_ros_application
source devel/setup.bash
roslaunch kuavo_tf2_web_republisher start_websocket_server.launch
用户电脑运行
确保已阅读(三) 预设参数调整部分, 并完成相应配置内容的检查
- 打开终端一,启动gazebo场景
cd ~/kuavo-ros-opensource
./docker/run_with_gpu.sh
source devel/setup.zsh
roslaunch humanoid_controllers load_kuavo_gazebo_manipulate.launch joystick_type:=bt2pro
- 打开终端二,启动ar_tag转换码操作和virtual操作
cd ~/kuavo-ros-opensource
./docker/run_with_gpu.sh
source devel/setup.zsh
roslaunch ar_control robot_strategies.launch
- 打开终端三,运行搬箱子示例:
cd ~/kuavo-ros-opensource
./docker/run_with_gpu.sh
source devel/setup.zsh
cd src/kuavo_humanoid_sdk/examples/strategies
python3 grasp_box_example.py --sim
(五) 实物运行
⚠️⚠️⚠️ 注意： 确保已通过零点工装完成机器人关节标定
⚠️⚠️⚠️ 注意： 确保机器人头部零点和手臂零点正确
⚠️⚠️⚠️ 注意： 机器人站立后,建议先执行原地踏步,观察机器人状态,确保机器人状态正常再进行后续操作
环境配置
- 上位机需要先修改
~/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'}
]
- 机器人会以站立时的位置作为其坐标系原点,而箱子在坐标系原点的右侧(y负方向),货架在坐标系原点的左侧(y正方向)**
启动场景说明
我们已经在gazebo仿真中模拟了搬箱子的场景, 实物场景可以参考仿真场景进行布置
机器人会以站立时的位置作为其坐标系原点
而箱子在坐标系原点的y负方向(机器人右侧),货架在坐标系原点的y正方向(机器人左侧)
机器人站立时, 需要保证站立位置在箱子和货架中间, 且机器人左手边为货架, 右手边为箱子
摆放箱子和货架的详细位置参数
以机器人站立位置为参照, 箱子需在机器人站立点右手边150cm处, 箱子上二维码中心点距地高度约为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
- 下位机打开终端三,运行搬箱子示例:
cd ~/kuavo-ros-opensource
sudo su
source devel/setup.bash
cd src/kuavo_humanoid_sdk/examples/strategies
python3 grasp_box_example.py
实物运行参数调整
因为每台机的机况不同, 箱子也可能不同, 因此用户可能需要根据实际情况调整抓取或放置的参数, 以下为两个调参示例
若箱子抓取点偏前(机器人的x正方向)
- 调整文件
src/kuavo_humanoid_sdk/examples/strategies/grasp_box_example.py - 调整前为:
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 # 箱子重量，单位：千克
)需要调整
xyz_offset=(box_size[0]/2, 0.0, -0.00),的第一个参数调整示例:修改为
xyz_offset=(box_size[0]/2 - box_size[0]/6, 0.0, -0.00),
- 调整文件
若放置点偏前(机器人的x正方向)
- 调整文件
src/kuavo_humanoid_sdk/examples/strategies/grasp_box_example.py - 调整前为:
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 # 箱子重量，单位：千克
)需要调整
xyz_offset=(box_size[0]/2, 0.0, -0.5),的第一个参数调整示例:修改为
xyz_offset=(box_size[0]/2 - box_size[0]/6, 0.0, -0.5),
- 调整文件