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 _init_scene(self): self._scene = pyrender.Scene() camera = pyrender.PerspectiveCamera( yfov=self._fov, aspectRatio=1.0, znear=0.001) # do not change aspect ratio theta, phi = 0, np.pi / 4 + np.random.rand() * (np.pi / 8) radius = 3 + (np.random.rand() - 1) x = radius * np.sin(phi) * np.cos(theta) y = radius * np.sin(phi) * np.sin(theta) z = radius * np.cos(phi) camera_pos = np.array([x, y, z]) #camera_pos = np.array([0,0,10]) #light_pos = np.array([0,0,10]) camera_pose = tra.euler_matrix(0, phi, 0) camera_pose[:3, 3] = camera_pos camera_pose[:3, :3] = camera_pose[:3, :3] self._scene.add(camera, pose=camera_pose, name='camera') self.camera_pose = camera_pose light = pyrender.SpotLight(color=np.ones(4), intensity=50., innerConeAngle=np.pi / 16, outerConeAngle=np.pi / 6.0) self._scene.add(light, pose=camera_pose, name='light')
def _predict_grasps( self, sess, pc, latents, num_refine_steps, threshold, return_grasp_indexes=False, base_to_camera_rt=None, grasps_rt=None, ): metadata = {'type': 'metropolis'} grasps = [] scores = [] output_latents = [] improved_probs, improved_ts, improved_eulers, selection_mask = self._predict_and_refine_grasps( sess, pc, num_refine_steps, latents, threshold=threshold, choose_fn=self._choose_fns[self._cfg.grasp_selection_mode], metadata=metadata, grasps_rt=grasps_rt, ) refine_indexes, sample_indexes = np.where(selection_mask) if latents is None: output_latents = None grasp_indexes = [] num_removed_grasps = 0 for refine_index, sample_index in zip(refine_indexes, sample_indexes): rt = tra.euler_matrix(*improved_eulers[refine_index, sample_index, :]) rt[:3, 3] = improved_ts[refine_index, sample_index, :] if base_to_camera_rt is not None: dot = np.sum(rt[:3, 2] * -base_to_camera_rt[:3, 2]) if dot < 0: num_removed_grasps += 1 continue scores.append(improved_probs[refine_index, sample_index]) grasps.append(rt.copy()) if latents is not None: output_latents.append(latents[sample_index]) grasp_indexes.append([int(refine_index), int(sample_index)]) if num_removed_grasps > 0: print('removed {} grasps based on camera_to_base_rt'.format( num_removed_grasps)) if return_grasp_indexes: return grasps, scores, output_latents, grasp_indexes return grasps, scores, output_latents
def _init_camera_renderer(self): """ If not in visualizing mode, initialize camera with given intrinsics """ if self._viewer: return if self._intrinsics in ['kinect_azure', 'realsense']: camera = pyrender.IntrinsicsCamera(self._fx, self._fy, self._cx, self._cy, self._znear, self._zfar) self._camera_node = self._scene.add(camera, pose=np.eye(4), name='camera') self.renderer = pyrender.OffscreenRenderer( viewport_width=self._width, viewport_height=self._height, point_size=1.0) else: camera = pyrender.PerspectiveCamera( yfov=self._fov, aspectRatio=1.0, znear=0.001) # do not change aspect ratio self._camera_node = self._scene.add(camera, pose=tra.euler_matrix( np.pi, 0, 0), name='camera') self.renderer = pyrender.OffscreenRenderer(400, 400)
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 rot_and_trans_to_grasps(euler_angles, translations, selection_mask): grasps = [] refine_indexes, sample_indexes = np.where(selection_mask) for refine_index, sample_index in zip(refine_indexes, sample_indexes): rt = tra.euler_matrix(*euler_angles[refine_index, sample_index, :]) rt[:3, 3] = translations[refine_index, sample_index, :] grasps.append(rt) return grasps
def to_point_cloud_world_frame(self, depth): pc = self._to_pointcloud(depth, xyz=False) R = self.camera_pose[:3, :3] t = t = self.camera_pose[:3, 3] return (R.T @ tra.euler_matrix(0, 0, np.pi)[:3, :3] @ pc.T).T #R.T@pc + R.T@t
def __init__(self, q=None, num_contact_points_per_finger=10, root_folder=''): """Create a Franka Panda parallel-yaw gripper object. Keyword Arguments: q {list of int} -- configuration (default: {None}) num_contact_points_per_finger {int} -- contact points per finger (default: {10}) root_folder {str} -- base folder for model files (default: {''}) """ self.joint_limits = [0.0, 0.04] self.default_pregrasp_configuration = 0.04 if q is None: q = self.default_pregrasp_configuration self.q = q fn_base = root_folder + 'gripper_models/panda_gripper/hand.stl' fn_finger = root_folder + 'gripper_models/panda_gripper/finger.stl' self.base = trimesh.load(fn_base) self.finger_l = trimesh.load(fn_finger) self.finger_r = self.finger_l.copy() # transform fingers relative to the base self.finger_l.apply_transform(tra.euler_matrix(0, 0, np.pi)) self.finger_l.apply_translation([+q, 0, 0.0584]) self.finger_r.apply_translation([-q, 0, 0.0584]) self.fingers = trimesh.util.concatenate([self.finger_l, self.finger_r]) self.hand = trimesh.util.concatenate([self.fingers, self.base]) self.ray_origins = [] self.ray_directions = [] for i in np.linspace(-0.01, 0.02, num_contact_points_per_finger): self.ray_origins.append(np.r_[self.finger_l.bounding_box.centroid + [0, 0, i], 1]) self.ray_origins.append(np.r_[self.finger_r.bounding_box.centroid + [0, 0, i], 1]) self.ray_directions.append( np.r_[-self.finger_l.bounding_box.primitive.transform[:3, 0]]) self.ray_directions.append( np.r_[+self.finger_r.bounding_box.primitive.transform[:3, 0]]) self.ray_origins = np.array(self.ray_origins) self.ray_directions = np.array(self.ray_directions) self.standoff_range = np.array([ max(self.finger_l.bounding_box.bounds[0, 2], self.base.bounding_box.bounds[1, 2]), self.finger_l.bounding_box.bounds[1, 2] ]) self.standoff_range[0] += 0.001
def _init_scene(self): self._scene = pyrender.Scene() camera = pyrender.PerspectiveCamera( yfov=self._fov, aspectRatio=1.0, znear=0.001) # do not change aspect ratio camera_pose = tra.euler_matrix(np.pi, 0, 0) self._scene.add(camera, pose=camera_pose, name='camera') #light = pyrender.SpotLight(color=np.ones(4), intensity=3., innerConeAngle=np.pi/16, outerConeAngle=np.pi/6.0) #self._scene.add(light, pose=camera_pose, name='light') self.renderer = None
def perturb_grasp(grasp, num, min_translation, max_translation, min_rotation, max_rotation): """ Self explanatory. """ output_grasps = [] for _ in range(num): sampled_translation = [np.random.uniform(lb, ub) for lb, ub in zip(min_translation, max_translation)] sampled_rotation = [np.random.uniform(lb, ub) for lb, ub in zip(min_rotation, max_rotation)] grasp_transformation = tra.euler_matrix(*sampled_rotation) grasp_transformation[:3, 3] = sampled_translation output_grasps.append(np.matmul(grasp, grasp_transformation)) return output_grasps
def move_camera_auto(scenes, motion_id): transforms = np.array( [ [ [0.65291082, -0.10677561, 0.74987094, -0.42925002], [0.75528109, 0.166384, -0.63392968, 0.3910296], [-0.0570783, 0.98026289, 0.18927951, 0.48834561], [0.0, 0.0, 0.0, 1.0], ], [ [0.99762283, 0.04747429, -0.04994869, 0.04963055], [-0.04687166, -0.06385564, -0.99685781, 0.5102746], [-0.05051463, 0.9968293, -0.06147865, 0.63364497], [0.0, 0.0, 0.0, 1.0], ], np.eye(4), ] ) if motion_id == 0: transforms = transforms[[0]] elif motion_id == 1: transforms = transforms[[1, 2]] else: raise ValueError transform_prev = morefusion.extra.trimesh.from_opengl_transform( list(scenes.values())[0].camera_transform ) for transform_next in transforms: point_prev = np.r_[ ttf.translation_from_matrix(transform_prev), ttf.euler_from_matrix(transform_prev), ] point_next = np.r_[ ttf.translation_from_matrix(transform_next), ttf.euler_from_matrix(transform_next), ] for w in np.linspace(0, 1, 50): point = point_prev + w * (point_next - point_prev) transform = ttf.translation_matrix(point[:3]) @ ttf.euler_matrix( *point[3:] ) for scene in scenes.values(): scene.camera_transform[ ... ] = morefusion.extra.trimesh.to_opengl_transform(transform) yield scenes transform_prev = transform_next
def render_all_and_save_to_h5(self, output_path, all_eulers, vis=False): """ Args: output_path: path of the h5 file. all_eulers: list of 3 elemenet-tuples indicating the euler angles in degrees. """ if len(self._object_nodes) != 1: raise ValueError( 'object nodes should have 1 element, not {}'.format( len(self._object_nodes))) hf = h5py.File(output_path) #point_grp = hf.create_group('points') #pose_grp = hf.create_group('object_poses') mean_grp = hf.create_dataset('object_mean', data=self._object_means[0]) pcs = [] rotations = [] #import mayavi.mlab as mlab for i, euler in enumerate(all_eulers): assert isinstance(euler, tuple) and len(euler) == 3 rotation = tra.euler_matrix(*euler) color, _, pc, final_rotation = self.render([rotation]) MAX_POINTS = 3000 if pc.shape[0] > MAX_POINTS: pc = pc[np.random.choice( range(pc.shape[0]), replace=False, size=MAX_POINTS), :] elif pc.shape[0] < MAX_POINTS: pc = pc[np.random.choice( range(pc.shape[0]), replace=True, size=MAX_POINTS), :] #print('{}/{}: {}'.format(i, len(all_eulers), pc.shape)) cv2.imshow('w', color) cv2.waitKey(1) # mlab.figure() #mlab.points3d(pc[:, 0], pc[:, 1], pc[:, 2]) # mlab.show() key = '{}_{}_{}'.format(euler[0], euler[1], euler[2]) pcs.append(pc) rotations.append(final_rotation[0]) hf.create_dataset('pcs', data=pcs, compression='gzip') hf.create_dataset('object_poses', data=rotations, compression='gzip') hf.close()
def __init__(self, q=None, num_contact_points_per_finger=10, root_folder=os.path.dirname(os.path.dirname(os.path.abspath(__file__)))): """Create a Franka Panda parallel-yaw gripper object. Keyword Arguments: q {list of int} -- opening configuration (default: {None}) num_contact_points_per_finger {int} -- contact points per finger (default: {10}) root_folder {str} -- base folder for model files (default: {''}) """ self.joint_limits = [0.0, 0.04] self.root_folder = root_folder self.default_pregrasp_configuration = 0.04 if q is None: q = self.default_pregrasp_configuration self.q = q fn_base = os.path.join(root_folder, 'gripper_models/panda_gripper/hand.stl') fn_finger = os.path.join(root_folder, 'gripper_models/panda_gripper/finger.stl') self.base = trimesh.load(fn_base) self.finger_l = trimesh.load(fn_finger) self.finger_r = self.finger_l.copy() # transform fingers relative to the base self.finger_l.apply_transform(tra.euler_matrix(0, 0, np.pi)) self.finger_l.apply_translation([+q, 0, 0.0584]) self.finger_r.apply_translation([-q, 0, 0.0584]) self.fingers = trimesh.util.concatenate([self.finger_l, self.finger_r]) self.hand = trimesh.util.concatenate([self.fingers, self.base]) self.contact_ray_origins = [] self.contact_ray_directions = [] # coords_path = os.path.join(root_folder, 'gripper_control_points/panda_gripper_coords.npy') with open(os.path.join(root_folder,'gripper_control_points/panda_gripper_coords.pickle'), 'rb') as f: self.finger_coords = pickle.load(f, encoding='latin1') finger_direction = self.finger_coords['gripper_right_center_flat'] - self.finger_coords['gripper_left_center_flat'] self.contact_ray_origins.append(np.r_[self.finger_coords['gripper_left_center_flat'], 1]) self.contact_ray_origins.append(np.r_[self.finger_coords['gripper_right_center_flat'], 1]) self.contact_ray_directions.append(finger_direction / np.linalg.norm(finger_direction)) self.contact_ray_directions.append(-finger_direction / np.linalg.norm(finger_direction)) self.contact_ray_origins = np.array(self.contact_ray_origins) self.contact_ray_directions = np.array(self.contact_ray_directions)
def main_check_pointcloud(): from train_vae_clean import draw_scene, inverse_transform import mayavi.mlab as mlab import matplotlib.pyplot as plt renderer = ObjectRenderer(object_paths=['panda_gripper.obj'], object_scales=[1]) pcs = [] for _ in range(10): object_pose = tra.euler_matrix( np.random.rand() * np.pi * 2, np.random.rand() * np.pi * 2, np.random.rand() * np.pi * 2, ) #object_pose = np.eye(4) #object_pose[2, 3] = 0.8 color, depth, pc, object_pose = renderer.render([object_pose]) object_pose = object_pose[0] plt.imshow(color) draw_scene(np.matmul(pc, inverse_transform(object_pose).T), None, mesh=renderer.tmesh) mlab.figure() print(np.mean(pc, 0)) mlab.points3d(pc[:, 0], pc[:, 1], pc[:, 2], pc[:, 2], colormap='plasma') #pcs.append(np.matmul(pc, inverse_transform(object_pose).T)) #print(pcs[-1].shape) #draw_scene(pc, None) plt.show() mlab.show() pc = np.concatenate(pcs, 0) draw_scene(pc, None) mlab.show()
def test_rotation(self): np.random.seed(int(time.time())) batch_size = 30 control_points = 16 rot_matrix = np.zeros((batch_size, control_points, 3, 3), dtype=np.float32) quat_matrix = np.zeros((batch_size, control_points, 4), dtype=np.float32) points = np.random.rand(batch_size, control_points, 3) rotated_points = np.random.rand(batch_size, control_points, 3) for b in range(batch_size): for c in range(control_points): angles = np.random.uniform(low=0, high=math.pi * 2., size=[ 3, ]) rot_matrix[b, c, :, :] = tra.euler_matrix(angles[0], angles[1], angles[2])[:3, :3] quat_matrix[b, c, :] = tra.quaternion_from_euler( angles[0], angles[1], angles[2]) rotated_points[b, c, :] = np.matmul(rot_matrix[b, c, :, :], points[b, c, :]) tf_rotated_points = rotate_point_by_quaternion( tf.convert_to_tensor(points, dtype=tf.float32), tf.convert_to_tensor(quat_matrix, dtype=tf.float32)) with self.test_session(): if np.all( np.abs(tf_rotated_points.eval() - rotated_points) < 1e-4): print('----------> Rotation passed') else: raise ValueError( 'test rotatation did not match {} != {}'.format( tf_rotated_points.eval(), rotated_points))
def get_gripper_control_points_o3d(grasp, show_sweep_volume=False, color=(0.2, 0.8, 0)): """ Open3D Visualization of parallel-jaw grasp grasp: [4, 4] np array """ meshes = [] align = tra.euler_matrix(np.pi / 2, 0, 0) # Cylinder 3,5,6 cylinder_1 = o3d.geometry.TriangleMesh.create_cylinder(radius=0.005, height=0.139) transform = np.eye(4) transform[0, 3] = -0.03 transform = np.matmul(align, transform) transform = np.matmul(grasp, transform) cylinder_1.paint_uniform_color(color) cylinder_1.transform(transform) # Cylinder 1 and 2 cylinder_2 = o3d.geometry.TriangleMesh.create_cylinder(radius=0.005, height=0.07) transform = tra.euler_matrix(0, np.pi / 2, 0) transform[0, 3] = -0.065 transform = np.matmul(align, transform) transform = np.matmul(grasp, transform) cylinder_2.paint_uniform_color(color) cylinder_2.transform(transform) # Cylinder 5,4 cylinder_3 = o3d.geometry.TriangleMesh.create_cylinder(radius=0.005, height=0.06) transform = tra.euler_matrix(0, np.pi / 2, 0) transform[2, 3] = 0.065 transform = np.matmul(align, transform) transform = np.matmul(grasp, transform) cylinder_3.paint_uniform_color(color) cylinder_3.transform(transform) # Cylinder 6, 7 cylinder_4 = o3d.geometry.TriangleMesh.create_cylinder(radius=0.005, height=0.06) transform = tra.euler_matrix(0, np.pi / 2, 0) transform[2, 3] = -0.065 transform = np.matmul(align, transform) transform = np.matmul(grasp, transform) cylinder_4.paint_uniform_color(color) cylinder_4.transform(transform) cylinder_1.compute_vertex_normals() cylinder_2.compute_vertex_normals() cylinder_3.compute_vertex_normals() cylinder_4.compute_vertex_normals() meshes.append(cylinder_1) meshes.append(cylinder_2) meshes.append(cylinder_3) meshes.append(cylinder_4) # Just for visualizing - sweep volume if show_sweep_volume: finger_sweep_volume = o3d.geometry.TriangleMesh.create_box(width=0.06, height=0.02, depth=0.14) transform = np.eye(4) transform[0, 3] = -0.06 / 2 transform[1, 3] = -0.02 / 2 transform[2, 3] = -0.14 / 2 transform = np.matmul(align, transform) transform = np.matmul(grasp, transform) finger_sweep_volume.paint_uniform_color([0, 0.2, 0.8]) finger_sweep_volume.transform(transform) finger_sweep_volume.compute_vertex_normals() meshes.append(finger_sweep_volume) return meshes
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 nonuniform_quaternions(): all_poses = [] for az in np.linspace(0, np.pi * 2, 30): for el in np.linspace(-np.pi / 2, np.pi / 2, 30): all_poses.append(tra.euler_matrix(el, az, 0)) return all_poses
def render_scene_object_renderer(): path = './processed' table_path = './surfacemesh/CoffeeTable.obj' object_names = os.listdir(path)[12:13] object_paths = [f'{path}/{obj}/model.obj' for obj in object_names] meshes = [] surface_mesh = trimesh.load(table_path) z_coordinates = surface_mesh.vertices[:, 2] table_height = z_coordinates.max() - z_coordinates.min() object_paths.append(table_path) scales = [1] * len(object_paths) scales[-1] = 0.03 renderer = ObjectRenderer(object_paths=list(np.array(object_paths)), object_scales=scales) for object in object_paths: meshes.append(trimesh.load(object)) while True: all_poses = [] for i, object in enumerate(object_paths[:-1]): mesh = meshes[i] homogenous_matrix = np.eye(4) theta = np.random.rand() * 2 * np.pi z_euler_rotation_matrix = tra.euler_matrix(0, 0, theta) jitter = 1 translation = np.random.rand(2) * jitter - jitter / 2 homogenous_matrix[:2, 3] = translation homogenous_matrix[:3, :3] = z_euler_rotation_matrix[:3, :3] lowest_point_in_mesh = mesh.vertices[:, 2].min() homogenous_matrix[2, 3] = -lowest_point_in_mesh all_poses.append(copy.deepcopy(homogenous_matrix)) collusion_manager = trimesh.collision.CollisionManager() for i, object in enumerate(object_paths[:-1]): mesh = meshes[i] collusion_manager.add_object(name=object.split('/')[2], mesh=mesh, transform=all_poses[i]) collusions = collusion_manager.in_collision_internal( return_names=True)[1] objects_to_remove = [] while collusions: collided_objects = list(itertools.chain.from_iterable(collusions)) collusions_per_object = collections.Counter(collided_objects) most_collided_object = collusions_per_object.most_common(1)[0][0] objects_to_remove.append(most_collided_object) collusions = [ c for c in collusions if most_collided_object not in c ] object_mask = [name not in objects_to_remove for name in object_names] object_mask.append(True) homogenous_matrix = np.eye(4) homogenous_matrix[2, 3] = -table_height * 0.015 all_poses.append(homogenous_matrix) color, depth, xyz = renderer.render( object_paths=list(np.array(object_paths)[object_mask]), object_poses=list(np.array(all_poses)[object_mask])) camera_pose = renderer.camera_pose savefile = {} savefile['rgb'] = color savefile['xyz'] = xyz savefile['camera_pose'] = camera_pose savefile['object_paths'] = list(np.array(object_paths)[object_mask]) savefile['object_poses'] = list(np.array(all_poses)[object_mask]) fuckoff = renderer._to_pointcloud(depth, xyz=False) camera_pose = renderer.camera_pose with open(f'./data/{time.time()}.npy', 'wb') as f: np.save(f, savefile) cv2.imwrite(f'./data/{time.time()}.png', color) return color, xyz, depth
with open(f'./data/{time.time()}.npy', 'wb') as f: np.save(f, savefile) cv2.imwrite(f'./data/{time.time()}.png', color) return color, xyz, depth if __name__ == '__main__': pc, pose, color = render_scene_object_renderer() R = pose[:3, :3] t = pose[:3, 3] o = pc R = tra.euler_matrix(0, np.pi, 0)[:3, :3] @ R o = (R.T @ pc.T).T - t o -= np.array([0, 0, o[:, 2].min()]) fig = plt.figure() ax = fig.add_subplot(111, projection='3d') ax.scatter(o[:, 0], o[:, 1], o[:, 2]) plt.show() ''' plt.imshow(color) plt.show() '''
def run(): outputGui.set("starting") fileName = FileGUI.get() YOffset = startHGui.get() desiredSize = sizeGui.get() buildCenterX = buildCenterXGui.get() print(buildCenterX) buildCenterY = buildCenterYGui.get() print(buildCenterY) templateWorld = BaseGUI.get() ## setup parameters outputFileName = fileName.replace(".STL", "").replace(".stl", "").replace( ".obj", "").replace(".OBJ", "") path_to_save = "temp" blocks = 0 solidBlocks = 0 wb = Workbook() xrot = rotXGui.get() yrot = rotYGui.get() zrot = rotZGui.get() ##Make import mesh and turn it into a big m = trimesh.load(fileName) #import a mesh HTM = a = euler_matrix(xrot * numpy.pi / 180, yrot * numpy.pi / 180, zrot * numpy.pi / 180) m.apply_transform(HTM) v = m.voxelized(pitch=1) #voxelize a mesh with a simple scale outputGui.set("sizing...") biggest = max(v.matrix.shape) #get biggest dimension v = m.voxelized(pitch=biggest / desiredSize) #scale to match biggest dimension. dims = v.matrix.shape outV = v.matrix.copy() outputGui.set("voxel magic done") ##make temp folder if not os.path.isdir(path_to_save): #make a temp folder to work out of os.mkdir(path_to_save) else: for filename in os.listdir(path_to_save): file_path = os.path.join(path_to_save, filename) try: if os.path.isfile(file_path) or os.path.islink(file_path): os.unlink(file_path) elif os.path.isdir(file_path): shutil.rmtree(file_path) except Exception as e: print('Failed to delete %s. Reason: %s' % (file_path, e)) with ZipFile(templateWorld, 'r') as zipObj: zipObj.extractall(path_to_save) outputGui.set("hollowing the model") ##Processing the world to make it hollow and put the excel sheet together with bedrock.World(path_to_save) as world: selectedBlock = bedrock.Block("minecraft:smooth_stone") for z in range(dims[2]): ws1 = wb.create_sheet("Y layer " + str(z + YOffset)) for y in range(dims[1]): for x in range(dims[0]): if x > 0 and y > 0 and z > 0: if x + 1 < dims[0] and y + 1 < dims[ 1] and z + 1 < dims[2]: if v.matrix[x + 1][y][z] and v.matrix[x - 1][y][z]: if v.matrix[x][y + 1][z] and v.matrix[x][y - 1][z]: if v.matrix[x][y][z + 1] and v.matrix[x][y][z - 1]: outV[x][y][z] = False if outV[x][y][z]: blocks += 1 world.setBlock(x - round(dims[0] / 2), z + YOffset, y - round(dims[2] / 2), selectedBlock) ws1.cell(row=y + 1, column=x + 1).value = str( -x + round(dims[0] / 2) + buildCenterX) + "/" + str(-y + round(dims[2] / 2) + buildCenterY) if v.matrix[x][y][z]: solidBlocks += 1 world.save() with worldNBT.BedrockLevelFile.load(os.path.join(path_to_save, "level.dat")) as lvlNBT: lvlNBT["LastPlayed"] = time.time() lvlNBT["LevelName"] = ntpath.basename(outputFileName) wb.save(filename=outputFileName + '.xlsx') wb.close() outputGui.set("saving") #Clean up temp files startDir = os.getcwd() os.chdir(os.path.join(startDir, path_to_save)) with ZipFile(outputFileName + ".mcworld", 'w') as zipF: zipF.write("world_icon.jpeg") zipF.write("levelname.txt") zipF.write("level.dat_old") zipF.write("level.dat") for root, dirs, files in os.walk("db"): for file in files: zipF.write(os.path.join(root, file)) ##pack up world shutil.move(os.path.join(os.getcwd(), outputFileName + ".mcworld"), os.path.join(startDir, outputFileName + ".mcworld")) #print blocks required. outputGui.set("numblocks:%d num stacks:%f num shulkers:%f" % (blocks, blocks / 64, blocks / 64 / 27))