def basic_test():
env = FetchDiscreteEnv(dis_tolerance=0.001, step_ds=0.005)
obs = env.reset()
done = False
ori_pos = (obs['eeinfo'][0]).copy()
print('---ori_pos = ', obs['eeinfo'][0], '----')
step = 0
robot_step = 0
# env.render()
s_time = time.time()
for _ in range(2000):
env.render()
a = [1, 0, 0, 0, 0]
s, r, d, info = env.step(a)
print('now pos = ', env.pos)
# env.render()
# a = [0, 0, 1, 0, 0]
# s,r, d, info = env.step(a)
time.sleep(1)
# env.gripper_close()
a = [0, 0, 0, 0, 1]
s, r, d, info = env.step(a)
env.render()
time.sleep(1)
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))
def __init__(self,
dis_tolerance=0.001,
step_ds=0.005,
gray_img=True,
hsv_color=False,
is_render=False):
self.env = FetchDiscreteEnv(dis_tolerance=0.001,
step_ds=0.005,
is_render=is_render)
self.gray_img = gray_img
self.target_pic = None
self.hsv_color = hsv_color
self.is_render = is_render
def __init__(self,
dis_tolerance=0.001,
step_ds=0.005,
gray_img=True,
use_tray=True,
is_render=False,
only_show_obj0=False):
self.env = FetchDiscreteEnv(dis_tolerance=0.001,
step_ds=0.005,
use_tray=use_tray,
is_render=is_render)
self.gray_img = gray_img
self.is_render = is_render
self.only_show_obj0 = only_show_obj0
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():
# 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 = ', obs['eeinfo'][0], '----')
step = 0
robot_step = 0
# env.render()
s_time = time.time()
sum_r = 0
while True:
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
s, r, d, info = env.step(a)
sum_r += r
a = [0, 0, 0, 0, 1]
s, r, d, info = env.step(a)
sum_r += r
env.render()
print('epsoide sum_r = ', sum_r)
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))
def test_limit(go_pos):
step_ds = 0.005
env = FetchDiscreteEnv(dis_tolerance=0.001, step_ds=0.005)
obs = env.reset()
done = False
sum_r = 0
while True:
env.render()
diff_x = go_pos[0] - env.pos[0]
diff_y = go_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
s, r, d, info = env.step(a)
sum_r += r
print('now pos = ', env.pos)
print('Sum reward = ', sum_r)
print('Final pos = ', env.pos)
def test_one_axis():
env = FetchDiscreteEnv(dis_tolerance=0.001, step_ds=0.005)
obs = env.reset()
ori_pos = (obs['eeinfo'][0]).copy()
print('---ori_pos = ', obs['eeinfo'][0], '----')
for _ in range(2000):
env.render()
a = [0, 0, 0, 1, 0]
s, r, d, info = env.step(a)
print('now pos = ', env.pos)
class FetchDiscreteCamSiamenseEnv:
def __init__(self,
dis_tolerance=0.001,
step_ds=0.005,
gray_img=True,
hsv_color=False,
is_render=False):
self.env = FetchDiscreteEnv(dis_tolerance=0.001,
step_ds=0.005,
is_render=is_render)
self.gray_img = gray_img
self.target_pic = None
self.hsv_color = hsv_color
self.is_render = is_render
def img_preprocess(self, img):
# not support gray image
if self.hsv_color:
resize_img = cv2.resize(img, (IMG_W_H, IMG_W_H),
interpolation=cv2.INTER_AREA)
resize_img = resize_img / 255.0
# print('resize_img = ', resize_img)
hsv_img = rgb_to_hsv(resize_img)
return hsv_img
else:
resize_img = cv2.resize(img, (IMG_W_H, IMG_W_H),
interpolation=cv2.INTER_AREA)
return resize_img
def step(self, action):
# print('i action = ', action)
a_one_hot = np.zeros(5)
a_one_hot[action] = 1
s_old, r, d, _ = self.env.step(a_one_hot)
# no use, but you need preserve it; otherwise, you will get error image
rgb_external = self.env.sim.render(width=256,
height=256,
camera_name="external_camera_0",
depth=False,
mode='offscreen',
device_id=-1)
rgb_gripper = self.env.sim.render(width=256,
height=256,
camera_name="gripper_camera_rgb",
depth=False,
mode='offscreen',
device_id=-1)
if self.is_render:
self.render_gripper_img(rgb_gripper)
# rgb_img = cv2.cvtColor(rgb_gripper, cv2.COLOR_BGR2RGB)
# cv2.imshow('Gripper Image',rgb_img)
# cv2.waitKey(50)
s = [self.img_preprocess(rgb_gripper), self.target_pic]
return s, r, d, None
@property
def pos(self):
return self.env.pos
@property
def obj_pos(self):
return self.env.obj_pos
def get_obj_pos(self, obj_id):
return self.env.get_obj_pos(obj_id)
@property
def gripper_state(self):
return self.env.gripper_state
def take_only_obj0_pic(self):
try:
self.env.rand_obj0_hide_obj1_obj2()
self.env.render()
# time.sleep(2)
rgb_external = self.env.sim.render(width=256,
height=256,
camera_name="external_camera_0",
depth=False,
mode='offscreen',
device_id=-1)
rgb_gripper = self.env.sim.render(width=256,
height=256,
camera_name="gripper_camera_rgb",
depth=False,
mode='offscreen',
device_id=-1)
if self.is_render:
self.render_target_img(rgb_gripper)
self.render_gripper_img(rgb_gripper)
time.sleep(2)
# time.sleep(2)
# resize_img = cv2.resize(rgb_gripper, (IMG_W_H, IMG_W_H), interpolation=cv2.INTER_AREA)
# self.target_pic = resize_img.copy()
self.target_pic = self.img_preprocess(rgb_gripper)
self.env.recover_obj0_obj1_obj2_pos()
self.env.render()
# time.sleep(2)
except Exception as e:
print(' Exception e -> ', e)
pass
# print(' Exception e -> ', e )
def reset(self):
if self.hsv_color:
self.env.rand_objs_hsv()
else:
self.env.rand_objs_color()
self.env.reset()
self.take_only_obj0_pic()
self.env.render()
rgb_external = self.env.sim.render(width=256,
height=256,
camera_name="external_camera_0",
depth=False,
mode='offscreen',
device_id=-1)
rgb_gripper = self.env.sim.render(width=256,
height=256,
camera_name="gripper_camera_rgb",
depth=False,
mode='offscreen',
device_id=-1)
if self.is_render:
self.render_gripper_img(rgb_gripper)
time.sleep(1)
s = [self.img_preprocess(rgb_gripper), self.target_pic]
return s
def render(self):
self.env.render()
def render_target_img(self, img):
# if self.is_render:
rgb_img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
cv2.imshow('Target Image', rgb_img)
cv2.waitKey(50)
def render_gripper_img(self, gripper_img):
# if self.is_render:
rgb_img = cv2.cvtColor(gripper_img, cv2.COLOR_BGR2RGB)
cv2.imshow('Gripper Image', rgb_img)
cv2.waitKey(50)
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))
class FetchDiscreteCamEnv:
def __init__(self,
dis_tolerance=0.001,
step_ds=0.005,
gray_img=True,
use_tray=True,
is_render=False,
only_show_obj0=False):
self.env = FetchDiscreteEnv(dis_tolerance=0.001,
step_ds=0.005,
use_tray=use_tray,
is_render=is_render)
self.gray_img = gray_img
self.is_render = is_render
self.only_show_obj0 = only_show_obj0
def state_preprocess(self, img):
resize_img = cv2.resize(img, (IMG_W_H, IMG_W_H),
interpolation=cv2.INTER_AREA)
gray_img = cv2.cvtColor(resize_img, cv2.COLOR_RGB2GRAY)
return np.reshape(gray_img, (IMG_W_H, IMG_W_H, 1))
def step(self, action):
# print('i action = ', action)
a_one_hot = np.zeros(6)
a_one_hot[action] = 1
s, r, d, _ = self.env.step(a_one_hot)
# no use, but you need preserve it; otherwise, you will get error image
rgb_external = self.env.sim.render(width=256,
height=256,
camera_name="external_camera_0",
depth=False,
mode='offscreen',
device_id=-1)
rgb_gripper = self.env.sim.render(width=256,
height=256,
camera_name="gripper_camera_rgb",
depth=False,
mode='offscreen',
device_id=-1)
rgb_gripper = cv2.cvtColor(rgb_gripper, cv2.COLOR_BGR2RGB)
if self.is_render:
if self.gray_img:
# resize_img = cv2.resize(rgb_gripper, (256, 256), interpolation=cv2.INTER_AREA)
gray_img = cv2.cvtColor(rgb_gripper, cv2.COLOR_RGB2GRAY)
cv2.imshow('Gripper Image', gray_img)
cv2.waitKey(50)
else:
self.render_gripper_img(rgb_gripper)
# s = self.state_preprocess(rgb_gripper)
if self.gray_img:
s = self.state_preprocess(rgb_gripper)
return s, r, d, None
else:
resize_img = cv2.resize(rgb_gripper, (IMG_W_H, IMG_W_H),
interpolation=cv2.INTER_AREA)
return resize_img, r, d, None
# return s, r, d, None
@property
def pos(self):
return self.env.pos
@property
def obj_pos(self):
return self.env.obj_pos
@property
def red_tray_pos(self):
return self.env.red_tray_pos
@property
def gripper_state(self):
return self.env.gripper_state
@property
def is_gripper_close(self):
return self.env.is_gripper_close
def reset(self):
# self.env.rand_objs_color(exclude_obj0 = True)
# self.env.rand_red_or_not(obj_name='object0', use_red_color=True)
self.env.rand_red_or_not(obj_name='object1', use_red_color=False)
self.env.rand_red_or_not(obj_name='object2', use_red_color=False)
self.env.reset()
if self.only_show_obj0:
self.env.hide_obj1_obj2()
self.env.render()
rgb_external = self.env.sim.render(width=256,
height=256,
camera_name="external_camera_0",
depth=False,
mode='offscreen',
device_id=-1)
rgb_gripper = self.env.sim.render(width=256,
height=256,
camera_name="gripper_camera_rgb",
depth=False,
mode='offscreen',
device_id=-1)
if self.gray_img:
s = self.state_preprocess(rgb_gripper)
return s
else:
resize_img = cv2.resize(rgb_gripper, (IMG_W_H, IMG_W_H),
interpolation=cv2.INTER_AREA)
return resize_img
def render(self):
self.env.render()
def render_gripper_img(self, gripper_img):
# if self.is_render:
# rgb_img = cv2.cvtColor(gripper_img, cv2.COLOR_BGR2RGB)
cv2.imshow('Gripper Image', gripper_img)
cv2.waitKey(50)