示例#1
0
def test_pour(visualize):
    arms = [LEFT_ARM]
    #cup_name, bowl_name = 'bluecup', 'bluebowl'
    #cup_name, bowl_name = 'cup_7', 'cup_8'
    #cup_name, bowl_name = 'cup_7-1', 'cup_7-2'
    #cup_name, bowl_name = get_name('bluecup', 1), get_name('bluecup', 2)
    cup_name = create_name('bluecup', 1) # bluecup | purple_cup | orange_cup | blue_cup | olive1_cup | blue3D_cup
    bowl_name = create_name('bowl', 1) # bowl | steel_bowl

    world, table_body = create_world([bowl_name, cup_name, bowl_name], visualize=visualize)
    with ClientSaver(world.perception.client):
        cup_z = stable_z(world.perception.sim_bodies[cup_name], table_body) + Z_EPSILON
        bowl_z = stable_z(world.perception.sim_bodies[bowl_name], table_body) + Z_EPSILON
    world.perception.set_pose(cup_name, Point(0.6, 0.2, cup_z), unit_quat())
    world.perception.set_pose(bowl_name, Point(0.6, 0.0, bowl_z), unit_quat())
    update_world(world, table_body)

    init = [
        ('Contains', cup_name, COFFEE),
        #('Graspable', bowl_name),
    ]
    goal = [
        ('Contains', bowl_name, COFFEE),
    ]
    task = Task(init=init, goal=goal, arms=arms,
                empty_arms=True, reset_arms=True, reset_items=True)

    return world, task
示例#2
0
def mirror_robot(robot1, node_points):
    # TODO: place robots side by side or diagonal across
    set_extrusion_camera(node_points, theta=-np.pi / 3)
    #draw_pose(Pose())
    centroid = np.average(node_points, axis=0)
    centroid_pose = Pose(point=centroid)
    #draw_pose(Pose(point=centroid))

    # print(centroid)
    scale = 0.  # 0.15
    vector = get_point(robot1) - centroid
    set_pose(robot1, Pose(point=Point(*+scale * vector[:2])))
    # Inner product of end-effector z with base->centroid or perpendicular to this line
    # Partition by sides

    robot2 = load_robot()
    set_pose(
        robot2,
        Pose(point=Point(*-(2 + scale) * vector[:2]), euler=Euler(yaw=np.pi)))

    # robots = [robot1]
    robots = [robot1, robot2]
    for robot in robots:
        set_configuration(robot, DUAL_CONF)
        # joint1 = get_movable_joints(robot)[0]
        # set_joint_position(robot, joint1, np.pi / 8)
        draw_pose(get_pose(robot), length=0.25)
    return robots
示例#3
0
 def fix_pose(self, name, pose=None, fraction=0.5):
     body = self.get_body(name)
     if pose is None:
         pose = get_pose(body)
     else:
         set_pose(body, pose)
     # TODO: can also drop in simulation
     x, y, z = point_from_pose(pose)
     roll, pitch, yaw = euler_from_quat(quat_from_pose(pose))
     quat = quat_from_euler(Euler(yaw=yaw))
     set_quat(body, quat)
     surface_name = self.get_supporting(name)
     if surface_name is None:
         return None, None
     if fraction == 0:
         new_pose = (Point(x, y, z), quat)
         return new_pose, surface_name
     surface_aabb = compute_surface_aabb(self, surface_name)
     new_z = (1 - fraction) * z + fraction * stable_z_on_aabb(
         body, surface_aabb)
     point = Point(x, y, new_z)
     set_point(body, point)
     print('{} error: roll={:.3f}, pitch={:.3f}, z-delta: {:.3f}'.format(
         name, roll, pitch, new_z - z))
     new_pose = (point, quat)
     return new_pose, surface_name
示例#4
0
文件: push.py 项目: lyltc1/LTAMP
def sample_push_contact(world, feature, parameter, under=False):
    robot = world.robot
    arm = feature['arm_name']
    body = world.get_body(feature['block_name'])
    push_yaw = feature['push_yaw']

    center, (width, _, height) = approximate_as_prism(body, body_pose=Pose(euler=Euler(yaw=push_yaw)))
    max_backoff = width + 0.1 # TODO: add gripper bounding box
    tool_link = link_from_name(robot, TOOL_FRAMES[arm])
    tool_pose = get_link_pose(robot, tool_link)
    gripper_link = link_from_name(robot, GRIPPER_LINKS[arm])
    collision_links = get_link_subtree(robot, gripper_link)

    urdf_from_center = Pose(point=center)
    reverse_z = Pose(euler=Euler(pitch=math.pi))
    rotate_theta = Pose(euler=Euler(yaw=push_yaw))
    #translate_z = Pose(point=Point(z=-feature['block_height']/2. + parameter['gripper_z'])) # Relative to base
    translate_z = Pose(point=Point(z=parameter['gripper_z'])) # Relative to center
    tilt_gripper = Pose(euler=Euler(pitch=parameter['gripper_tilt']))

    grasps = []
    for i in range(1 + under):
        flip_gripper = Pose(euler=Euler(yaw=i * math.pi))
        for x in np.arange(0, max_backoff, step=0.01):
            translate_x = Pose(point=Point(x=-x))
            grasp_pose = multiply(flip_gripper, tilt_gripper, translate_x, translate_z, rotate_theta,
                                  reverse_z, invert(urdf_from_center))
            set_pose(body, multiply(tool_pose, TOOL_POSE, grasp_pose))
            if not link_pairs_collision(robot, collision_links, body, collision_buffer=0.):
                grasps.append(grasp_pose)
                break
    return grasps
