예제 #1
0
파일: objects.py 프로젝트: galou/symoro
 def set_length(self, new_length):
     if not self.init_length:
         self.rod_vertices, self.rod_indices, self.rod_normals = \
             Primitives.rod_array(new_length)
         self.init_length = new_length
     self.length = new_length
     if self.has_base or self.has_end:
         self.sph_vertices, self.sph_indices, self.sph_normals = \
             Primitives.sph_array(1.5 * new_length)
     super(JointObject, self).set_length(new_length)
예제 #2
0
 def set_length(self, new_length):
     if not self.init_length:
         self.rod_vertices, self.rod_indices, self.rod_normals = \
             Primitives.rod_array(new_length)
         self.init_length = new_length
     self.length = new_length
     if self.has_base or self.has_end:
         self.sph_vertices, self.sph_indices, self.sph_normals = \
             Primitives.sph_array(1.5 * new_length)
     super(JointObject, self).set_length(new_length)
예제 #3
0
파일: rt.py 프로젝트: bodil/lolisp
  def __init__(self, ns = None):
    if ns:
      self.ns = ns
    else:
      prims = Primitives(self)

      import primitives.math
      prims.extend(primitives.math)
      import primitives.concurrent
      prims.extend(primitives.concurrent)

      self.ns = Scope(extend = prims)
예제 #4
0
 def set_length(self, new_length):
     if not self.init_length:
         self.rod_vertices, self.rod_indices, self.rod_normals = \
             Primitives.rod_array(new_length)
         self.init_length = new_length
     self.length = new_length
     super(JointObject, self).set_length(new_length)
예제 #5
0
 def set_length(self, new_length):
     if not self.init_length:
         self.rod_vertices, self.rod_indices, self.rod_normals = \
             Primitives.rod_array(new_length)
         self.init_length = new_length
     self.length = new_length
     super(JointObject, self).set_length(new_length)
    def args_generator(self, state: Program, op_id: int):
        n_args = self.get_n_args(op_id)
        # Syntactically valid ranges
        jump_range = range(state.min, state.max + 1 + 1 + n_args + 1)
        content_range = range(state.min, state.max + 1 + n_args + 1)

        if abs(state.min) >= state.work_tape_size:
            allocate_range = None
        else:
            allocate_range = (
                range(1, min(5, state.work_tape_size - abs(state.min)) + 1),
            )

        if state.min == 0:
            free_range = None
        else:
            free_range = (range(1, min(abs(state.min), 5) + 1),)

        write_range = range(state.min, -1 + 1)
        get_input_range = range(19 + 1)

        op_args = [
            (content_range, content_range, jump_range),
            (content_range,),
            (jump_range,),
            [],
            (content_range, content_range, write_range),
            (get_input_range, write_range),
            (content_range, write_range),
            allocate_range,
            (write_range,),
            (write_range,),
            (content_range, content_range, write_range),
            (content_range, content_range, write_range),
            free_range,
        ]

        return Primitives._generator(op_args, op_id)
예제 #7
0
 def set_length(self, new_length):
     self.sph_vertices, self.sph_indices, self.sph_normals = Primitives.sph_array(new_length)
     super(FixedJoint, self).set_length(new_length)
예제 #8
0
 def set_length(self, new_length):
     self.box_vertices, self.box_normals = Primitives.box_array(new_length)
     super(PrismaticJoint, self).set_length(new_length)
예제 #9
0
 def set_length(self, new_length):
     self.cyl_vertices, self.cyl_indices, self.cyl_normals = Primitives.cyl_array(new_length)
     super(RevoluteJoint, self).set_length(new_length)
예제 #10
0
 def set_length(self, new_length):
     self.arr_vertices, self.arr_indices, self.arr_normals = \
         Primitives.arr_array(new_length)
예제 #11
0
 def set_length(self, new_length):
     self.sph_vertices, self.sph_indices, self.sph_normals = \
         Primitives.sph_array(new_length)
     super(FixedJoint, self).set_length(new_length)
