def pick_problem(arm='left', grasp_type='side'): other_arm = get_other_arm(arm) initial_conf = get_carry_conf(arm, grasp_type) pr2 = create_pr2() set_base_values(pr2, (0, -1.2, np.pi / 2)) set_arm_conf(pr2, arm, initial_conf) open_arm(pr2, arm) set_arm_conf(pr2, other_arm, arm_conf(other_arm, REST_LEFT_ARM)) close_arm(pr2, other_arm) plane = create_floor() table = load_pybullet(TABLE_URDF) # table = create_table(height=0.8) # table = load_pybullet("table_square/table_square.urdf") box = create_box(.07, .05, .25) set_point(box, (-0.25, -0.3, TABLE_MAX_Z + .25 / 2)) # set_point(box, (0.2, -0.2, 0.8 + .25/2 + 0.1)) return Problem(robot=pr2, movable=[box], arms=[arm], grasp_types=[grasp_type], surfaces=[table], goal_conf=get_pose(pr2), goal_holding=[(arm, box)])
def main(use_pr2_drake=True): connect(use_gui=True) add_data_path() plane = p.loadURDF("plane.urdf") table_path = "models/table_collision/table.urdf" table = load_pybullet(table_path, fixed_base=True) set_quat(table, quat_from_euler(Euler(yaw=PI / 2))) # table/table.urdf, table_square/table_square.urdf, cube.urdf, block.urdf, door.urdf obstacles = [plane, table] pr2_urdf = DRAKE_PR2_URDF if use_pr2_drake else PR2_URDF with HideOutput(): pr2 = load_model(pr2_urdf, fixed_base=True) # TODO: suppress warnings? dump_body(pr2) z = base_aligned_z(pr2) print(z) #z = stable_z_on_aabb(pr2, AABB(np.zeros(3), np.zeros(3))) print(z) set_point(pr2, Point(z=z)) print(get_aabb(pr2)) wait_if_gui() base_start = (-2, -2, 0) base_goal = (2, 2, 0) arm_start = SIDE_HOLDING_LEFT_ARM #arm_start = TOP_HOLDING_LEFT_ARM #arm_start = REST_LEFT_ARM arm_goal = TOP_HOLDING_LEFT_ARM #arm_goal = SIDE_HOLDING_LEFT_ARM left_joints = joints_from_names(pr2, PR2_GROUPS['left_arm']) right_joints = joints_from_names(pr2, PR2_GROUPS['right_arm']) torso_joints = joints_from_names(pr2, PR2_GROUPS['torso']) set_joint_positions(pr2, left_joints, arm_start) set_joint_positions(pr2, right_joints, rightarm_from_leftarm(REST_LEFT_ARM)) set_joint_positions(pr2, torso_joints, [0.2]) open_arm(pr2, 'left') # test_ikfast(pr2) add_line(base_start, base_goal, color=RED) print(base_start, base_goal) if use_pr2_drake: test_drake_base_motion(pr2, base_start, base_goal, obstacles=obstacles) else: test_base_motion(pr2, base_start, base_goal, obstacles=obstacles) test_arm_motion(pr2, left_joints, arm_goal) # test_arm_control(pr2, left_joints, arm_start) wait_if_gui('Finish?') disconnect()
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 main(use_pr2_drake=False): connect(use_gui=True) add_data_path() plane = p.loadURDF("plane.urdf") #table_path = "table/table.urdf" # table_path = "models/table_collision/table.urdf" # table = p.loadURDF(table_path, 0, 0, 0, 0, 0, 0.707107, 0.707107) # table_square/table_square.urdf, cube.urdf, block.urdf, door.urdf pr2_urdf = DRAKE_PR2_URDF if use_pr2_drake else PR2_URDF with HideOutput(): pr2 = load_model(pr2_urdf, fixed_base=True) # TODO: suppress warnings? dump_body(pr2) base_start = (-2, -2, 0) base_goal = (2, 2, 0) arm_start = SIDE_HOLDING_LEFT_ARM #arm_start = TOP_HOLDING_LEFT_ARM #arm_start = REST_LEFT_ARM arm_goal = TOP_HOLDING_LEFT_ARM #arm_goal = SIDE_HOLDING_LEFT_ARM left_joints = joints_from_names(pr2, PR2_GROUPS['left_arm']) right_joints = joints_from_names(pr2, PR2_GROUPS['right_arm']) torso_joints = joints_from_names(pr2, PR2_GROUPS['torso']) set_joint_positions(pr2, left_joints, arm_start) set_joint_positions(pr2, right_joints, rightarm_from_leftarm(REST_LEFT_ARM)) set_joint_positions(pr2, torso_joints, [0.2]) open_arm(pr2, 'left') # test_ikfast(pr2) p.addUserDebugLine(base_start, base_goal, lineColorRGB=(1, 0, 0)) # addUserDebugText print(base_start, base_goal) if use_pr2_drake: test_drake_base_motion(pr2, base_start, base_goal) else: test_base_motion(pr2, base_start, base_goal) test_arm_motion(pr2, left_joints, arm_goal) # test_arm_control(pr2, left_joints, arm_start) user_input('Finish?') disconnect()