Ejemplo n.º 1
0
def test_close_gripper(robot, arm):
    gripper_joints = get_gripper_joints(robot, arm)
    extend_fn = get_extend_fn(robot, gripper_joints)
    for positions in extend_fn(get_open_positions(robot, arm), get_closed_positions(robot, arm)):
        set_joint_positions(robot, gripper_joints, positions)
        print(positions)
        wait_if_gui('Continue?')
Ejemplo n.º 2
0
def compute_door_paths(world,
                       joint_name,
                       door_conf1,
                       door_conf2,
                       obstacles=set(),
                       teleport=False):
    door_paths = []
    if door_conf1 == door_conf2:
        return door_paths
    door_joint = joint_from_name(world.kitchen, joint_name)
    door_joints = [door_joint]
    # TODO: could unify with grasp path
    door_extend_fn = get_extend_fn(world.kitchen,
                                   door_joints,
                                   resolutions=[DOOR_RESOLUTION])
    door_path = [door_conf1.values] + list(
        door_extend_fn(door_conf1.values, door_conf2.values))
    if teleport:
        door_path = [door_conf1.values, door_conf2.values]
    # TODO: open until collision for the drawers

    sign = world.get_door_sign(door_joint)
    pull = (sign * door_path[0][0] < sign * door_path[-1][0])
    # door_obstacles = get_descendant_obstacles(world.kitchen, door_joint)
    for handle_grasp in get_handle_grasps(world, door_joint, pull=pull):
        link, grasp, pregrasp = handle_grasp
        handle_path = []
        for door_conf in door_path:
            set_joint_positions(world.kitchen, door_joints, door_conf)
            # if any(pairwise_collision(door_obst, obst)
            #       for door_obst, obst in product(door_obstacles, obstacles)):
            #    return
            handle_path.append(get_link_pose(world.kitchen, link))
            # Collide due to adjacency

        # TODO: check pregrasp path as well
        # TODO: check gripper self-collisions with the robot
        set_configuration(world.gripper, world.open_gq.values)
        tool_path = [
            multiply(handle_pose, invert(grasp)) for handle_pose in handle_path
        ]
        for i, tool_pose in enumerate(tool_path):
            set_joint_positions(world.kitchen, door_joints, door_path[i])
            set_tool_pose(world, tool_pose)
            # handles = draw_pose(handle_path[i], length=0.25)
            # handles.extend(draw_aabb(get_aabb(world.kitchen, link=link)))
            # wait_for_user()
            # for handle in handles:
            #    remove_debug(handle)
            if any(
                    pairwise_collision(world.gripper, obst)
                    for obst in obstacles):
                break
        else:
            door_paths.append(
                DoorPath(door_path, handle_path, handle_grasp, tool_path))
    return door_paths
Ejemplo n.º 3
0
 def iterate(self, world, attachments):
     joints = get_gripper_joints(world.robot, self.arm)
     start_conf = get_joint_positions(world.robot, joints)
     end_conf = [self.position] * len(joints)
     extend_fn = get_extend_fn(world.robot, joints)
     path = [start_conf] + list(extend_fn(start_conf, end_conf))
     for positions in path:
         set_joint_positions(world.robot, joints, positions)
         yield positions