示例#5
0
def test_push_pour(visualize):
    arms = ARMS
    cup_name = create_name('bluecup', 1)
    bowl_name = create_name('bowl', 1)

    items = [bowl_name, cup_name, bowl_name]
    world, table_body = create_world(items, visualize=visualize)
    set_point(table_body, np.array(TABLE_POSE[0]) + np.array([0, -0.1, 0]))

    with ClientSaver(world.perception.client):
        cup_z = stable_z(world.perception.sim_bodies[cup_name], table_body) + Z_EPSILON
        bowl_z = stable_z(world.perception.sim_bodies[bowl_name], table_body) + Z_EPSILON
    world.perception.set_pose(cup_name, Point(0.75, 0.4, cup_z), unit_quat())
    world.perception.set_pose(bowl_name, Point(0.5, -0.6, bowl_z), unit_quat())
    update_world(world, table_body)

    # TODO: can prevent the right arm from being able to pick
    init = [
        ('Contains', cup_name, COFFEE),
        #('Graspable', bowl_name),
        ('CanPush', bowl_name, LEFT_ARM), # TODO: intersection of these regions
    ]
    goal = [
        ('Contains', bowl_name, COFFEE),
        ('InRegion', bowl_name, LEFT_ARM),
    ]
    task = Task(init=init, goal=goal, arms=arms, pushable=[bowl_name])
    # Most of the time places the cup to exchange arms

    return world, task
示例#6
0
def test_block(visualize):
    #arms = [LEFT_ARM]
    arms = ARMS
    block_name = create_name('greenblock', 1)
    tray_name = create_name('tray', 1)

    world, table_body = create_world([tray_name, block_name], visualize=visualize)
    with ClientSaver(world.perception.client):
        block_z = stable_z(world.perception.sim_bodies[block_name], table_body) + Z_EPSILON
        tray_z = stable_z(world.perception.sim_bodies[tray_name], table_body) + Z_EPSILON
        #attach_viewcone(world.perception.pr2, depth=1, head_name='high_def_frame')
        #dump_body(world.perception.pr2)
    #block_y = 0.0
    block_y = -0.4
    world.perception.set_pose(block_name, Point(0.6, block_y, block_z), unit_quat())
    world.perception.set_pose(tray_name, Point(0.6, 0.4, tray_z), unit_quat())
    update_world(world, table_body)

    init = [
        ('Stackable', block_name, tray_name, TOP),
    ]
    goal = [
        ('On', block_name, tray_name, TOP),
    ]
    #goal = [
    #    ('Grasped', arms[0], block_name),
    #]
    task = Task(init=init, goal=goal, arms=arms, empty_arms=True)

    return world, task
