def basic_info():
env = FetchDiscreteEnv(dis_tolerance=0.001, step_ds=0.005)
obs = env.reset()
print("Start position ->", env.pos)
print("Start (Close) gripper state ->", env.gripper_state)
env.gripper_close(False)
print("Open gripper state ->", env.gripper_state)
# Open gripper state -> [0.0500507 0.0500507]
env.gripper_close(True)
print("Close gripper state ->", env.gripper_state)
# Close gripper state -> [0.00184229 0.00184229]
env.measure_obj_reward()
def go_obj_savepic(is_render=True):
save_dir = 'z_fetch_run_pic'
create_dir(save_dir)
# dis_tolerance = 0.0001 # 1mm
step_ds = 0.005
env = FetchDiscreteEnv(dis_tolerance=0.001, step_ds=0.005)
# obs = env.reset()
# done = False
# ori_pos = (obs['eeinfo'][0]).copy()
print('---ori_pos = ', env.pos, '----')
# step = 0
# robot_step = 0
# # env.render()
s_time = time.time()
# env.render()
# step_count = 0
for i in range(5):
obs = env.reset()
env.gripper_close(False)
env.render()
save_dir = 'z_fetch_run_pic_%02d' % i
create_dir(save_dir)
step_count = 0
print('------start ep %03d--------' % i)
sum_r = 0
while True:
if is_render:
env.render()
diff_x = env.obj_pos[0] - env.pos[0]
diff_y = env.obj_pos[1] - env.pos[1]
if diff_x > step_ds:
a = [1, 0, 0, 0, 0]
elif diff_x < 0 and abs(diff_x) > step_ds:
a = [0, 0, 1, 0, 0]
elif diff_y > step_ds:
a = [0, 1, 0, 0, 0]
elif diff_y < 0 and abs(diff_y) > step_ds:
a = [0, 0, 0, 1, 0]
else:
break
step_count += 1
s, r, d, info = env.step(a)
sum_r += r
# rgb_external = env.sim.render(width=256, height=256, camera_name="external_camera_0", depth=False,
# mode='offscreen', device_id=-1)
# rgb_gripper = env.sim.render(width=256, height=256, camera_name="gripper_camera_rgb", depth=False,
# mode='offscreen', device_id=-1)
rgb_external = env.sim.render(width=256,
height=256,
camera_name="external_camera_0",
depth=False,
mode='offscreen',
device_id=-1)
rgb_gripper = env.sim.render(width=256,
height=256,
camera_name="gripper_camera_rgb",
depth=False,
mode='offscreen',
device_id=-1)
# print('type(rgb_gripper) = ', type(rgb_gripper),', shape=', np.shape(rgb_gripper))
img = Image.fromarray(rgb_gripper, 'RGB')
# img.save(save_dir + '/%03d.jpg' % step_count)
img.save(save_dir + '/%03d_r%3.2f.jpg' % (step_count, r))
a = [0, 0, 0, 0, 1]
s, r, d, info = env.step(a)
sum_r += r
print('sum_r = ', sum_r)
print("use step = ", step_count)
env.render()
# print('---final_pos = ' , obs['eeinfo'][0],'----')
# pos_diff = obs['eeinfo'][0] - ori_pos
# formattedList = ["%.2f" % member for member in pos_diff]
# print('---pos_diff = ' ,formattedList ,'----')
print('use time = {:.2f}'.format(time.time() - s_time))