def get_obs(current_pose): pos, orn = func_pose2posAndorn(current_pose, mapper_scene2z[scene_name]) env.robot.reset_new_pose(pos, orn) obs, _, _, _ = env.step(4) obs_rgb = obs['rgb_filled'] obs_depth = obs['depth'] return obs_rgb.copy(), obs_depth.copy()
## create diff_theta_list from -75 to +75 gap 15 degree diff_theta_list = [] ## -45, -30, -15, 0, 15, 30, 45 for i in range(7): theta = -3/12.0*math.pi + i * math.pi/12 diff_theta_list.append(theta) source_node_idx = random.randint(0, num_nodes-1) x0 = path_finder.nodes_x[source_node_idx] y0 = path_finder.nodes_y[source_node_idx] theta0 = pose_theta_list[random.randint(0, 23)] left_pose = [x0, y0, theta0] file_addr = '/home/reza/Datasets/GibsonEnv/my_code/visual_servoing/sample_image_pairs' current_pose = left_pose pos, orn = func_pose2posAndorn(current_pose, mapper_scene2z[scene_name]) env.robot.reset_new_pose(pos, orn) obs, _, _, _ = env.step(4) obs_rgb = obs['rgb_filled'] cv2.imwrite('{}/left_img.png'.format(file_addr), obs_rgb[:,:,::-1]) right_pose_list = [] for i in range(len(theta_list)): for j in range(len(dist_list)): #temp_theta = plus_theta_fn(theta0, theta_list[random.randint(0, 6)]) #temp_dist = dist_list[random.randint(0, 6)] location_theta = plus_theta_fn(theta0, theta_list[i]) location_dist = dist_list[j] x1 = x0 + location_dist * math.cos(location_theta) y1 = y0 + location_dist * math.sin(location_theta)