示例#7
0
def pour_path_from_parameter(world, bowl_name, cup_name):
    bowl_body = world.get_body(bowl_name)
    bowl_center, (bowl_d, bowl_h) = approximate_as_cylinder(bowl_body)
    cup_body = world.get_body(cup_name)
    cup_center, (cup_d, _, cup_h) = approximate_as_prism(cup_body)

    #####

    obj_type = type_from_name(cup_name)
    if obj_type in [MUSTARD]:
        initial_pitch = final_pitch = -np.pi
        radius = 0
    else:
        initial_pitch = 0  # different if mustard
        final_pitch = -3 * np.pi / 4
        radius = bowl_d / 2

    #axis_in_cup_center_x = -0.05
    axis_in_cup_center_x = 0  # meters
    #axis_in_cup_center_z = -cup_h/2.
    axis_in_cup_center_z = 0.  # meters
    #axis_in_cup_center_z = +cup_h/2.

    # tl := top left | tr := top right
    cup_tl_in_center = np.array([-cup_d / 2, 0, cup_h / 2])
    cup_tl_in_axis = cup_tl_in_center - Point(z=axis_in_cup_center_z)
    cup_tl_angle = np.math.atan2(cup_tl_in_axis[2], cup_tl_in_axis[0])
    cup_tl_pour_pitch = final_pitch - cup_tl_angle

    cup_radius2d = np.linalg.norm([cup_tl_in_axis])
    pivot_in_bowl_tr = Point(
        x=-(cup_radius2d * np.math.cos(cup_tl_pour_pitch) + 0.01),
        z=(cup_radius2d * np.math.sin(cup_tl_pour_pitch) + Z_OFFSET))

    pivot_in_bowl_center = Point(x=radius, z=bowl_h / 2) + pivot_in_bowl_tr
    base_from_pivot = Pose(
        Point(x=axis_in_cup_center_x, z=axis_in_cup_center_z))

    #####

    assert -np.pi <= final_pitch <= initial_pitch
    pitches = [initial_pitch]
    if final_pitch != initial_pitch:
        pitches = list(np.arange(final_pitch, initial_pitch,
                                 np.pi / 16)) + pitches
    cup_path_in_bowl = []
    for pitch in pitches:
        rotate_pivot = Pose(
            euler=Euler(pitch=pitch)
        )  # Can also interpolate directly between start and end quat
        cup_path_in_bowl.append(
            multiply(Pose(point=bowl_center), Pose(pivot_in_bowl_center),
                     rotate_pivot, invert(base_from_pivot),
                     invert(Pose(point=cup_center))))
    return cup_path_in_bowl
示例#8
0
def main():
    parser = argparse.ArgumentParser()  # Automatically includes help
    parser.add_argument('-viewer', action='store_true', help='enable viewer.')
    args = parser.parse_args()

    connect(use_gui=True)

    with LockRenderer():
        draw_pose(unit_pose(), length=1, width=1)
        floor = create_floor()
        set_point(floor, Point(z=-EPSILON))

        table1 = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH/2, height=TABLE_WIDTH/2,
                              top_color=TAN, cylinder=False)
        set_point(table1, Point(y=+0.5))

        table2 = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH/2, height=TABLE_WIDTH/2,
                              top_color=TAN, cylinder=False)
        set_point(table2, Point(y=-0.5))

        tables = [table1, table2]

        #set_euler(table1, Euler(yaw=np.pi/2))
        with HideOutput():
            # data_path = add_data_path()
            # robot_path = os.path.join(data_path, WSG_GRIPPER)
            robot_path = get_model_path(WSG_50_URDF)  # WSG_50_URDF | PANDA_HAND_URDF
            robot = load_pybullet(robot_path, fixed_base=True)
            #dump_body(robot)

        block1 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=RED)
        block_z = stable_z(block1, table1)
        set_point(block1, Point(y=-0.5, z=block_z))

        block2 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=GREEN)
        set_point(block2, Point(x=-0.25, y=-0.5, z=block_z))

        block3 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=BLUE)
        set_point(block3, Point(x=-0.15, y=+0.5, z=block_z))

        blocks = [block1, block2, block3]

        set_camera_pose(camera_point=Point(x=-1, z=block_z+1), target_point=Point(z=block_z))

    block_pose = get_pose(block1)
    open_gripper(robot)
    tool_link = link_from_name(robot, 'tool_link')
    base_from_tool = get_relative_pose(robot, tool_link)
    #draw_pose(unit_pose(), parent=robot, parent_link=tool_link)
    grasps = get_side_grasps(block1, tool_pose=Pose(euler=Euler(yaw=np.pi/2)),
                             top_offset=0.02, grasp_length=0.03, under=False)[1:2]
    for grasp in grasps:
        gripper_pose = multiply(block_pose, invert(grasp))
        set_pose(robot, multiply(gripper_pose, invert(base_from_tool)))
        wait_for_user()

    wait_for_user('Finish?')
    disconnect()
示例#9
0
def get_handle_grasps(world, joint, pull=True, pre_distance=APPROACH_DISTANCE):
    pre_direction = pre_distance * get_unit_vector([0, 0, 1])
    #half_extent = 1.0*FINGER_EXTENT[2] # Collides
    half_extent = 1.05 * FINGER_EXTENT[2]

    grasps = []
    for link in get_link_subtree(world.kitchen, joint):
        if 'handle' in get_link_name(world.kitchen, link):
            # TODO: can adjust the position and orientation on the handle
            for yaw in [0, np.pi]:  # yaw=0 DOESN'T WORK WITH LULA
                handle_grasp = (Point(z=-half_extent),
                                quat_from_euler(
                                    Euler(roll=np.pi, pitch=np.pi / 2,
                                          yaw=yaw)))
                #if not pull:
                #    handle_pose = get_link_pose(world.kitchen, link)
                #    for distance in np.arange(0., 0.05, step=0.001):
                #        pregrasp = multiply(([0, 0, -distance], unit_quat()), handle_grasp)
                #        tool_pose = multiply(handle_pose, invert(pregrasp))
                #        set_tool_pose(world, tool_pose)
                #        # TODO: check collisions
                #        wait_for_user()
                handle_pregrasp = multiply((pre_direction, unit_quat()),
                                           handle_grasp)
                grasps.append(HandleGrasp(link, handle_grasp, handle_pregrasp))
    return grasps
