Пример #1
0
 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)
Пример #3
0
 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)
Пример #4
0
 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()
Пример #5
0
    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)
Пример #7
0
 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])
Пример #8
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()
Пример #9
0
    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()