예제 #12
0
def state_lattice_planner(n: int, m: int, file_name: str = "test", g_weight: float = 0.5, h_weight: float = 0.5,
                          costmap_file: str = "",
                          start_pos: tuple = (20, 10, 0), goal_pos: tuple = (20, 280, 0),
                          initial_heading: float = math.pi / 2, padding: int = 0,
                          turning_radius: int = 8, vel: int = 10, num_headings: int = 8,
                          num_obs: int = 130, min_r: int = 1, max_r: int = 8, upper_offset: int = 20,
                          lower_offset: int = 20, allow_overlap: bool = False,
                          obstacle_density: int = 6, obstacle_penalty: float = 3,
                          Kp: float = 3, Ki: float = 0.08, Kd: float = 0.5, inf_stream: bool = False,
                          save_animation: bool = False, save_costmap: bool = False, smooth_path: bool = False,
                          replan: bool = False, horizon: int = np.inf, y_axis_limit: int = 100, buffer: int = None,
                          move_yaxis_threshold: int = 20, new_obs_dist: int = None):
    # PARAM SETUP
    # --- costmap --- #
    load_costmap_file = costmap_file
    ship_vertices = np.array([[-1, -4],
                              [1, -4],
                              [1, 2],
                              [0, 4],
                              [-1, 2]])

    # load costmap object from file if specified
    if load_costmap_file:
        with open(load_costmap_file, "rb") as fd:
            costmap_obj = pickle.load(fd)
            # recompute costmap costs if obstacle penalty is different than original
            if costmap_obj.obstacle_penalty != obstacle_penalty:
                costmap_obj.update2(obstacle_penalty)
    else:
        # initialize costmap
        costmap_obj = CostMap(
            n, m, obstacle_penalty, min_r, max_r, inf_stream, y_axis_limit, num_obs, new_obs_dist
        )

        # generate obs up until buffer if in inf stream mode
        max_y = y_axis_limit + buffer if inf_stream else goal_pos[1]
        # generate random obstacles
        costmap_obj.generate_obstacles(start_pos[1], max_y, num_obs,
                                       upper_offset, lower_offset, allow_overlap)

    orig_obstacles = costmap_obj.obstacles.copy()

    # initialize ship object
    ship = Ship(ship_vertices, start_pos, initial_heading, turning_radius, padding)

    # get the primitives
    prim = Primitives(turning_radius, initial_heading, num_headings)

    # generate swath dict
    swath_dict = swath.generate_swath(ship, prim)

    print("WEIGHTS", g_weight, h_weight)
    # initialize a star object
    a_star = AStar(g_weight, h_weight, cmap=costmap_obj,
                   primitives=prim, ship=ship, first_initial_heading=initial_heading)

    # compute current goal
    curr_goal = (goal_pos[0], min(goal_pos[1], (start_pos[1] + horizon)), goal_pos[2])

    t0 = time.time()
    worked, smoothed_edge_path, nodes_visited, x1, y1, x2, y2, orig_path = \
        a_star.search(start_pos, curr_goal, swath_dict, smooth_path)

    init_plan_time = time.time() - t0
    print("Time elapsed: ", init_plan_time)
    print("Hz", 1 / init_plan_time)

    if worked:
        plot_obj = Plot(
            costmap_obj, prim, ship, nodes_visited, smoothed_edge_path.copy(),
            path_nodes=(x1, y1), smoothing_nodes=(x2, y2), horizon=horizon,
            inf_stream=inf_stream, y_axis_limit=y_axis_limit
        )
        path = Path(plot_obj.full_path)
    else:
        print("Failed to find path at step 0")
        exit(1)

    # init pymunk sim
    space = pymunk.Space()
    space.add(ship.body, ship.shape)
    space.gravity = (0, 0)
    staticBody = space.static_body  # create a static body for friction constraints

    # create the pymunk objects and the polygon patches for the ice
    polygons = [
        create_polygon(
            space, staticBody, (obs['vertices'] - np.array(obs['centre'])).tolist(),
            *obs['centre'], density=obstacle_density
        )
        for obs in costmap_obj.obstacles
    ]

    # From pure pursuit
    state = State(x=start_pos[0], y=start_pos[1], yaw=0.0, v=0.0)
    target_course = TargetCourse(path.path[0], path.path[1])
    target_ind = target_course.search_target_index(state)

    # init PID controller
    pid = PID(Kp, Ki, Kd, 0)
    pid.output_limits = (-1, 1)  # limit on PID output

    # generator to end matplotlib animation when it reaches the goal
    def gen():
        nonlocal at_goal
        i = 0
        while not at_goal:
            i += 1
            yield i
        raise StopIteration  # should stop animation

    def animate(frame, queue_state, pipe_path):
        nonlocal at_goal

        steps = 10
        # move simulation forward 20 ms seconds:
        for x in range(steps):
            space.step(0.02 / steps)

        # get current state
        ship_pos = (ship.body.position.x, ship.body.position.y, 0)  # straight ahead of boat is 0

        # check if ship has made it past the goal line
        if ship.body.position.y >= goal_pos[1]:
            at_goal = True
            print("\nAt goal, shutting down...")
            plt.close(plot_obj.map_fig)
            plt.close(plot_obj.sim_fig)
            queue_state.close()
            shutdown_event.set()
            return []

        # Pymunk takes left turn as negative and right turn as positive in ship.body.angle
        # To get proper error, we must flip the sign on the angle, as to calculate the setpoint,
        # we look at a point one lookahead distance ahead, and find the angle to that point with
        # arctan2, but using this, we will get positive values on the left and negative values on the right
        # As the angular velocity in pymunk uses the same convention as ship.body.angle, we must flip the sign
        # of the output as well
        output = -pid(-ship.body.angle)

        # should play around with frequency at which new state data is sent
        if frame % 20 == 0 and frame != 0 and replan:
            # update costmap and polygons
            to_add, to_remove = costmap_obj.update(polygons, ship.body.position.y)
            assert len(costmap_obj.obstacles) <= costmap_obj.total_obs

            # remove polygons if any
            for obs in to_remove:
                polygons.remove(obs)

            # add polygons if any
            polygons.extend([
                create_polygon(
                    space, staticBody, (obs['vertices'] - np.array(obs['centre'])).tolist(),
                    *obs['centre'], density=obstacle_density
                )
                for obs in to_add
            ])
            print("Total polygons", len(polygons))

            try:
                # empty queue to ensure latest state data is pushed
                queue_state.get_nowait()
            except Empty:
                pass

            # send updated state via queue
            queue_state.put({
                'ship_pos': ship_pos,
                'ship_body_angle': ship.body.angle,
                'costmap': costmap_obj.cost_map,
                'obstacles': costmap_obj.obstacles,
            }, block=False)
            print('\nSent new state data!')

        # check if there is a new path
        if pipe_path.poll():
            # get new path
            path_data = pipe_path.recv()
            new_path = path_data['path']  # this is the full path and in the correct order i.e. start -> goal
            print('\nReceived replanned path!')

            # compute swath cost of new path up until the max y distance of old path for a fair comparison
            # note, we do not include the path length in the cost
            full_swath, full_cost, current_cost = swath.compute_swath_cost(
                costmap_obj.cost_map, new_path, ship.vertices, threshold_dist=path.path[1][-1]
            )
            try:
                assert full_cost >= current_cost  # sanity check
            except AssertionError:
                print("Full and partial swath costs", full_cost, current_cost)

            path_expired = False
            prev_cost = None

            # check if old path is 'expired' regardless of costs
            if (path.path[1][-1] - ship_pos[1]) < horizon / 2:
                path_expired = True

            else:
                # clip old path based on ship y position
                old_path = path.clip_path(ship_pos[1])

                # compute cost of clipped old path
                _, prev_cost, _ = swath.compute_swath_cost(costmap_obj.cost_map, old_path, ship.vertices)

                print('\nPrevious Cost: {prev_cost:.3f}'.format(prev_cost=prev_cost))
                print('Current Cost: {current_cost:.3f}\n'.format(current_cost=current_cost))

            if path_expired or current_cost < prev_cost:
                if path_expired:
                    print("Path expired, applying new path regardless of cost!")
                else:
                    print("New path better than old path!")
                    path.new_path_cnt += 1

                plot_obj.update_path(
                    new_path, full_swath, path_data['path_nodes'],
                    path_data['smoothing_nodes'], path_data['nodes_expanded']
                )

                # update to new path
                path.path = new_path
                ship.set_path_pos(0)

                # update pure pursuit objects with new path
                target_course.update(path.path[0], path.path[1])
                state.update(ship.body.position.x, ship.body.position.y, ship.body.angle)

                # update costmap and map fig
                plot_obj.update_map(costmap_obj.cost_map)
                plot_obj.map_fig.canvas.draw()

            else:
                print("Old path better than new path")
                path.old_path_cnt += 1

        if ship.path_pos < np.shape(path.path)[1] - 1:
            # Translate linear velocity into direction of ship
            x_vel = math.sin(ship.body.angle)
            y_vel = math.cos(ship.body.angle)
            mag = math.sqrt(x_vel ** 2 + y_vel ** 2)
            x_vel = x_vel / mag * vel
            y_vel = y_vel / mag * vel
            ship.body.velocity = Vec2d(x_vel, y_vel)

            # Assign output of PID controller to angular velocity
            ship.body.angular_velocity = output

            # Update the pure pursuit state
            state.update(ship.body.position.x, ship.body.position.y, ship.body.angle)

            # Get look ahead index
            ind = target_course.search_target_index(state)

            if ind != ship.path_pos:
                # Find heading from current position to look ahead point
                ship.set_path_pos(ind)
                dy = path.path[1][ind] - ship.body.position.y
                dx = path.path[0][ind] - ship.body.position.x
                angle = np.arctan2(dy, dx) - a_star.first_initial_heading
                # set setpoint for PID controller
                pid.setpoint = angle

        # at each step animate ship and obstacle patches
        plot_obj.animate_ship(ship, horizon, move_yaxis_threshold)
        plot_obj.animate_obstacles(polygons)

        return plot_obj.get_sim_artists()

    # multiprocessing setup
    lifo_queue = Queue(maxsize=1)  # LIFO queue to send state information to A*
    conn_recv, conn_send = Pipe(duplex=False)  # pipe to send new path to controller and for plotting
    shutdown_event = Event()

    # setup a process to run A*
    print('\nStart process...')
    gen_path_process = Process(
        target=gen_path, args=(lifo_queue, conn_send, shutdown_event, ship, prim,
                               costmap_obj, swath_dict, a_star, goal_pos, horizon, smooth_path)
    )
    gen_path_process.start()

    # init vars used in animation methods
    at_goal = False

    # start animation in main process
    anim = animation.FuncAnimation(plot_obj.sim_fig,
                                   animate,
                                   frames=gen,
                                   fargs=(lifo_queue, conn_recv,),
                                   interval=20,
                                   blit=False,
                                   repeat=False,
                                   )

    if save_animation:
        anim.save(os.path.join('gifs', file_name), writer=animation.PillowWriter(fps=30))
    plt.show()

    total_dist_moved = 0
    # Compare cost maps
    for i, j in zip(orig_obstacles, polygons):
        pos = (j.body.position.x, j.body.position.y)
        area = j.area
        if area > 4:
            total_dist_moved = a_star.dist(i['centre'], pos) * (area/2) + total_dist_moved

    print('TOTAL DIST MOVED', total_dist_moved)
    print('Old/new path counts', '\n\told path', path.old_path_cnt, '\n\tnew path', path.new_path_cnt)

    shutdown_event.set()
    print('\n...done with process')
    gen_path_process.join()
    print('Completed multiprocessing')

    # get response from user for saving costmap
    if save_costmap:
        costmap_obj.save_to_disk()
    return total_dist_moved, init_plan_time