示例#10
0
def main(display='control'):  # control | execute | step
    connect(use_gui=True)
    disable_real_time()
    # KUKA_IIWA_URDF | DRAKE_IIWA_URDF
    robot = load_model(DRAKE_IIWA_URDF, fixed_base=True)
    # floor = load_model('models/short_floor.urdf')
    floor = p.loadURDF("plane.urdf")
    block = load_model(
        "models/drake/objects/block_for_pick_and_place_heavy.urdf", fixed_base=False)
    set_pose(block, Pose(Point(y=0.5, z=stable_z(block, floor))))
    set_default_camera()
    dump_world()

    saved_world = WorldSaver()
    command = plan(robot, block, fixed=[floor], teleport=False)
    if (command is None) or (display is None):
        print('Unable to find a plan!')
        return

    saved_world.restore()
    update_state()
    user_input('{}?'.format(display))
    if display == 'control':
        enable_gravity()
        command.control(real_time=False, dt=0)
    elif display == 'execute':
        command.refine(num_steps=10).execute(time_step=0.005)
    elif display == 'step':
        command.step()
    else:
        raise ValueError(display)

    print('Quit?')
    wait_for_user()
    disconnect()
示例#11
0
def observe_pybullet(world):
    # TODO: randomize robot's pose
    # TODO: probabilities based on whether in viewcone or not
    # TODO: sample from poses on table
    # world_saver = WorldSaver()
    visible_entities = are_visible(world)
    detections = {}
    assert OBS_P_FP == 0
    for name in visible_entities:
        # TODO: false positives
        if random.random() < OBS_P_FN:
            continue
        body = world.get_body(name)
        pose = get_pose(body)
        dx, dy = np.random.multivariate_normal(mean=np.zeros(2),
                                               cov=math.pow(OBS_POS_STD, 2) *
                                               np.eye(2))
        dyaw, = np.random.multivariate_normal(mean=np.zeros(1),
                                              cov=math.pow(OBS_ORI_STD, 2) *
                                              np.eye(1))
        print('{}: dx={:.3f}, dy={:.3f}, dyaw={:.5f}'.format(
            name, dx, dy, dyaw))
        noise_pose = Pose(Point(x=dx, y=dy), Euler(yaw=dyaw))
        observed_pose = multiply(pose, noise_pose)
        #world.get_body_type(name)
        detections.setdefault(name, []).append(
            observed_pose)  # TODO: use type instead
    #world_saver.restore()
    return detections
示例#12
0
def test_retraction(robot, info, tool_link, distance=0.1, **kwargs):
    ik_joints = get_ik_joints(robot, info, tool_link)
    start_pose = get_link_pose(robot, tool_link)
    end_pose = multiply(start_pose, Pose(Point(z=-distance)))
    handles = [
        add_line(point_from_pose(start_pose),
                 point_from_pose(end_pose),
                 color=BLUE)
    ]
    #handles.extend(draw_pose(start_pose))
    #handles.extend(draw_pose(end_pose))
    path = []
    pose_path = list(
        interpolate_poses(start_pose, end_pose, pos_step_size=0.01))
    for i, pose in enumerate(pose_path):
        print('Waypoint: {}/{}'.format(i + 1, len(pose_path)))
        handles.extend(draw_pose(pose))
        conf = next(
            either_inverse_kinematics(robot, info, tool_link, pose, **kwargs),
            None)
        if conf is None:
            print('Failure!')
            path = None
            wait_for_user()
            break
        set_joint_positions(robot, ik_joints, conf)
        path.append(conf)
        wait_for_user()
        # for conf in islice(ikfast_inverse_kinematics(robot, info, tool_link, pose, max_attempts=INF, max_distance=0.5), 1):
        #    set_joint_positions(robot, joints[:len(conf)], conf)
        #    wait_for_user()
    remove_handles(handles)
    return path
