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