def test_static_gripper(self): scene = h5py.File('../data/scenes/shapenetsem40_5.hdf5')['scene'][31] sim = Simulator(gui=True, use_egl=False) sim.restore(scene, '../data/3d_models/shapenetsem40') sim.add_gripper('../simulator/gripper.urdf') sim.load('../simulator/bin.urdf', pos=sim.bin_pos) sim.run(epochs=50000)
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_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()
def test_move_gripper(self): sim = Simulator(gui=True, debug=False, use_egl=False) sim.add_gripper('../simulator/gripper.urdf') sim.load('../simulator/data/cube_small.urdf') pos = [0, 0, 0.05] # pos = [0, 0, 0.02] pose = np.append(pos, np.radians([0, 0, 0])) sim.run(epochs=240) sim.set_gripper_width(0.07) sim.move_gripper_to(pose, angvel=4, linvel=0.6) sim.close_gripper() pose[2] = 0.5 sim.move_gripper_to(pose, angvel=4, linvel=0.6) sim.run(epochs=500)
# scenes = [scenes[5]] # Hack to fix pybullet-pylab incompatibility on mac os if args.gui: import pylab as plt plt.figure() sim = Simulator(use_egl=False, gui=args.gui) # Change to no gui sim.cam.pos = [ 0., np.cos(np.deg2rad(args.angle)) * args.distance, np.sin(np.deg2rad(args.angle)) * args.distance ] sim.cam.width = args.cam_resolution sim.cam.height = args.cam_resolution sim.add_gripper(os.environ['GRIPPER_PATH']) onet = OrthoNet(model_fn=args.network) _global_start = time.time() for scene_idx in range(len(scenes)): try: # TODO: remove after fixing all bugs scene_name = scenes_ds['name'][scene_idx] logging.debug('Testing scene %s' % scene_name) sim.restore(scenes[scene_idx], os.environ['MODELS_PATH']) # Get the gripper out of the way so it doesn't interfere with cloud sim.teleport_to_pose([0., 0., 10.], [0., 0., 0.], 0.) logging.debug('Generating point cloud') _start = time.time() cloud = transform_world_to_camera(sim.cam.point_cloud(), sim.cam)
def test_gripper_autostop(self): sim = Simulator(gui=True, use_egl=False) sim.add_gripper('../simulator/gripper.urdf') result = sim.move_gripper_to([0, 0, 0, 0, 0, 0])
def test_close_gripper(self): sim = Simulator(gui=True, use_egl=False) sim.add_gripper('../simulator/gripper.urdf') sim.run(epochs=100) result = sim.close_gripper()
def test_teleport_to_pose(self): sim = Simulator(gui=True, use_egl=False, debug=True) sim.add_gripper('../simulator/gripper.urdf') sim.run_debug_teleport()