__author__ = 'cxwang, mhgou'
__version__ = '1.0'
import os
import time
import numpy as np
import open3d as o3d
from transforms3d.euler import euler2mat, quat2mat
from .rotation import batch_viewpoint_params_to_matrix, matrix_to_dexnet_params
from .dexnet.grasping.quality import PointGraspMetrics3D
from .dexnet.grasping.grasp import ParallelJawPtGrasp3D
from .dexnet.grasping.graspable_object import GraspableObject3D
from .dexnet.grasping.grasp_quality_config import GraspQualityConfigFactory
from .dexnet.grasping.contacts import Contact3D
from .dexnet.grasping.meshpy.obj_file import ObjFile
from .dexnet.grasping.meshpy.sdf_file import SdfFile
[docs]def get_scene_name(num):
'''
**Input:**
- num: int of the scene number.
**Output:**
- string of the scene name.
'''
return ('scene_%04d' % (num,))
[docs]def create_table_points(lx, ly, lz, dx=0, dy=0, dz=0, grid_size=0.01):
'''
**Input:**
- lx:
- ly:
- lz:
**Output:**
- numpy array of the points with shape (-1, 3).
'''
xmap = np.linspace(0, lx, int(lx/grid_size))
ymap = np.linspace(0, ly, int(ly/grid_size))
zmap = np.linspace(0, lz, int(lz/grid_size))
xmap, ymap, zmap = np.meshgrid(xmap, ymap, zmap, indexing='xy')
xmap += dx
ymap += dy
zmap += dz
points = np.stack([xmap, ymap, zmap], axis=-1)
points = points.reshape([-1, 3])
return points
[docs]def parse_posevector(posevector):
'''
**Input:**
- posevector: list of pose
**Output:**
- obj_idx: int of the index of object.
- mat: numpy array of shape (4, 4) of the 6D pose of object.
'''
mat = np.zeros([4,4],dtype=np.float32)
alpha, beta, gamma = posevector[4:7]
alpha = alpha / 180.0 * np.pi
beta = beta / 180.0 * np.pi
gamma = gamma / 180.0 * np.pi
mat[:3,:3] = euler2mat(alpha, beta, gamma)
mat[:3,3] = posevector[1:4]
mat[3,3] = 1
obj_idx = int(posevector[0])
return obj_idx, mat
[docs]def load_dexnet_model(data_path):
'''
**Input:**
- data_path: path to load .obj & .sdf files
**Output:**
- obj: dexnet model
'''
of = ObjFile('{}.obj'.format(data_path))
sf = SdfFile('{}.sdf'.format(data_path))
mesh = of.read()
sdf = sf.read()
obj = GraspableObject3D(sdf, mesh)
return obj
[docs]def compute_point_distance(A, B):
'''
**Input:**
- A: (N, 3)
- B: (M, 3)
**Output:**
- dists: (N, M)
'''
A = A[:, np.newaxis, :]
B = B[np.newaxis, :, :]
dists = np.linalg.norm(A-B, axis=-1)
return dists
[docs]def compute_closest_points(A, B):
'''
**Input:**
- A: (N, 3)
- B: (M, 3)
**Output:**
- indices: (N,) closest point index in B for each point in A
'''
dists = compute_point_distance(A, B)
indices = np.argmin(dists, axis=-1)
return indices
[docs]def voxel_sample_points(points, voxel_size=0.008):
'''
**Input:**
- points: (N, 3)
**Output:**
- points: (n, 3)
'''
cloud = o3d.geometry.PointCloud()
cloud.points = o3d.utility.Vector3dVector(points)
cloud = cloud.voxel_down_sample(voxel_size)
points = np.array(cloud.points)
return points
[docs]def topk_grasps(grasps, k=10):
'''
**Input:**
- grasps: (N, 17)
- k: int
**Output:**
- topk_grasps: (k, 17)
'''
assert(k > 0)
grasp_confidence = grasps[:, 0]
indices = np.argsort(-grasp_confidence)
topk_indices = indices[:min(k, len(grasps))]
topk_grasps = grasps[topk_indices]
return topk_grasps
[docs]def get_grasp_score(grasp, obj, fc_list, force_closure_quality_config):
tmp, is_force_closure = False, False
quality = -1
for ind_, value_fc in enumerate(fc_list):
value_fc = round(value_fc, 2)
tmp = is_force_closure
is_force_closure = PointGraspMetrics3D.grasp_quality(grasp, obj, force_closure_quality_config[value_fc])
if tmp and not is_force_closure:
quality = round(fc_list[ind_ - 1], 2)
break
elif is_force_closure and value_fc == fc_list[-1]:
quality = value_fc
break
elif value_fc == fc_list[0] and not is_force_closure:
break
return quality
[docs]def collision_detection(grasp_list, model_list, dexnet_models, poses, scene_points, outlier=0.05, empty_thresh=10, return_dexgrasps=False):
'''
**Input:**
- grasp_list: [(k1, 17), (k2, 17), ..., (kn, 17)] in camera coordinate
- model_list: [(N1, 3), (N2, 3), ..., (Nn, 3)] in camera coordinate
- dexnet_models: [GraspableObject3D,] in object coordinate
- poses: [(4, 4),] from model coordinate to camera coordinate
- scene_points: (Ns, 3) in camera coordinate
- outlier: float, used to compute workspace mask
- empty_thresh: int, 'num_inner_points < empty_thresh' means empty grasp
- return_dexgrasps: bool, return grasps in dex-net format while True
**Output:**
- collsion_mask_list: [(k1,), (k2,), ..., (kn,)]
- empty_mask_list: [(k1,), (k2,), ..., (kn,)]
- dexgrasp_list: [[ParallelJawPtGrasp3D,],] in object coordinate
'''
height = 0.02
depth_base = 0.02
finger_width = 0.01
collision_mask_list = list()
num_models = len(model_list)
empty_mask_list = list()
dexgrasp_list = list()
for i in range(num_models):
if len(grasp_list[i]) == 0:
collision_mask_list.append(list())
empty_mask_list.append(list())
if return_dexgrasps:
dexgrasp_list.append(list())
continue
## parse grasp parameters
model = model_list[i]
obj_pose = poses[i]
dexnet_model = dexnet_models[i]
grasps = grasp_list[i]
grasp_points = grasps[:, 13:16]
grasp_poses = grasps[:, 4:13].reshape([-1,3,3])
grasp_depths = grasps[:, 3]
grasp_widths = grasps[:, 1]
## crop scene, remove outlier
xmin, xmax = model[:,0].min(), model[:,0].max()
ymin, ymax = model[:,1].min(), model[:,1].max()
zmin, zmax = model[:,2].min(), model[:,2].max()
xlim = ((scene_points[:,0] > xmin-outlier) & (scene_points[:,0] < xmax+outlier))
ylim = ((scene_points[:,1] > ymin-outlier) & (scene_points[:,1] < ymax+outlier))
zlim = ((scene_points[:,2] > zmin-outlier) & (scene_points[:,2] < zmax+outlier))
workspace = scene_points[xlim & ylim & zlim]
# transform scene to gripper frame
target = (workspace[np.newaxis,:,:] - grasp_points[:,np.newaxis,:])
target = np.matmul(target, grasp_poses)
# compute collision mask
mask1 = ((target[:,:,2]>-height/2) & (target[:,:,2]<height/2))
mask2 = ((target[:,:,0]>-depth_base) & (target[:,:,0]<grasp_depths[:,np.newaxis]))
mask3 = (target[:,:,1]>-(grasp_widths[:,np.newaxis]/2+finger_width))
mask4 = (target[:,:,1]<-grasp_widths[:,np.newaxis]/2)
mask5 = (target[:,:,1]<(grasp_widths[:,np.newaxis]/2+finger_width))
mask6 = (target[:,:,1]>grasp_widths[:,np.newaxis]/2)
mask7 = ((target[:,:,0]>-(depth_base+finger_width)) & (target[:,:,0]<-depth_base))
left_mask = (mask1 & mask2 & mask3 & mask4)
right_mask = (mask1 & mask2 & mask5 & mask6)
bottom_mask = (mask1 & mask3 & mask5 & mask7)
inner_mask = (mask1 & mask2 &(~mask4) & (~mask6))
collision_mask = np.any((left_mask | right_mask | bottom_mask), axis=-1)
empty_mask = (np.sum(inner_mask, axis=-1) < empty_thresh)
collision_mask = (collision_mask | empty_mask)
collision_mask_list.append(collision_mask)
empty_mask_list.append(empty_mask)
## generate grasps in dex-net format
if return_dexgrasps:
dexgrasps = list()
for grasp_id,_ in enumerate(grasps):
grasp_point = grasp_points[grasp_id]
R = grasp_poses[grasp_id]
width = grasp_widths[grasp_id]
depth = grasp_depths[grasp_id]
points_in_gripper = target[grasp_id][inner_mask[grasp_id]]
if empty_mask[grasp_id]:
dexgrasps.append(None)
continue
center = np.array([depth, 0, 0]).reshape([3, 1]) # gripper coordinate
center = np.dot(grasp_poses[grasp_id], center).reshape([3])
center = (center + grasp_point).reshape([1,3]) # camera coordinate
center = transform_points(center, np.linalg.inv(obj_pose)).reshape([3]) # object coordinate
R = np.dot(obj_pose[:3,:3].T, R)
binormal, approach_angle = matrix_to_dexnet_params(R)
grasp = ParallelJawPtGrasp3D(ParallelJawPtGrasp3D.configuration_from_params(
center, binormal, width, approach_angle), depth)
dexgrasps.append(grasp)
dexgrasp_list.append(dexgrasps)
if return_dexgrasps:
return collision_mask_list, empty_mask_list, dexgrasp_list
else:
return collision_mask_list, empty_mask_list
[docs]def eval_grasp(grasp_group, models, dexnet_models, poses, config, table=None, voxel_size=0.008, TOP_K = 50):
'''
**Input:**
- grasp_group: GraspGroup instance for evaluation.
- models: in model coordinate
- dexnet_models: models in dexnet format
- poses: from model to camera coordinate
- config: dexnet config.
- table: in camera coordinate
- voxel_size: float of the voxel size.
- TOP_K: int of the number of top grasps to evaluate.
'''
num_models = len(models)
## grasp nms
grasp_group = grasp_group.nms(0.03, 30.0/180*np.pi)
## assign grasps to object
# merge and sample scene
model_trans_list = list()
seg_mask = list()
for i,model in enumerate(models):
model_trans = transform_points(model, poses[i])
seg = i * np.ones(model_trans.shape[0], dtype=np.int32)
model_trans_list.append(model_trans)
seg_mask.append(seg)
seg_mask = np.concatenate(seg_mask, axis=0)
scene = np.concatenate(model_trans_list, axis=0)
# assign grasps
indices = compute_closest_points(grasp_group.translations, scene)
model_to_grasp = seg_mask[indices]
pre_grasp_list = list()
for i in range(num_models):
grasp_i = grasp_group[model_to_grasp==i]
grasp_i.sort_by_score()
pre_grasp_list.append(grasp_i[:10].grasp_group_array)
all_grasp_list = np.vstack(pre_grasp_list)
remain_mask = np.argsort(all_grasp_list[:,0])[::-1]
min_score = all_grasp_list[remain_mask[min(49,len(remain_mask) - 1)],0]
grasp_list = []
for i in range(num_models):
remain_mask_i = pre_grasp_list[i][:,0] >= min_score
grasp_list.append(pre_grasp_list[i][remain_mask_i])
# grasp_list = pre_grasp_list
## collision detection
if table is not None:
scene = np.concatenate([scene, table])
collision_mask_list, empty_list, dexgrasp_list = collision_detection(
grasp_list, model_trans_list, dexnet_models, poses, scene, outlier=0.05, return_dexgrasps=True)
## evaluate grasps
# score configurations
force_closure_quality_config = dict()
fc_list = np.array([1.2, 1.0, 0.8, 0.6, 0.4, 0.2])
for value_fc in fc_list:
value_fc = round(value_fc, 2)
config['metrics']['force_closure']['friction_coef'] = value_fc
force_closure_quality_config[value_fc] = GraspQualityConfigFactory.create_config(config['metrics']['force_closure'])
# get grasp scores
score_list = list()
for i in range(num_models):
dexnet_model = dexnet_models[i]
collision_mask = collision_mask_list[i]
dexgrasps = dexgrasp_list[i]
scores = list()
num_grasps = len(dexgrasps)
for grasp_id in range(num_grasps):
if collision_mask[grasp_id]:
scores.append(-1.)
continue
if dexgrasps[grasp_id] is None:
scores.append(-1.)
continue
grasp = dexgrasps[grasp_id]
score = get_grasp_score(grasp, dexnet_model, fc_list, force_closure_quality_config)
scores.append(score)
score_list.append(np.array(scores))
return grasp_list, score_list, collision_mask_list