예제 #13
0
 def set_length(self, new_length):
     self.cyl_vertices, self.cyl_indices, self.cyl_normals = \
         Primitives.cyl_array(new_length)
     super(RevoluteJoint, self).set_length(new_length)
예제 #14
0
def main(write):
    """
    Currently the different primitives and names are tracked in lists an arrays. Kemp and Regier 
    do it by using 3D arrays, I did that to. 

    The way it is generating contains a lot of double looping but generates the relational matrixes in
    the exact same order als Kemp and Regier which made comparing them easier. Now that it works the 
    can be made more efficiently again. It is on the list. 
    """

    def append_category(name, matrix, score):
        """
        Should do more in the future when these components are stored separatly.
        Written inside the function so that the variable category can be accessed easily.
        """
        if sum(sum(matrix)) == 0:
            return
        categories.append((name, matrix, score))
        return 

    primitives = Primitives()
    rules = Rules()
    ts = Tools()

    # Load the initial primitives
    singles = [(name, ts.filter_familymembers(matrix), score) for name, matrix, score in primitives.singles]
    doubles =  [(name, ts.filter_familymembers(matrix), score) for name, matrix, score in primitives.doubles]

#   Concepts contains the categories generated by previous rounds while categories contains all the newly generated categories
#   as well. This is to prevent that categories generated in for example round two aren't immediately used for generating. 
    categories = doubles.copy()
    concepts = categories.copy()

    close_max = 12
    depth = 3
    ego = False

    for depth in range(depth): 
        start_time = time.clock()
        print("\nStarting depth {} with {} categories".format(depth+1, len(concepts)))

        for name_d, matrix_d, score_d in concepts:
            # One binary (self refering rules)
            name_inv = [name_d, name_d, "inv"]
            name_symm_clos = [name_d, name_d, "sy_clos"]
            name_tran_clos = [name_d, name_d, "tr_clos"]
            score = 1 + score_d
            append_category(name_inv, rules.inverse(matrix_d), score)
            append_category(name_symm_clos, rules.symmetric_closure(matrix_d), score)
            append_category(name_tran_clos, rules.transitive_closure(matrix_d, close_max), score)

        for name_s, matrix_s, score_s in singles:
            # Binary - Unary (A(x,y) -- B(x) case)
            for name_d, matrix_d, score_d in concepts:
                score = 1 + score_d + score_s
                name_conj_x = [name_s, name_d, "conjx"]
                name_conj_y = [name_d, name_s, "conjy"]
                matrix_conj_x, matrix_conj_y = rules.conjunction(matrix_d, matrix_s, "singles")
                append_category(name_conj_x, matrix_conj_x, score)
                append_category(name_conj_y, matrix_conj_y, score)


                # Disjunctive rules
                name_disj_x = [name_d, name_s, "disjx"]
                name_disj_y = [name_d, name_s, "disjy"]
                matrix_disj_x, matrix_disj_y = rules.disjunction(matrix_d, matrix_s, "singles")
                append_category(name_conj_x, matrix_disj_x, score)
                append_category(name_conj_y, matrix_disj_y, score)
                

        i = 0
        for name_d, matrix_d, score_d in concepts:
            # Two Binary rules (A(x,y) --- B(x,y))
            j = 0
            for name_d2, matrix_d2, score_d2 in concepts:
                score = 1 + score_d + score_d2
                if j > i:
                    # Conjuctive rule.
                    name_conj = [name_d, name_d2, "conj"]
                    append_category(name_conj, rules.conjunction(matrix_d, matrix_d2), score)

                    # Disjuctive rule.
                    name_disj = [name_d, name_d2, "disj"]
                    append_category(name_disj, rules.disjunction(matrix_d, matrix_d2), score)

                # Transtive rule.
                name_tran = [name_d, name_d2, "tran"]
                append_category(name_tran, rules.transitive(matrix_d, matrix_d2), score)
                j +=1
            i +=1

        # Filter by ego at depth 3 (or depth == 2)
        if depth == 2:
            ego = True

        print("Total categories:", len(categories))

        categories = ts.filter_by_score(categories, ego)
        end_time = time.clock()
        concepts = categories.copy()

        print("Executing depth {} took {}.".format(depth+1, end_time-start_time))
        print("Remaining categories:", len(categories))

        if write == "True":
            ts.write_matrix_to_file(categories, path_matrix="concepts/bn_matrix_depth_" + str(depth+1), path_names="concepts/bn_names_depth_" + str(depth+1))
