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_object_to_world(self):
sim = Simulator(gui=False, 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 = 300
sim.cam.height = 300
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')
points, orientations, angles, widths, scores = net.predict(cloud, net.network_predictor,
debug=False)
best_idx = np.argmax(scores)
p = points[best_idx]
z = orientations[best_idx]
x = angles[best_idx]
w = widths[best_idx]
y = np.cross(z, x)
# Camera to world transformation
p = ortho_pipeline.transform_camera_to_world(p, sim.cam)
R = ortho_pipeline.get_camera_frame(sim.cam)
z = R.dot(z.T).T
x = R.dot(x.T).T
recloud = ortho_pipeline.transform_camera_to_world(cloud, sim.cam)
orthographic.render_pose(PointCloud(recloud), p, z, x, w)
def test_ortho_pipeline(self):
import pybullet as p
from simulator.simulator import Simulator
from core.orthographic import PointCloud, OrthoNet
from core.evaluate_orthographic_pipeline import transform_world_to_camera
import h5py
plt.figure()
sim = Simulator(use_egl=False, gui=True)
sim.cam.pos = [1., 1., 1.]
sim.cam.width = 500
sim.cam.height = 500
scene = h5py.File(
'../data/scenes/200210_1654_manually_generated_scenes.hdf5'
)['scene'][0]
sim.restore(scene, '../data/3d_models/shapenetsem40')
cloud = transform_world_to_camera(sim.cam.point_cloud(), sim.cam)
net = OrthoNet(
model_fn=
'/Users/mario/Developer/msc-thesis/data/networks/ggcnn_rss/epoch_29_model.hdf5'
)
net.predict(cloud,
predictor=net.network_predictor,
debug=True,
roi=[-1, 1, -1, 1, -1, 1],
padding=80)
plt.pause(0)
def test_transform_world_to_camera(self):
sim = Simulator(use_egl=False)
scenes_ds = h5py.File('../data/scenes/shapenetsem40_5.hdf5', 'r')
scene = scenes_ds['scene'][np.random.randint(len(scenes_ds['scene'][:]))]
sim.restore(scene, os.environ['MODELS_PATH'])
sim.cam.pos = [1, 1, 1.5]
sim.cam.width = 600
sim.cam.height = 600
cloud = sim.cam.point_cloud()
print 'cloud was generated'
cloud = ortho_pipeline.transform_world_to_camera(cloud, sim.cam)
eye = np.eye(3)
orthographic.render_frame([0, 0, 0], eye[0], eye[1], eye[2], cloud=PointCloud(cloud))
def test_transform_camera_to_world(self):
sim = Simulator(use_egl=False)
scenes_ds = h5py.File('../data/scenes/shapenetsem40_5.hdf5', 'r')
scene = scenes_ds['scene'][np.random.randint(len(scenes_ds['scene'][:]))]
sim.restore(scene, os.environ['MODELS_PATH'])
sim.cam.pos = [1, 1, 1.5]
sim.cam.width = 200
sim.cam.height = 200
cloud_orig = sim.cam.point_cloud()
print 'cloud was generated'
cloud_transformed = ortho_pipeline.transform_world_to_camera(cloud_orig, sim.cam)
cloud_recovered = ortho_pipeline.transform_camera_to_world(cloud_transformed, sim.cam)
diff = np.abs(cloud_orig - cloud_recovered)
assert np.all(diff < 1e-2)
from simulator.simulator import Simulator
import h5py
import pptk
import numpy as np
import core.evaluate_orthographic_pipeline as ortho_pipeline
SCENE_FN = '/Users/mario/Developer/msc-thesis/data/scenes/200220_1700_manually_generated_scenes.hdf5'
scene = h5py.File(SCENE_FN)['scene'][0]
sim = Simulator(use_egl=False, gui=False)
sim.cam.pos = [.6, 0., .3]
sim.restore(scene, '../data/3d_models/shapenetsem40')
cloud = sim.cam.point_cloud()
cloud = ortho_pipeline.transform_world_to_camera(cloud, sim.cam)
pptk.viewer(cloud)
np.save('../test/horizontal_bottle.npy', cloud)