dex-net
dex-net copied to clipboard
BerkeleyAutomation
Suction grasp database generation from DexNet 3.0
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?
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.
Hi all, no, I ended up writing my own code following the procedure in the paper. It was painful.
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
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.
@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.
