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 move_to_initial_config(ros_world, open_grippers={}, timeout=5.0, blocking=True): for arm, is_open in open_grippers.items(): if is_open: ros_world.controller.open_gripper(get_arm_prefix(arm), blocking=False) else: ros_world.controller.close_gripper(get_arm_prefix(arm), blocking=False) ros_world.controller.command_torso(TORSO_POSITION, timeout=timeout, blocking=False) ros_world.controller.command_head(angles=HEAD_CONF, timeout=timeout, blocking=False) ros_world.controller.command_arm('l', LEFT_ARM_CONF, timeout=timeout, blocking=False) ros_world.controller.command_arm('r', rightarm_from_leftarm(LEFT_ARM_CONF), timeout=timeout, blocking=blocking)
def main(): connect(use_gui=True) with HideOutput(): pr2 = load_model( "models/drake/pr2_description/urdf/pr2_simplified.urdf") set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['left_arm']), REST_LEFT_ARM) set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['right_arm']), rightarm_from_leftarm(REST_LEFT_ARM)) set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['torso']), [0.2]) dump_body(pr2) block = load_model(BLOCK_URDF, fixed_base=False) set_point(block, [2, 0.5, 1]) target_point = point_from_pose(get_pose(block)) # head_link = link_from_name(pr2, HEAD_LINK) head_joints = joints_from_names(pr2, PR2_GROUPS['head']) head_link = head_joints[-1] #max_detect_distance = 2.5 max_register_distance = 1.0 distance_range = (max_register_distance / 2, max_register_distance) base_generator = visible_base_generator(pr2, target_point, distance_range) base_joints = joints_from_names(pr2, PR2_GROUPS['base']) for i in range(5): base_conf = next(base_generator) set_joint_positions(pr2, base_joints, base_conf) p.addUserDebugLine(point_from_pose(get_link_pose(pr2, head_link)), target_point, lineColorRGB=(1, 0, 0)) # addUserDebugText p.addUserDebugLine(point_from_pose( get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))), target_point, lineColorRGB=(0, 0, 1)) # addUserDebugText # head_conf = sub_inverse_kinematics(pr2, head_joints[0], HEAD_LINK, ) head_conf = inverse_visibility(pr2, target_point) set_joint_positions(pr2, head_joints, head_conf) print(get_detections(pr2)) # TODO: does this detect the robot sometimes? detect_mesh, z = get_detection_cone(pr2, block) detect_cone = create_mesh(detect_mesh, color=(0, 1, 0, 0.5)) set_pose(detect_cone, get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))) view_cone = get_viewcone(depth=2.5, color=(1, 0, 0, 0.25)) set_pose(view_cone, get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))) wait_for_user() remove_body(detect_cone) remove_body(view_cone) disconnect()
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()
def main(): # TODO: update this example connect(use_gui=True) with HideOutput(): pr2 = load_model(DRAKE_PR2_URDF) set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['left_arm']), REST_LEFT_ARM) set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['right_arm']), rightarm_from_leftarm(REST_LEFT_ARM)) set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['torso']), [0.2]) dump_body(pr2) block = load_model(BLOCK_URDF, fixed_base=False) set_point(block, [2, 0.5, 1]) target_point = point_from_pose(get_pose(block)) draw_point(target_point) head_joints = joints_from_names(pr2, PR2_GROUPS['head']) #head_link = child_link_from_joint(head_joints[-1]) #head_name = get_link_name(pr2, head_link) head_name = 'high_def_optical_frame' # HEAD_LINK_NAME | high_def_optical_frame | high_def_frame head_link = link_from_name(pr2, head_name) #max_detect_distance = 2.5 max_register_distance = 1.0 distance_range = (max_register_distance / 2, max_register_distance) base_generator = visible_base_generator(pr2, target_point, distance_range) base_joints = joints_from_names(pr2, PR2_GROUPS['base']) for i in range(5): base_conf = next(base_generator) set_joint_positions(pr2, base_joints, base_conf) handles = [ add_line(point_from_pose(get_link_pose(pr2, head_link)), target_point, color=RED), add_line(point_from_pose( get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))), target_point, color=BLUE), ] # head_conf = sub_inverse_kinematics(pr2, head_joints[0], HEAD_LINK, ) head_conf = inverse_visibility(pr2, target_point, head_name=head_name, head_joints=head_joints) assert head_conf is not None set_joint_positions(pr2, head_joints, head_conf) print(get_detections(pr2)) # TODO: does this detect the robot sometimes? detect_mesh, z = get_detection_cone(pr2, block) detect_cone = create_mesh(detect_mesh, color=(0, 1, 0, 0.5)) set_pose(detect_cone, get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))) view_cone = get_viewcone(depth=2.5, color=(1, 0, 0, 0.25)) set_pose(view_cone, get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))) wait_if_gui() remove_body(detect_cone) remove_body(view_cone) disconnect()