예제 #15
0
 def set_length(self, new_length):
     self.arr_vertices, self.arr_indices, self.arr_normals = Primitives.arr_array(new_length)
예제 #16
0
 def set_length(self, new_length):
     self.box_vertices, self.box_normals = \
         Primitives.box_array(new_length)
     super(PrismaticJoint, self).set_length(new_length)
예제 #17
0
def gen_path(queue_state: Queue,
             pipe_path: connection.Pipe,
             shutdown_event: Event,
             ship: Ship,
             prim: Primitives,
             costmap: CostMap,
             swath_dict: Swath,
             a_star: AStar,
             goal_pos: Tuple,
             horizon: int = np.inf,
             smooth_path: bool = False) -> None:
    planner_times = [
    ]  # list to keep track of time it takes for a* to find a path at each step
    while not shutdown_event.is_set():
        try:
            state_data = queue_state.get(block=True,
                                         timeout=1)  # blocking call
            print('\nReceived state data!')

            # update ship initial heading
            ship.body.angle = state_data['ship_body_angle']
            ship.initial_heading = -ship.body.angle + a_star.first_initial_heading

            # get new ship pos and computed snapped goal
            ship_pos = state_data['ship_pos']
            curr_goal = (goal_pos[0], min(goal_pos[1],
                                          (ship_pos[1] + horizon)),
                         goal_pos[2])
            snapped_goal = snap_to_lattice(
                ship_pos,
                curr_goal,
                ship.initial_heading,
                ship.turning_radius,
                prim.num_headings,
                abs_init_heading=ship.initial_heading)

            # update costmap
            costmap.cost_map = state_data['costmap']
            costmap.obstacles = state_data['obstacles']

            # update primitives and update swath
            prim.rotate(-ship.body.angle, orig=True)
            new_swath_dict = update_swath(theta=-ship.body.angle,
                                          swath_dict=swath_dict)

            # get a rough idea of planner speed
            print('Generating next path...')
            t0 = time.time()

            # compute path to goal
            _, new_path, nodes_visited, x1, y1, x2, y2, _ = \
                a_star.search(ship_pos, snapped_goal, new_swath_dict, smooth_path=smooth_path)

            # save time to list and update average frequency
            dt = time.time() - t0
            planner_times.append(dt)
            print('Time elapsed: ', dt)
            print('Average planner frequency: {:.4f} Hz'.format(
                1 / (sum(planner_times) / len(planner_times))))

            if new_path != 'Fail' and len(new_path) > 1:
                # sample points along path
                full_path = get_points_on_path(new_path,
                                               prim.num_headings,
                                               ship.initial_heading,
                                               ship.turning_radius,
                                               eps=1e-3)
                # send new path and node information to pipe
                print('Sending...')
                pipe_path.send({  # blocking call
                    'path': np.asarray(full_path),
                    'path_nodes': (x1, y1),
                    'smoothing_nodes': (x2, y2),
                    'nodes_expanded': nodes_visited
                })
                print('Sent path!')

        except queue.Empty:
            # nothing in queue so try again
            time.sleep(0.001)

        except ValueError as err:
            print("Queue closed: {}".format(err))
            break

    pipe_path.close()