def test_top_grasp(self):
sim = Simulator(gui=True, use_egl=False)
scene = h5py.File('../data/scenes/200210_1654_manually_generated_scenes.hdf5')['scene'][0]
sim.restore(scene, '../data/3d_models/shapenetsem40')
sim.cam.pos = [0, 0, 1.5]
sim.cam.width = 600
sim.cam.height = 600
cloud = sim.cam.point_cloud()
cloud = ortho_pipeline.transform_world_to_camera(cloud, sim.cam)
net = orthographic.OrthoNet(model_fn='/Users/mario/Developer/msc-thesis/data/networks/ggcnn_rss/epoch_29_model.hdf5')
ps, zs, xs, ws, scores = net.predict(cloud, net.network_predictor, debug=True, predict_best_only=True)
best_idx = np.argmax(scores)
print best_idx, ps, scores
p = ortho_pipeline.transform_camera_to_world(ps[best_idx], sim.cam)
R = ortho_pipeline.get_camera_frame(sim.cam)
z = R.dot(zs[best_idx].T).T
x = R.dot(xs[best_idx].T).T
w = ws[best_idx]
sim.add_gripper('../simulator/gripper.urdf')
sim.add_debug_pose(p, z, x, w)
sim.teleport_to_pre_grasp(p, z, x, w)
sim.grasp_along(z)
sim.move_to_post_grasp()
sim.run(1000)
def test_add_debug_pose(self):
sim = Simulator(gui=True, use_egl=False)
scene = h5py.File(
'../data/scenes/200210_1654_manually_generated_scenes.hdf5'
)['scene'][0]
sim.restore(scene, '../data/3d_models/shapenetsem40')
cam_rot = R.from_euler('ZXZ', [0, 90, -30], degrees=True).as_dcm()
print cam_rot
sim.add_debug_pose([0, 0, 1], cam_rot[:, 2], cam_rot[:, 0], .5)
sim.run()
def test_teleport_rotation_from_axis(self):
sim = Simulator(gui=True, use_egl=False)
sim.add_gripper('../simulator/gripper.urdf')
width = .2
angles = np.random.rand(3) * 2 * np.pi - np.pi
target_rot = R.from_euler('ZXZ', angles)
debug_rot = target_rot.as_dcm()
sim.add_debug_pose([0, 0, 1], debug_rot[:, 2], debug_rot[:, 0], width)
sim.run(epochs=100)
gripper_rot = target_rot * R.from_euler('X', np.pi)
sim.teleport_to_pose([0, 0, 1], gripper_rot.as_euler('XYZ'), width)
sim.run(epochs=10000)
def test_teleport_to_pre_grasp(self):
sim = Simulator(gui=True, use_egl=False)
scene = h5py.File(
'../data/scenes/200210_1654_manually_generated_scenes.hdf5'
)['scene'][0]
sim.restore(scene, '../data/3d_models/shapenetsem40')
sim.add_gripper('../simulator/gripper.urdf')
p = [-.032, -.032, .1]
z = [1, 1, 0]
x = [1, -1, 0]
w = .1
sim.add_debug_pose(p, z, x, w)
sim.run(epochs=100)
sim.teleport_to_pre_grasp(p, z, x, w)
sim.run()
bbox_inches='tight',
dpi=60)
_end = time.time()
logging.debug('Done in %ss' % (_end - _start))
best_idx = np.argmax(scores)
p = transform_camera_to_world(ps[best_idx], sim.cam)
R = get_camera_frame(sim.cam)
z = R.dot(zs[best_idx].T).T
x = R.dot(xs[best_idx].T).T
w = ws[best_idx]
logging.debug('Evaluating')
_start = time.time()
sim.add_debug_pose(p, z, x, w)
sim.teleport_to_pre_grasp(p, z, x, w)
sim.grasp_along(z)
sim.move_to_post_grasp()
result = sim.move_to_drop_off()
_end = time.time()
logging.debug('Evaluation %s and took %ss' %
(['failed', 'succeeded'][result], _end - _start))
if not args.omit_results:
results_f.write(','.join([
scene_name,
str(p),
str(z),
str(x),
str(w), metadata[best_idx]['view'],
str(result)
