Esempio n. 1
0
def detect_collision(spec, config):
    if test_environment_bounds(config) and test_angle_constraints(config, spec) and \
            test_length_constraints(config, spec) and test_grapple_point_constraint(config, spec) and \
            test_self_collision(config, spec) and test_obstacle_collision(config, spec, spec.obstacles):
        return True
    else:
        return False
Esempio n. 2
0
def uniformly_sampling(spec, obstacles, stage):
    while True:
        sampling_angle = []
        sampling_length = []
        for i in range(spec.num_segments):
            angle = random.uniform(-165, 165)
            sampling_angle.append(Angle(degrees=float(angle)))
            length = random.uniform(spec.min_lengths[i], spec.max_lengths[i])
            sampling_length.append(length)

        if stage % 2 == 0:
            new_config = make_robot_config_from_ee1(
                spec.grapple_points[stage][0],
                spec.grapple_points[stage][1],
                sampling_angle,
                sampling_length,
                ee1_grappled=True)
        else:
            new_config = make_robot_config_from_ee2(
                spec.grapple_points[stage][0],
                spec.grapple_points[stage][1],
                sampling_angle,
                sampling_length,
                ee2_grappled=True)

        if test_obstacle_collision(new_config, spec, obstacles) and test_self_collision(new_config, spec) and \
                test_environment_bounds(new_config):
            return new_config
Esempio n. 3
0
def check_valid(config, spec):
    if len(config.points) <= 1:
        return True
    return (tester.test_angle_constraints(config, spec)
            and tester.test_self_collision(config)
            and tester.test_obstacle_collision(config, spec, spec.obstacles)
            and tester.test_environment_bounds(config))
Esempio n. 4
0
def config_validity(config: robot_config.RobotConfig,
                    spec: problem_spec.ProblemSpec):
    #tester.test_obstacle_collision(config, spec, spec.obstacles
    return (custom_test_obstacle_collision(config, problem, custom_get_lenient_obstacles(spec))\
        and tester.test_self_collision(config, spec)\
        and tester.test_length_constraints(config, spec)\
        and tester.test_angle_constraints(config, spec)\
        and tester.test_environment_bounds(config))
Esempio n. 5
0
def individual_config_collision_checking(spec, config):
    # Obstacle Collision - true for pass, false for fail
    # Self Collision - return true for pass, false for fail
    # Environment Bound - return true for pass, false for fail
    # Angle Constraints - true for pass, false for fail
    return test_obstacle_collision(
        config, spec, spec.obstacles) and test_self_collision(
            config, spec) and test_environment_bounds(
                config) and test_angle_constraints(config, spec)
Esempio n. 6
0
def check_path(config1, config2, spec, obstacles):
    angles1 = config1.ee1_angles
    angles2 = config2.ee1_angles
    angle_distance = []
    if config1.ee2_grappled is True:
        angles1 = config1.ee2_angles
        angles2 = config2.ee2_angles

    lengths1 = config1.lengths
    lengths2 = config2.lengths
    length_distance = []

    for j in range(len(angles1)):
        angle_distance.append(angles2[j].__sub__(angles1[j]))
        length_distance.append(lengths2[j] - lengths1[j])

    for j in range(20):
        new_angle_list = []
        new_length_list = []
        for k in range(len(angles1)):
            if config1.ee1_grappled is True:
                new_angle_list.append(config1.ee1_angles[k].__add__(
                    angle_distance[k].__mul__(0.05 * j)))
            elif config1.ee2_grappled is True:
                new_angle_list.append(config1.ee2_angles[k].__add__(
                    angle_distance[k].__mul__(0.05 * j)))

            new_length_list.append(config1.lengths[k] +
                                   length_distance[k] * 0.05 * j)

        x, y = config1.points[0]
        new_config = make_robot_config_from_ee1(x,
                                                y,
                                                new_angle_list,
                                                new_length_list,
                                                ee1_grappled=True,
                                                ee2_grappled=False)
        if config1.ee2_grappled is True:
            x, y = config1.points[-1]
            new_config = make_robot_config_from_ee2(
                x,
                y,
                new_angle_list,
                list(reversed(new_length_list)),
                ee1_grappled=False,
                ee2_grappled=True)

        if not (test_obstacle_collision(new_config, spec, obstacles)
                and test_self_collision(new_config, spec)
                and test_environment_bounds(new_config)):
            return False

    return True
