机器人抓取（六）—— 抓取点检测（抓取位姿估计） gqcnn代码测试与解读

grasp detection

两个开源工作做得比较好的二维平面抓取点检测项目。
gcnn, 文档

GR-ConvNet

1. gqcnn

Dex-Net是 Berkeley Auto Lab 维护的一个项目，旨在训练机器人学习抓取。项目主要包含两个部分：Dex-net用于生成数据集，GQ-CNN (Grasp Quality Convolutional Neural Networks)用于预测平行夹爪用候选抓取位姿从点云中成功抓取物体的可能性。
GQ-CNN 可用于快速规划抓取位姿。
gqcnn文档
本文旨在利用预训练的GQ-CNN 模型，进行grasp planning。

请注意：GQ-CNN 模型对数据生成过程中使用的以下参数敏感：

1. The robot gripper
2. The depth camera
3. The distance between the camera and workspace.

1.1 安装

建议在 Pip 和 ROS 安装中用 Conda 或 Virtualenv 创建一个新的 Python 环境。

1.1.1 ros 安装

# 下载项目代码
cd catkin_workspace/src
git clone https://github.com/BerkeleyAutomation/gqcnn.git

#  配置环境
cd gqcnn
pip install . 
1
2
3
4
5
6
7

注：pip install . 的安装方法很有可能不成功，可以找到gpu_requirements.txt文件，逐个安装。其中autolab_core需要安装为ros package，方法如下：

cd catkin_workspace/src
git clone https://github.com/BerkeleyAutomation/autolab_core.git

cd autolab_core
python setup.py install

1
2
3
4
5
6

编译功能包

cd catkin_workspace
catkin_make
1
2

1.1.2 下载预训练模型

./scripts/downloads/models/download_models.sh
1

预训练模型下载至 gqcnn/models文件夹，对于平行夹爪来说主要有两个模型：GQCNN-4.0-PJ和FC-GQCNN-4.0-PJ 。两者都是用PhotoNeo PhoXi S相机数据为平行夹爪训练的模型，图片中的物体杂乱摆放。

On-Policy Dataset Synthesis for Learning Robot Grasping Policies Using Fully Convolutional Deep Networks表明FC-GQCNN-4.0-PJ效率更高,精度也更高。 FC-GQ-CNN无需依赖交叉熵方法（CEM）来迭代的搜索抓取动作空间以获得最佳抓取策略，而是密集而有效地并行评估整个动作空间。它可以在 0.625s 内评估 5000x 此以上的 grasp，进而每小时可进行 296 次抓取.

1.2 测试

测试的预训练模型为：GQCNN-4.0-PJ 和 FC-GQCNN-4.0-PJ。测试用例在data/examples/clutter/phoxi/文件夹下，GQCNN-4.0-PJ模型测试图片在dex-net_4.0文件夹，FC-GQCNN-4.0-PJ测试图片在gqcnn 文件夹。

上述示例都是Phoxi S相机的图片，如果要测试其他输入数据，则需要修改cfg/examples/fc_gqcnn_pj.yaml文件下[“policy”][“metric”][“fully_conv_gqcnn_config”] 的高度和宽度配置。

1.2.1 python 测试

测试预训练的GQ-CNN 平行夹爪网络，需要使用的深度图和分割图（data/examples中有测试用例）

python examples/policy.py GQCNN-4.0-PJ --depth_image data/examples/clutter/phoxi/dex-net_4.0/depth_0.npy --segmask data/examples/clutter/phoxi/dex-net_4.0/segmask_0.png --camera_intr data/calib/phoxi/phoxi.intr
1

如果要测试 FC-GQ-CNN 网络，需要添加 --fully_conv 标记。例如：

python examples/policy.py FC-GQCNN-4.0-PJ --fully_conv --depth_image data/examples/clutter/phoxi/fcgqcnn/depth_0.npy --segmask data/examples/clutter/phoxi/fcgqcnn/segmask_0.png --camera_intr data/calib/phoxi/phoxi.intr
1

rgb原图及抓取点的预测图如下(GQCNN-4.0-PJ)：

也可以直接运行测试 the pre-trained parallel jaw FC-GQ-CNN model 的脚本：

 ./scripts/policies/run_all_dex-net_4.0_fc_pj_examples.sh
1

1.2.2 ros 测试

1. 启动预训练的 Dex-Net 4.0 平行夹爪 ros 抓取规划服务：

roslaunch gqcnn grasp_planning_service.launch model_name:=GQCNN-4.0-PJ
1