Ejemplo n.º 4
0
def plan_approach(end_effector,
                  print_traj,
                  collision_fn,
                  approach_distance=APPROACH_DISTANCE):
    # TODO: collisions at the ends of elements
    if approach_distance == 0:
        return Command([print_traj])
    robot = end_effector.robot
    joints = get_movable_joints(robot)
    extend_fn = get_extend_fn(robot,
                              joints,
                              resolutions=0.25 * JOINT_RESOLUTIONS)
    tool_link = link_from_name(robot, TOOL_LINK)
    approach_pose = Pose(Point(z=-approach_distance))

    #element = print_traj.element
    #midpoint = get_point(element)
    #points = map(point_from_pose, [print_traj.tool_path[0], print_traj.tool_path[-1]])
    #midpoint = np.average(list(points), axis=0)
    #draw_point(midpoint)
    #element_to_base = 0.05*get_unit_vector(get_point(robot) - midpoint)
    #retreat_pose = Pose(Point(element_to_base))
    # TODO: perpendicular to element

    # TODO: solve_ik
    start_conf = print_traj.path[0]
    set_joint_positions(robot, joints, start_conf)
    initial_pose = multiply(print_traj.tool_path[0], approach_pose)
    #draw_pose(initial_pose)
    #wait_if_gui()
    initial_conf = inverse_kinematics(robot, tool_link, initial_pose)
    if initial_conf is None:
        return None
    initial_path = [initial_conf] + list(extend_fn(initial_conf, start_conf))
    if any(map(collision_fn, initial_path)):
        return None
    initial_traj = MotionTrajectory(robot, joints, initial_path)

    end_conf = print_traj.path[-1]
    set_joint_positions(robot, joints, end_conf)
    final_pose = multiply(print_traj.tool_path[-1], approach_pose)
    final_conf = inverse_kinematics(robot, tool_link, final_pose)
    if final_conf is None:
        return None
    final_path = [end_conf] + list(extend_fn(
        end_conf, final_conf))  # TODO: version that includes the endpoints
    if any(map(collision_fn, final_path)):
        return None
    final_traj = MotionTrajectory(robot, joints, final_path)
    return Command([initial_traj, print_traj, final_traj])
Ejemplo n.º 5
0
 def fn(gq1, gq2):
     #if gq1 == gq2:
     #    return None
     if teleport:
         path = [gq1.values, gq2.values]
     else:
         extend_fn = get_extend_fn(gq2.body,
                                   gq2.joints,
                                   resolutions=resolutions)
         path = [gq1.values] + list(extend_fn(gq1.values, gq2.values))
     cmd = Sequence(State(world),
                    commands=[
                        Trajectory(world, gq2.body, gq2.joints, path),
                    ],
                    name='gripper')
     return (cmd, )
Ejemplo n.º 6
0
Archivo: move.py Proyecto: lyltc1/LTAMP
def plan_water_motion(body,
                      joints,
                      end_conf,
                      attachment,
                      obstacles=None,
                      attachments=[],
                      self_collisions=True,
                      disabled_collisions=set(),
                      max_distance=MAX_DISTANCE,
                      weights=None,
                      resolutions=None,
                      reference_pose=unit_pose(),
                      custom_limits={},
                      **kwargs):
    assert len(joints) == len(end_conf)
    sample_fn = get_sample_fn(body, joints, custom_limits=custom_limits)
    distance_fn = get_distance_fn(body, joints, weights=weights)
    extend_fn = get_extend_fn(body, joints, resolutions=resolutions)
    collision_fn = get_collision_fn(body,
                                    joints,
                                    obstacles, {attachment} | set(attachments),
                                    self_collisions,
                                    disabled_collisions,
                                    max_distance=max_distance,
                                    custom_limits=custom_limits)

    def water_test(q):
        if attachment is None:
            return False
        set_joint_positions(body, joints, q)
        attachment.assign()
        attachment_pose = get_pose(attachment.child)
        pose = multiply(attachment_pose,
                        reference_pose)  # TODO: confirm not inverted
        roll, pitch, _ = euler_from_quat(quat_from_pose(pose))
        violation = (MAX_ROTATION < abs(roll)) or (MAX_ROTATION < abs(pitch))
        #if violation: # TODO: check whether different confs can be waypoints for each object
        #    print(q, violation)
        #    wait_for_user()
        return violation

    invalid_fn = lambda q, **kwargs: water_test(q) or collision_fn(q, **kwargs)
    start_conf = get_joint_positions(body, joints)
    if not check_initial_end(start_conf, end_conf, invalid_fn):
        return None
    return birrt(start_conf, end_conf, distance_fn, sample_fn, extend_fn,
                 invalid_fn, **kwargs)
