__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 ..grasp import Grasp
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.0)
continue
if dexgrasps[grasp_id] is None:
scores.append(-1.0)
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
[docs]def model_free_collision_detection(grasp: Grasp, scene_points, collision_thresh=0):
"""Execute collision detection given point cloud and grasp pose.
Args:
grasp(graspnetAPI.Grasp): grasp pose in camera coordinate.
scene_points(np.array): (Ns, 3) in camera coordinate.
collision_thresh(int): 'num_collision_points > collision_thresh' means collision grasp
Returns:
bool: if collision.
"""
height = 0.02
depth_base = 0.02
finger_width = 0.01
## parse grasp parameters
grasp_array = grasp.grasp_array
grasp_points = grasp_array[13:16]
grasp_poses = grasp_array[4:13].reshape([3, 3])
grasp_depths = grasp_array[3]
grasp_widths = grasp_array[1]
# scene_points: [n, 3], grasp_points: [3]
# transform scene to gripper frame
target = scene_points - grasp_points
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)
mask3 = target[:, 1] > -(grasp_widths / 2 + finger_width)
mask4 = target[:, 1] < -grasp_widths / 2
mask5 = target[:, 1] < (grasp_widths / 2 + finger_width)
mask6 = target[:, 1] > grasp_widths / 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
is_collision = (
np.sum((left_mask | right_mask | bottom_mask), axis=-1) > collision_thresh
)
return is_collision