def main(): parser = argparse.ArgumentParser() # Automatically includes help parser.add_argument('-arm', required=True) parser.add_argument('-grasp', required=True) parser.add_argument('-viewer', action='store_true', help='enable viewer.') args = parser.parse_args() arm = args.arm other_arm = get_other_arm(arm) grasp_type = args.grasp connect(use_gui=args.viewer) add_data_path() with HideOutput(): robot = load_pybullet(DRAKE_PR2_URDF) set_group_conf(robot, 'torso', [0.2]) set_arm_conf(robot, arm, get_carry_conf(arm, grasp_type)) set_arm_conf(robot, other_arm, arm_conf(other_arm, REST_LEFT_ARM)) #plane = p.loadURDF("plane.urdf") #table = p.loadURDF("table/table.urdf", 0, 0, 0, 0, 0, 0.707107, 0.707107) table = create_table() box = create_box(.07, .07, .14) #create_inverse_reachability(robot, box, table, arm=arm, grasp_type=grasp_type) create_inverse_reachability2(robot, box, table, arm=arm, grasp_type=grasp_type) disconnect()
def create_inverse_reachability(robot, body, table, arm, grasp_type, max_attempts=500, num_samples=500): tool_link = get_gripper_link(robot, arm) robot_saver = BodySaver(robot) gripper_from_base_list = [] grasps = GET_GRASPS[grasp_type](body) start_time = time.time() while len(gripper_from_base_list) < num_samples: box_pose = sample_placement(body, table) set_pose(body, box_pose) grasp_pose = random.choice(grasps) gripper_pose = multiply(box_pose, invert(grasp_pose)) for attempt in range(max_attempts): robot_saver.restore() base_conf = next(uniform_pose_generator(robot, gripper_pose)) #, reachable_range=(0., 1.))) #set_base_values(robot, base_conf) set_group_conf(robot, 'base', base_conf) if pairwise_collision(robot, table): continue grasp_conf = pr2_inverse_kinematics(robot, arm, gripper_pose) #, nearby_conf=USE_CURRENT) #conf = inverse_kinematics(robot, link, gripper_pose) if (grasp_conf is None) or pairwise_collision(robot, table): continue gripper_from_base = multiply(invert(get_link_pose(robot, tool_link)), get_base_pose(robot)) #wait_if_gui() gripper_from_base_list.append(gripper_from_base) print('{} / {} | {} attempts | [{:.3f}]'.format( len(gripper_from_base_list), num_samples, attempt, elapsed_time(start_time))) wait_if_gui() break else: print('Failed to find a kinematic solution after {} attempts'.format(max_attempts)) return save_inverse_reachability(robot, arm, grasp_type, tool_link, gripper_from_base_list)
def parse_robot(robot_json): pose = parse_pose(robot_json) if robot_json['name'] == 'pr2': with HideOutput(True): robot_id = load_model(DRAKE_PR2_URDF, fixed_base=True) set_group_conf(robot_id, 'base', base_values_from_pose(pose)) else: # TODO: set the z? #set_pose(robot_id, pose) raise NotImplementedError(robot_json['name']) for joint, values in robot_json['conf'].items(): [value] = values if has_joint(robot_id, joint): set_joint_position(robot_id, joint_from_name(robot_id, joint), value) else: print('Robot {} lacks joint {}'.format(robot_json['name'], joint)) if robot_json['name'] == 'pr2': set_group_conf(robot_id, 'torso', [0.2]) set_group_conf(robot_id, 'left_arm', REST_LEFT_ARM) set_group_conf(robot_id, 'right_arm', rightarm_from_leftarm(REST_LEFT_ARM)) return robot_id
def update_world(world, target_body, camera_point=VIEWER_POINT): pr2 = world.perception.pr2 with ClientSaver(world.perception.client): open_arm(pr2, LEFT_ARM) open_arm(pr2, RIGHT_ARM) set_group_conf(pr2, 'torso', [TORSO_POSITION]) set_group_conf(pr2, arm_from_arm('left'), WIDE_LEFT_ARM) set_group_conf(pr2, arm_from_arm('right'), rightarm_from_leftarm(WIDE_LEFT_ARM)) target_point = get_point(target_body) head_conf = inverse_visibility(pr2, target_point, head_name=CAMERA_OPTICAL_FRAME) # Must come after torso set_group_conf(pr2, 'head', head_conf) set_camera_pose(camera_point, target_point=target_point)
def parse_robot(robot): name = robot.find('name').text urdf = robot.find('urdf').text fixed_base = not parse_boolean(robot.find('movebase')) print(name, urdf, fixed_base) pose = parse_pose(robot.find('basepose')) torso = parse_array(robot.find('torso')) left_arm = parse_array(robot.find('left_arm')) right_arm = parse_array(robot.find('right_arm')) assert (name == 'pr2') with HideOutput(): robot_id = load_model(DRAKE_PR2_URDF, fixed_base=True) set_group_conf(robot_id, 'base', base_values_from_pose(pose)) set_group_conf(robot_id, 'torso', torso) set_group_conf(robot_id, 'left_arm', left_arm) set_group_conf(robot_id, 'right_arm', right_arm) #set_point(robot_id, Point(z=point_from_pose(pose)[2])) # TODO: base pose isn't right # print(robot.tag) # print(robot.attrib) # print(list(robot.iter('basepose'))) return robot_id