Ejemplo n.º 7
0
def compute_motion(robot, fixed_obstacles, element_bodies,
                   printed_elements, start_conf, end_conf,
                   collisions=True, max_time=INF, buffer=0.1, smooth=100):
    # TODO: can also just plan to initial conf and then shortcut
    joints = get_movable_joints(robot)
    assert len(joints) == len(end_conf)
    weights = JOINT_WEIGHTS
    resolutions = np.divide(RESOLUTION * np.ones(weights.shape), weights)
    disabled_collisions = get_disabled_collisions(robot)
    custom_limits = {}
    #element_from_body = {b: e for e, b in element_bodies.items()}

    hulls, obstacles = {}, []
    if collisions:
        element_obstacles = {element_bodies[e] for e in printed_elements}
        obstacles = set(fixed_obstacles) | element_obstacles
        #hulls, obstacles = decompose_structure(fixed_obstacles, element_bodies, printed_elements)
    #print(hulls)
    #print(obstacles)
    #wait_for_user()

    #printed_elements = set(element_bodies)
    bounding = None
    if printed_elements:
        # TODO: pass in node_points
        bounding = create_bounding_mesh(printed_elements, element_bodies=element_bodies, node_points=None,
                                        buffer=0.01) # buffer=buffer)
        #wait_for_user()

    sample_fn = get_sample_fn(robot, joints, custom_limits=custom_limits)
    distance_fn = get_distance_fn(robot, joints, weights=weights)
    extend_fn = get_extend_fn(robot, joints, resolutions=resolutions)
    #collision_fn = get_collision_fn(robot, joints, obstacles, attachments={}, self_collisions=SELF_COLLISIONS,
    #                                disabled_collisions=disabled_collisions, custom_limits=custom_limits, max_distance=0.)
    collision_fn = get_element_collision_fn(robot, obstacles)

    fine_extend_fn = get_extend_fn(robot, joints, resolutions=1e-1*resolutions) #, norm=INF)

    def test_bounding(q):
        set_joint_positions(robot, joints, q)
        collision = (bounding is not None) and pairwise_collision(robot, bounding, max_distance=buffer) # body_collision
        return q, collision

    def dynamic_extend_fn(q_start, q_end):
        # TODO: retime trajectories to be move more slowly around the structure
        for (q1, c1), (q2, c2) in get_pairs(map(test_bounding, extend_fn(q_start, q_end))):
            # print(c1, c2, len(list(fine_extend_fn(q1, q2))))
            # set_joint_positions(robot, joints, q2)
            # wait_for_user()
            if c1 and c2:
                for q in fine_extend_fn(q1, q2):
                    # set_joint_positions(robot, joints, q)
                    # wait_for_user()
                    yield q
            else:
                yield q2

    def element_collision_fn(q):
        if collision_fn(q):
            return True
        #for body in get_bodies_in_region(get_aabb(robot)): # Perform per link?
        #    if (element_from_body.get(body, None) in printed_elements) and pairwise_collision(robot, body):
        #        return True
        for hull, bodies in hulls.items():
            if pairwise_collision(robot, hull) and any(pairwise_collision(robot, body) for body in bodies):
                return True
        return False

    path = None
    if check_initial_end(start_conf, end_conf, collision_fn):
        path = birrt(start_conf, end_conf, distance_fn, sample_fn, dynamic_extend_fn, element_collision_fn,
                     restarts=50, iterations=100, smooth=smooth, max_time=max_time)
    # path = plan_joint_motion(robot, joints, end_conf, obstacles=obstacles,
    #                          self_collisions=SELF_COLLISIONS, disabled_collisions=disabled_collisions,
    #                          weights=weights, resolutions=resolutions,
    #                          restarts=50, iterations=100, smooth=100)

    # if (bounding is not None) and (path is not None):
    #     for i, q in enumerate(path):
    #         print('{}/{}'.format(i, len(path)))
    #         set_joint_positions(robot, joints, q)
    #         wait_for_user()

    if bounding is not None:
        remove_body(bounding)
    for hull in hulls:
        remove_body(hull)
    if path is None:
        print('Failed to find a motion plan!')
        return None
    return MotionTrajectory(robot, joints, path)