def test_duck_on_plane():
ground_plane = RandomTexturedGroundPlaneHook(
size=1.,
texture_image_dir=os.path.join(TEST_DATA_DIR, 'textures/wood'),
)
duck = RandomFreeFallObject(
category_name='rubber_duck',
path_to_obj=DUCK_OBJ_PATH,
x=(-.1, .1),
y=(-.1, .1),
height=1.,
scale=0.1,
)
sim = BulletSimulator(
mode=CONNECT_MODE,
max_time=3.,
hooks=[
ground_plane,
duck,
],
)
for _ in range(3):
sim.reset()
while sim.running:
sim.step()
assert len(sim.objects) == 2
sim.close()
def test_obj_adder_single():
ground_plane = GroundPlaneHook()
duck = BasicOBJHook(
category_name='rubber_duck',
path_to_obj=DUCK_OBJ_PATH,
position=np.array([0, 0, 1]),
)
sim = BulletSimulator(
mode=CONNECT_MODE,
max_time=2.,
hooks=[
ground_plane,
duck,
],
)
for _ in range(2):
sim.reset()
while sim.running:
sim.step()
sim.close()
assert len(sim.objects) == 1
def test_r2d2():
ground_plane = GroundPlaneHook()
r2d2 = R2D2CreatorHook()
sim = BulletSimulator(
mode=CONNECT_MODE,
max_time=2,
hooks=[
ground_plane,
r2d2,
],
)
sim.reset()
while not sim.terminated:
output = sim.step()
def test_plane():
ground_plane = RandomTexturedGroundPlaneHook(
size=1.,
texture_image_dir=os.path.join(TEST_DATA_DIR, 'textures/carpet'),
)
sim = BulletSimulator(
mode=CONNECT_MODE,
max_time=3.,
hooks=[
ground_plane,
],
)
for _ in range(5):
sim.reset()
while sim.running:
sim.step()
assert len(sim.objects) == 1
sim.close()
def test_obj_adder_multiple_in_sequence():
ground_plane = GroundPlaneHook()
height = 3.
duck_1 = BasicOBJHook(
category_name='rubber_duck',
path_to_obj=DUCK_OBJ_PATH,
position=np.array([0, 0, height]),
time_to_create=0.,
)
duck_2 = BasicOBJHook(
category_name='rubber_duck',
path_to_obj=DUCK_OBJ_PATH,
position=np.array([0, 0, height]),
time_to_create=.5,
)
duck_3 = BasicOBJHook(
category_name='rubber_duck',
path_to_obj=DUCK_OBJ_PATH,
position=np.array([0, 0, height]),
time_to_create=1.,
)
sim = BulletSimulator(
mode=CONNECT_MODE,
max_time=3.,
hooks=[
ground_plane,
duck_1,
duck_2,
duck_3,
],
)
out = sim.reset()
assert len(sim.objects) == 1
for _ in range(2):
while sim.running:
out = sim.step()
sim.close()
assert len(sim.objects) == 3
def test_obj_adder_multiple_simultaneous():
ground_plane = GroundPlaneHook()
time_to_create = 1.
duck_1 = BasicOBJHook(
category_name='rubber_duck',
path_to_obj=DUCK_OBJ_PATH,
position=np.array([-1, -1, 2]),
time_to_create=time_to_create,
)
duck_2 = BasicOBJHook(
category_name='rubber_duck',
path_to_obj=DUCK_OBJ_PATH,
position=np.array([0, 0, 4]),
time_to_create=time_to_create,
)
duck_3 = BasicOBJHook(
category_name='rubber_duck',
path_to_obj=DUCK_OBJ_PATH,
position=np.array([1, 1, 6]),
time_to_create=time_to_create,
)
sim = BulletSimulator(
mode=CONNECT_MODE,
max_time=3.,
hooks=[
ground_plane,
duck_1,
duck_2,
duck_3,
],
)
for _ in range(2):
out = sim.reset()
while sim.running:
out = sim.step()
sim.close()
assert len(sim.objects) == 3
def test_dataset_two_cams():
ground_plane = RandomTexturedGroundPlaneHook(
size = 1.2,
texture_image_dir=os.path.join(TEST_DATA_DIR, 'textures/carpet'),
)
tray = RandomTexturedTrayHook(
size=0.6,
texture_image_dir=os.path.join(TEST_DATA_DIR, 'textures/wood'),
)
duck_1 = RandomFreeFallObject(
category_name = 'rubber_duck',
path_to_obj = DUCK_OBJ_PATH,
x = (-.05, .05),
y = (-.05, .05),
height = .5,
scale = 0.1,
time_to_create = .3
)
duck_2 = RandomFreeFallObject(
category_name = 'rubber_duck',
path_to_obj = DUCK_OBJ_PATH,
x = (-.05, .05),
y = (-.05, .05),
height = .5,
scale = 0.1,
time_to_create = 1.3
)
duck_3 = RandomFreeFallObject(
category_name = 'rubber_duck',
path_to_obj = DUCK_OBJ_PATH,
x = (-.05, .05),
y = (-.05, .05),
height = .5,
scale = 0.1,
time_to_create = 2.3
)
cache_dir = tempfile.mkdtemp()
print('\ncache_dir: %s' % cache_dir)
dataset_writer_1 = DatasetWriter(
path_to_dataset = cache_dir,
base_format = 'C1_%04d.png',
yml_filename = 'anno_c1.yml',
)
dataset_writer_2 = DatasetWriter(
path_to_dataset = cache_dir,
base_format = 'C2_%04d.png',
yml_filename = 'anno_c2.yml',
)
camera_1 = StaticOGLCameraHook(
K = [1075.65091572, 0.0, 210.06888344, 0.0, 1073.90347929, 174.72159802, 0.0, 0.0, 1.0],
img_shape = (400,400),
position = np.array([.8, .8, 1.3]),
lookat = np.array([0., 0., 0.]),
up = 'up',
start = np.inf,
# start = 0.,
# interval = .25,
dataset_writer = dataset_writer_1,
)
camera_2 = StaticOGLCameraHook(
K = [1075.65091572, 0.0, 210.06888344, 0.0, 1073.90347929, 174.72159802, 0.0, 0.0, 1.0],
img_shape = (400,400),
position = np.array([-.7, -.7, 1.2]),
lookat = np.array([0., 0., 0.]),
up = 'up',
start = np.inf,
# start = 0.,
# interval = .25,
dataset_writer = dataset_writer_2,
)
sim = BulletSimulator(
mode=CONNECT_MODE,
max_time=5.,
hooks=[
ground_plane,
tray,
duck_1,
duck_2,
duck_3,
camera_1,
camera_2,
],
)
for _ in range(3):
out = sim.reset()
while sim.running:
out = sim.step()
assert len(sim.objects) == 3 + 1 + 1
sim.close()
with open( os.path.join(cache_dir, 'anno_c1.yml') ) as f:
anno_c1 = yaml.load(f)
with open( os.path.join(cache_dir, 'anno_c2.yml') ) as f:
anno_c2 = yaml.load(f)
for anno in [anno_c1, anno_c2]:
for example in anno:
rgb = imread( os.path.join(cache_dir, example['path_to_rgb']) )
depth = imread( os.path.join(cache_dir, example['path_to_depth']) )
labels = imread( os.path.join( cache_dir, example['path_to_label'] ) )
K = np.reshape(example['intrinsics'], (3,3))
img_shape = (400,400)
world_to_cam = example['world_to_cam']
world_to_cam = (quat2R(world_to_cam['R'])[:3,:3], np.array(world_to_cam['t']) )
projected_foreground = np.zeros(img_shape)
for obj in example['objects']:
if obj['category_name'] in ['plane', 'tray']:
continue
mesh_scale = obj['mesh_scale']
mesh = object_loader.OBJFile( os.path.join(cache_dir, obj['path_to_obj']), None)
mesh.vertices = [ [mesh_scale * x for x in v] for v in mesh.vertices ]
object_to_world = obj['object_to_world']
object_to_world = (quat2R(object_to_world['R'])[:3,:3], np.array(object_to_world['t']) )
object_to_cam = chain(object_to_world, world_to_cam)
R,t = object_to_cam
mask = project_mesh(mesh, R, t, K, img_shape)
projected_foreground[mask>0] = 1.
foreground_from_labels = np.float32(labels > 1)
inter = np.float32( np.logical_and(projected_foreground>0, foreground_from_labels>0) )
union = np.float32( np.logical_or(projected_foreground>0, foreground_from_labels>0) )
overlap = inter.sum() / union.sum()
assert overlap > .9
def test_tray_tless():
ground_plane = RandomTexturedGroundPlaneHook(
size = .3,
texture_image_dir=os.path.join(TEST_DATA_DIR, 'textures/carpet'),
)
tray = RandomTexturedTrayHook(
size=0.15,
texture_image_dir=os.path.join(TEST_DATA_DIR, 'textures/wood'),
)
N = 7
tless_objects = []
for t in range(N):
time_to_create = .4 * t
obj_id = np.random.choice([1,10])
tless_obj = RandomFreeFallObject(
category_name = 'tless_obj_%02d' % obj_id,
path_to_obj = TLESS_OBJ_PATH % obj_id,
x = (-.05, .05),
y = (-.05, .05),
height = .2,
time_to_create = time_to_create,
scale = .001, # tless obj file use millimeter unit. E.g. obj_01 is about 6cm.
)
tless_objects.append(tless_obj)
cache_dir = tempfile.mkdtemp()
dataset_writer_1 = DatasetWriter(
path_to_dataset = cache_dir,
base_format = 'C1_%04d.png',
yml_filename = 'anno_c1.yml',
)
dataset_writer_2 = DatasetWriter(
path_to_dataset = cache_dir,
base_format = 'C2_%04d.png',
yml_filename = 'anno_c2.yml',
)
camera_1 = StaticOGLCameraHook(
K = [1075.65091572, 0.0, 210.06888344, 0.0, 1073.90347929, 174.72159802, 0.0, 0.0, 1.0],
img_shape = (400,400),
position = np.array([.3, .3, .5]),
lookat = np.array([0., 0., 0.]),
up = 'up',
start = np.inf,
dataset_writer = dataset_writer_1,
)
camera_2 = StaticOGLCameraHook(
K = [1075.65091572, 0.0, 210.06888344, 0.0, 1073.90347929, 174.72159802, 0.0, 0.0, 1.0],
img_shape = (400,400),
position = np.array([-.4, -.4, .6]),
lookat = np.array([0., 0., 0.]),
up = 'up',
start = np.inf,
dataset_writer = dataset_writer_2,
)
sim = BulletSimulator(
mode=CONNECT_MODE,
max_time=3.2,
hooks=[
ground_plane,
tray,
camera_1,
camera_2,
] + tless_objects,
)
for _ in range(2):
out = sim.reset()
while sim.running:
out = sim.step()
assert len(sim.objects) == N + 2
sim.close()
with open( os.path.join(cache_dir, 'anno_c1.yml') ) as f:
anno_c1 = yaml.load(f)
with open( os.path.join(cache_dir, 'anno_c2.yml') ) as f:
anno_c2 = yaml.load(f)
for ann1, ann2 in zip(anno_c1, anno_c2):
rgb1 = imread( os.path.join(cache_dir, ann1['path_to_rgb']) )
rgb2 = imread( os.path.join(cache_dir, ann2['path_to_rgb']) )
assert rgb1.shape == (400,400,4)
assert rgb2.shape == (400,400,4)
depth1 = imread( os.path.join(cache_dir, ann1['path_to_depth']) )
depth2 = imread( os.path.join(cache_dir, ann2['path_to_depth']) )
assert depth1.shape == (400,400)
assert depth2.shape == (400,400)
label1 = imread( os.path.join(cache_dir, ann1['path_to_label']) )
label2 = imread( os.path.join(cache_dir, ann2['path_to_label']) )
assert label1.shape == (400,400)
assert label2.shape == (400,400)
if VIS:
plt.figure(1), plt.clf(), plt.imshow(np.concatenate([rgb1, rgb2], axis=1))
plt.figure(2), plt.clf(), plt.imshow(np.concatenate([depth1, depth2], axis=1))
plt.figure(3), plt.clf(), plt.imshow(np.concatenate([label1, label2], axis=1))
plt.show(block=False)
plt.pause(.5)
print('\ncache dir: %s' % cache_dir)
def test_hello_pybullet():
sim = BulletSimulator(mode=CONNECT_MODE, max_time=5.)
sim.reset()
while not sim.terminated:
sim.step()
def test_random_static_cameras_one_shot_at_2s():
'''
Identical to above, but take only one pictures at t=2s
'''
ground_plane = GroundPlaneHook()
height = 3.
duck_1 = BasicOBJHook(
category_name='rubber_duck',
path_to_obj=DUCK_OBJ_PATH,
position=np.array([0, 0, height]),
time_to_create=0.,
)
duck_2 = BasicOBJHook(
category_name='rubber_duck',
path_to_obj=DUCK_OBJ_PATH,
position=np.array([0, 0, height]),
time_to_create=1.,
)
duck_3 = BasicOBJHook(
category_name='rubber_duck',
path_to_obj=DUCK_OBJ_PATH,
position=np.array([0, 0, height]),
time_to_create=2.,
)
camera_1 = RandomStaticCameraHook(
K=[
1075.65091572, 0.0, 210.06888344, 0.0, 1073.90347929, 174.72159802,
0.0, 0.0, 1.0
],
img_shape=(400, 400),
# position = np.array([10., 10., 7.]),
# lookat = np.array([0., 0., 0.]),
position=[(-10., 10.), (-10., 10.), 7],
lookat=[(-0.2, 0.2), (-0.2, 0.2), (0, 0.1)],
up='up',
start=2.,
light_src=[(-1, 1), (-1, 1), 1],
)
camera_2 = RandomStaticCameraHook(
K=[
1075.65091572, 0.0, 210.06888344, 0.0, 1073.90347929, 174.72159802,
0.0, 0.0, 1.0
],
img_shape=(400, 400),
position=[(-10., 10.), (-10., 10.), 6],
lookat=[(-0.2, 0.2), (-0.2, 0.2), (0, 0.1)],
up='up',
start=2.,
)
sim = BulletSimulator(
mode=CONNECT_MODE,
max_time=5.,
hooks=[
ground_plane,
duck_1,
duck_2,
duck_3,
camera_1,
camera_2,
],
)
images_in_all_rollouts = []
for _ in range(2):
out = sim.reset()
images = maybe_collect_images(out)
assert len(images) == 0
# num_images = 0
images_in_rollout = []
while sim.running:
out = sim.step()
images = maybe_collect_images(out)
if len(images) > 0:
images_in_rollout.extend(images)
images_in_all_rollouts.append(images_in_rollout)
sim.close()
# images across different rollout should be different.
assert not np.allclose(images_in_all_rollouts[0][0],
images_in_all_rollouts[1][0])
assert not np.allclose(images_in_all_rollouts[0][1],
images_in_all_rollouts[1][1])
def test_static_opengl_cameras():
'''
Based on test_obj_adder.test_obj_adder_multiple_in_sequence().
Take the first picture upon reset (start=0.),
and then take pictures every 0.1 seconds until time is up.
'''
# copy-and-paste
ground_plane = GroundPlaneHook()
height = 3.
duck_1 = BasicOBJHook(
category_name='rubber_duck',
path_to_obj=DUCK_OBJ_PATH,
position=np.array([0, 0, height]),
time_to_create=0.,
)
duck_2 = BasicOBJHook(
category_name='rubber_duck',
path_to_obj=DUCK_OBJ_PATH,
position=np.array([0, 0, height]),
time_to_create=1.,
)
duck_3 = BasicOBJHook(
category_name='rubber_duck',
path_to_obj=DUCK_OBJ_PATH,
position=np.array([0, 0, height]),
time_to_create=2.,
)
camera_1 = StaticOGLCameraHook(
K=[
1075.65091572, 0.0, 210.06888344, 0.0, 1073.90347929, 174.72159802,
0.0, 0.0, 1.0
],
img_shape=(400, 400),
position=np.array([10., 10., 7.]),
lookat=np.array([0., 0., 0.]),
up='up',
start=0.,
# interval = .5,
interval=.1,
)
camera_2 = StaticOGLCameraHook(
K=[
1075.65091572, 0.0, 210.06888344, 0.0, 1073.90347929, 174.72159802,
0.0, 0.0, 1.0
],
img_shape=(400, 400),
position=np.array([-10., -10., 6.]),
lookat=np.array([0., 0., 0.]),
up='up',
start=0.,
# interval = .5,
interval=.1,
)
sim = BulletSimulator(
mode=CONNECT_MODE,
max_time=5.,
hooks=[
ground_plane,
duck_1,
duck_2,
duck_3,
camera_1,
camera_2,
],
)
for _ in range(2):
out = sim.reset()
num_images = 0
images = maybe_collect_images(out)
for I in images:
assert I.shape == (400, 400, 4)
num_images += len(images)
while sim.running:
out = sim.step()
images = maybe_collect_images(out)
for I in images:
assert I.shape == (400, 400, 4)
num_images += len(images)
assert num_images == 100
sim.close()
def test_static_opengl_cameras_one_final_shot():
'''
Identical to above, but take only one pictures before simulation terminates (start=np.inf).
'''
ground_plane = GroundPlaneHook()
height = 3.
duck_1 = BasicOBJHook(
category_name='rubber_duck',
path_to_obj=DUCK_OBJ_PATH,
position=np.array([0, 0, height]),
time_to_create=0.,
)
duck_2 = BasicOBJHook(
category_name='rubber_duck',
path_to_obj=DUCK_OBJ_PATH,
position=np.array([0, 0, height]),
time_to_create=1.,
)
duck_3 = BasicOBJHook(
category_name='rubber_duck',
path_to_obj=DUCK_OBJ_PATH,
position=np.array([0, 0, height]),
time_to_create=2.,
)
camera_1 = StaticOGLCameraHook(
K=[
1075.65091572, 0.0, 210.06888344, 0.0, 1073.90347929, 174.72159802,
0.0, 0.0, 1.0
],
img_shape=(400, 400),
position=np.array([10., 10., 7.]),
lookat=np.array([0., 0., 0.]),
up='up',
start=np.inf,
)
camera_2 = StaticOGLCameraHook(
K=[
1075.65091572, 0.0, 210.06888344, 0.0, 1073.90347929, 174.72159802,
0.0, 0.0, 1.0
],
img_shape=(400, 400),
position=np.array([-10., -10., 6.]),
lookat=np.array([0., 0., 0.]),
up='up',
start=np.inf,
)
sim = BulletSimulator(
mode=CONNECT_MODE,
max_time=5.,
hooks=[
ground_plane,
duck_1,
duck_2,
duck_3,
camera_1,
camera_2,
],
)
for _ in range(2):
out = sim.reset()
num_images = 0
images = maybe_collect_images(out)
assert len(images) == 0
while sim.running:
out = sim.step()
images = maybe_collect_images(out)
if len(images) > 0:
assert len(images) == 2
assert sim.terminated
for I in images:
assert I.shape == (400, 400, 4)
sim.close()
def test_pack_box():
ground_plane = RandomTexturedGroundPlaneHook(
size = 30.,
texture_image_dir=os.path.join(TEST_DATA_DIR, 'textures/carpet'),
)
# mesh_scale = 1.0
mesh_scale = .7
box_in_box = BoxPackingHook(
path_to_obj = CUBOID_OBJ_PATH,
category_name = 'boxy_obj',
mesh_scale = mesh_scale,
box_dim = [ (2.5, 2.8), (3.5, 3.8) ],
# box_center = [(-0.3, 0.3), (-0.2, 0.2)],
box_center = [(-1.3, -0.7), (-0.2, 0.2)],
inplane_rot_angles = [-6, -4, -2, 0, 2, 4, 6],
slack = (0, 0.1),
block_filter = block_filter,
)
cache_dir = tempfile.mkdtemp()
print('\ncache_dir: %s' % cache_dir)
dataset_writer = DatasetWriter(
path_to_dataset = cache_dir,
base_format = 'C1_%04d.png',
yml_filename = 'anno_c1.yml',
)
camera = RandomStaticCameraHook(
K = [616.262451172, 0.0, 330.200531006, 0.0, 616.415588379, 235.531219482, 0.0, 0.0, 1.0],
img_shape = (480, 640),
position = [(-6., 6.), (-6., 6.), 8.],
lookat = np.zeros(3),
up = 'up',
light_src = [(-5.0, 5.0), (-5.0, 5.0), (5.0, 8.0)],
start = 0.,
dataset_writer = dataset_writer,
)
sim = BulletSimulator(
mode=CONNECT_MODE,
max_time=0.00001,
# max_time=1000.0,
hooks=[
ground_plane,
box_in_box,
camera,
]
)
start_time = time.time()
for _ in range(4):
out = sim.reset()
while sim.running:
out = sim.step()
# assert len(sim.objects) == N + 2
elapsed = time.time() - start_time
sim.close()
with open( os.path.join(cache_dir, 'anno_c1.yml') ) as f:
anno = yaml.load(f)
for example in anno:
rgb = imread( os.path.join(cache_dir, example['path_to_rgb']) )
depth = imread( os.path.join(cache_dir, example['path_to_depth']) )
labels = imread( os.path.join( cache_dir, example['path_to_label'] ) )
K = np.reshape(example['intrinsics'], (3,3))
img_shape = (480,640)
world_to_cam = example['world_to_cam']
world_to_cam = (quat2R(world_to_cam['R'])[:3,:3], np.array(world_to_cam['t']) )
projected_foreground = np.zeros(img_shape)
for obj in example['objects']:
if obj['category_name'] in ['plane', 'tray']:
continue
mesh_scale = obj['mesh_scale']
mesh = object_loader.OBJFile( os.path.join(cache_dir, obj['path_to_obj']), None)
mesh.vertices = [ [mesh_scale * x for x in v] for v in mesh.vertices ]
object_to_world = obj['object_to_world']
object_to_world = (quat2R(object_to_world['R'])[:3,:3], np.array(object_to_world['t']) )
object_to_cam = chain(object_to_world, world_to_cam)
R,t = object_to_cam
mask = project_mesh(mesh, R, t, K, img_shape)
projected_foreground[mask>0] = 1.
foreground_from_labels = np.float32(labels > 0)
inter = np.float32( np.logical_and(projected_foreground>0, foreground_from_labels>0) )
union = np.float32( np.logical_or(projected_foreground>0, foreground_from_labels>0) )
overlap = inter.sum() / union.sum()
assert overlap > .9