Esempio n. 7
0
def collison_checking(new_config, spec, obstacles):
    """
    return true for pass, false for fail
    1. It will not collide with any of the obstacles
    2. It will not collide with itself
    3. The entire Canadarm robotic arm must lie inside the workspace
    """
    if (test_obstacle_collision(new_config, spec, obstacles)
            and test_self_collision(new_config, spec)
            and test_environment_bounds(new_config)):
        return True
    return False
Esempio n. 8
0
def interpolate_path(start_config, end_config, is_end, spec):
    """
    produces a series of primitive steps between two RobotConfigs
    :param start_config: Beginning RobotConfig to check
    :param end_config: End RobotConfig to check
    :param is_end: Whether a path has been found
    :param spec: The ProblemSpec object defining problem
    :return: List of primitive steps if valid, None otherwise
    """
    primitive_steps = []
    angle_diffs = []
    length_diffs = []
    primitive_angles = []
    primitive_lengths = []

    for i in range(len(start_config.ee1_angles)):
        angle_diffs.append(end_config.ee1_angles[i].in_radians() - start_config.ee1_angles[i].in_radians())
        length_diffs.append(end_config.lengths[i] - start_config.lengths[i])

    abs_angles = [abs(angle) for angle in angle_diffs]
    abs_lengths = [abs(length) for length in length_diffs]
    segs = max(max(abs_angles), max(abs_lengths))
    total_configs = int(math.ceil(segs/0.001))

    for arm in range(len(start_config.ee1_angles)):
        primitive_angles.append(angle_diffs[arm] / total_configs)
        primitive_lengths.append(length_diffs[arm] / total_configs)

    for prim_step in range(total_configs):
        ee1_angles = []
        lengths = []
        for arm in range(len(start_config.ee1_angles)):
            angle = Angle(radians=start_config.ee1_angles[arm].in_radians() + (prim_step * primitive_angles[arm]))
            ee1_angles.append(angle)

            length = start_config.lengths[arm] + prim_step * primitive_lengths[arm]
            lengths.append(length)

        config = make_robot_config_from_ee1(start_config.get_ee1()[0], start_config.get_ee1()[1], ee1_angles, lengths,
                                            start_config.ee1_grappled, start_config.ee2_grappled)

        # If collision check happened, don't do it again
        if not is_end:
            if not test_obstacle_collision(config, spec, spec.obstacles) or not test_environment_bounds(config) \
                    or not test_self_collision(config, spec):
                return None

        primitive_steps.append(config)

    primitive_steps.append(end_config)

    return primitive_steps
Esempio n. 9
0
def solve(spec, output):
    """
    An example solve method containing code to perform a breadth first search of the state graph and return a list of
    configs which form a path through the state graph between the initial and the goal. Note that this path will not
    satisfy the primitive step requirement - you will need to interpolate between the configs in the returned list.

    If you wish to use this code, you may either copy this code into your own file or add your existing code to this
    file.

    :param spec: ProblemSpec object
    :return: List of configs forming a path through the graph from initial to goal
    """

    init_node = GraphNode(spec, spec.initial)
    goal_node = GraphNode(spec, spec.goal)

    # TODO: Insert your code to build the state graph here
    # *** example for adding neighbors ***
    # if path between n1 and n2 is collision free:
    #   n1.neighbors.append(n2)
    #   n2.neighbors.append(n1)
    vertices, edges = state_graph(350, 45, 5, init_node, goal_node, spec)

    # search the graph
    init_container = [init_node]

    # here, each key is a graph node, each value is the list of configs visited on the path to the graph node
    init_visited = {init_node: [init_node.config]}

    while len(init_container) > 0:
        current = init_container.pop(0)
        # print(current.__hash__())
        if test_config_equality(current.config, spec.goal, spec):
            # found path to goal
            # print(init_visited[current])
            print("found")
            # for i in init_visited[current]:
            #     print(str(i))
            write_robot_config_list_to_file(output, primitive_step(init_visited[current]))
            print("finish")
            return init_visited[current]

        successors = current.get_successors()
        for suc in successors:
            if suc not in init_visited:
                if test_self_collision(suc.config, spec) \
                        and test_environment_bounds(suc.config) \
                        and test_obstacle_collision(suc.config, spec, spec.obstacles):
                    init_container.append(suc)
                    init_visited[suc] = init_visited[current] + [suc.config]
