pybullet+graspnet实现多相机点云融合与物体抓取（二）

在上篇文中，介绍了在windows上部署graspnet的一些细节，同时完成了在仿真环境以及现实世界中生成对点云的抓取姿态。本篇文章主要介绍如何获取抓取位姿的世界坐标，以完成抓取。

抓取效果参考视频：

基于pybullet+graspnet的抓取复现

下面介绍实现的具体细节：

1. 摄像机拍摄原理：

首先要明确pybullet里照片获取的原理，由视角矩阵计算函数可知：

viewMatrix = p.computeViewMatrix(
        cameraEyePosition=cameraPos,
        cameraTargetPosition=targetPos,
        cameraUpVector=cameraupPos,
        physicsClientId=0
    )  # 计算视角矩阵

需要有三个必要的参数：相机眼位置，相机目标位置和相机上向量组成。值得注意的是，相机的视角矩阵很可能与相机的位姿齐次矩阵不同。但是我们可以在相机的位姿矩阵和相机视角矩阵间建立联系，我使用这种办法：

rot = R.from_quat(qq).as_matrix()#qq是相机姿态四元数
targetPos = np.matmul(rot, np.array([0, 0, -1])) + cameraPos
cameraupPos = np.matmul(rot, np.array([-1, 0, 0]))

之后 保留这个rot（比如设置为全局变量），通过以下旋转变换：

R1 = rot
gg1 = gg[0]#gg是demo函数返回的抓取列表，经过gg.sort_by_score()后，取分数最高的抓取
ori = np.array(gg1.rotation_matrix)#gg的旋转矩阵
tran=np.array(gg1.translation)#gg的位移向量
R1=np.matmul( R1,np.array([[0, 1, 0],  [1, 0, 0],[0, 0, -1]]))#后面的这个矩阵是我调整出来的，至于为什么，跟bullet里面坐标系变换，以及graspnet模型的一些设置有关
ori=np.matmul(R1,ori)#camera的世界旋转矩阵
camera_tran=np.dot(R1, tran.T).T
world_tran =camera_tran + cameraPos#camera的世界坐标

通过这个方法获取旋转矩阵后，就可以控制夹爪抓取物体了。不过实际应用的时候，还可以考虑一下夹爪的行进路线，最好是沿着抓取方向（x轴）行进，这样可以避免把物体打乱。

2.生成随机物体

其实这也不是随机的生成，是因为pybullet里面自带了很多个物体，一共一千个，可以实现一种伪随机的效果，代码如下：

    def load_obj(self):
        # load Object
        objid=np.random.randint(0,999)
        objid=str(objid).zfill(3)
        obj_path=os.path.join(pybullet_data.getDataPath(), "random_urdfs/"+objid+'/'+objid+'.urdf')
        ObjectID = p.loadURDF(obj_path, [0, 0, 0.20], globalScaling=0.7)
        #适当通过globalScaling调整物块大小，以方便夹爪抓取
        p.changeDynamics(ObjectID,-1,lateralFriction=10)
        #适当改变物体摩擦系数属性以方便夹爪抓取

然后通过一个按键来加载随机物体：

        if ord("c") in keys and keys[ord("c")] & p.KEY_WAS_RELEASED:
            robotiq.load_obj()

 3. 通过分割图来实现对某个特定物体的抓取

pybullet在拍照时还会返回一个seg图，将这个seg图替换掉

def process_data(self,color,depth,seg=None):

函数里面

workspace_mask = np.array([True for i in range(1280*720)]).reshape(720,1280)

的workspace_mask就好了。但你需要先将其转换为bool矩阵。比如你想抓取序号为num的一个物体：

def mask_pic(seg,num):
#该函数目的将seg分割矩阵里面所有值为num设置为True，否则False
    sp=np.array(seg).shape
    mask=np.array([False for i in range(sp[0]*sp[1])]).reshape(sp)#np.full(sp, False, dtype=bool)
    for i in range(sp[0]):
        for j in range(sp[1]):
            if seg[i][j]==num:
                mask[i][j]=True

    return mask

用这个函数即可。