• model_name: Name of the GQ-CNN model to use. Default is GQCNN-4.0-PJ.
• model_dir: GQ-CNN models 的存储路径. Default is models/. If you are using the provided download_models.sh script, you shouldn’t have to modify this.
• fully_conv：注意与model_name相对应。如果采用了FC-GQCNN-4.0-PJ，fully_conv则为 True：

roslaunch gqcnn grasp_planning_service.launch model_name:=FC-GQCNN-4.0-PJ fully_conv:=True
1

2. 启动示例 ROS policy，（使用预先保存的图片）

python examples/policy_ros.py --depth_image data/examples/clutter/phoxi/dex-net_4.0/depth_0.npy --segmask data/examples/clutter/phoxi/dex-net_4.0/segmask_0.png --camera_intr data/calib/phoxi/phoxi.intr
# 也可以只用深度图
python examples/policy_ros.py --depth_image data/examples/clutter/phoxi/dex-net_4.0/depth_0.npy --camera_intr data/calib/phoxi/phoxi.intr
1
2
3

• depth_image_filename: Path to a depth image (float array in .npy format).
• segmask_filename: Path to an object segmentation mask (binary image in .png format).
• camera_intr_filename: 相机内参路径 (.intr file generated with BerkeleyAutomation’s perception package).

注：ros测试中segmask 图不是必须的。如果segmask is None，程序会根据深度图和rgb 图自动生成 segmask。但 python 测试中 segmask 图是必须的。

debug：

1. ModuleNotFoundError: No module named 'rospkg'
   因为运行环境为python3。
   解决方法: pip3 install rospkg。

2. cannot open shared object file
   OSError: /home/ur/miniforge3/envs/grasp/lib/libgeos_c.so: cannot open shared object file: No such file or directory
   解决办法：locate libgeos_c.so 找到 libgeos_c.so 的路径。然后将这三个文件复制到/home/ur/miniforge3/envs/grasp/lib/文件夹。
   

3. ImportError: dynamic module does not define module export function (PyInit_cv_bridge_boost)
   这是因为ros 自带的 cv_bridge只支持 python2。想要使用Python3需要另外创建 ros 工作空间,自行编译cv_bridge包。然后在 python3 环境执行：

source ~/cvbridge_build_ws/install/setup.bash --extend
1

详细解决方法参见连接：Jetson AGX Xavier避坑指南(五)——环境搭建2.(python3、conda、ros、pytorch、tensorflow)

1.3 ros 代码导读

本文仅解读 FC-GQCNN-PJ 部分。PJ 表示平行夹爪。
输入深度图(.npy)和 sagmask 图(可选)，输出抓取点。
采用了ros service 通信机制。

1.3.1 客户端：ros_policy.py

客户端节点：grasp_planning_example。

主要输入：depth_image (.npy), segmask(也可以没有)，camera_intr(相机内参配置文件路径)。

订阅/grasp_planner和/grasp_planner_segmask服务。前者适用于仅有depth_image的情况，后者用于有 depth_image和 segmask 图的情况。下面我们仅查看不需要 segmask 图的/grasp_planner服务。注意：虽然服务端需要输入 color_image,但客户端并没有提供 color_image输入，而是以一个与深度图同尺寸的全零矩阵替代。

客户端调用/grasp_planner服务，得到grasp=grasp_resp.grasp数据。进而转换为抓取动作。
Grasp2D (gqcnn/grasping/grasp.py)：Parallel-jaw grasp in image space。其 pose()函数会返回 grasp 在相机坐标系中的坐标(type: autolab_core.RigidTransform)，该函数有一个输入参数grasp_approach_dir，是相对于相机坐标系的。
GraspAction:进一步封装grasp 和Grasp2D数据。

1.3.2 服务端：graspy_planner_node.py

main

1. 创建Grasp_Sampler_Server node。

2. 加载配置文件：fully_conv、parallel_jaw 类型的配置文件路径为 "cfg/examples/ros/fc_gqcnn_pj.yaml" 该文件引入了"cfg/examples/fc_gqcnn_pj.yaml"文件，并重新定义配置文件中的 visualization 部分（vis）。

3. 声明一个Publiser： grasp_pose_publisher，用于发布抓取位姿信息（"/gqcnn_grasp/pose",PoseStamped）。

4. 初始化抓取策略：grasping_policy = FullyConvolutionalGraspingPolicyParallelJaw(policy_cfg)。

5. 初始化抓取规划器：grasp_planner = GraspPlanner(cfg, cv_bridge, grasping_policy, grasp_pose_publisher)。

6. 声明一个 Service： grasp_planner，服务类型GQCNNGraspPlanner，回调函数grasp_planner.plan_grasp。

srv

srv类型为GQCNNGraspPlanner.srv：

