机器人学习之ReorientBot: Learning Object Reorientation for Specific-Posed Placement（ICRA 2022）

1 简介

Object reorientation 面向的问题是：物体初始化的位姿，无法使机器人立刻抓取并以特定的位姿放置物体。

本文提出基于视觉的操作系统ReorientBot，包含以下步骤：

1. 视觉场景理解：使用板载RGBD相机进行物体姿态估计(pose estimation)和体积重构(volumetric reconstruction)；
2. 可学习的路径点 (waypoint) 选择（waypoint指物体的抓取位姿和物体的重定位位姿）；
3. 基于选择的waypoint，使用传统motion planning生成无碰撞的轨迹。

每组waypoints包括start waypoint (grasp/regrasp pose) 和end waypoint (reorientation/placement pose).

该方法面向模型已知的物体，在仿真和实际场景中，达到93%的成功率。

与启发式方法相比，成功率提高81% (精度差值/baseline精度)，运行效率提高22%。

本文方法的主要创新：单步重定位。

以前的方法必须假定重定位的位姿是稳定的，使得一些重定位动作实现起来比较困难，如翻转物体（机械手在物体翻转后，会挡在物体下面）。本文方法结合使用稳定重定位姿态和不稳定重定位姿态，其中不稳定姿态在被机械手释放后会由于重力最终变成稳定姿态，使机器人每一步重定位都可以实现显著的旋转，实现单步重定位。

机械手类型：真空吸附。

2 方法

给定target object (当前要操作的物体)的目标状态，通过视觉场景感知、waypoint 生成和motion planning来将物体放置在目标位置。

2.1 视觉场景理解

对于场景中的target object，使用MoreFusion算法进行2D目标检测和6D姿态估计。

对于场景中的non-target object (target object 之外的所有物体)，使用体积重构算法？使用heightmap表示物体上表面的每个点到地面的距离，用来表示non-target object的状态。

算法在已知物体集合上训练。

2.2 motion waypoint 生成

使用随机采样和学习的过滤算法来选择waypoint。

2.2.1 Waypoint 采样 for pick-and-reorient

在pick-and-reorient任务中，waypoint包括开始的grasp pose，和最后的reorientation pose。

采样grasp pose：

根据target物体初始位姿，在仿真中搭建物体物体场景，使用虚拟相机获得物体mask深度图，计算物体表面法线，在物体表面提取约30个点和法线，得到抓取位置和四元数。

在物体堆附近的开放、平坦的空间中采样reorientation pose：

首先采样XY坐标：将一个$0.5m\times 0.3m$的矩形空间离散化为$10\times 8=80$个点，在每个XY位置处，构建一个立方体，边长为target物体的最长轴，检测立方体与其他non-target object是否碰撞，过滤距离物体堆太近的位置。

然后计算旋转和Z坐标：在每个无碰撞的XY坐标处，将旋转姿态的三个欧拉角都均分为8份，共$8^3=512$份。对于每个旋转姿态，计算target物体在$Z=0$时的底部到平面的距离$l$，物体的Z坐标设为$l+0.02m$，使物体位于平面上方。在所有XY处的旋转姿态和Z坐标都是可复用的。

最终得到最大$80\times 512=40960$个候选reorientation poses。

采样的reorientation poses中包含不稳定姿态，不稳定姿态在被机械手释放后会由于重力最终变成稳定姿态，使机器人每一步重定位都可以实现显著的旋转，提高运行效率。

2.2.2 reorientation pose 选择

训练一个神经网络模型来预测物体在reorientation pose被机械手放开且物体落下并稳定后，物体可以被grasp pose抓取的概率。

这里的grasp pose和上面采样的grasp pose不是一个，这里的grasp pose是怎么得到的？

使用模型从40960个候选中选取top-1000个得分最高的reorientation pose。

模型输入：object label、initial pose、reorientation pose、物体 grasp pose、物体堆的heightmap，

模型结构：CNN+maxpool+relu处理heightmap，然后与其他四个信息concat，然后经过MLP处理，输出得分。

标签采集：对于一个随机采样的reorientation pose，在仿真中物体在从pose自然下落至稳定位姿，使用motion planning来测试target grasp pose是否是可实现的。这里的grasp pose是怎么得到的？

损失：二值交叉熵

模型如下：

2.2.3 Waypoint 采样 for pick-and-place

在pick-and-reorient任务中，waypoint包括开始的grasp pose，和最后的placement pose(提前给定)。

grasp pose：与pick-and-reorient任务不同，该任务的grasp pose从物体的final pose下采样。

将仿真物体放置在final pose，在容器的开口方向上生成虚拟相机(柜子的水平方向，盒子的垂直方向)，然后采用与pick-and-reorient任务中相同的方式，在物体表面采集30个grasp pose。

placement pose：提前给定。

2.3 运动轨迹生成

由于遍历每一对waypoints来生成轨迹然后判断是否碰撞很耗时，因此引入另一个learned model来预测一对waypoints的有效性，即motion planner 基于这一对waypoints可以找到无碰撞的路径的概率。得分低的waypoints被过滤。

该模型只用于pick-and-reorient任务，不用于pick-to-place任务。因为pick-to-place任务中只有一个placement pose，waypoints对很少；而reorientation pose很多。

模型输入：和上一个模型结构类似，输入grasp pose、reorient pose、initial pose、object label、heightmap。

模型输出：grasp pose 得分、轨迹 得分、reorient 得分、关节空间中的轨迹长度。

其中，grasp pose 得分、轨迹 得分、reorient 得分 分别表示机器人在轨迹的开始、中间、最后状态是不是无碰撞的。

预测这三个变量的原因是，帮助机器人来推理为什么整个轨迹是无效的，或者说start/mid/end中哪个状态是无效的。但是，grasp pose和reorient pose本来不就是无碰撞的吗，这样会不会冗余？

损失：前三个得分采用二值交叉熵，轨迹长度采用L1 loss。

模型如下：

有效waypoints对的选择标准：先选择轨迹 得分top-10的waypoints，然后按照关节空间中的轨迹长度进行排序，长度最短的送入motion planner得到无碰撞轨迹。

3 实验

采用6个物体，尺寸中等偏大，全部来自YCB模型库。

使用OMPL实现的RRT-Connect算法生成轨迹，然后进行碰撞检测。

placement pose由程序随机分配，作为任务目标。

测试了两种不同的真空吸附器（I-shape、L-shape），上述提到的两个模型需要针对每个吸附器分别训练。

对比的启发式方法（baseline）：为了使reorientation pose是稳定的，且target grasp point可达，对于80个XY坐标，物体只沿z轴旋转，且target抓取点面向机械臂，如下右图：

结果表示，本文方法比baseline高很多。

详细结果看论文。

问题：

1、如何确定物体重定位的位姿？

答：先去除可能碰撞的XY坐标，再均匀采样旋转姿态并计算Z坐标，然后根据学习模型选出 重定位后还可以被抓取的概率高的重定位位姿（reorientation pose）。

2、已知重定位的位姿，如何生成抓取位姿和运动轨迹？

答：先采样生成抓取位姿，然后使用学习模型预测抓取-重定位位姿中间的轨迹无碰撞的抓取-重定位位姿对。使用传统的motion planner生成轨迹。