# kuka_uid=kuka_env.kuka.kukaUid,
# fixed_config=fixed_config)
# grasp loop
top_1 = [0, 0]
top_2 = [0, 0]
top_3 = [0, 0]
for _ in range(opts.num_trials + 1):
kuka_env.drop_objects(opts.num_objects)
for _ in range(100):
p.stepSimulation()
time.sleep(1)
# take picture
img, depth, view_mat, proj_mat, width, height = camera.render()
view_inv = np.linalg.inv(view_mat)
proj_inv = np.linalg.inv(proj_mat)
#camera_pos = view_inv[:3,-1]
#f_x = proj_mat[0,0]
#f_y = proj_mat[1,1]
#c_x = proj_mat[2,1]
#c_y = proj_mat[1,1]
# RQ decomp to reconstruct camera mat from proj mat
#R, K = np.linalg.qr(proj_mat[:,:3])
#T = np.diag(np.sign(np.diag(K)))
#if np.linalg.det(T) < 0:
# T[1,1] *= -1
right.material = material()
right.material.color = color(0.5, 1, 0.1)
right.material.diffuse = 0.7
right.material.specular = 0.3
w.objects.append(right)
left = sphere()
left.transform = translation(-1.5, 0.33, -0.75) * scaling(0.33, 0.33, 0.33)
left.material = material()
left.material.color = color(1, 0.8, 0.1)
left.material.diffuse = 0.7
left.material.specular = 0.3
w.objects.append(left)
cam = camera(400, 200, pi / 3)
cam.transform = view_transform(point(0, 1.5, -5), point(0, 1, 0),
vector(0, 1, 0))
canvas = render(cam, w)
end = time.time()
print("Finished render.")
print(str(round(end - start, 2)) + "s")
start = time.time()
print("Start writing file...")
canvas_to_ppm(canvas).write_file("images/planes.ppm")
end = time.time()
print("Finished writing file.")
print(str(round(end - start, 2)) + "s")
def step_impl(context):
context.image = render(context.c, context.w)