# request params
sensor_msgs/Image color_image
sensor_msgs/Image depth_image
sensor_msgs/CameraInfo camera_info
---
# response params
GQCNNGrasp grasp
1
2
3
4
5
6
7

GQCNNGrasp.msg 定义如下

geometry_msgs/Pose pose
float64 q_value

uint8 PARALLEL_JAW=0
uint8 SUCTION=1
uint8 grasp_type

# grasp representation in image space.
float64[2] center_px
float64 angle
float64 depth
sensor_msgs/Image thumbnail
1
2
3
4
5
6
7
8
9
10
11
12

grasp_planner服务的回调函数为 plan_grasp().接收参数为深度图、sagmask图、相机参数。
如果没有 sagmask 图，程序会根据深度图自动生成。

如果要用自己的相机，注意修改相机参数配置。配置文件在 data/calib/文件夹下，新建一个自己相机的文件名如 k4a 然后添加两个配置文件：k4a.intr，k4a_to_world.tf。注意修改 cfg/examples/fc_gqcnn_pj.yaml 中输入的图像尺寸参数im_height和im_width。（或resize 图像尺寸，并修改 k4a.intr参数）。

grasp planner

GraspPlanner类中的plan_grasp(req)函数是服务的回调函数。

plan_grasp(req)

首先调用self.read_images(req)函数，将输入数据用 cv_bridge解析为 color_im、depth_im 和 camera_intr，数据类别为 perception 定义的 ColorImage, DepthImage。

然后调用self._plan_grasp(color_im, depth_im, camera_intr)函数Planning Grasp。

self._plan_grasp()

如果没有segmask，该函数会用 perception 定义的 BinaryImage 根据 depth_im自动生成segmask图。
然后将rgbd_im, camera_intr,segmask封装为rgbd_state（RgbdImageState类）。

然后调用execute_policy函数。

execute_policy(rgbd_state, self.grasping_policy, self.grasp_pose_publisher, camera_intr.frame)
1

execute_policy()

grasp = grasping_policy(rgbd_state) 。（ graspy_planner_node.py 中有定义：grasping_policy = FullyConvolutionalGraspingPolicyParallelJaw(policy_cfg)。稍后我们会看一下FullyConvolutionalGraspingPolicyParallelJaw是如何运行的）

然后将数据解析并赋给GQCNNGrasp类型的gqcnn_grasp。

将 gqcnn_grasp.pose封装为PoseStamped类型的pose_stamped，然后grasp_pose_publisher.publish(pose_stamped)

最后返回gqcnn_grasp。

1.3.3 grasping policy

下面我们来看一下 grasping_policy是如何工作的。

所有的GQ-CNN 抓取策略都是 GraspingPolicy 的子类。输入 RgbdImageStates，返回 GraspAction。
FC-GQCNN-PJ模型的grasping_policy为FullyConvolutionalGraspingPolicyParallelJaw (定义在gqcnn/grasping/policy/fc_policy.py)，派生关系如下：

Policy -> GraspingPolicy -> FullyConvolutionalGraspingPolicy->FullyConvolutionalGraspingPolicyParallelJaw

Policy 是抽象基类，定义了抽象方法 action(state)，Returns an action for a given state, GraspingPolicy 类引入了grasp_quality_fn实例属性, 然后在 action() 函数中定义了抽象方法_action(state)。最后由FullyConvolutionalGraspingPolicy类实现_action(state)方法。

execute_policy() 函数中主要用到了grasping_policy的三个属性(或方法)：

• .q_value
• .grasp（type ：Grasp2D, 包含.pose()、.center、.depth、.angle等方法和属性）、
• .image.rosmsg

1.3.1 Policy

gqcnn/grasping/policy/policy.py

Policy 是抽象基类，定义如下：

class Policy(ABC):
    """Abstract policy class."""
    
    def __call__(self, state):
        """Execute the policy on a state."""
        return self.action(state)

    @abstractmethod
    def action(self, state):
        """Returns an action for a given state."""
        pass
1
2
3
4
5
6
7
8
9
10
11

__call__()函数使类的实例可以当做函数调用。即：

grasping_policy = Policy() #实例化
grasp = grasping_policy(rgbd_state) #函数调用
# 等同于
grasp = grasping_policy.action(rgbd_state)
1
2
3
4

而 action(state)是抽象方法，需要在子类GraspingPolicy中实现。

1.3.2 GraspingPolicy

gqcnn/grasping/policy/policy.py

1.__init__(self,config)：
载入 policy config 数据，即"cfg/examples/ros/fc_gqcnn_pj.yaml"的 "policy"部分。

定义属性： self._grasp_quality_fn ，并允许作为 grasp_quality_fn 属性调用。

