def propose_grasps(pc, radius, num_grasps=1, vis=False):
output_grasps = []
for _ in range(num_grasps):
center_index = np.random.randint(pc.shape[0])
center_point = pc[center_index, :].copy()
d = np.sqrt(np.sum(np.square(pc - np.expand_dims(center_point, 0)),
-1))
index = np.where(d < radius)[0]
neighbors = pc[index, :]
eigen_values, eigen_vectors = cov_matrix(center_point, neighbors)
direction = eigen_vectors[:, 2]
direction = choose_direction(direction, center_point)
surface_orientation = trimesh.geometry.align_vectors([0, 0, 1],
direction)
roll_orientation = tra.quaternion_matrix(
tra.quaternion_about_axis(np.random.uniform(0, 2 * np.pi),
[0, 0, 1]))
gripper_transform = surface_orientation.dot(roll_orientation)
gripper_transform[:3, 3] = center_point
translation_transform = np.eye(4)
translation_transform[2, 3] = -np.random.uniform(0.0669, 0.1122)
gripper_transform = gripper_transform.dot(translation_transform)
output_grasps.append(gripper_transform.copy())
if vis:
draw_scene(pc, grasps=output_grasps)
mlab.show()
return np.asarray(output_grasps)
def main_check_pointcloud(iterate_all_viewpoints=True):
from visualization_utils import draw_scene
import mayavi.mlab as mlab
import matplotlib.pyplot as plt
import glob
import random
import cv2
import time
import json
quaternions = [
l[:-1].split('\t')
for l in open('uniform_quaternions/data2_4608.qua', 'r').readlines()
]
quaternions = [[float(t[0]),
float(t[1]),
float(t[2]),
float(t[3])] for t in quaternions]
quaternions = np.asarray(quaternions)
quaternions = np.roll(quaternions, 1, axis=1)
all_eulers = [tra.quaternion_matrix(q) for q in quaternions]
all_eulers = []
for az in np.linspace(0, math.pi * 2, 30):
for el in np.linspace(-math.pi / 2, math.pi / 2, 30):
all_eulers.append(tra.euler_matrix(el, az, 0))
renderer = OnlineObjectRendererMultiProcess(caching=True)
renderer.start()
grasps_path = glob.glob('unified_grasp_data/grasps/*.json')
random.shuffle(grasps_path)
#mesh_paths = ['unified_grasp_data/meshes/Bowl/9a52843cc89cd208362be90aaa182ec6.stl']
for main_iter in range(5 * len(grasps_path)):
gpath = grasps_path[main_iter % len(grasps_path)]
json_dict = json.load(open(gpath))
mpath = os.path.join('unified_grasp_data', json_dict['object'])
scale = json_dict['object_scale']
#mpath = 'unified_grasp_data/meshes/Bottle/10d3d5961e00b2133ff038bc77759685.stl'
#mpath = 'unified_grasp_data/meshes/Bottle/ef631a2ce94fae3ab8966911a5afa22c.stl'
print(main_iter, mpath)
start_time = time.time()
renderer.change_object(mpath, scale)
if iterate_all_viewpoints == True:
viewpoints = all_eulers
else:
viewpoints = [all_eulers[np.random.randint(len(all_eulers))]]
for view in viewpoints:
image, depth, pc, _ = renderer.render(view)
print(time.time() - start_time)
print('depth min = {} max = {} npoints = {}'.format(
np.min(depth), np.max(depth), pc.shape))
draw_scene(pc, None)
mlab.show()
renderer.terminate()
renderer.join()
def visualize(self, visualize_all: bool = True):
"""Visualize point cloud and last batch of computed grasps in a 3D visualizer
"""
candidates_to_display = self.n_of_candidates if (
(self.n_of_candidates > 0) and
(self.n_of_candidates < len(self.latest_grasps))
and not visualize_all) else len(self.latest_grasps)
mlab.figure(bgcolor=(1, 1, 1))
visualization_utils.draw_scene(
pc=self.scene_pc,
grasps=self.latest_grasps[:candidates_to_display],
grasp_scores=self.latest_grasp_scores[:candidates_to_display],
pc_color=self.scene_pc_colors,
show_gripper_mesh=True)
print('[INFO] Close visualization window to proceed')
mlab.show()
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)
if self._eval_vae_and_evaluator:
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 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
def read_eval_scene(self, file_path, visualize=False):
if not os.path.isfile(file_path):
if not self._should_create_data:
raise ValueError('could not find data {}'.format(file_path))
json_path = self._grasp_reader.generate_object_set(
self._cfg.eval_split)
obj_grasp_data = self._grasp_reader.read_grasp_file(
os.path.join(self._cfg.dataset_root_folder, json_path), True)
obj_pose = self._grasp_reader.arrange_objects(
obj_grasp_data[-3])[0]
in_camera_pose = None
print('changing object to ', obj_grasp_data[-2])
self._grasp_reader.change_object(obj_grasp_data[-2],
obj_grasp_data[-1])
pc, camera_pose, in_camera_pose = self._grasp_reader.render_random_scene(
None)
folder_path = file_path[:file_path.rfind('/')]
create_directory(folder_path)
print('writing {}'.format(file_path))
np.save(
file_path, {
'json': json_path,
'obj_pose': obj_pose,
'camera_pose': in_camera_pose
})
else:
d = np.load(file_path).item()
json_path = d['json']
obj_pose = d['obj_pose']
obj_grasp_data = self._grasp_reader.read_grasp_file(
os.path.join(self._cfg.dataset_root_folder, json_path), True)
in_camera_pose = d['camera_pose']
self._grasp_reader.change_object(obj_grasp_data[-2],
obj_grasp_data[-1])
pc, camera_pose, _ = self._grasp_reader.render_random_scene(
in_camera_pose)
pos_grasps = np.matmul(np.expand_dims(camera_pose, 0),
obj_grasp_data[0])
neg_grasps = np.matmul(np.expand_dims(camera_pose, 0),
obj_grasp_data[2])
grasp_labels = np.hstack(
(np.ones(pos_grasps.shape[0]),
np.zeros(neg_grasps.shape[0]))).astype(np.int32)
grasps = np.concatenate((pos_grasps, neg_grasps), 0)
if visualize:
from visualization_utils import draw_scene
import mayavi.mlab as mlab
pos_mask = np.logical_and(
grasp_labels == 1,
np.random.rand(*grasp_labels.shape) < 0.1)
neg_mask = np.logical_and(
grasp_labels == 0,
np.random.rand(*grasp_labels.shape) < 0.01)
print(grasps[pos_mask, :, :].shape, grasps[neg_mask, :, :].shape)
draw_scene(pc, grasps[pos_mask, :, :])
mlab.show()
draw_scene(pc, grasps[neg_mask, :, :])
mlab.show()
return pc[:, :3], grasps, grasp_labels, {
'cad_path':
obj_grasp_data[-2],
'cad_scale':
obj_grasp_data[-1],
'to_canonical_transformation':
grasp_data_reader.inverse_transform(camera_pose)
}
assert(np.all(pose == output_pc_poses[0]))
pc = output_pcs[0]
pose = output_pc_poses[0]
cad_file = output_cad_files[0]
cad_scale = output_cad_scales[0]
obj = sample.Object(cad_file)
obj.rescale(cad_scale)
obj = obj.mesh
obj.vertices -= np.expand_dims(np.mean(obj.vertices, 0), 0)
print('mean_pc', np.mean(pc, 0))
print('pose', pose)
draw_scene(
pc,
grasps=output_grasps,
grasp_scores=None if args.vae_mode else output_labels,
)
mlab.figure()
draw_scene(
pc.dot(pose.T),
grasps=[pose.dot(g) for g in output_grasps],
mesh=obj,
grasp_scores=None if args.vae_mode else output_labels,
)
mlab.show()
grasps = np.concatenate((pos_grasp
s, neg_grasps), 0)
if visualize:
from visualization_utils import draw_scene
import mayavi.mlab as mlab
pos_mask = np.logical_and(grasp_labels == 1, np.random.rand(*grasp_labels.shape) < 0.1)
neg_mask = np.logical_and(
grasp_labels == 0,
np.random.rand(*grasp_labels.shape) < 0.01)
print(grasps[pos_mask, :, :].shape, grasps[neg_mask, :, :].shape)
draw_scene(pc, grasps[pos_mask, :, :])
mlab.show()
draw_scene(pc, grasps[neg_mask, :, :])
mlab.show()
return pc[:, :3], grasps, grasp_labels, {'cad_path': obj_grasp_data[-2], 'cad_scale': obj_grasp_data[-1], 'to_canonical_transformation': grasp_data_reader.inverse_transform(camera_pose)}
}
def eval_scene(self, file_path, visualize=False):