示例#13
0
def create_door(width=0.08,
                length=1,
                height=2,
                mass=1,
                handle=True,
                frame=True,
                **kwargs):
    # TODO: hinge, cylinder on end, sliding door, knob
    # TODO: explicitly disable self collisions (happens automatically)
    geometry = get_box_geometry(width, length, height)
    hinge = 0  # -width/2
    com_point = Point(x=hinge, y=-length / 2., z=height / 2.)
    door_collision, door_visual = create_shape(geometry,
                                               pose=Pose(com_point),
                                               **kwargs)
    door_link = LinkInfo(
        mass=mass,
        collision_id=door_collision,
        visual_id=door_visual,
        #point=com_point,
        inertial_point=com_point,  # TODO: be careful about the COM
        parent=0,
        joint_type=p.JOINT_REVOLUTE,
        joint_axis=[0, 0, 1])
    links = [door_link]
    if handle:
        links.append(create_handle(width, length, height))
    if frame:
        links.append(create_frame(width, length, height))
    body = create_multi_body(base_link=LinkInfo(), links=links)
    #draw_circle(center=unit_point(), diameter=width/2., parent=body, parent_link=0)
    #set_joint_limits(body, link=0, lower=-PI, upper=PI)
    return body
示例#14
0
def add_scales(task, ros_world):
    scale_stackings = {}
    holding = {grasp.obj_name for grasp in task.init_holding.values()}
    with ClientSaver(ros_world.client):
        perception = ros_world.perception
        items = sorted(set(perception.get_items()) - holding,
                       key=lambda n: get_point(ros_world.get_body(n))[1],
                       reverse=False)  # Right to left
        for i, item in enumerate(items):
            if not POUR and (get_type(item) != SCOOP_CUP):
                continue
            item_body = ros_world.get_body(item)
            scale = create_name(SCALE_TYPE, i + 1)
            with HideOutput():
                scale_body = load_pybullet(get_body_urdf(get_type(scale)),
                                           fixed_base=True)
            ros_world.perception.sim_bodies[scale] = scale_body
            ros_world.perception.sim_items[scale] = None
            item_z = stable_z(item_body, scale_body)
            scale_pose_item = Pose(
                point=Point(z=-item_z))  # TODO: relies on origin in base
            set_pose(scale_body, multiply(get_pose(item_body),
                                          scale_pose_item))
            roll, pitch, _ = get_euler(scale_body)
            set_euler(scale_body, [roll, pitch, 0])
            scale_stackings[scale] = item
        #wait_for_user()
    return scale_stackings
示例#15
0
def main():
    # TODO: move to pybullet-planning for now
    parser = argparse.ArgumentParser()
    parser.add_argument('-viewer',
                        action='store_true',
                        help='enable the viewer while planning')
    args = parser.parse_args()
    print(args)

    connect(use_gui=True)
    with LockRenderer():
        draw_pose(unit_pose(), length=1)
        floor = create_floor()
        with HideOutput():
            robot = load_pybullet(get_model_path(ROOMBA_URDF),
                                  fixed_base=True,
                                  scale=2.0)
            for link in get_all_links(robot):
                set_color(robot, link=link, color=ORANGE)
            robot_z = stable_z(robot, floor)
            set_point(robot, Point(z=robot_z))
        #set_base_conf(robot, rover_confs[i])

        data_path = add_data_path()
        shelf, = load_model(os.path.join(data_path, KIVA_SHELF_SDF),
                            fixed_base=True)  # TODO: returns a tuple
        dump_body(shelf)
        #draw_aabb(get_aabb(shelf))

    wait_for_user()
    disconnect()
示例#16
0
def main(display='execute'):  # control | execute | step
    connect(use_gui=True)
    disable_real_time()
    draw_global_system()
    with HideOutput():
        robot = load_model(DRAKE_IIWA_URDF)  # KUKA_IIWA_URDF | DRAKE_IIWA_URDF
        floor = load_model('models/short_floor.urdf')
    block = load_model(BLOCK_URDF, fixed_base=False)
    set_pose(block, Pose(Point(y=0.5, z=stable_z(block, floor))))
    set_default_camera(distance=2)
    dump_world()

    saved_world = WorldSaver()
    command = plan(robot, block, fixed=[floor], teleport=False)
    if (command is None) or (display is None):
        print('Unable to find a plan!')
        return

    saved_world.restore()
    update_state()
    wait_if_gui('{}?'.format(display))
    if display == 'control':
        enable_gravity()
        command.control(real_time=False, dt=0)
    elif display == 'execute':
        command.refine(num_steps=10).execute(time_step=0.005)
    elif display == 'step':
        command.step()
    else:
        raise ValueError(display)

    print('Quit?')
    wait_if_gui()
    disconnect()
示例#17
0
def load_plane(z=-1e-3):
    add_data_path()
    plane = load_pybullet('plane.urdf', fixed_base=True)
    #plane = load_model('plane.urdf')
    if z is not None:
        set_point(plane, Point(z=z))
    return plane
