两个开源工作做得比较好的二维平面抓取位姿估计项目:
gqcnn, 文档

GR-ConvNet
本文测试了 gqcnn。

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 pip 安装

 git clone https://github.com/BerkeleyAutomation/gqcnn.git
#  配置环境
cd gqcnn
pip install .

1.1.2 ros 安装

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

# 编译功能包
cd catkin_workspace
catkin_make

1.1.3 提示

pip install . 的安装方法很有可能不成功,可以找到gpu_requirements.txt文件,逐个安装。
例如,经常安装失败的autolab_core

pip install autolab_core

如果使用 ros ,autolab_core需要安装为ros package,方法如下:

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

cd autolab_core
python setup.py install

cd catkin_workspace
catkin_make

1.1.4 下载预训练模型

./scripts/downloads/models/download_models.sh

预训练模型下载至 gqcnn/models文件夹,对于平行夹爪来说主要有两个模型:GQCNN-4.0-PJFC-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

如果要测试 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

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.2.2 ros 测试

  1. 启动预训练的 Dex-Net 4.0 平行夹爪 ros 抓取规划服务:
    roslaunch gqcnn grasp_planning_service.launch model_name:=GQCNN-4.0-PJ
    
  • 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. 启动示例 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
    
  • 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

    1. 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/文件夹。
      在这里插入图片描述
  1. 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
    
    详细解决方法参见连接:Jetson AGX Xavier避坑指南(五)——环境搭建2.(python3、conda、ros、pytorch、tensorflow)

2. ros 代码导读

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

2.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数据。

2.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

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

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

如果要用自己的相机,注意修改相机参数配置。配置文件在 data/calib/文件夹下,新建一个自己相机的文件名如 k4a 然后添加两个配置文件:k4a.intrk4a_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)
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。

2.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

2.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

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

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

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

2.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 函数。

2.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)

camera_intr.deproject_pixel函数主要内容为point_3d = depth * np.linalg.inv(self._K).dot(np.r_[pixel.data, 1.0]),即

\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]

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

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.
    在这里插入图片描述

本文最初发表在 csdn 上,略有改动。原文链接:https://blog.csdn.net/zxxxiazai/article/details/112246220