def find_second_closest(path, x, y, index_star):
one = dy.int32(1)
ip1 = index_star + one
x_test_ip1 = dy.memory_read(memory=path['X'], index=ip1)
y_test_ip1 = dy.memory_read(memory=path['Y'], index=ip1)
distance_ip1 = distance_between(x, y, x_test_ip1, y_test_ip1)
im1 = index_star - one
x_test_im1 = dy.memory_read(memory=path['X'], index=im1)
y_test_im1 = dy.memory_read(memory=path['Y'], index=im1)
distance_im1 = distance_between(x, y, x_test_im1, y_test_im1)
which = distance_ip1 > distance_im1
second_clostest_distance = distance_ip1
second_clostest_distance = dy.conditional_overwrite(
second_clostest_distance, condition=which, new_value=distance_im1)
index_second_star = ip1
index_second_star = dy.conditional_overwrite(index_second_star,
condition=which,
new_value=im1)
return second_clostest_distance, index_second_star
def sample_path(path, index):
y_r = dy.memory_read(memory=path['Y'], index=index)
x_r = dy.memory_read(memory=path['X'], index=index)
psi_r = dy.memory_read(memory=path['PSI'], index=index)
K_r = dy.memory_read(memory=path['K'], index=index)
return x_r, y_r, psi_r, K_r
def sample_path_xy(path, index):
"""
Read a sample (x,y) of the given path at a given array-index
"""
if path['buffer_type'] == 'dy.memory':
y = dy.memory_read(memory=path['Y'], index=index)
x = dy.memory_read(memory=path['X'], index=index)
return x, y
elif path['buffer_type'] == 'circular_buffer':
x = cb.read_from_absolute_index(path['X'], index)
y = cb.read_from_absolute_index(path['Y'], index)
return x, y
def sample_path_finite_difference(path, index):
y1 = dy.memory_read(memory=path['Y'], index=index)
y2 = dy.memory_read(memory=path['Y'], index=index + dy.int32(1))
x1 = dy.memory_read(memory=path['X'], index=index)
x2 = dy.memory_read(memory=path['X'], index=index + dy.int32(1))
Delta_x = x2 - x1
Delta_y = y2 - y1
psi_r = dy.atan2(Delta_y, Delta_x)
x_r = x1
y_r = y1
return x_r, y_r, psi_r
def sample_path(path, index):
"""
Read a sample of the given path at a given array-index
"""
if path['buffer_type'] == 'dy.memory':
d = dy.memory_read(memory=path['D'], index=index)
x = dy.memory_read(memory=path['X'], index=index)
y = dy.memory_read(memory=path['Y'], index=index)
psi = dy.memory_read(memory=path['PSI'], index=index)
K = dy.memory_read(memory=path['K'], index=index)
return d, x, y, psi, K
elif path['buffer_type'] == 'circular_buffer':
d = cb.read_from_absolute_index(path['D'], index)
x = cb.read_from_absolute_index(path['X'], index)
y = cb.read_from_absolute_index(path['Y'], index)
psi = cb.read_from_absolute_index(path['PSI'], index)
K = cb.read_from_absolute_index(path['K'], index)
return d, x, y, psi, K
def tracker(path, x, y):
index_track = dy.signal()
with dy.sub_loop(max_iterations=1000) as system:
search_index_increment = dy.int32(
1) # positive increment assuming positive velocity
Delta_index = dy.sum(search_index_increment, initial_state=-1)
Delta_index_previous_step = Delta_index - search_index_increment
x_test = dy.memory_read(memory=path['X'],
index=index_track + Delta_index)
y_test = dy.memory_read(memory=path['Y'],
index=index_track + Delta_index)
distance = distance_between(x_test, y_test, x, y)
distance_previous_step = dy.delay(distance, initial_state=100000)
minimal_distance_reached = distance_previous_step < distance
# introduce signal names
distance.set_name('tracker_distance')
minimal_distance_reached.set_name('minimal_distance_reached')
# break condition
system.loop_until(minimal_distance_reached)
# return
system.set_outputs([Delta_index_previous_step, distance_previous_step])
Delta_index = system.outputs[0].set_name('tracker_Delta_index')
distance = system.outputs[1].set_name('distance')
index_track_next = index_track + Delta_index
index_track << dy.delay(index_track_next, initial_state=1)
return index_track_next, Delta_index, distance
def sample_path_d(path, index):
"""
Read a sample (d) of the given path at a given array-index
"""
if path['buffer_type'] == 'dy.memory':
d = dy.memory_read(memory=path['D'], index=index)
return d
elif path['buffer_type'] == 'circular_buffer':
d = cb.read_from_absolute_index(path['D'], index)
return d
def path_horizon_head_index(path):
"""
Get the current head-index position in the horizon and the distance at the head
"""
if path['buffer_type'] == 'dy.memory':
head_index = dy.int32( path['samples'] - 1 )
distance_at_the_end_of_horizon = dy.memory_read( memory=path['D'], index=head_index )
elif path['buffer_type'] == 'circular_buffer':
head_index = cb.get_current_absolute_write_index(path['D']) - 1
distance_at_the_end_of_horizon = cb.read_from_absolute_index(path['D'], head_index)
return head_index, distance_at_the_end_of_horizon
def path_horizon_tail_index(path):
"""
Get the current tail-index position in the horizon and the distance at the tail
"""
if path['buffer_type'] == 'dy.memory':
tail_index = dy.int32(0)
distance_at_the_begin_of_horizon = dy.memory_read(memory=path['D'],
index=tail_index)
elif path['buffer_type'] == 'circular_buffer':
tail_index = cb.get_absolute_minimal_index(path['D'])
distance_at_the_begin_of_horizon = cb.read_from_absolute_index(
path['D'], tail_index)
return tail_index, distance_at_the_begin_of_horizon
if testname == 'memory':
import math
#vector = np.linspace(-1.11, 2.22, 400)
it = np.linspace(0,1, 400)
vector = np.sin( 10 * it * math.pi ) + it
data = dy.memory(datatype=dy.DataTypeFloat64(1), constant_array=vector ).set_name('data')
looked_up_element = dy.memory_read( memory=data, index=dy.counter() ).set_name('looked_up_element')
looked_up_element_delayed = dy.delay( dy.memory_read( memory=data, index=dy.counter() ) ).set_name('looked_up_element_delayed')
output_signals = [ looked_up_element, looked_up_element_delayed ]
if testname == 'memory_machine':
vector = [ 1, 2, 3, 4, 5, 6, 7, 8, 9, 3 ]
data = dy.memory(datatype=dy.DataTypeInt32(1), constant_array=vector ).set_name('data')
position_index = dy.signal()
next_position = dy.memory_read( memory=data, index=position_index )
def J(index):
d_test = dy.memory_read(memory=path['D'], index=index)
distance = dy.abs(d_test - target_distance)
return distance
def tracker_distance_ahead(path, current_index, distance_ahead):
"""
<----- Delta_index_track ----->
array: X X X X X X X X X X X X X X X
^
current_index
"""
if 'Delta_d' in path:
# constant sampling interval in distance
# computation can be simplified
pass
target_distance = dy.float64(distance_ahead) + dy.memory_read(
memory=path['D'], index=current_index)
def J(index):
d_test = dy.memory_read(memory=path['D'], index=index)
distance = dy.abs(d_test - target_distance)
return distance
Delta_index_track = dy.signal()
# initialize J_star
J_star_0 = J(current_index + Delta_index_track)
J_star_0.set_name('J_star_0')
#
# compute the direction in which J has its decent
# if true: with increasing index J increases --> decrease search index
# if false: with increasing index J decreases --> increase search index
#
J_next_index = J(current_index + Delta_index_track + dy.int32(1))
J_Delta_to_next_index = J_next_index - J_star_0
direction_flag = J_Delta_to_next_index > dy.float64(0)
search_index_increment = dy.int32(1)
search_index_increment = dy.conditional_overwrite(search_index_increment,
direction_flag,
dy.int32(-1))
search_index_increment.set_name('search_index_increment')
# loop to find the minimum of J
with dy.sub_loop(max_iterations=1000) as system:
# J_star(k) - the smallest J found so far
J_star = dy.signal()
# inc- / decrease the search index
Delta_index_prev_it, Delta_index = dy.sum2(search_index_increment,
initial_state=0)
Delta_index.set_name('Delta_index')
# sample the cost function and check if it got smaller in this step
J_to_verify = J(current_index + Delta_index_track + Delta_index)
J_to_verify.set_name('J_to_verify')
step_caused_improvment = J_to_verify < J_star
# replace the
J_star_next = dy.conditional_overwrite(J_star, step_caused_improvment,
J_to_verify)
# state for J_star
J_star << dy.delay(J_star_next,
initial_state=J_star_0).set_name('J_star')
# loop break condition
system.loop_until(dy.logic_not(step_caused_improvment))
# return the results computed in the loop
system.set_outputs([Delta_index_prev_it, J_to_verify, J_star])
Delta_index = system.outputs[0]
Delta_index_track_next = Delta_index_track + Delta_index
Delta_index_track << dy.delay(Delta_index_track_next, initial_state=0)
Delta_index_track.set_name('Delta_index_track')
# compute the residual distance
optimal_distance = dy.memory_read(memory=path['D'],
index=current_index +
Delta_index_track_next)
distance_residual = target_distance - optimal_distance
return Delta_index_track_next, distance_residual, Delta_index
