def _load_object(self, path, scale):
if (path, scale) in self._cache:
return self._cache[(path, scale)]
obj = sample.Object(path)
obj.rescale(scale)
tmesh = obj.mesh
tmesh_mean = np.mean(tmesh.vertices, 0)
tmesh.vertices -= np.expand_dims(tmesh_mean, 0)
lbs = np.min(tmesh.vertices, 0)
ubs = np.max(tmesh.vertices, 0)
object_distance = np.max(ubs - lbs) * 5
mesh = pyrender.Mesh.from_trimesh(tmesh)
context = {
'tmesh': copy.deepcopy(tmesh),
'distance': object_distance,
'node': pyrender.Node(mesh=mesh),
'mesh_mean': np.expand_dims(tmesh_mean, 0),
}
self._cache[(path, scale)] = context
return self._cache[(path, scale)]
def _load_object(self, path, scale=1.0):
obj = sample.Object(path)
if scale != 1.0:
obj.rescale(scale)
print('rescaling with scale', scale)
tmesh = obj.mesh
tmesh_mean = np.mean(tmesh.vertices, 0)
tmesh.vertices -= np.expand_dims(tmesh_mean, 0)
self.tmesh = copy.deepcopy(tmesh)
mesh = pyrender.Mesh.from_trimesh(tmesh)
self._meshes[path] = self._scene.add(mesh, name='object')
def _load_object(self, path, scale=1.0):
obj = sample.Object(path)
obj.rescale(scale)
print('rescaling with scale', scale)
tmesh = obj.mesh
tmesh_mean = np.mean(tmesh.vertices, 0)
tmesh.vertices -= np.expand_dims(tmesh_mean, 0)
lbs = np.min(tmesh.vertices, 0)
ubs = np.max(tmesh.vertices, 0)
self._object_distances.append(np.max(ubs - lbs) * 5)
print(self._object_distances)
self.tmesh = copy.deepcopy(tmesh)
mesh = pyrender.Mesh.from_trimesh(tmesh)
return self._scene.add(mesh,
name='object'), np.expand_dims(tmesh_mean, 0)
def draw_scene(pc,
grasps=[],
grasp_scores=None,
grasp_color=None,
gripper_color=(0, 1, 0),
mesh=None,
show_gripper_mesh=False,
grasps_selection=None,
visualize_diverse_grasps=False,
min_seperation_distance=0.03,
pc_color=None,
plasma_coloring=False,
target_cps=None):
"""
Draws the 3D scene for the object and the scene.
Args:
pc: point cloud of the object
grasps: list of 4x4 numpy array indicating the transformation of the grasps.
grasp_scores: grasps will be colored based on the scores. If left
empty, grasps are visualized in green.
grasp_color: if it is a tuple, sets the color for all the grasps. If list
is provided it is the list of tuple(r,g,b) for each grasp.
mesh: If not None, shows the mesh of the object. Type should be trimesh
mesh.
show_gripper_mesh: If True, shows the gripper mesh for each grasp.
grasp_selection: if provided, filters the grasps based on the value of
each selection. 1 means select ith grasp. 0 means exclude the grasp.
visualize_diverse_grasps: sorts the grasps based on score. Selects the
top score grasp to visualize and then choose grasps that are not within
min_seperation_distance distance of any of the previously selected
grasps. Only set it to True to declutter the grasps for better
visualization.
pc_color: if provided, should be a n x 3 numpy array for color of each
point in the point cloud pc. Each number should be between 0 and 1.
plasma_coloring: If True, sets the plasma colormap for visualizting the
pc.
"""
# ipdb.set_trace()
max_grasps = 100
grasps = np.array(grasps)
if grasp_scores is not None:
grasp_scores = np.array(grasp_scores)
if len(grasps) > max_grasps:
print('Downsampling grasps, there are too many')
chosen_ones = np.random.randint(low=0,
high=len(grasps),
size=max_grasps)
grasps = grasps[chosen_ones]
if grasp_scores is not None:
grasp_scores = grasp_scores[chosen_ones]
if mesh is not None:
if type(mesh) == list:
for elem in mesh:
plot_mesh(elem)
else:
plot_mesh(mesh)
if pc_color is None and pc is not None:
if plasma_coloring:
mlab.points3d(pc[:, 0],
pc[:, 1],
pc[:, 2],
pc[:, 2],
colormap='plasma')
else:
mlab.points3d(pc[:, 0],
pc[:, 1],
pc[:, 2],
color=(0.1, 0.1, 1),
scale_factor=0.01)
elif pc is not None:
if plasma_coloring:
mlab.points3d(pc[:, 0],
pc[:, 1],
pc[:, 2],
pc_color[:, 0],
colormap='plasma')
else:
# ipdb.set_trace()
rgba = np.zeros((pc.shape[0], 4), dtype=np.uint8)
rgba[:, :3] = np.asarray(pc_color)
rgba[:, 3] = 255
src = mlab.pipeline.scalar_scatter(pc[:, 0], pc[:, 1], pc[:, 2])
src.add_attribute(rgba, 'colors')
src.data.point_data.set_active_scalars('colors')
g = mlab.pipeline.glyph(src)
g.glyph.scale_mode = "data_scaling_off"
g.glyph.glyph.scale_factor = 0.01
grasp_pc = np.squeeze(utils.get_control_point_tensor(1, False), 0)
grasp_pc[2, 2] = 0.059
grasp_pc[3, 2] = 0.059
mid_point = 0.5 * (grasp_pc[2, :] + grasp_pc[3, :])
modified_grasp_pc = []
modified_grasp_pc.append(np.zeros((3, ), np.float32))
modified_grasp_pc.append(mid_point)
modified_grasp_pc.append(grasp_pc[2])
modified_grasp_pc.append(grasp_pc[4])
modified_grasp_pc.append(grasp_pc[2])
modified_grasp_pc.append(grasp_pc[3])
modified_grasp_pc.append(grasp_pc[5])
grasp_pc = np.asarray(modified_grasp_pc)
def transform_grasp_pc(g):
output = np.matmul(grasp_pc, g[:3, :3].T)
output += np.expand_dims(g[:3, 3], 0)
return output
if grasp_scores is not None:
indexes = np.argsort(-np.asarray(grasp_scores))
else:
indexes = range(len(grasps))
print('draw scene ', len(grasps))
selected_grasps_so_far = []
removed = 0
if grasp_scores is not None:
min_score = np.min(grasp_scores)
max_score = np.max(grasp_scores)
top5 = np.array(grasp_scores).argsort()[-5:][::-1]
for ii in range(len(grasps)):
i = indexes[ii]
if grasps_selection is not None:
if grasps_selection[i] == False:
continue
g = grasps[i]
is_diverse = True
for prevg in selected_grasps_so_far:
distance = np.linalg.norm(prevg[:3, 3] - g[:3, 3])
if distance < min_seperation_distance:
is_diverse = False
break
if visualize_diverse_grasps:
if not is_diverse:
removed += 1
continue
else:
if grasp_scores is not None:
print('selected', i, grasp_scores[i], min_score, max_score)
else:
print('selected', i)
selected_grasps_so_far.append(g)
if isinstance(gripper_color, list):
pass
elif grasp_scores is not None:
normalized_score = (grasp_scores[i] -
min_score) / (max_score - min_score + 0.0001)
if grasp_color is not None:
gripper_color = grasp_color[ii]
else:
gripper_color = get_color_plasma(normalized_score)
if min_score == 1.0:
gripper_color = (0.0, 1.0, 0.0)
if show_gripper_mesh:
gripper_mesh = sample.Object(
'gripper_models/panda_gripper.obj').mesh
gripper_mesh.apply_transform(g)
# ipdb.set_trace()
mlab.triangular_mesh(
gripper_mesh.vertices[:, 0],
gripper_mesh.vertices[:, 1],
gripper_mesh.vertices[:, 2],
gripper_mesh.faces,
color=gripper_color,
opacity=1 if visualize_diverse_grasps else 0.5)
else:
pts = np.matmul(grasp_pc, g[:3, :3].T)
pts += np.expand_dims(g[:3, 3], 0)
if isinstance(gripper_color, list):
mlab.plot3d(pts[:, 0],
pts[:, 1],
pts[:, 2],
color=gripper_color[i],
tube_radius=0.003,
opacity=1)
else:
tube_radius = 0.001
mlab.plot3d(pts[:, 0],
pts[:, 1],
pts[:, 2],
color=gripper_color,
tube_radius=tube_radius,
opacity=1)
if target_cps is not None:
mlab.points3d(target_cps[ii, :, 0],
target_cps[ii, :, 1],
target_cps[ii, :, 2],
color=(1.0, 0.0, 0),
scale_factor=0.01)
print('removed {} similar grasps'.format(removed))
def simple_draw_scene(pc,
grasps=[],
grasp_scores=None,
grasp_color=None,
gripper_color=(0, 1, 0),
mesh=None,
show_gripper_mesh=False,
grasps_selection=None,
visualize_diverse_grasps=False,
min_seperation_distance=0.03,
pc_color=None,
plasma_coloring=False,
target_cps=None):
ipdb.set_trace()
max_grasps = 10
grasps = np.array(grasps)
fig = plt.figure()
ax = plt.axes(projection='3d')
ax.scatter3D(0, 0, 0, s=2)
if grasp_scores is not None:
grasp_scores = np.array(grasp_scores)
if len(grasps) > max_grasps:
print('Downsampling grasps, there are too many')
chosen_ones = np.random.randint(low=0,
high=len(grasps),
size=max_grasps)
grasps = grasps[chosen_ones]
if grasp_scores is not None:
grasp_scores = grasp_scores[chosen_ones]
if mesh is not None:
if type(mesh) == list:
for elem in mesh:
plot_mesh(elem)
else:
plot_mesh(mesh)
if pc_color is None and pc is not None:
if plasma_coloring:
mlab.points3d(pc[:, 0],
pc[:, 1],
pc[:, 2],
pc[:, 2],
colormap='plasma')
else:
mlab.points3d(pc[:, 0],
pc[:, 1],
pc[:, 2],
color=(0.1, 0.1, 1),
scale_factor=0.01)
elif pc is not None:
if plasma_coloring:
mlab.points3d(pc[:, 0],
pc[:, 1],
pc[:, 2],
pc_color[:, 0],
colormap='plasma')
else:
# ipdb.set_trace()
rgba = np.zeros((pc.shape[0], 3))
rgba[:, :3] = np.asarray(pc_color)
# src = mlab.pipeline.scalar_scatter(pc[:, 0], pc[:, 1], pc[:, 2])
# src.add_attribute(rgba, 'colors')
# src.data.point_data.set_active_scalars('colors')
# g = mlab.pipeline.glyph(src)
# g.glyph.scale_mode = "data_scaling_off"
# g.glyph.glyph.scale_factor = 0.01
decimation_factor = 10
pc0 = pc[:, 0]
pc1 = pc[:, 1]
pc2 = pc[:, 2]
red_pc0 = pc0[:-decimation_factor:decimation_factor]
red_pc1 = pc1[:-decimation_factor:decimation_factor]
red_pc2 = pc2[:-decimation_factor:decimation_factor]
rgba = rgba[:-decimation_factor:decimation_factor]
rgba = rgba.astype(int)
#ax.scatter3D(red_pc0.tolist(), red_pc1.tolist(), red_pc2.tolist(), rgba )
ax.scatter3D(red_pc0, red_pc1, red_pc2, c=rgba / 255.0, s=1)
grasp_pc = np.squeeze(utils.get_control_point_tensor(1, False), 0)
grasp_pc[2, 2] = 0.059
grasp_pc[3, 2] = 0.059
mid_point = 0.5 * (grasp_pc[2, :] + grasp_pc[3, :])
modified_grasp_pc = []
modified_grasp_pc.append(np.zeros((3, ), np.float32))
modified_grasp_pc.append(mid_point)
modified_grasp_pc.append(grasp_pc[2])
modified_grasp_pc.append(grasp_pc[4])
modified_grasp_pc.append(grasp_pc[2])
modified_grasp_pc.append(grasp_pc[3])
modified_grasp_pc.append(grasp_pc[5])
grasp_pc = np.asarray(modified_grasp_pc)
def transform_grasp_pc(g):
output = np.matmul(grasp_pc, g[:3, :3].T)
output += np.expand_dims(g[:3, 3], 0)
return output
if grasp_scores is not None:
indexes = np.argsort(-np.asarray(grasp_scores))
else:
indexes = range(len(grasps))
print('draw scene ', len(grasps))
selected_grasps_so_far = []
removed = 0
if grasp_scores is not None:
min_score = np.min(grasp_scores)
max_score = np.max(grasp_scores)
top5 = np.array(grasp_scores).argsort()[-5:][::-1]
for ii in range(len(grasps)):
i = indexes[ii]
if grasps_selection is not None:
if grasps_selection[i] == False:
continue
g = grasps[i]
is_diverse = True
for prevg in selected_grasps_so_far:
distance = np.linalg.norm(prevg[:3, 3] - g[:3, 3])
if distance < min_seperation_distance:
is_diverse = False
break
if visualize_diverse_grasps:
if not is_diverse:
removed += 1
continue
else:
if grasp_scores is not None:
print('selected', i, grasp_scores[i], min_score, max_score)
else:
print('selected', i)
selected_grasps_so_far.append(g)
if isinstance(gripper_color, list):
pass
elif grasp_scores is not None:
normalized_score = (grasp_scores[i] -
min_score) / (max_score - min_score + 0.0001)
if grasp_color is not None:
gripper_color = grasp_color[ii]
else:
#gripper_color = get_color_plasma(normalized_score)
gripper_color = (0.0, 1.0, 0.0)
if min_score == 1.0:
gripper_color = (0.0, 1.0, 0.0)
if show_gripper_mesh:
gripper_mesh = sample.Object(
'gripper_models/panda_gripper.obj').mesh
gripper_mesh.apply_transform(g)
# ipdb.set_trace()
mlab.triangular_mesh(
gripper_mesh.vertices[:, 0],
gripper_mesh.vertices[:, 1],
gripper_mesh.vertices[:, 2],
gripper_mesh.faces,
color=gripper_color,
opacity=1 if visualize_diverse_grasps else 0.5)
else:
pts = np.matmul(grasp_pc, g[:3, :3].T)
pts += np.expand_dims(g[:3, 3], 0)
if isinstance(gripper_color, list):
mlab.plot3d(pts[:, 0],
pts[:, 1],
pts[:, 2],
color=gripper_color[i],
tube_radius=0.003,
opacity=1)
else:
tube_radius = 0.001
# mlab.plot3d(pts[:, 0],
# pts[:, 1],
# pts[:, 2],
# color=gripper_color,
# tube_radius=tube_radius,
# opacity=1)
tfTPC = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0.1],
[0, 0, 0, 1]])
TPC = np.matmul(g, tfTPC)
ax.scatter3D(TPC[0, 3], TPC[1, 3], TPC[2, 3], c="b", s=16)
ax.scatter3D(g[0, 3], g[1, 3], g[2, 3], c="b", s=8)
ax.plot3D(pts[:, 0], pts[:, 1], pts[:, 2], c=gripper_color)
if target_cps is not None:
mlab.points3d(target_cps[ii, :, 0],
target_cps[ii, :, 1],
target_cps[ii, :, 2],
color=(1.0, 0.0, 0),
scale_factor=0.01)
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
ax.set_xlim3d(0.2, 0.8)
ax.set_ylim3d(-0.3, 0.3)
ax.set_zlim3d(-0.2, 0.4)
#ax.axis('equal')
ipdb.set_trace()
plt.show()
print('removed {} similar grasps'.format(removed))
def eval_scene(self, file_path, visualize=False):
"""
Returns full_results, evaluator_results.
full_results: Contains information about grasps in canonical pose, scores,
ground truth positive grasps, and also cad path and scale that is used for
flex evaluation.
evaluator_results: Only contains information for the classification of positive
and negative grasps. The info is gt label of each grasp, predicted score for
each grasp, and the 4x4 transformation of each grasp.
"""
pc, grasps, grasps_label, flex_info = self.read_eval_scene(file_path)
canonical_transform = flex_info['to_canonical_transformation']
evaluator_result = None
full_results = None
if self._eval_grasp_evaluator:
latents = self._grasp_estimator.sample_latents()
output_grasps, output_scores, _ = self._grasp_estimator.predict_grasps(
self._sess, pc, latents, 0, grasps_rt=grasps)
evaluator_result = (grasps_label, output_scores, output_grasps)
latents = np.random.rand(self._cfg.num_samples,
self._cfg.latent_size) * 4 - 2
print(pc.shape)
generated_grasps, generated_scores, _ = self._grasp_estimator.predict_grasps(
self._sess,
pc,
latents,
num_refine_steps=self._cfg.num_refine_steps,
)
gt_pos_grasps = [g for g, l in zip(grasps, grasps_label) if l == 1]
gt_pos_grasps = np.asarray(gt_pos_grasps).copy()
gt_pos_grasps_canonical = np.matmul(canonical_transform,
gt_pos_grasps)
generated_grasps = np.asarray(generated_grasps)
print(generated_grasps.shape)
generated_grasps_canonical = np.matmul(canonical_transform,
generated_grasps)
obj = sample.Object(flex_info['cad_path'])
obj.rescale(flex_info['cad_scale'])
mesh = obj.mesh
mesh_mean = np.mean(mesh.vertices, 0, keepdims=True)
canonical_pc = pc.dot(canonical_transform[:3, :3].T)
canonical_pc += np.expand_dims(canonical_transform[:3, 3], 0)
gt_pos_grasps_canonical[:, :3, 3] += mesh_mean
canonical_pc += mesh_mean
generated_grasps_canonical[:, :3, 3] += mesh_mean
if visualize:
from utils.visualization_utils import draw_scene
import mayavi.mlab as mlab
draw_scene(canonical_pc,
grasps=gt_pos_grasps_canonical,
mesh=mesh)
mlab.show()
draw_scene(canonical_pc + mesh_mean,
grasps=generated_grasps_canonical,
mesh=mesh,
grasp_scores=generated_scores)
mlab.show()
full_results = (generated_grasps_canonical, generated_scores,
gt_pos_grasps_canonical, flex_info['cad_path'],
flex_info['cad_scale'])
return full_results, evaluator_result
