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))