2.实现action(state)方法:
action 函数并没有实现啥功能，而是调用了_action(state) 函数，而 _action(state) 是抽象方法，在子类 FullyConvolutionalGraspingPolicy 中实现。

self._grasp_quality_fn

我们来看一下self._grasp_quality_fn干了什么。

self._grasp_quality_fn = GraspQualityFunctionFactory.quality_function( metric_type, self._metric_config)
然后quality_function返回了 FCGQCnnQualityFunction(config) 。 FCGQCnnQualityFunction类加载FCGQCNNTF网络模型，返回quality()。quality函数return FCGQCNNTF模型的predict(images, depths)。predict 根据深度图像预测抓取成功的概率和抓握姿势。

FCGQCnnQualityFunction类的父类是抽象基类GraspQualityFunction，GraspQualityFunction在 call 函数中 return self.quality(state, actions, params)。即可以将FCGQCnnQualityFunction类的实例当做方法调用，返回的是 quality 函数。

1.3.3 FullyConvolutionalGraspingPolicy

gqcnn/grasping/policy/fc_policy.py

实现_action(state)方法：

1. 解包 state(type: RgbdImageState) 数据。然后提取 images 和 depths。

2. preds = self._grasp_quality_fn.quality(images, depths)：预测抓取成功概率。

3. _mask_predictions() ：用裁剪和降采样的 sagmask 图像，进一步筛选出在物体上的预测成功的grasps.

4. _sample_predictions()：采样 num_actions=1 个predictions。

5. _get_actions() :将返回数据封装为 GraspAction 类型。_get_actions() 函数为抽象方法定义在子类FullyConvolutionalGraspingPolicyParallelJaw中。

GraspAction

GraspAction 实例主要有四个属性，execute_policy() 函数用到了前三个。

• q_value
• grasp（type ：Grasp2D, 包含.pose()、.center、.depth、.angle等方法和属性）
• image.rosmsg
• policy_name

Grasp2D

• pose(grasp_approach_dir=None):计算grasp 在相机坐标系的 3d 位姿。grasp_approach_dir是接近目标物体的方向(相机坐标系)。
• depth grasp: 中心点的深度(3d)
• center:图像坐标系下的grasp 中心点。
• angle ：Grasp axis angle with the camera x-axis。

Grasp2D.pose()

pose(grasp_approach_dir=None)
计算grasp 在相机坐标系的 3d 位姿。
参数：grasp_approach_dir (numpy.ndarray) 是接近目标物体的方向(相机坐标系),如果未指定，则使用相机光轴方向。
在相机坐标系下抓取位姿矩阵：grasp_rot_camera的 x,y,z 轴定义如下：
grasp_x_camera = grasp_approach_dir （type: np.array:(3,)）
grasp_y_camera = grasp_axis_camera （np.array:(3,)）
grasp_z_camera = np.cross(grasp_x_camera, grasp_y_camera) ( np.array:(3,))
相机坐标系下抓取中心点坐标 grasp_center_camera通过下列函数计算

 grasp_center_camera = self.camera_intr.deproject_pixel(self.depth, center_px_im)
1

camera_intr.deproject_pixel函数主要内容为point_3d = depth * np.linalg.inv(self._K).dot(np.r_[pixel.data, 1.0]),即
[ x y z ] = d e p t h ∗ [ f x γ c x 0 f y c y 0 0 1 ] − 1 ∗ [ pixel.x pixel.x 1 ] \left[

$$\begin{matrix} x\\ y\\ z\end{matrix}$$ \right] =depth * \left[ $$\begin{array}{ccc}f_x & \gamma & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1\end{array}$$ \right]^{-1} * \left[ $$\begin{matrix} \text{pixel.x}\\ \text{pixel.x} \\ 1\end{matrix}$$ \right] ⎣⎡​xyz​⎦⎤​=depth∗⎣⎡​fx​00​γfy​0​cx​cy​1​⎦⎤​−1∗⎣⎡​pixel.xpixel.x1​⎦⎤​

Returns：从 grasp 到相机坐标系的转换。

T_grasp_camera = RigidTransform(rotation=grasp_rot_camera,
                                translation=grasp_center_camera,
                                from_frame="grasp", 
                                to_frame=self.camera_intr.frame)  
                                # from_frame and to_frame just a name
return T_grasp_camera
1
2
3
4
5
6

Return type：autolab_core.RigidTransform

RigidTransform 似乎啥也没干，仅仅把输入的参数以各种输出形式（如四元数、欧拉角、geometry_msgs.msg.Pose）封装一下。

RigidTransform

autolab_core.RigidTransform
刚体从一个坐标系到另一个坐标系的变换。

重要属性

• pose_msg
  geometry_msgs.msg.Pose， The rigid transform as a geometry_msg pose.