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)
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)
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)
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 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)
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)
def set_length(self, new_length):
self.box_vertices, self.box_normals = Primitives.box_array(new_length)
super(PrismaticJoint, self).set_length(new_length)
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)
def set_length(self, new_length):
self.arr_vertices, self.arr_indices, self.arr_normals = \
Primitives.arr_array(new_length)
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)
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
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)
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))
def set_length(self, new_length):
self.arr_vertices, self.arr_indices, self.arr_normals = Primitives.arr_array(new_length)
def set_length(self, new_length):
self.box_vertices, self.box_normals = \
Primitives.box_array(new_length)
super(PrismaticJoint, self).set_length(new_length)
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()
