Suction grasp database generation from DexNet 3.0

[atirehgram]

Hi, as per title I was looking for the code to generate a database of annotated depth images for suction grasps, as done in DexNet 3.0. I was not able to find it in this repo, but maybe it has been released somewhere else?

[TAO-TAO-TAO-TAO-TAO]

Hello, have you found the code to generate DexNet 3.0

[NZII]

I‘m in the same situiation, have you found a way to generate Dex-Net 3.0 datasets? I've spent a lot of time looking for it, but nothing helps.

[atirehgram]

Hi all, no, I ended up writing my own code following the procedure in the paper. It was painful.

[atirehgram]

This is part of my code:

def find_good_suction_grasps(index,pc_normals,obj_pb_id,T_w_o,process_id):

    # See dexnet
    
    disc_radius = 0.01
    h = 0.038 # TODO: set parameter

    # Point and normal in object coordinates
    contact_point_np = pc_normals[index,:3]
    normal_np = pc_normals[index,3:]
    
    x_new = np.random.randn(3)  # take a random vector
    x_new -= x_new.dot(normal_np) * normal_np  # make it orthogonal to normal
    x_new /= np.linalg.norm(x_new)  # normalize it
    y_new = np.cross(normal_np, x_new)  # cross product with normal
    
    r1 = (x_new-contact_point_np) / np.linalg.norm(x_new-contact_point_np)
    r2 = (y_new-contact_point_np) / np.linalg.norm(y_new-contact_point_np)
    r3 = (normal_np-contact_point_np) / np.linalg.norm(normal_np-contact_point_np)
    # Find T matrix from local surface point coordinates to object coordinates (new z coincides with outwards normal)
    R = np.vstack((r1,r2,r3)).T
    #print("R", np.linalg.det(R))
    T_o_c = np.vstack((R.T, contact_point_np)).T
    T_o_c = np.vstack((T_o_c, [0, 0, 0, 1]))

    inward_normal = - normal_np

    n_vertices = 8
    octagon_side = disc_radius*np.sqrt(2-np.sqrt(2))
    n_intermediate_points = 5

    octagon_vertices_contact = [[disc_radius*np.cos(np.deg2rad(theta)),disc_radius*np.sin(np.deg2rad(theta))] for theta in range(0,360,int(360/n_vertices))]
    octagon_vertices_contact.append(octagon_vertices_contact[0]) #repeat the first point to create a 'closed loop'
    x, y = zip(*octagon_vertices_contact) #create lists of x and y values
    
    tck, u = splprep([x, y], u=None, s=0.0, per=1, k=1) 
    u_new = np.unique(np.concatenate([np.linspace(u[i],u[i+1], num=n_intermediate_points+1) for i in range(len(u)-1)]))
    x_new, y_new = splev(u_new, tck, der=0)

    ones = np.ones((len(x_new)))
    z_new = 0.1*ones
    rays_from_contact = np.column_stack((x_new,y_new,z_new,ones))[:-1,:]
    rays_to_contact = np.column_stack((x_new,y_new,-z_new,ones))[:-1,:]

    rays_from_world = np.array(np.matmul(np.matmul(T_w_o,T_o_c),rays_from_contact.T)).T
    rays_to_world = np.array(np.matmul(np.matmul(T_w_o,T_o_c),rays_to_contact.T)).T

    hit_infos = pb.rayTestBatch(rays_from_world[:,:3].tolist(),rays_to_world[:,:3].tolist(),physicsClientId=process_id)
    
    hit_obj_id = [hit_info[0] for hit_info in hit_infos]
    hit_obj_id_np = np.array(hit_obj_id)

    if not np.all(hit_obj_id_np == obj_pb_id):
        quality = 0
        label = 0
        result = [0,0,0]
        result.extend(inward_normal.tolist())
        result.append(quality)
        result.append(label)
        return result

    hit_position_world = [hit_info[3] for hit_info in hit_infos]
    hit_position_world.append(hit_position_world[0])
    hit_position_world_np = np.array(hit_position_world)

    #for position in hit_position_world_np:
        #pb.loadURDF("cube.urdf",globalScaling=0.001,basePosition=position)
    
    dsides = np.sqrt(np.sum(np.diff(hit_position_world_np,axis=0)**2, axis=1))
    sides = np.sum(np.reshape(dsides, (-1,n_intermediate_points)), axis=1)
    
    if not np.all((sides <= octagon_side*1.1) & (sides >= octagon_side*0.9)):
        quality = 0
        label = 0
        result = [0,0,0]
        result.extend(inward_normal.tolist())
        result.append(quality)
        result.append(label)
        return result

    ones = np.ones(hit_position_world_np.shape[0])
    hit_position_world_aug_np = np.vstack((hit_position_world_np.T,ones)).T

    hit_position_np = np.matmul(T_w_o,hit_position_world_aug_np.T)[:,:3]
    apex_distance = np.amin((1/n_vertices)*np.sum(np.matmul((hit_position_np - contact_point_np), inward_normal)) - h,0)
    apex_position = contact_point_np + apex_distance*inward_normal

    A_up = np.vstack((R.T, np.zeros((3,3)))).T
    A_down = np.vstack((np.cross(contact_point_np,R).T, R.T)).T
    A = np.vstack((A_up, A_down))
    #print(np.linalg.det(A))

    w = np.array([0,0,0.2,0,0,0])#np.ones(6) # TODO: wrench with one in each direction to resist?
    W = np.eye(6)
    mu = 0.5
    G = np.matmul(A,W) # minimise ||Ga + w||^2, G = AW, subject to Ka <= m

    P = 2*np.matmul(G.T,G)
    #print("P", P)
    q = 2*np.matmul(G.T,w)
    #print("q", q)

    K = np.array([  np.sqrt(3),     0,  -mu,  0,  0,  0,
                    -np.sqrt(3),     0,  -mu,  0,  0,  0,
                    0, np.sqrt(3), -mu,0,0,0,
                    0, -np.sqrt(3), -mu,0,0,0,
                    0,0,-1,0,0,0,
                    0,0,0,np.sqrt(2),0,0,
                    0,0,0,-np.sqrt(2),0,0,
                    0,0,0,0,np.sqrt(2),0,
                    0,0,0,0,-np.sqrt(2),0,
                    0,0,disc_radius*mu,0,0,np.sqrt(3),
                    0,0,-disc_radius*mu,0,0,np.sqrt(3),])

    K = np.reshape(K, (11,6))
    
    kappa = 0.005
    V = 2
    m = np.array([  mu*V, mu*V, mu*V, mu*V,
                    V,
                    np.pi*disc_radius*kappa,np.pi*disc_radius*kappa, np.pi*disc_radius*kappa, np.pi*disc_radius*kappa,
                    disc_radius*mu*V, 0])

    #print("P^TP", np.matmul(P_sym.T,P_sym))
    #print("det", np.linalg.eig(P_sym))

    try:
        optimal_wrench = qp.solve_qp(P,q,-K.T,-m,0)[0]
        #print(res)
        quality = np.linalg.norm(np.matmul(G,optimal_wrench)+w)
    except Exception as e:
        print(e)
        quality = 0
        label = 0
        result = [0,0,0]
        result.extend(inward_normal.tolist())
        result.append(quality)
        result.append(label)
        return result

    quality = np.linalg.norm(optimal_wrench)
    #print(quality)
    if quality < 0.2:
        label = 1
    else:
        label = 0
    result = [x for x in apex_position]
    result.extend(inward_normal.tolist())
    result.append(quality)
    result.append(label)
    return result

[NZII]

It's incredible, you're so smart and well executed. This procedure is really hard, but you do it very well. Thank you very much for your solution.

[miura123]

@atirehgram
I am also having trouble creating a database for suction. By all means, I would appreciate it if you could share your wonderful code in its entirety. Thank you very much for your solution.