def gltf_trimesh(): sphere1 = primitives.Sphere(radius=1.0) sphere2 = primitives.Sphere(radius=2.0) # transformations.euler_from_quaternion(obj.transform.rotation, axes='sxyz') node1_transform = transformations.translation_matrix([0, 0, -2]) node2_transform = transformations.translation_matrix([5, 5, 5]) scene1 = scene.Scene() scene1.add_geometry(sphere1, node_name="Sphere1", geom_name="Geom Sphere1", transform=node1_transform, extras=test_metadata) scene1.add_geometry(sphere2, node_name="Sphere2", geom_name="Geom Sphere2", parent_node_name="Sphere1", transform=node2_transform, extras=test_metadata) scene1.metadata['extras'] = test_metadata files = scene1.export(None, "gltf") #, extras=test_metadata) print(files["model.gltf"].decode('utf8')) #for k, v in files.items(): # print(k, v) #"gltf-hierarchy-trimesh.gltf" #scene1.show() # Save scene with extras scene1.export("gltf-hierarchy-trimesh.glb") #, extras=test_metadata) '''
def check_compose_transform(self, R, t): T = compose_transform(R=R[:3, :3]) testing.assert_allclose(T, R) T = compose_transform(t=t) testing.assert_allclose(T, tf.translation_matrix(cuda.to_cpu(t))) T = compose_transform(R=R[:3, :3], t=t) testing.assert_allclose( T, tf.translation_matrix(cuda.to_cpu(t)) @ cuda.to_cpu(R))
def __init__(self, update_time=1 / 30, resolution=0.01): self.update_time = update_time self.resolution = resolution self.octree = octomap.OcTree(resolution) self.point_cloud = None self.is_changed = False self.window = pyglet.window.Window(width=1024, height=720) @self.window.event def on_key_press(symbol, modifiers): if modifiers == 0: if symbol == pyglet.window.key.Q: self.window.on_close() # self.gui = None aabb_min = np.array([-1.4, -1., 0.]) aabb_max = np.array([0.45, 0.75, 2.]) initial_bbox = trimesh.path.creation.box_outline( aabb_max - aabb_min, tf.translation_matrix((aabb_min + aabb_max) / 2), ) self.scene = trimesh.Scene(geometry=[initial_bbox]) # initial_bbox print(self.scene) # self.scene = None self.app_visualiser = None self.start_visualiser()
def update_visualisation(self, dt): if self.is_changed: if self.point_cloud is not None: self.octree.insertPointCloud( pointcloud=np.double(self.point_cloud), origin=np.array([0, 0, 0], dtype=float), # TODO this maxrange=2, ) occupied, _ = self.octree.extractPointCloud() aabb_min = self.octree.getMetricMin() aabb_max = self.octree.getMetricMax() bbox = trimesh.path.creation.box_outline( aabb_max - aabb_min, tf.translation_matrix((aabb_min + aabb_max) / 2), ) geom = trimesh.voxel.ops.multibox( occupied, pitch=self.resolution, colors=[1., 0, 0, 0.5] ) self.scene = trimesh.Scene(geometry=[bbox, geom]) # self.update_hbox(occupied, aabb_min, aabb_max) self.is_changed = False else: print("Didn't changed") pass
def main(): dataset = morefusion.datasets.YCBVideoDataset("train") frame = dataset[1000] class_ids = frame["meta"]["cls_indexes"] instance_ids = class_ids depth = frame["depth"] K = frame["meta"]["intrinsic_matrix"] rgb = frame["color"] pcd = morefusion.geometry.pointcloud_from_depth(depth, fx=K[0, 0], fy=K[1, 1], cx=K[0, 2], cy=K[1, 2]) instance_label = frame["label"] Ts_cad2cam_true = np.tile(np.eye(4), (len(instance_ids), 1, 1)) Ts_cad2cam_true[:, :3, :4] = frame["meta"]["poses"].transpose(2, 0, 1) Ts_cad2cam_pred = np.zeros_like(Ts_cad2cam_true) for i, ins_id in enumerate(instance_ids): mask = instance_label == ins_id centroid = np.nanmean(pcd[mask], axis=0) Ts_cad2cam_pred[i] = tf.translation_matrix(centroid) return refinement( instance_ids, class_ids, rgb, pcd, instance_label, Ts_cad2cam_true, Ts_cad2cam_pred, )
def normalize_mesh(mesh, inverse_padding=0.98): # normalize mesh: center on origin, and scale to unit cube translate = translation_matrix(-mesh.bounds.mean(axis=0)) # use 0.98 for padding scale = scale_matrix(inverse_padding / max(mesh.extents)) mesh.apply_transform(translate) mesh.apply_transform(scale)
def create_scene(): """ Create a scene with a Fuze bottle, some cubes, and an axis. Returns ---------- scene : trimesh.Scene Object with geometry """ scene = trimesh.Scene() # plane geom = trimesh.creation.box((0.5, 0.5, 0.01)) geom.apply_translation((0, 0, -0.005)) geom.visual.face_colors = (.6, .6, .6) scene.add_geometry(geom) # axis geom = trimesh.creation.axis(0.02) scene.add_geometry(geom) box_size = 0.1 # box1 geom = trimesh.creation.box((box_size, ) * 3) geom.visual.face_colors = np.random.uniform(0, 1, (len(geom.faces), 3)) transform = tf.translation_matrix([0.1, 0.1, box_size / 2]) scene.add_geometry(geom, transform=transform) # box2 geom = trimesh.creation.box((box_size, ) * 3) geom.visual.face_colors = np.random.uniform(0, 1, (len(geom.faces), 3)) transform = tf.translation_matrix([-0.1, 0.1, box_size / 2]) scene.add_geometry(geom, transform=transform) # fuze geom = trimesh.load(str(here / '../models/fuze.obj')) transform = tf.translation_matrix([-0.1, -0.1, 0]) scene.add_geometry(geom, transform=transform) # sphere geom = trimesh.creation.icosphere(radius=0.05) geom.visual.face_colors = np.random.uniform(0, 1, (len(geom.faces), 3)) transform = tf.translation_matrix([0.1, -0.1, box_size / 2]) scene.add_geometry(geom, transform=transform) return scene
def find_object_placement(self, obj_mesh, max_iter): """Try to find a non-colliding stable pose on top of any support surface. Args: obj_mesh (trimesh.Trimesh): Object mesh to be placed. max_iter (int): Maximum number of attempts to place to object randomly. Raises: RuntimeError: In case the support object(s) do not provide any support surfaces. Returns: bool: Whether a placement pose was found. np.ndarray: Homogenous 4x4 matrix describing the object placement pose. Or None if none was found. """ support_polys, support_T = self._get_support_polygons() if len(support_polys) == 0: raise RuntimeError("No support polygons found!") # get stable poses for object stable_obj = obj_mesh.copy() stable_obj.vertices -= stable_obj.center_mass stable_poses, stable_poses_probs = stable_obj.compute_stable_poses( threshold=0, sigma=0, n_samples=20 ) # stable_poses, stable_poses_probs = obj_mesh.compute_stable_poses(threshold=0, sigma=0, n_samples=1) # Sample support index support_index = max(enumerate(support_polys), key=lambda x: x[1].area)[0] iter = 0 colliding = True while iter < max_iter and colliding: # Sample position in plane pts = trimesh.path.polygons.sample( support_polys[support_index], count=1 ) # To avoid collisions with the support surface pts3d = np.append(pts, 0) # Transform plane coordinates into scene coordinates placement_T = np.dot( support_T[support_index], trimesh.transformations.translation_matrix(pts3d), ) pose = self._get_random_stable_pose(stable_poses, stable_poses_probs) placement_T = np.dot( np.dot(placement_T, pose), tra.translation_matrix(-obj_mesh.center_mass) ) # Check collisions colliding = self.is_colliding(obj_mesh, placement_T) iter += 1 return not colliding, placement_T if not colliding else None
def create_scene2(): scene = trimesh.Scene() for _ in range(5): geom = trimesh.load('models/fuze.obj') R = tf.random_rotation_matrix() T = tf.translation_matrix(np.random.uniform(-0.3, 0.3, (3, ))) scene.add_geometry(geom, transform=T @ R) return scene
def create_scene1(): scene = trimesh.Scene() for _ in range(20): geom = trimesh.creation.axis(0.02) R = tf.random_rotation_matrix() T = tf.translation_matrix(np.random.uniform(-1, 1, (3, ))) scene.add_geometry(geom, transform=T @ R) return scene
def visualize(occupied, empty, K, width, height, rgb, pcd, mask, resolution, aabb): window = pyglet.window.Window(width=int(640 * 0.9 * 3), height=int(480 * 0.9)) @window.event def on_key_press(symbol, modifiers): if modifiers == 0: if symbol == pyglet.window.key.Q: window.on_close() gui = glooey.Gui(window) hbox = glooey.HBox() hbox.set_padding(5) camera = trimesh.scene.Camera( resolution=(width, height), focal=(K[0, 0], K[1, 1]), transform=np.eye(4), ) camera_marker = trimesh.creation.camera_marker(camera, marker_height=0.1) # initial camera pose camera.transform = np.array( [[0.73256052, -0.28776419, 0.6168848, 0.66972396], [-0.26470017, -0.95534823, -0.13131483, -0.12390466], [0.62712751, -0.06709345, -0.77602162, -0.28781298], [ 0., 0., 0., 1., ]], ) aabb_min, aabb_max = aabb bbox = trimesh.path.creation.box_outline( aabb_max - aabb_min, tf.translation_matrix((aabb_min + aabb_max) / 2), ) geom = trimesh.PointCloud(vertices=pcd[mask], colors=rgb[mask]) scene = trimesh.Scene(camera=camera, geometry=[bbox, geom, camera_marker]) hbox.add(labeled_scene_widget(scene, label='pointcloud')) geom = trimesh.voxel.multibox(occupied, pitch=resolution, colors=[1., 0, 0, 0.5]) scene = trimesh.Scene(camera=camera, geometry=[bbox, geom, camera_marker]) hbox.add(labeled_scene_widget(scene, label='occupied')) geom = trimesh.voxel.multibox(empty, pitch=resolution, colors=[0.5, 0.5, 0.5, 0.5]) scene = trimesh.Scene(camera=camera, geometry=[bbox, geom, camera_marker]) hbox.add(labeled_scene_widget(scene, label='empty')) gui.add(hbox) pyglet.app.run()
def export_mesh(self, filename, layer_defs): from functools import reduce from trimesh.primitives import Extrusion from trimesh.transformations import translation_matrix reduce(lambda a, b: a + b, (Extrusion(polygon=geometry, height=min_max[1] - min_max[0], transform=translation_matrix((0, 0, min_max[0]))) for layer, min_max in layer_defs.items() for geometry in self.get_reduced_layer(layer))).export(filename)
def mesh(self): components = [] groups = [[]] last_position = translation_matrix([0, 0, 0]) last_height = 0 for comp in self.components: disp = comp.disp sign = np.sign(disp) if disp != 0 else 1 # move coordinates to edge last_position.dot(translation_matrix( [0., 0., sign * last_height / 2]), out=last_position) last_height = comp.height mesh = comp.mesh(last_position, groups) components.extend(mesh) space = merge_meshes(components) return components, groups, space
def get_frame(self, index): frame_dir = self.ids[index] rgb_file = frame_dir / "image.png" rgb = imgviz.io.imread(rgb_file)[:, :, :3] depth_file = frame_dir / "depth.npz" depth = np.load(depth_file)["arr_0"] depth = depth.astype(np.float32) / 1000.0 depth[depth == 0] = np.nan assert rgb.shape[:2] == depth.shape detections_file = frame_dir / "detections.npz" detections = np.load(detections_file) masks = detections["masks"] class_ids = detections["class_ids"] # scores = detections['scores'] camera_info_file = frame_dir / "camera_info.yaml" with open(camera_info_file) as f: camera_info = yaml.safe_load(f) K = np.array(camera_info["K"]).reshape(3, 3) instance_ids = [] instance_label = np.zeros(rgb.shape[:2], dtype=np.int32) for i, mask in enumerate(masks): instance_id = i + 1 instance_ids.append(instance_id) instance_label[mask] = instance_id instance_ids = np.array(instance_ids, dtype=np.int32) pcd = geometry_module.pointcloud_from_depth( depth, fx=K[0, 0], fy=K[1, 1], cx=K[0, 2], cy=K[1, 2] ) Ts_cad2cam = [] for instance_id in instance_ids: mask = instance_label == instance_id translation_rough = np.nanmean(pcd[mask], axis=0) T_cad2cam = tf.translation_matrix(translation_rough) Ts_cad2cam.append(T_cad2cam) Ts_cad2cam = np.array(Ts_cad2cam, dtype=float) return dict( instance_ids=instance_ids, class_ids=class_ids, rgb=rgb, depth=depth, instance_label=instance_label, intrinsic_matrix=K, T_cam2world=np.eye(4), Ts_cad2cam=Ts_cad2cam, cad_files={}, )
def __init__( self, rgb, pcd, instance_label, instance_ids, class_ids, Ts_cad2cam_true, Ts_cad2cam_pred=None, ): N = len(instance_ids) assert instance_ids.shape == (N, ) and 0 not in instance_ids assert class_ids.shape == (N, ) and 0 not in class_ids assert Ts_cad2cam_true.shape == (N, 4, 4) H, W = pcd.shape[:2] assert pcd.shape == (H, W, 3) assert instance_label.shape == (H, W) self._instance_ids = instance_ids self._class_ids = dict(zip(instance_ids, class_ids)) self._Ts_cad2cam_true = dict(zip(instance_ids, Ts_cad2cam_true)) self._rgb = rgb self._pcd = pcd self._instance_label = instance_label self._mapping = morefusion.contrib.MultiInstanceOctreeMapping() pitch = 0.01 nonnan = ~np.isnan(pcd).any(axis=2) for ins_id in np.unique(instance_label): if ins_id == -1: continue mask = instance_label == ins_id self._mapping.initialize(ins_id, pitch=pitch) self._mapping.integrate(ins_id, mask, pcd) self._cads = {} for instance_id in self._instance_ids: class_id = self._class_ids[instance_id] cad = self._models.get_cad(class_id=class_id) cad.visual = cad.visual.to_color() self._cads[instance_id] = cad if Ts_cad2cam_pred is None: self._Ts_cad2cam_pred = {} nonnan = ~np.isnan(pcd).any(axis=2) for instance_id in instance_ids: mask = instance_label == instance_id centroid = pcd[nonnan & mask].mean(axis=0) T_cad2cam_pred = ttf.translation_matrix(centroid) self._Ts_cad2cam_pred[instance_id] = T_cad2cam_pred else: assert len(instance_ids) == len(Ts_cad2cam_pred) self._Ts_cad2cam_pred = dict(zip(instance_ids, Ts_cad2cam_pred))
def setUp(self): batch_size = 5 self.translations = np.array( [tf.random_vector((3, )) for _ in range(batch_size)], dtype=np.float32, ) self.transforms = np.array( [tf.translation_matrix(q) for q in self.translations], 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 mesh(self, at, groups=None): """Generate mesh for gear pair at position given by `at`. .. note: `at` is/must be edited by reference """ sign = np.sign(self.disp) if self.disp != 0 else 1 if abs(self.disp) < self.stretch_margin: stretch = self.disp + sign * self.height / 2 else: stretch = sign * (self.height + self.gears.p.stretch + 2 * self.stretch_margin) / 2 at.dot(translation_matrix([0, 0, stretch]), out=at) at.dot(rotation_matrix(self.angle, [0, 0, 1]), out=at) p_mesh = self.gears.p.mesh(at) at.dot(translation_matrix( [self.ap, 0, sign * self.gears.p.stretch / 2]), out=at) g_mesh = self.gears.g.mesh(at) if groups is not None: groups[-1].append(p_mesh) groups.append([g_mesh]) return (p_mesh, g_mesh)
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 get_scenes(): dataset = morefusion.datasets.YCBVideoDataset("train") index = 0 video_id_prev = None scene = trimesh.Scene() while index < len(dataset): example = dataset[index] video_id, frame_id = dataset.ids[index].split("/") clear = False if video_id_prev is not None and video_id_prev != video_id: clear = True for node_name in scene.graph.nodes_geometry: scene.graph.transforms.remove_node(node_name) video_id_prev = video_id rgb = example["color"] depth = example["depth"] K = example["meta"]["intrinsic_matrix"] T_world2cam = np.r_[example["meta"]["rotation_translation_matrix"], [[0, 0, 0, 1]]] T_cam2world = np.linalg.inv(T_world2cam) pcd = morefusion.geometry.pointcloud_from_depth(depth, fx=K[0, 0], fy=K[1, 1], cx=K[0, 2], cy=K[1, 2]) nonnan = ~np.isnan(depth) geom = trimesh.PointCloud(vertices=pcd[nonnan], colors=rgb[nonnan]) scene.add_geometry(geom, transform=T_cam2world) # A kind of current camera view, but a bit far away to see whole scene. scene.camera.resolution = (rgb.shape[1], rgb.shape[0]) scene.camera.focal = (K[0, 0], K[1, 1]) scene.camera_transform = morefusion.extra.trimesh.to_opengl_transform( T_cam2world @ tf.translation_matrix([0, 0, -0.5])) # scene.set_camera() geom = trimesh.creation.camera_marker(scene.camera, marker_height=0.05)[1] geom.colors = [(0, 1.0, 0)] * len(geom.entities) scene.add_geometry(geom, transform=T_cam2world) index += 15 print(f"[{index:08d}] video_id={video_id}") yield {"__clear__": clear, "rgb": rgb, "scene": scene}
def init_visualiser(self): gui = glooey.Gui(self.window) # pyglet.clock.schedule_interval(func=self.update_visualisation, interval=self.update_time) aabb_min = np.array([-1.4, -1., 0.]) aabb_max = np.array([0.45, 0.75, 2.]) initial_bbox = trimesh.path.creation.box_outline( aabb_max - aabb_min, tf.translation_matrix((aabb_min + aabb_max) / 2), ) scene = trimesh.Scene(geometry=[initial_bbox]) # initial_bbox gui.add(trimesh.viewer.SceneWidget(scene)) pyglet.app.run()
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 export_mesh(self, filename, layer_defs): """ Saves the current geometry as a mesh-file. :param filename: Name of the file which will be created. The file ending determines the format. :param layer_defs: Definition of the layers, should be a list like [(layer,(z_min,z_max)),...] """ from functools import reduce from trimesh.primitives import Extrusion from trimesh.transformations import translation_matrix reduce(lambda a, b: a + b, (Extrusion(polygon=geometry, height=min_max[1] - min_max[0], transform=translation_matrix((0, 0, min_max[0]))) for layer, min_max in layer_defs.items() for geometry in (lambda x: x if hasattr(x, '__iter__') else [x, ])( self.get_reduced_layer(layer)))).export(filename)
def mesh(self, previous_edge, groups=None): """Return the mesh of the motor described in mesh_data and centered around `center` """ sign = np.sign(self.disp) if self.disp != 0 else 1 previous_edge.dot(translation_matrix( [0, 0, self.disp + sign * self.height / 2]), out=previous_edge) mesh_data = self.motor_data['mesh'] mesh = Cylinder(radius=mesh_data['r'], height=mesh_data['h'], transform=previous_edge.copy()) if groups is not None: groups[-1].append(mesh) return mesh,
def initialize_scene(self, scene, regressor, custom_regressor=None, smooth=True, profile=False): lights = {} for i, light in enumerate(scene.lights[:7]): matrix = scene.graph.get(light.name)[0] color = light.color.astype(np.float64) / 255.0 lights[i] = matrix, color w = self.frameGeometry().width() h = self.frameGeometry().height() scene.camera.resolution = [w, h] self.initialize_widget(lights=lights, fov=scene.camera.fov, z_near=scene.camera.z_near, z_far=scene.camera.z_far) self._scene = scene self._regressor = regressor self._custom_regressor = custom_regressor self._smooth = smooth # save initial camera transform self._initial_camera_transform = scene.camera_transform.copy() self._initial_camera_scale = scene.scale.copy() self._initial_camera_centroid = scene.centroid.copy() self._line_offset = tf.translation_matrix([0, 0, scene.scale / 1000]) self._reset_view() self._batch = pyglet.graphics.Batch() self._vertex_list = {} # store scene geometry as vertex lists self._vertex_list_hash = {} # store geometry hashes self._vertex_list_mode = {} # store geometry rendering mode self._profile = bool(profile) if self._profile: from pyinstrument import Profiler self.Profiler = Profiler
def check_max_voxelization_3d(origin, pitch, points, values, gpu, **kwargs): batch_indices = np.zeros((values.shape[0], ), dtype=np.int32) intensities = np.linalg.norm(values, axis=1) if gpu >= 0: cuda.get_device_from_id(gpu).use() values = cuda.to_gpu(values) points = cuda.to_gpu(points) batch_indices = cuda.to_gpu(batch_indices) intensities = cuda.to_gpu(intensities) y = morefusion.functions.max_voxelization_3d( values, points, batch_indices=batch_indices, intensities=intensities, batch_size=1, origin=origin, pitch=pitch, dimensions=(32, 32, 32), ) y = y[0] y = y.transpose(1, 2, 3, 0) matrix_values = cuda.to_cpu(y.array) matrix_filled = (matrix_values != 0).any(axis=3) scene = trimesh.Scene() scene.angles = np.zeros(3) transform = ttf.scale_matrix(pitch) transform = ttf.translation_matrix(origin) @ transform geom = trimesh.voxel.VoxelGrid(encoding=matrix_filled, transform=transform).as_boxes() I, J, K = zip(*np.argwhere(matrix_filled)) geom.visual.face_colors = matrix_values[I, J, K].repeat(12, axis=0) scene.add_geometry(geom) def callback(scene): scene.set_camera(angles=scene.angles) scene.angles += [0, np.deg2rad(1), 0] trimesh.viewer.SceneViewer(scene=scene, callback=callback, **kwargs)
def get_transform(self, obj_id, frame="com"): """Get object transformation in scene coordinates. Args: obj_id (str): Name of the object. frame (str, optional): Object reference frame to use. Either 'com' (center of mass) or 'mesh' (origin of mesh file). Defaults to 'com'. Raises: ValueError: If frame is not 'com' or 'mesh'. Returns: np.ndarray: Homogeneous 4x4 matrix. """ if frame == "com": return np.dot( self._poses[obj_id], tra.translation_matrix(self._objects[obj_id].center_mass), ) elif frame == "mesh": return self._poses[obj_id] raise ValueError("Unknown argument:", frame)
def __init__( self, rgb, pcd, instance_label, instance_ids, class_ids, Ts_cad2cam_true, Ts_cad2cam_pred=None, ): self._rgb = rgb self._pcd = pcd self._instance_label = instance_label self._instance_label_viz = imgviz.label2rgb(instance_label) self._instance_ids = instance_ids self._class_ids = dict(zip(instance_ids, class_ids)) self._Ts_cad2cam_true = dict(zip(instance_ids, Ts_cad2cam_true)) self._cads = {} for instance_id in self._instance_ids: class_id = self._class_ids[instance_id] cad = self._models.get_cad(class_id=class_id) cad.visual = cad.visual.to_color() self._cads[instance_id] = cad if Ts_cad2cam_pred is None: self._Ts_cad2cam_pred = {} nonnan = ~np.isnan(pcd).any(axis=2) for instance_id in instance_ids: mask = instance_label == instance_id centroid = pcd[nonnan & mask].mean(axis=0) T_cad2cam_pred = tf.translation_matrix(centroid) self._Ts_cad2cam_pred[instance_id] = T_cad2cam_pred else: assert len(instance_ids) == len(Ts_cad2cam_pred) self._Ts_cad2cam_pred = dict(zip(instance_ids, Ts_cad2cam_pred))
aabb_max_eye = (1, 1, 1) distance = np.full((n_keypoints, ), 1, dtype=float) elevation = np.random.uniform(30, 90, (n_keypoints, )) azimuth = np.random.uniform(0, 360, (n_keypoints, )) eyes = morefusion.geometry.points_from_angles(distance, elevation, azimuth) indices = indices = np.linspace(0, 127, num=len(eyes)) indices = indices.round().astype(int) eyes = morefusion.geometry.trajectory.sort_by(eyes, key=targets[indices]) eyes = morefusion.geometry.trajectory.interpolate(eyes, n_points=128) # ----------------------------------------------------------------------------- scene = trimesh.Scene() box = trimesh.path.creation.box_outline((2, 2, 2)) scene.add_geometry(box) axis = trimesh.creation.axis(0.01) point = trimesh.creation.icosphere(radius=0.01, color=(1.0, 0, 0)) for eye, target in zip(eyes, targets): transform = tf.translation_matrix(eye) scene.add_geometry(axis, transform=transform) transform = tf.translation_matrix(target) scene.add_geometry(point, transform=transform) ray = trimesh.load_path([eye, target]) scene.add_geometry(ray) morefusion.extra.trimesh.display_scenes({"scene": scene})
def find_object_placement(self, obj_mesh, max_iter, distance_above_support, gaussian=None): """Try to find a non-colliding stable pose on top of any support surface. Args: obj_mesh (trimesh.Trimesh): Object mesh to be placed. max_iter (int): Maximum number of attempts to place to object randomly. distance_above_support (float): Distance the object mesh will be placed above the support surface. gaussian (list[float], optional): Normal distribution for position in plane (mean_x, mean_y, std_x, std_y). Defaults to None. Raises: RuntimeError: In case the support object(s) do not provide any support surfaces. Returns: bool: Whether a placement pose was found. np.ndarray: Homogenous 4x4 matrix describing the object placement pose. Or None if none was found. """ support_polys, support_T = self._get_support_polygons() if len(support_polys) == 0: raise RuntimeError("No support polygons found!") # get stable poses for object stable_obj = obj_mesh.copy() stable_obj.vertices -= stable_obj.center_mass stable_poses, stable_poses_probs = stable_obj.compute_stable_poses( threshold=0, sigma=0, n_samples=1) # stable_poses, stable_poses_probs = obj_mesh.compute_stable_poses(threshold=0, sigma=0, n_samples=1) # Sample support index support_index = max(enumerate(support_polys), key=lambda x: x[1].area)[0] iter = 0 colliding = True while iter < max_iter and colliding: # Sample position in plane if gaussian: while True: p = Point( np.random.normal( loc=np.array(gaussian[:2]) + support_polys[support_index].centroid, scale=gaussian[2:], )) if p.within(support_polys[support_index]): pts = [p.x, p.y] break else: pts = trimesh.path.polygons.sample( support_polys[support_index], count=1) # To avoid collisions with the support surface pts3d = np.append(pts, distance_above_support) # Transform plane coordinates into scene coordinates placement_T = np.dot( support_T[support_index], trimesh.transformations.translation_matrix(pts3d), ) pose = self._get_random_stable_pose(stable_poses, stable_poses_probs) placement_T = np.dot(np.dot(placement_T, pose), tra.translation_matrix(-obj_mesh.center_mass)) # placement_T = np.dot(placement_T, pose) # Check collisions colliding = self.in_collision_with( obj_mesh, placement_T, min_distance=distance_above_support) iter += 1 return not colliding, placement_T if not colliding else None
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