cnt[0] += 1
for i in range(M2.length, 1500):
idx = np.random.choice(len(empty_color))
pixel_index = [1, np.random.choice(224), np.random.choice(224)]
t2 = Transition(empty_color[idx], empty_depth[idx], pixel_index, -R,
empty_color[idx], empty_depth[idx], True)
M2.add(t2)
cnt[1] += 1
for i in range(M3.length, 1500):
idx = np.random.choice(len(empty_color))
pixel_index = [
np.random.choice(range(2, 6)),
np.random.choice(224),
np.random.choice(224)
]
t3 = Transition(empty_color[idx], empty_depth[idx], pixel_index, -R,
empty_color[idx], empty_depth[idx], True)
M3.add(t3)
cnt[2] += 1
print "Collect %d final state" % len(empty_color)
print "Suction 1 memory synthesized %d data" % cnt[0]
print "Suction 2 memory synthesized %d data" % cnt[1]
print "Gripper memory synthesized %d data" % cnt[2]
print M1.length, M2.length, M3.length
M1.save_memory("", "suction_1_augment.pkl")
M2.save_memory("", "suction_2_augment.pkl")
M3.save_memory("", "gripper_augment.pkl")
success_cnt += 1
print success_cnt
pixel_idx_list.append(pixel_index)
else:
pixel_index = [0, pixel_x, pixel_y]
transition = Transition(color_name, depth_name, pixel_index, current_reward, next_color_name, next_depth_name, is_empty)
suction_1_memory.add(transition)
success_cnt = 0
for i in range(suction_1_memory.length):
if suction_1_memory.tree.data[i].reward == reward:
success_cnt += 1
print success_cnt
pixel_idx_list.append(pixel_index)
iteration += 1
if __name__ == "__main__":
rospy.init_node("manual_collect_data")
go_home()
vacuum_pump_control(SetBoolRequest(False))
sub = rospy.Subscriber("/clicked_point", PointStamped, cb)
while not rospy.is_shutdown():
rospy.spin()
gripper_memory.save_memory(root_path, "gripper_memory.pkl")
suction_2_memory.save_memory(root_path, "suction_2_memory.pkl")
suction_1_memory.save_memory(root_path, "suction_1_memory.pkl")
np.savetxt("action_target.csv", pixel_idx_list, delimiter=",")
np.savetxt("empty_result.csv", is_empty_list, delimiter=",")
np.savetxt("action_result.csv", is_success_list, delimiter=",")