示例#18
0
def test_grasps(robot, node_points, elements):
    element = elements[-1]
    [element_body] = create_elements(node_points, [element])

    phi = 0
    link = link_from_name(robot, TOOL_NAME)
    link_pose = get_link_pose(robot, link)
    draw_pose(link_pose)  #, parent=robot, parent_link=link)
    wait_for_user()

    n1, n2 = element
    p1 = node_points[n1]
    p2 = node_points[n2]
    length = np.linalg.norm(p2 - p1)
    # Bottom of cylinder is p1, top is p2
    print(element, p1, p2)
    for phi in np.linspace(0, np.pi, 10, endpoint=True):
        theta = np.pi / 4
        for t in np.linspace(-length / 2, length / 2, 10):
            translation = Pose(
                point=Point(z=-t)
            )  # Want to be moving backwards because +z is out end effector
            direction = Pose(euler=Euler(roll=np.pi / 2 + theta, pitch=phi))
            angle = Pose(euler=Euler(yaw=1.2))
            grasp_pose = multiply(angle, direction, translation)
            element_pose = multiply(link_pose, grasp_pose)
            set_pose(element_body, element_pose)
            wait_for_user()
示例#19
0
文件: scoop.py 项目: lyltc1/LTAMP
def lower_spoon(world, bowl_name, spoon_name, min_spoon_z, max_spoon_z):
    assert min_spoon_z <= max_spoon_z
    bowl_body = world.get_body(bowl_name)
    bowl_urdf_from_center = get_urdf_from_base(
        bowl_body)  # get_urdf_from_center

    spoon_body = world.get_body(spoon_name)
    spoon_quat = lookup_orientation(spoon_name, STIR_QUATS)
    spoon_urdf_from_center = get_urdf_from_base(
        spoon_body, reference_quat=spoon_quat)  # get_urdf_from_center
    # Keeping the orientation consistent for generalization purposes

    # TODO: what happens when the base isn't flat (e.g. bowl)
    bowl_pose = get_pose(bowl_body)
    stir_z = None
    for z in np.arange(max_spoon_z, min_spoon_z, step=-0.005):
        bowl_from_stirrer = multiply(bowl_urdf_from_center, Pose(Point(z=z)),
                                     invert(spoon_urdf_from_center))
        set_pose(spoon_body, multiply(bowl_pose, bowl_from_stirrer))
        #wait_for_user()
        if pairwise_collision(bowl_body, spoon_body):
            # Want to be careful not to make contact with the base
            break
        stir_z = z
    return stir_z
示例#20
0
def pose2d_on_surface(world, entity_name, surface_name, pose2d=UNIT_POSE2D):
    x, y, yaw = pose2d
    body = world.get_body(entity_name)
    surface_aabb = compute_surface_aabb(world, surface_name)
    z = stable_z_on_aabb(body, surface_aabb)
    pose = Pose(Point(x, y, z), Euler(yaw=yaw))
    set_pose(body, pose)
    return pose
示例#21
0
def main():
    parser = argparse.ArgumentParser()
    parser.add_argument('-shape',
                        default='box',
                        choices=['box', 'sphere', 'cylinder', 'capsule'])
    parser.add_argument('-video', action='store_true')
    args = parser.parse_args()
    video = 'video.mp4' if args.video else None

    connect(use_gui=True, mp4=video)
    if video is not None:
        set_renderer(enable=False)

    draw_global_system()
    set_camera_pose(camera_point=Point(+1.5, -1.5, +1.5),
                    target_point=Point(-1.5, +1.5, 0))

    add_data_path()
    plane = load_pybullet('plane.urdf', fixed_base=True)
    #plane = load_model('plane.urdf')
    set_point(plane, Point(z=-1e-3))

    ramp = create_ramp()
    dump_body(ramp)

    obj = create_object(args.shape)
    set_euler(obj, np.random.uniform(-np.math.radians(1), np.math.radians(1),
                                     3))
    set_point(obj, np.random.uniform(-1e-3, +1e-3, 3))
    #set_velocity(obj, linear=Point(x=-1))
    set_position(obj, z=2.)
    #set_position(obj, z=base_aligned_z(obj))
    dump_body(obj)

    #add_pose_constraint(obj, pose=(target_point, target_quat), max_force=200)

    set_renderer(enable=True)
    if video is None:
        wait_if_gui('Begin?')
    simulate(controller=condition_controller(lambda step:
                                             (step != 0) and at_rest(obj)))
    if video is None:
        wait_if_gui('Finish?')
    disconnect()