Esempio n. 10
0
def valid_config(config, spec):
    """
    #Imported from tester
    #Self Collision
    #Bound check
    #Angle constraints
    #Length constraints
    #Obstacle collision
    """
    if test_self_collision(config, spec) == False:
        return False
    if test_environment_bounds(config) == False:
        return False
    if test_angle_constraints(config, spec) == False:
        return False
    if test_length_constraints(config, spec) == False:
        return False
    if test_obstacle_collision(config, spec, spec.obstacles) == False:
        return False
    return True
Esempio n. 11
0
def create_state_graph(spec, obstacles):
    init_node = GraphNode(spec, spec.initial)
    goal_node = GraphNode(spec, spec.goal)

    nodes = [init_node, goal_node]
    for stage in range(spec.num_grapple_points):
        for i in range(500):
            sample_point = uniformly_sampling(spec, obstacles, stage)
            new_node = GraphNode(spec, sample_point)
            nodes.append(new_node)

            for node in nodes:
                if check_distance(
                        spec, sample_point, node.config) and check_path(
                            sample_point, node.config, spec, obstacles):
                    node.neighbors.append(new_node)
                    new_node.neighbors.append(node)

    init_container = [init_node]
    init_visited = {init_node: [init_node.config]}

    while len(init_container) > 0:
        current = init_container.pop(0)

        if test_config_equality(current.config, spec.goal, spec):
            result = init_visited[current]
            # return result
            new_list = []
            for i in range(len(result) - 1):
                angles1 = result[i].ee1_angles
                angles2 = result[i + 1].ee1_angles
                if result[i].ee2_grappled is True:
                    angles1 = result[i].ee2_angles
                    angles2 = result[i + 1].ee2_angles

                angle_distance = []
                lengths1 = result[i].lengths
                lengths2 = result[i + 1].lengths
                length_distance = []

                for j in range(len(angles1)):
                    angle_distance.append(angles2[j].__sub__(angles1[j]))
                    length_distance.append(lengths2[j] - lengths1[j])

                for j in range(501):
                    new_angle_list = []
                    new_length_list = []
                    for k in range(len(angles1)):
                        if result[i].ee1_grappled is True:
                            new_angle_list.append(
                                result[i].ee1_angles[k].__add__(
                                    angle_distance[k].__mul__(0.002 * j)))
                        elif result[i].ee2_grappled is True:
                            new_angle_list.append(
                                result[i].ee2_angles[k].__add__(
                                    angle_distance[k].__mul__(0.002 * j)))

                        new_length_list.append(result[i].lengths[k] +
                                               length_distance[k] * 0.002 * j)

                    if result[i].ee1_grappled is True:
                        x, y = result[i].points[0]
                        new_config = make_robot_config_from_ee1(
                            x,
                            y,
                            new_angle_list,
                            new_length_list,
                            ee1_grappled=True,
                            ee2_grappled=False)
                        if test_obstacle_collision(
                                new_config, spec,
                                obstacles) and test_self_collision(
                                    new_config, spec
                                ) and test_environment_bounds(new_config):
                            new_list.append(new_config)
                    elif result[i].ee2_grappled is True:
                        x, y = result[i].points[-1]
                        new_config = make_robot_config_from_ee2(
                            x,
                            y,
                            new_angle_list,
                            list(reversed(new_length_list)),
                            ee1_grappled=False,
                            ee2_grappled=True)
                        if test_obstacle_collision(
                                new_config, spec,
                                obstacles) and test_self_collision(
                                    new_config, spec
                                ) and test_environment_bounds(new_config):
                            new_list.append(new_config)

            return new_list

        successors = current.get_successors()
        for suc in successors:
            if suc not in init_visited:
                init_container.append(suc)
                init_visited[suc] = init_visited[current] + [suc.config]
