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 rotate(self, v, origin="local"): obj = self.copy() rot = quaternion_from_euler(v[0], v[1], v[2], "sxyz") rotation_matrix = transformations.quaternion_matrix(rot) #rot = transformations.euler_matrix(v[0], v[1], v[2], 'sxyz') center_coords = None if origin == 'local': center_coords = None elif origin == 'bounds_center': # group_centroid, use for children ((xmin, ymin, zmin), (xmax, ymax, zmax)) = self.bounds() center_coords = [(xmin + xmax) / 2, (ymin + ymax) / 2, (zmin + zmax) / 2] elif origin: center_coords = origin if center_coords: #translate_before = transformations.translation_matrix(np.array(center_coords) * -1) #translate_after = transformations.translation_matrix(np.array(center_coords)) #transf = translate_before * rot # * rot * translate_after # doesn't work, these matrifes are 4x3, not 4x4 HTM obj.center = obj.center - np.array(center_coords) obj.center = np.dot(rotation_matrix, list(obj.center) + [1])[:3] # Hack: use matrices obj.center = obj.center + np.array(center_coords) obj.size = np.abs(np.dot(rotation_matrix, list(obj.size) + [1])[:3]) # Hack: use matrices else: #transf = rot obj.center = np.dot(rotation_matrix, list(obj.center) + [1])[:3] # Hack: use matrices obj.size = np.abs(np.dot(rotation_matrix, list(obj.size) + [1])[:3]) # Hack: use matrices return obj
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 __init__( self, root_folder, batch_size=1, raw_num_points = 20000, estimate_normals = False, caching=True, use_uniform_quaternions=False, scene_obj_scales=None, scene_obj_paths=None, scene_obj_transforms=None, num_train_samples=None, num_test_samples=None, use_farthest_point = False, intrinsics = None, distance_range = (0.9,1.3), elevation = (30,150), pc_augm_config = None, depth_augm_config = None ): self._root_folder = root_folder self._batch_size = batch_size self._raw_num_points = raw_num_points self._caching = caching self._num_train_samples = num_train_samples self._num_test_samples = num_test_samples self._estimate_normals = estimate_normals self._use_farthest_point = use_farthest_point self._scene_obj_scales = scene_obj_scales self._scene_obj_paths = scene_obj_paths self._scene_obj_transforms = scene_obj_transforms self._distance_range = distance_range self._pc_augm_config = pc_augm_config self._depth_augm_config = depth_augm_config self._current_pc = None self._cache = {} self._renderer = SceneRenderer(caching=True, intrinsics=intrinsics) if use_uniform_quaternions: quat_path = os.path.join(self._root_folder, 'uniform_quaternions/data2_4608.qua') quaternions = [l[:-1].split('\t') for l in open(quat_path, '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) self._all_poses = [tra.quaternion_matrix(q) for q in quaternions] else: self._cam_orientations = [] self._elevation = np.array(elevation)/180. for az in np.linspace(0, np.pi * 2, 30): for el in np.linspace(self._elevation[0], self._elevation[1], 30): self._cam_orientations.append(tra.euler_matrix(0, -el, az)) self._coordinate_transform = tra.euler_matrix(np.pi/2, 0, 0).dot(tra.euler_matrix(0, np.pi/2, 0))
def rotate(angle, mesh: trimesh): # matrix_x = tf.rotation_matrix(math.radians(angle[0]), [1,0,0], mesh.centroid) # matrix_y = tf.rotation_matrix(math.radians(angle[1]), [0,1,0], mesh.centroid) # matrix_z = tf.rotation_matrix(math.radians(angle[2]), [0,0,1], mesh.centroid) mesh.apply_transform( tf.quaternion_matrix( tf.quaternion_from_euler(math.radians(angle[0]), math.radians(angle[1]), math.radians(angle[2]))))
def _transform(self): link = self._optimizer.target quaternion = cuda.to_cpu(link.quaternion.array) translation = cuda.to_cpu(link.translation.array) transform = tf.quaternion_matrix(quaternion) transform = morefusion.geometry.compose_transform( transform[:3, :3], translation ) return transform
def rotate(self, v): obj = self.copy() rot = quaternion_from_euler(v[0], v[1], v[2], "sxyz") rotation_matrix = transformations.quaternion_matrix(rot) obj.center = np.dot(rotation_matrix, list(obj.center) + [1])[:3] # Hack: use matrices obj.size = np.abs(np.dot(rotation_matrix, list(obj.size) + [1])[:3]) # Hack: use matrices return obj
def main(): models = morefusion.datasets.YCBVideoModels() points = models.get_pcd(class_id=2) quaternion_true = tf.random_quaternion() quaternion_pred = quaternion_true + [0.1, 0, 0, 0] transform_true = tf.quaternion_matrix(quaternion_true) transform_pred = tf.quaternion_matrix(quaternion_pred) scenes = {} for use_translation in [False, True]: if use_translation: translation_true = np.random.uniform(-0.02, 0.02, (3,)) translation_pred = np.random.uniform(-0.02, 0.02, (3,)) transform_true[:3, 3] = translation_true transform_pred[:3, 3] = translation_pred add = morefusion.metrics.average_distance( [points], [transform_true], [transform_pred] )[0][0] # --------------------------------------------------------------------- scene = trimesh.Scene() points_true = tf.transform_points(points, transform_true) colors = np.full((points_true.shape[0], 3), [1.0, 0, 0]) geom = trimesh.PointCloud(vertices=points_true, color=colors) scene.add_geometry(geom) points_pred = tf.transform_points(points, transform_pred) colors = np.full((points_true.shape[0], 3), [0, 0, 1.0]) geom = trimesh.PointCloud(vertices=points_pred, color=colors) scene.add_geometry(geom) scenes[f"use_translation: {use_translation}, add: {add}"] = scene if scenes: camera_transform = list(scenes.values())[0].camera_transform scene.camera_transform = camera_transform morefusion.extra.trimesh.display_scenes(scenes)
def uniform_quaternions(): 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) return [tra.quaternion_matrix(q) for q in quaternions]
def setUp(self): batch_size = 5 self.quaternions = np.array( [tf.random_quaternion() for _ in range(batch_size)], dtype=np.float32, ) self.transforms = np.array( [tf.quaternion_matrix(q) for q in self.quaternions], dtype=np.float32, ) self.gTs = np.random.uniform(-1, 1, (batch_size, 4, 4)).astype( np.float32 ) self.check_backward_options = {"atol": 5e-4, "rtol": 5e-3}
def main(): models = morefusion.datasets.YCBVideoModels() points = models.get_pcd(class_id=2) n_test = 1000 transforms_true = [] transforms_pred = [] for i in range(n_test): quaternion_true = tf.random_quaternion() quaternion_pred = quaternion_true + [0.1, 0, 0, 0] translation_true = np.random.uniform(-0.02, 0.02, (3, )) translation_pred = np.random.uniform(-0.02, 0.02, (3, )) transform_true = tf.quaternion_matrix(quaternion_true) transform_true[:3, 3] = translation_true transform_pred = tf.quaternion_matrix(quaternion_pred) transform_pred[:3, 3] = translation_pred transforms_true.append(transform_true) transforms_pred.append(transform_pred) adds = morefusion.metrics.average_distance([points] * n_test, transforms_true, transforms_pred)[0] max_distance = 0.1 auc, x, y = morefusion.metrics.auc_for_errors(adds, max_threshold=max_distance, return_xy=True) print("auc:", auc) plt.plot(x, y) plt.xlabel("add threshold") plt.xlim(0, max_distance) plt.ylabel("accuracy") plt.ylim(0, 1) plt.show()
def main(): models = morefusion.datasets.YCBVideoModels() points = models.get_pcd(class_id=2) quaternion_true = tf.random_quaternion() quaternion_pred = quaternion_true + [0.1, 0, 0, 0] transform_true = tf.quaternion_matrix(quaternion_true) transform_pred = tf.quaternion_matrix(quaternion_pred) # translation translation_true = np.random.uniform(-0.02, 0.02, (3, )) translation_pred = np.random.uniform(-0.02, 0.02, (3, )) transform_true[:3, 3] = translation_true transform_pred[:3, 3] = translation_pred for symmetric in [False, True]: add = morefusion.functions.loss.average_distance( points, transform_true, transform_pred[None], symmetric=symmetric, ) print(f"symmetric={symmetric}, add={float(add.array[0])}")
def spherical_rotate(max_rot_degrees): """Calculate a random rotation matrix using angle magnitudes in max_rot.""" assert isinstance(max_rot_degrees, tuple) and len(max_rot_degrees) == 3 xrot, yrot, zrot = max_rot_degrees # Build the global rotation matrix using quaternions q_xr = tf.quaternion_about_axis(rand_step(xrot), [1, 0, 0]) q_yr = tf.quaternion_about_axis(rand_step(yrot), [0, 1, 0]) q_zr = tf.quaternion_about_axis(rand_step(zrot), [0, 0, 1]) # Multiply global and local rotations rotation = tf.quaternion_multiply(q_xr, q_yr) rotation = tf.quaternion_multiply(rotation, q_zr) return tf.quaternion_matrix(rotation)
def get_empty_scene(self): """Creates an empty scene with a camera and spot light.""" scene = pyrender.Scene(ambient_light=[0.2, 0.2, 0.2], bg_color=[0.1, 0.1, 0.1]) camera = pyrender.PerspectiveCamera(yfov=np.pi / 3.0, aspectRatio=1) camera_pose = translation_matrix([0, 3, 4]) @ quaternion_matrix( quaternion_about_axis(np.deg2rad(-45.0), [1, 0, 0])) scene.add(camera, pose=camera_pose) light = pyrender.SpotLight(color=np.ones(3), intensity=3.0, innerConeAngle=np.pi / 16.0) scene.add(light, pose=camera_pose) return scene
def _filter_robot_poses(self, robot_poses): filtered_robot_poses = list() for robot_pose in robot_poses: ori = robot_pose.orientation R = ttf.quaternion_matrix(np.array([ori.w, ori.x, ori.y, ori.z]))[0:3, 0:3] z_vector = np.array([0.0, 0.0, 1.0]).transpose().reshape((3, 1)) down_z_vector = (np.array([0.0, 0.0, -1.0]).transpose().reshape( (3, 1))) robot_z_vector = np.matmul(R, z_vector) angle_between = ttf.angle_between_vectors(robot_z_vector, down_z_vector) if angle_between < self._angle_from_vertical_limit: filtered_robot_poses.append(robot_pose) return filtered_robot_poses
def main(argv): import argparse parser = argparse.ArgumentParser( description='Pre-render objects', formatter_class=argparse.ArgumentDefaultsHelpFormatter) parser.add_argument('--cad_path', type=str, default=1) parser.add_argument('--cad_scale', type=float, default=-1) parser.add_argument('--quaternion_file', type=str, default='uniform_quaternions/data2_4608.qua') parser.add_argument('--output_path', type=str) args = parser.parse_args(sys.argv[1:]) if args.cad_scale <= 0: raise ValueError("you have to set --cad_scale to positive number") quaternions = [ l[:-1].split('\t') for l in open(args.quaternion_file, '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.euler_from_matrix(tra.quaternion_matrix(q)) for q in quaternions ] renderer = ObjectRenderer(object_paths=[args.cad_path], object_scales=[args.cad_scale]) try: renderer.render_all_and_save_to_h5(args.output_path, all_eulers, vis=True) except Exception as e: print(e) os.system('rm {}'.format(args.output_path))
def _get_grid_full(self, examples, pitch, origin): dims = (self._voxel_dim, ) * 3 grid_full = np.zeros(dims, dtype=np.int32) for i, example in enumerate(examples): T = tf.quaternion_matrix(example["quaternion_true"]) T = geometry_module.compose_transform( R=T[:3, :3], t=example["translation_true"]) vox = self._models.get_solid_voxel_grid(example["class_id"]) points = trimesh.transform_points(vox.points, T) indices = trimesh.voxel.ops.points_to_indices(points, pitch=pitch, origin=origin) I, J, K = indices[:, 0], indices[:, 1], indices[:, 2] keep = ((0 <= I) & (I < dims[0]) & (0 <= J) & (J < dims[1]) & (0 <= K) & (K < dims[2])) I, J, K = I[keep], J[keep], K[keep] grid_full[I, J, K] = i + 1 # starts from 1 return grid_full
def get_adds(result_file): dataset = morefusion.datasets.YCBVideoDataset models = morefusion.datasets.YCBVideoModels() result = scipy.io.loadmat(result_file, chars_as_strings=True, squeeze_me=True) frame_id = "/".join(result["frame_id"].split("/")[1:]) frame = dataset.get_frame(frame_id) adds = collections.defaultdict(list) for gt_index, cls_id in enumerate(frame["meta"]["cls_indexes"]): try: pred_index = np.where(result["labels"] == cls_id)[0][0] pose = result["poses"][pred_index] T_pred = morefusion.geometry.compose_transform( R=tf.quaternion_matrix(pose[:4])[:3, :3], t=pose[4:], ) T_true = np.r_[frame["meta"]["poses"][:, :, gt_index], [[0, 0, 0, 1]], ] pcd = models.get_pcd(class_id=cls_id) add, add_s = morefusion.metrics.average_distance( [pcd], transform1=[T_true], transform2=[T_pred], ) add = add[0] add_s = add_s[0] except IndexError: add = np.inf add_s = np.inf print(f"{result_file}, {cls_id:04d}, {add:.3f}, {add_s:.3f}") adds[cls_id].append((add, add_s)) return adds
def _pcd_files_to_pts(pcd_files, pts_file_npy, pts_file, obj_locations, obj_rotations, min_pts_size=0, debug=False): """ Convert pcd blensor results to xyz or directly to npy files. Merge front and back scans. Moving the object instead of the camera because the point cloud is in some very weird space that behaves crazy when the camera moves. A full day wasted on this shit! :param pcd_files: :param pts_file_npy: :param pts_file: :param trafos_inv: :param debug: :return: """ import gzip def revert_offset(pts_data: np.ndarray, inv_offset: np.ndarray): pts_reverted = pts_data # don't just check the header because missing rays may be added with NaNs if pts_reverted.shape[0] > 0: pts_offset_correction = np.broadcast_to(inv_offset, pts_reverted.shape) pts_reverted += pts_offset_correction return pts_reverted # https://www.blensor.org/numpy_import.html def extract_xyz_from_blensor_numpy(arr_raw): # timestamp # yaw, pitch # distance,distance_noise # x,y,z # x_noise,y_noise,z_noise # object_id # 255*color[0] # 255*color[1] # 255*color[2] # idx hits = arr_raw[arr_raw[:, 3] != 0.0] # distance != 0.0 --> hit noisy_xyz = hits[:, [8, 9, 10]] return noisy_xyz pts_data_to_cat = [] for fi, f in enumerate(pcd_files): try: if f.endswith('.numpy.gz'): pts_data_vs = extract_xyz_from_blensor_numpy( np.loadtxt(gzip.GzipFile(f, "r"))) elif f.endswith('.numpy'): pts_data_vs = extract_xyz_from_blensor_numpy(np.loadtxt(f)) elif f.endswith('.pcd'): pts_data_vs, header_info = point_cloud.load_pcd(file_in=f) else: raise ValueError( 'Input file {} has an unknown format!'.format(f)) except EOFError as er: print('Error processing {}: {}'.format(f, er)) continue # undo coordinate system changes pts_data_vs = utils.right_handed_to_left_handed(pts_data_vs) # move back from camera distance, always along x axis obj_location = np.array(obj_locations[fi]) revert_offset(pts_data_vs, -obj_location) # get and apply inverse rotation matrix of camera scanner_rotation_inv = trafo.quaternion_matrix( trafo.quaternion_conjugate(obj_rotations[fi])) pts_data_ws_test_inv = trafo.transform_points(pts_data_vs, scanner_rotation_inv, translate=False) pts_data_ws = pts_data_ws_test_inv if pts_data_ws.shape[0] > 0: pts_data_to_cat += [pts_data_ws.astype(np.float32)] # debug outputs to check the rotations... the pointcloud MUST align exactly with the mesh if debug: point_cloud.write_xyz(file_path=os.path.join( 'debug', 'test_{}.xyz'.format(str(fi))), points=pts_data_ws) if len(pts_data_to_cat) > 0: pts_data = np.concatenate(tuple(pts_data_to_cat), axis=0) if pts_data.shape[0] > min_pts_size: point_cloud.write_xyz(file_path=pts_file, points=pts_data) np.save(pts_file_npy, pts_data)
mask_bg = np.ones(rgb.shape[:2], dtype=bool) for ins_id, mask in zip(result["labels"], result["masks"]): mask = mask.astype(bool) mapping.initialize(ins_id, pitch=0.01) mapping.integrate(ins_id, mask, pcd_scene) mask_bg = mask_bg & (~mask) mapping.initialize(0, pitch=0.01) mapping.integrate(0, mask_bg, pcd_scene) instance_ids_all = np.r_[0, frame["meta"]["cls_indexes"]] with morefusion.utils.timer(frame_id): poses_refined = np.zeros_like(result["poses"]) for i, (cls_id, mask, pose) in enumerate( zip(result["labels"], result["masks"], result["poses"])): transform_init = morefusion.geometry.compose_transform( R=tf.quaternion_matrix(pose[:4])[:3, :3], t=pose[4:], ) pcd_cad = models.get_pcd(class_id=cls_id) mask = mask.astype(bool) & nonnan pcd_depth = pcd_scene[mask] centroid = np.mean(pcd_depth, axis=0) diagonal = models.get_bbox_diagonal(class_id=cls_id) aabb_min = centroid - diagonal / 2 aabb_max = aabb_min + diagonal occupied_t, empty_i = mapping.get_target_pcds( cls_id, aabb_min, aabb_max) occupied_u = []
def __init__( self, root_folder, batch_size, num_grasp_clusters, npoints, min_difference_allowed=(0, 0, 0), max_difference_allowed=(3, 3, 0), occlusion_nclusters=0, occlusion_dropout_rate=0., caching=True, run_in_another_process=True, collision_hard_neg_min_translation=(-0.03,-0.03,-0.03), collision_hard_neg_max_translation=(0.03,0.03,0.03), collision_hard_neg_min_rotation=(-0.6,-0.2,-0.6), collision_hard_neg_max_rotation=(+0.6,+0.2,+0.6), collision_hard_neg_num_perturbations=10, use_uniform_quaternions=False, ratio_of_grasps_used=1.0, ratio_positive=0.3, ratio_hardnegative=0.4, balanced_data=True, ): self._root_folder = root_folder self._batch_size = batch_size self._num_grasp_clusters = num_grasp_clusters self._max_difference_allowed = max_difference_allowed self._min_difference_allowed = min_difference_allowed self._npoints = npoints self._occlusion_nclusters = occlusion_nclusters self._occlusion_dropout_rate = occlusion_dropout_rate self._caching = caching self._collision_hard_neg_min_translation = collision_hard_neg_min_translation self._collision_hard_neg_max_translation = collision_hard_neg_max_translation self._collision_hard_neg_min_rotation = collision_hard_neg_min_rotation self._collision_hard_neg_max_rotation = collision_hard_neg_max_rotation self._collision_hard_neg_num_perturbations = collision_hard_neg_num_perturbations self._collision_hard_neg_queue = {} self._ratio_of_grasps_used = ratio_of_grasps_used self._ratio_positive = ratio_positive self._ratio_hardnegative = ratio_hardnegative self._balanced_data = balanced_data for i in range(3): assert(collision_hard_neg_min_rotation[i] <= collision_hard_neg_max_rotation[i]) assert(collision_hard_neg_min_translation[i] <= collision_hard_neg_max_translation[i]) self._current_pc = None self._cache = {} if run_in_another_process: self._renderer = OnlineObjectRendererMultiProcess(caching=True) else: self._renderer = OnlineObjectRenderer(caching=True) self._renderer.start() if use_uniform_quaternions: 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) self._all_poses = [tra.quaternion_matrix(q) for q in quaternions] else: self._all_poses = [] for az in np.linspace(0, np.pi * 2, 30): for el in np.linspace(-np.pi / 2, np.pi / 2, 30): self._all_poses.append(tra.euler_matrix(el, az, 0)) self._eval_files = [json.load(open(f)) for f in glob.glob(os.path.join(self._root_folder, 'splits', '*.json'))]
def sample_multiple_grasps(number_of_candidates, mesh, gripper_name, systematic_sampling, surface_density=0.005 * 0.005, standoff_density=0.01, roll_density=15, type_of_quality='antipodal', min_quality=-1.0, silent=False): """Sample a set of grasps for an object. Arguments: number_of_candidates {int} -- Number of grasps to sample mesh {trimesh} -- Object mesh gripper_name {str} -- Name of gripper model systematic_sampling {bool} -- Whether to use grid sampling for roll Keyword Arguments: surface_density {float} -- surface density, in m^2 (default: {0.005*0.005}) standoff_density {float} -- density for standoff, in m (default: {0.01}) roll_density {float} -- roll density, in deg (default: {15}) type_of_quality {str} -- quality metric (default: {'antipodal'}) min_quality {float} -- minimum grasp quality (default: {-1}) silent {bool} -- verbosity (default: {False}) Raises: Exception: Unknown quality metric Returns: [type] -- points, normals, transforms, roll_angles, standoffs, collisions, quality """ origins = [] orientations = [] transforms = [] standoffs = [] roll_angles = [] gripper = create_gripper(gripper_name) if systematic_sampling: # systematic sampling. input: # - Surface density: # - Standoff density: # - Rotation density: # Resulting number of samples: # (Area/Surface Density) * (Finger length/Standoff density) * (360/Rotation Density) surface_samples = int(np.ceil(mesh.area / surface_density)) standoff_samples = np.linspace( gripper.standoff_range[0], gripper.standoff_range[1], max(1, (gripper.standoff_range[1] - gripper.standoff_range[0]) / standoff_density)) rotation_samples = np.arange(0, 1 * np.pi, np.deg2rad(roll_density)) number_of_candidates = surface_samples * \ len(standoff_samples) * len(rotation_samples) tmp_points, face_indices = mesh.sample(surface_samples, return_index=True) tmp_normals = mesh.face_normals[face_indices] number_of_candidates = len(tmp_points) * \ len(standoff_samples) * len(rotation_samples) print("Number of samples ", number_of_candidates, "(", len(tmp_points), " x ", len(standoff_samples), " x ", len(rotation_samples), ")") points = [] normals = [] position_idx = [] pos_cnt = 0 cnt = 0 batch_position_idx = [] batch_points = [] batch_normals = [] batch_roll_angles = [] batch_standoffs = [] batch_transforms = [] for point, normal in tqdm(zip(tmp_points, tmp_normals), total=len(tmp_points), disable=silent): for roll in rotation_samples: for standoff in standoff_samples: batch_position_idx.append(pos_cnt) batch_points.append(point) batch_normals.append(normal) batch_roll_angles.append(roll) batch_standoffs.append(standoff) orientation = tra.quaternion_matrix( tra.quaternion_about_axis(roll, [0, 0, 1])) origin = point + normal * standoff batch_transforms.append( np.dot( np.dot( tra.translation_matrix(origin), trimesh.geometry.align_vectors([0, 0, -1], normal)), orientation)) cnt += 1 pos_cnt += 1 if cnt % 1000 == 0 or cnt == len(tmp_points): valid = raycast_collisioncheck(np.asarray(batch_transforms), np.asarray(batch_points), mesh) transforms.extend(np.array(batch_transforms)[valid]) position_idx.extend(np.array(batch_position_idx)[valid]) points.extend(np.array(batch_points)[valid]) normals.extend(np.array(batch_normals)[valid]) roll_angles.extend(np.array(batch_roll_angles)[valid]) standoffs.extend(np.array(batch_standoffs)[valid]) batch_position_idx = [] batch_points = [] batch_normals = [] batch_roll_angles = [] batch_standoffs = [] batch_transforms = [] points = np.array(points) normals = np.array(normals) position_idx = np.array(position_idx) else: points, face_indices = mesh.sample(number_of_candidates, return_index=True) normals = mesh.face_normals[face_indices] # generate transformations for point, normal in tqdm(zip(points, normals), total=len(points), disable=silent): # roll along approach vector angle = np.random.rand() * 2 * np.pi roll_angles.append(angle) orientations.append( tra.quaternion_matrix( tra.quaternion_about_axis(angle, [0, 0, 1]))) # standoff from surface standoff = (gripper.standoff_range[1] - gripper.standoff_range[0]) * np.random.rand() \ + gripper.standoff_range[0] standoffs.append(standoff) origins.append(point + normal * standoff) transforms.append( np.dot( np.dot(tra.translation_matrix(origins[-1]), trimesh.geometry.align_vectors([0, 0, -1], normal)), orientations[-1])) verboseprint("Checking collisions...") collisions = in_collision_with_gripper(mesh, transforms, gripper_name=gripper_name, silent=silent) verboseprint("Labelling grasps...") quality = {} quality_key = 'quality_' + type_of_quality if type_of_quality == 'antipodal': quality[quality_key] = grasp_quality_antipodal( transforms, collisions, object_mesh=mesh, gripper_name=gripper_name, silent=silent) elif type_of_quality == 'number_of_contacts': quality[quality_key] = grasp_quality_point_contacts( transforms, collisions, object_mesh=mesh, gripper_name=gripper_name, silent=silent) else: raise Exception("Quality metric unknown: ", quality) # Filter out by quality quality_np = np.array(quality[quality_key]) collisions = np.array(collisions) f_points = [] f_normals = [] f_transforms = [] f_roll_angles = [] f_standoffs = [] f_collisions = [] f_quality = [] for i, _ in enumerate(transforms): if quality_np[i] >= min_quality: f_points.append(points[i]) f_normals.append(normals[i]) f_transforms.append(transforms[i]) f_roll_angles.append(roll_angles[i]) f_standoffs.append(standoffs[i]) f_collisions.append(int(collisions[i])) f_quality.append(quality_np[i]) points = np.array(f_points) normals = np.array(f_normals) transforms = np.array(f_transforms) roll_angles = np.array(f_roll_angles) standoffs = np.array(f_standoffs) collisions = f_collisions quality[quality_key] = f_quality return points, normals, transforms, roll_angles, standoffs, collisions, quality
def generate_candidates(mesh_properties): """Generates grasp candidates via predicted 6DoF pose of gripper.""" bbox = mesh_properties['bbox'] work2obj = mesh_properties['work2obj'] obj2grip = mesh_properties['obj2grip'] # Extract the bounding box corners and planes of the object _, planes = get_corners_and_plances(bbox) xyz_grid = [to_rad(x, y, z) for x in range(0, 360, GLOBAL_X) \ for y in range(0, 360, GLOBAL_Y) \ for z in range(0, 360, GLOBAL_Z)] total_trials = len(xyz_grid)**2 # Angles holds the information about global and local rotations # Matrices holds information about the final transformation. angles = np.empty((total_trials, 6)) matrices = np.empty((total_trials, 12)) # Going to extend a vector along z-direction of palm. The extra 1 o the end # is for multiplying with a 4x4 HT matrix palm_axis = np.atleast_2d([0, 0, 10, 1]).T curr_trial = 0 success = 0 # Exhaustively evaluate local and global rotations for gx, gy, gz in xyz_grid: for lx, ly, lz in xyz_grid: # Monitor our status, print something every 10% if curr_trial % int(0.1 * total_trials) == 0: print 'Trial %d/%d, successful: %d'%\ (curr_trial, total_trials, success) curr_trial += 1 # Build the global rotation matrix using quaternions q_gx = tf.quaternion_about_axis(gx + rand_step(GLOBAL_X), [1, 0, 0]) q_gy = tf.quaternion_about_axis(gy + rand_step(GLOBAL_Y), [0, 1, 0]) q_gz = tf.quaternion_about_axis(gz + rand_step(GLOBAL_Z), [0, 0, 1]) # Build the local rotation matrix using quaternions q_lx = tf.quaternion_about_axis(lx + rand_step(GLOBAL_X), [1, 0, 0]) q_ly = tf.quaternion_about_axis(ly + rand_step(GLOBAL_Y), [0, 1, 0]) q_lz = tf.quaternion_about_axis(lz + rand_step(GLOBAL_Z), [0, 0, 1]) # Multiply global and local rotations global_rotation = tf.quaternion_multiply(q_gx, q_gy) global_rotation = tf.quaternion_multiply(global_rotation, q_gz) global_rotation = tf.quaternion_matrix(global_rotation) local_rotation = tf.quaternion_multiply(q_lx, q_ly) local_rotation = tf.quaternion_multiply(local_rotation, q_lz) local_rotation = tf.quaternion_matrix(local_rotation) rotation = np.dot(np.dot(global_rotation, obj2grip), local_rotation) # Don't want to test any candidates that are below the workspace workspace2grip = np.dot(work2obj, rotation) if workspace2grip[2, 3] < 0: continue # Check if a line from the center of the grippers palm intersects # with the planes of the object line_start = rotation[:3, 3] line_end = np.dot(rotation, palm_axis).flatten()[:3] for plane in planes: itx_plane = intersect_plane(line_start, line_end, *plane) if intersect_box(itx_plane, bbox) is False: continue angles[success] = np.asarray([gx, gy, gz, lx, ly, lz]) matrices[success] = rotation[:3].flatten() success = success + 1 break # Only keep the successful transformations angles = np.asarray(angles)[:success] matrices = np.asarray(matrices)[:success] return angles, matrices
def main(): parser = argparse.ArgumentParser( formatter_class=argparse.ArgumentDefaultsHelpFormatter, ) parser.add_argument("model", help="model file in a log dir") parser.add_argument("--gpu", type=int, default=0, help="gpu id") parser.add_argument("--save", action="store_true", help="save") args = parser.parse_args() args_file = path.Path(args.model).parent / "args" with open(args_file) as f: args_data = json.load(f) pprint.pprint(args_data) if args.gpu >= 0: chainer.cuda.get_device_from_id(args.gpu).use() model = singleview_3d.models.Model( n_fg_class=len(args_data["class_names"][1:]), pretrained_resnet18=args_data["pretrained_resnet18"], with_occupancy=args_data["with_occupancy"], loss=args_data["loss"], loss_scale=args_data["loss_scale"], ) if args.gpu >= 0: model.to_gpu() print(f"==> Loading trained model: {args.model}") chainer.serializers.load_npz(args.model, model) print("==> Done model loading") split = "val" dataset = morefusion.datasets.YCBVideoRGBDPoseEstimationDataset( split=split) dataset_reindexed = morefusion.datasets.YCBVideoRGBDPoseEstimationDatasetReIndexed( # NOQA split=split, class_ids=args_data["class_ids"], ) transform = Transform( train=False, with_occupancy=args_data["with_occupancy"], ) pprint.pprint(args.__dict__) # ------------------------------------------------------------------------- depth2rgb = imgviz.Depth2RGB() for index in range(len(dataset)): frame = dataset.get_frame(index) image_id = dataset._ids[index] indices = dataset_reindexed.get_indices_from_image_id(image_id) examples = dataset_reindexed[indices] examples = [transform(example) for example in examples] if not examples: continue inputs = chainer.dataset.concat_examples(examples, device=args.gpu) with chainer.no_backprop_mode() and chainer.using_config( "train", False): quaternion_pred, translation_pred, confidence_pred = model.predict( class_id=inputs["class_id"], rgb=inputs["rgb"], pcd=inputs["pcd"], pitch=inputs.get("pitch"), origin=inputs.get("origin"), grid_nontarget_empty=inputs.get("grid_nontarget_empty"), ) indices = model.xp.argmax(confidence_pred.array, axis=1) quaternion_pred = quaternion_pred[ model.xp.arange(quaternion_pred.shape[0]), indices] translation_pred = translation_pred[ model.xp.arange(translation_pred.shape[0]), indices] reporter = chainer.Reporter() reporter.add_observer("main", model) observation = {} with reporter.scope(observation): model.evaluate( class_id=inputs["class_id"], quaternion_true=inputs["quaternion_true"], translation_true=inputs["translation_true"], quaternion_pred=quaternion_pred, translation_pred=translation_pred, ) # TODO(wkentaro) observation_new = {} for k, v in observation.items(): if re.match(r"main/add_or_add_s/[0-9]+/.+", k): k_new = "/".join(k.split("/")[:-1]) observation_new[k_new] = v observation = observation_new print(f"[{index:08d}] {observation}") # --------------------------------------------------------------------- K = frame["intrinsic_matrix"] height, width = frame["rgb"].shape[:2] fovy = trimesh.scene.Camera(resolution=(width, height), focal=(K[0, 0], K[1, 1])).fov[1] batch_size = len(inputs["class_id"]) class_ids = cuda.to_cpu(inputs["class_id"]) quaternion_pred = cuda.to_cpu(quaternion_pred.array) translation_pred = cuda.to_cpu(translation_pred.array) quaternion_true = cuda.to_cpu(inputs["quaternion_true"]) translation_true = cuda.to_cpu(inputs["translation_true"]) Ts_pred = [] Ts_true = [] for i in range(batch_size): # T_cad2cam T_pred = tf.quaternion_matrix(quaternion_pred[i]) T_pred[:3, 3] = translation_pred[i] T_true = tf.quaternion_matrix(quaternion_true[i]) T_true[:3, 3] = translation_true[i] Ts_pred.append(T_pred) Ts_true.append(T_true) Ts = dict(true=Ts_true, pred=Ts_pred) vizs = [] depth_viz = depth2rgb(frame["depth"]) for which in ["true", "pred"]: pybullet.connect(pybullet.DIRECT) for i, T in enumerate(Ts[which]): cad_file = morefusion.datasets.YCBVideoModels().get_cad_file( class_id=class_ids[i]) morefusion.extra.pybullet.add_model( cad_file, position=tf.translation_from_matrix(T), orientation=tf.quaternion_from_matrix(T)[[1, 2, 3, 0]], ) ( rgb_rend, depth_rend, segm_rend, ) = morefusion.extra.pybullet.render_camera( np.eye(4), fovy, height, width) pybullet.disconnect() segm_rend = imgviz.label2rgb(segm_rend + 1, img=frame["rgb"], alpha=0.7) depth_rend = depth2rgb(depth_rend) rgb_input = imgviz.tile(cuda.to_cpu(inputs["rgb"]), border=(255, 255, 255)) viz = imgviz.tile( [ frame["rgb"], depth_viz, rgb_input, segm_rend, rgb_rend, depth_rend, ], (1, 6), border=(255, 255, 255), ) viz = imgviz.resize(viz, width=1800) if which == "pred": text = [] for class_id in np.unique(class_ids): add = observation[f"main/add_or_add_s/{class_id:04d}"] text.append(f"[{which}] [{class_id:04d}]: " f"add/add_s={add * 100:.1f}cm") text = "\n".join(text) else: text = f"[{which}]" viz = imgviz.draw.text_in_rectangle( viz, loc="lt", text=text, size=20, background=(0, 255, 0), color=(0, 0, 0), ) if which == "true": viz = imgviz.draw.text_in_rectangle( viz, loc="rt", text="singleview_3d", size=20, background=(255, 0, 0), color=(0, 0, 0), ) vizs.append(viz) viz = imgviz.tile(vizs, (2, 1), border=(255, 255, 255)) if args.save: out_file = path.Path(args.model).parent / f"video/{index:08d}.jpg" out_file.parent.makedirs_p() imgviz.io.imsave(out_file, viz) yield viz
def _r_from_q(q): q_wxyz = np.roll(q, 1) return transformations.quaternion_matrix(q_wxyz)[:3, :3]