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
