策略模块搬箱子案例

策略模块搬箱子案例

(一)案例介绍

案例功能

基于二维码识别实现箱体与货架的精准定位
完成箱体的抓取、运输及摆放全流程

案例流程逻辑

机器人自主移动至预设箱体位置
机器人头部旋转扫描箱体二维码 → 精确定位 → 接近作业位置
控制机械臂位控与力控结合抓取箱体
自主转向并移动至预设货架位置
头部旋转扫描货架二维码 → 精确定位 → 接近放置位置
完成箱体放置 → 返回起始点

注意：经测试,使用夹板搬箱子的稳定性更好,因此建议用户将末端执行器更换为夹板

更换夹板说明

夹板物料准备

用户需要自行联系3D打印厂商打印夹板, 推荐使用金属加工, 效果更好
- 需要先切换到beta分支
- 3D打印文件位置: ~/kuavo-ros-opensource/docs/models/手掌板.STEP
- 2D加工图纸位置: ~/kuavo-ros-opensource/docs/models/手掌板.pdf
关于材质和相关加工要求, 请参考2D加工图纸上的内容

灵巧手更换为夹板

若您的机器的末端执行器是灵巧手,可参考按如下步骤将灵巧手替换为夹板
- 这里以右手灵巧手更换夹板为例,左手按相同步骤操作即可
- 拧掉红圈中的螺丝(背面还有一个,共三个),将如图所示外壳拆下
- 拧掉红圈中的螺丝(背面还有两个,共四个),将灵巧手拆下(注意要扶住灵巧手防止掉落)
- 将灵巧手上的接线拔掉,按住箭头处拔掉即可
- 将夹板从下方插入图示结构件中, 并拧紧螺丝(背面还有两个,共四个),安装好夹板(注意夹板安装方向)
- 装上第一步拆下的手臂外壳,最终效果如图所示

自研夹爪更换为夹板

若您的机器的末端执行器是乐聚自研夹爪, 请先打印替换支架并进行替换
- 替换支架材料文件位置:
  - 需要先切换到beta分支
  - 3D打印文件位置: ~/kuavo-ros-opensource/docs/models/手掌连接主支架.STEP
  - 2D加工图纸位置: ~/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

环境配置

仿真使用时, 需确认机器人版本 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),

策略模块搬箱子案例

(一)案例介绍​

案例功能​

案例流程逻辑​

更换夹板说明​

夹板物料准备​

灵巧手更换为夹板​

自研夹爪更换为夹板​

案例物料说明​

视频展示​

(二)代码仓库配置​

上位机代码编译​

下位机代码编译​

下位机kuavo-humanoid-sdk安装​

报错解决​

(三) 预设参数调整​

箱子与货架位置设定​

箱子信息设定​

放置点设定​

安全区域调整​

(四) 仿真运行​

docker环境​

环境配置​

上位机运行​

用户电脑运行​

(五) 实物运行​

环境配置​

启动场景说明​

上位机运行​

下位机运行​

实物运行参数调整​

(一)案例介绍

案例功能

案例流程逻辑

更换夹板说明

夹板物料准备

灵巧手更换为夹板

自研夹爪更换为夹板

案例物料说明

视频展示

(二)代码仓库配置

上位机代码编译

下位机代码编译

下位机kuavo-humanoid-sdk安装

报错解决

(三) 预设参数调整

箱子与货架位置设定

箱子信息设定

放置点设定

安全区域调整

(四) 仿真运行

docker环境

环境配置

上位机运行

用户电脑运行

(五) 实物运行

环境配置

启动场景说明

上位机运行

下位机运行

实物运行参数调整