示例#22
0
文件: pour.py 项目: lyltc1/LTAMP
def sample_pour_parameter(world, feature):
    # TODO: adjust for RELATIVE_POUR
    cup_pour_pitch = -3 * np.pi / 4
    # pour_cup_pitch = -5*np.pi/6
    # pour_cup_pitch = -np.pi

    #axis_in_cup_center_x = -0.05
    axis_in_cup_center_x = 0
    #axis_in_cup_center_z = -feature['cup_height']/2.
    axis_in_cup_center_z = 0.  # This is in meters (not a fraction of the high)
    #axis_in_cup_center_z = feature['cup_height']/2.

    # tl := top left | tr := top right
    cup_tl_in_center = np.array(
        [-feature['cup_diameter'] / 2, 0, feature['cup_height'] / 2])
    cup_tl_in_axis = cup_tl_in_center - Point(x=axis_in_cup_center_x,
                                              z=axis_in_cup_center_z)
    cup_tl_angle = np.math.atan2(cup_tl_in_axis[2], cup_tl_in_axis[0])
    cup_tl_pour_pitch = cup_pour_pitch - cup_tl_angle

    cup_radius2d = np.linalg.norm([cup_tl_in_axis])
    pivot_in_bowl_tr = Point(
        x=-(cup_radius2d * np.math.cos(cup_tl_pour_pitch) + 0.01),
        z=(cup_radius2d * np.math.sin(cup_tl_pour_pitch) + 0.01))

    bowl_tr_in_bowl_center = Point(x=feature['bowl_diameter'] / 2,
                                   z=feature['bowl_height'] / 2)
    pivot_in_bowl_center = bowl_tr_in_bowl_center + pivot_in_bowl_tr

    parameter = {
        'pitch': cup_pour_pitch,
        'axis_in_cup_x': axis_in_cup_center_x,
        'axis_in_cup_z': axis_in_cup_center_z,
        'axis_in_bowl_x': pivot_in_bowl_center[0],
        'axis_in_bowl_z': pivot_in_bowl_center[2],
        #'velocity': None,
        #'bowl_yaw': None,
        #'cup_yaw': None,
        'relative': RELATIVE_POUR,
    }
    if RELATIVE_POUR:
        parameter = scale_parameter(feature, parameter, RELATIVE_POUR_SCALING)
    yield parameter
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()
示例#24
0
def main(display='execute'):  # control | execute | step
    connect(use_gui=True)
    disable_real_time()

    with HideOutput():
        root_directory = os.path.dirname(os.path.abspath(__file__))
        robot = load_pybullet(os.path.join(root_directory, IRB6600_TRACK_URDF), fixed_base=True)
    floor = load_model('models/short_floor.urdf')
    block = load_model(BLOCK_URDF, fixed_base=False)
    floor_x = 2
    set_pose(floor, Pose(Point(x=floor_x, z=0.5)))
    set_pose(block, Pose(Point(x=floor_x, y=0, z=stable_z(block, floor))))
    # set_default_camera()
    dump_world()

    saved_world = WorldSaver()
    with LockRenderer():
        command = plan(robot, block, fixed=[floor], teleport=False)
    if (command is None) or (display is None):
        print('Unable to find a plan!')
        print('Quit?')
        wait_for_interrupt()
        disconnect()
        return

    saved_world.restore()
    update_state()
    user_input('{}?'.format(display))
    if display == 'control':
        enable_gravity()
        command.control(real_time=False, dt=0)
    elif display == 'execute':
        command.refine(num_steps=10).execute(time_step=0.002)
    elif display == 'step':
        command.step()
    else:
        raise ValueError(display)

    print('Quit?')
    wait_for_interrupt()
    disconnect()
示例#25
0
def load_world():
    #print(get_data_path())
    #p.loadURDF("samurai.urdf", useFixedBase=True) # World
    #p.loadURDF("kuka_lwr/kuka.urdf", useFixedBase=True)
    #p.loadURDF("kuka_iiwa/model_free_base.urdf", useFixedBase=True)

    # TODO: store internal world info here to be reloaded
    robot = load_model(DRAKE_IIWA_URDF)
    # robot = load_model(KUKA_IIWA_URDF)
    floor = load_model('models/short_floor.urdf')
    sink = load_model(SINK_URDF, pose=Pose(Point(x=-0.5)))
    stove = load_model(STOVE_URDF, pose=Pose(Point(x=+0.5)))
    block = load_model(BLOCK_URDF, fixed_base=False)
    #cup = load_model('models/dinnerware/cup/cup_small.urdf',
    # Pose(Point(x=+0.5, y=+0.5, z=0.5)), fixed_base=False)

    set_pose(block, Pose(Point(y=0.5, z=stable_z(block, floor))))
    # print(get_camera())
    set_default_camera()

    return robot, block