Esempio n. 12
0
def generate_sub_goals(spec, stage, obstacles):
    start_point = spec.grapple_points[stage]
    goal_point = spec.grapple_points[stage+1]

    sub_goals = []
    for i in range(10):
        find_goal = False

        while find_goal is False:
            sampling_angle = []
            sampling_length = []
            for j in range(spec.num_segments - 1):
                angle = random.uniform(-165, 165)
                sampling_angle.append(Angle(degrees=float(angle)))
                length = random.uniform(spec.min_lengths[j], spec.max_lengths[j])
                sampling_length.append(length)

            if stage % 2 == 0:
                x1, y1 = start_point
                x2, y2 = goal_point
                partial_config = make_robot_config_from_ee1(x1, y1, sampling_angle, sampling_length, ee1_grappled=True)
                if (partial_config.points[-1][0] - x2) ** 2 + (partial_config.points[-1][1] - y2) ** 2 < spec.max_lengths[spec.num_segments-1] ** 2:
                    sampling_length.append(math.sqrt((partial_config.points[-1][0] - x2) ** 2 + (partial_config.points[-1][1] - y2) ** 2))

                    required_net_angle = math.atan((partial_config.points[-1][1] - y2) / (partial_config.points[-1][0] - x2))
                    if y2 > partial_config.points[-1][1] and x2 < partial_config.points[-1][0]:
                        required_net_angle += math.pi
                    elif y2 < partial_config.points[-1][1] and x2 < partial_config.points[-1][0]:
                        required_net_angle -= math.pi
                    current_net_angle = 0

                    for angle in sampling_angle:
                        current_net_angle += angle.in_radians()
                    sampling_angle.append(Angle(radians=float(required_net_angle-current_net_angle)))

                    sub_goal_config = make_robot_config_from_ee1(x1, y1, sampling_angle, sampling_length, ee1_grappled=True, ee2_grappled=True)
                    if test_obstacle_collision(sub_goal_config, spec, obstacles) and test_self_collision(sub_goal_config, spec)\
                            and test_environment_bounds(sub_goal_config):

                        sub_goals.append(sub_goal_config)
                        find_goal = True

            else:
                x1, y1 = start_point
                x2, y2 = goal_point
                partial_config = make_robot_config_from_ee2(x1, y1, sampling_angle, sampling_length, ee2_grappled=True)

                if (partial_config.points[0][0] - x2) ** 2 + (partial_config.points[0][1] - y2) ** 2 < spec.max_lengths[0] ** 2:
                    sampling_length.insert(0, math.sqrt((partial_config.points[0][0] - x2) ** 2 + (partial_config.points[0][1] - y2) ** 2))
                    required_net_angle = math.atan((partial_config.points[0][1] - y2) / (partial_config.points[0][0] - x2))
                    if required_net_angle < 0:
                        required_net_angle += math.pi
                    current_net_angle = 0
                    for angle in sampling_angle:
                        current_net_angle += angle.in_radians()
                    sampling_angle.append(Angle(radians=float(required_net_angle-current_net_angle)))
                    sub_goal_config = make_robot_config_from_ee2(x1, y1, sampling_angle, sampling_length, ee1_grappled=True, ee2_grappled=True)

                    if test_obstacle_collision(sub_goal_config, spec, obstacles) and test_self_collision(sub_goal_config, spec)\
                            and test_environment_bounds(sub_goal_config):
                        sub_goals.append(sub_goal_config)
                        find_goal = True

    return sub_goals