示例#26
0
def main(display='execute'): # control | execute | step
    # One of the arm's gantry carriage is fixed when the other is moving.
    connect(use_gui=True)
    set_camera(yaw=-90, pitch=-40, distance=10, target_position=(0, 7.5, 0))
    draw_pose(unit_pose(), length=1.0)
    disable_real_time()

    with HideOutput():
        root_directory = os.path.dirname(os.path.abspath(__file__))
        robot = load_pybullet(os.path.join(root_directory, ETH_RFL_URDF), fixed_base=True)
    # floor = load_model('models/short_floor.urdf')
    block = load_model(BLOCK_URDF, fixed_base=False)
    #link = link_from_name(robot, 'gantry_base_link')
    #print(get_aabb(robot, link))

    block_x = -0.2
    #block_y = 1 if ARM == 'right' else 13.5
    #block_x = 10
    block_y = 5.

    # set_pose(floor, Pose(Point(x=floor_x, y=1, z=1.3)))
    # set_pose(block, Pose(Point(x=floor_x, y=0.6, z=stable_z(block, floor))))
    set_pose(block, Pose(Point(x=block_x, y=block_y, z=3.5)))
    # set_default_camera()
    dump_world()

    #print(get_camera())
    saved_world = WorldSaver()
    with LockRenderer():
        command = plan(robot, block, fixed=[], teleport=False) # fixed=[floor],
    if (command is None) or (display is None):
        print('Unable to find a plan! Quit?')
        wait_for_interrupt()
        disconnect()
        return

    saved_world.restore()
    update_state()
    print('{}?'.format(display))
    wait_for_interrupt()
    if display == 'control':
        enable_gravity()
        command.control(real_time=False, dt=0)
    elif display == 'execute':
        command.refine(num_steps=10).execute(time_step=0.002)
    elif display == 'step':
        command.step()
    else:
        raise ValueError(display)

    print('Quit?')
    wait_for_interrupt()
    disconnect()
示例#27
0
文件: pour.py 项目: lyltc1/LTAMP
def get_bowl_from_pivot(world, feature, parameter):
    bowl_body = world.get_body(feature['bowl_name'])
    bowl_urdf_from_center = get_urdf_from_top(
        bowl_body)  # get_urdf_from_base | get_urdf_from_center
    if RELATIVE_POUR:
        parameter = scale_parameter(feature,
                                    parameter,
                                    RELATIVE_POUR_SCALING,
                                    descale=True)
    bowl_base_from_pivot = Pose(
        Point(x=parameter['axis_in_bowl_x'], z=parameter['axis_in_bowl_z']))
    return multiply(bowl_urdf_from_center, bowl_base_from_pivot)
示例#28
0
def move_occluding(world):
    # Prevent obstruction by other objects
    # TODO: this is a bit of a hack due to pybullet
    world.set_base_conf([-5.0, 0, 0])
    for joint in world.kitchen_joints:
        joint_name = get_joint_name(world.kitchen, joint)
        if joint_name in DRAWERS:
            world.open_door(joint)
        else:
            world.close_door(joint)
    for name in world.movable:
        set_pose(world.get_body(name), Pose(Point(z=-5.0)))
示例#29
0
def parse_region(region):
    lower = np.min(region['hull'], axis=0)
    upper = np.max(region['hull'], axis=0)
    x, y = (lower + upper) / 2.
    w, h = (upper - lower)  # / 2.
    geom = get_box_geometry(w, h, 1e-3)
    collision_id, visual_id = create_shape(geom,
                                           pose=Pose(Point(x, y)),
                                           color=parse_color(region['color']))
    #region_id = create_body(NULL_ID, visual_id)
    region_id = create_body(collision_id, visual_id)
    set_pose(region_id, parse_pose(region))
    return region_id
示例#30
0
def create_frame(width, length, height, side=0.1):
    # TODO: could be part of the base link instead
    shapes = [
        Shape(get_box_geometry(width=width,
                               length=length + 2 * side,
                               height=side),
              pose=Pose(Point(z=height / 2. + side / 2.)),
              color=BLACK),
        Shape(get_box_geometry(width=width, length=side, height=height),
              pose=Pose(Point(y=(length + side) / 2.)),
              color=BLACK),
        Shape(get_box_geometry(width=width, length=side, height=height),
              pose=Pose(Point(y=-(length + side) / 2.)),
              color=BLACK),
    ]
    frame_collision, frame_visual = create_shape_array(*unzip(shapes))
    return LinkInfo(mass=STATIC_MASS,
                    collision_id=frame_collision,
                    visual_id=frame_visual,
                    point=Point(y=-length / 2., z=height / 2.),
                    parent=0,
                    joint_type=p.JOINT_FIXED)