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_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 _get_line_segment(path, x, y, index_star):
"""
Given the index of the clostest point, compute the index of the 2nd clostest point.
"""
one = dy.int32(1)
x_star, y_star = sample_path_xy(path, index=index_star)
x_test_ip1, y_test_ip1 = sample_path_xy(path, index=index_star + one)
distance_ip1 = distance_between(x, y, x_test_ip1, y_test_ip1)
x_test_im1, y_test_im1 = sample_path_xy(path, index=index_star - one)
distance_im1 = distance_between(x, y, x_test_im1, y_test_im1)
# find out which point is the 2nd closest
# which = True means that the point referred by the index index_star - 1 is the 2nd closest
# which = False means that the point referred by the index index_star + 1 is the 2nd closest
which = distance_ip1 > distance_im1
second_clostest_distance = dy.conditional_overwrite(distance_ip1,
condition=which,
new_value=distance_im1)
index_second_star = dy.conditional_overwrite(index_star + one,
condition=which,
new_value=index_star - one)
#
i_s = dy.conditional_overwrite(index_star,
condition=which,
new_value=index_star - one)
i_e = dy.conditional_overwrite(index_star + one,
condition=which,
new_value=index_star)
#
# get start/end xy-points of the line segment which is closest
# the line is described by (x_s, y_s) --> (x_e, y_e)
#
# find start point (x_s, y_s)
x_s = dy.conditional_overwrite(x_star,
condition=which,
new_value=x_test_im1)
y_s = dy.conditional_overwrite(y_star,
condition=which,
new_value=y_test_im1)
# find stop point (x_e, y_e)
x_e = dy.conditional_overwrite(x_test_ip1,
condition=which,
new_value=x_star)
y_e = dy.conditional_overwrite(y_test_ip1,
condition=which,
new_value=y_star)
return i_s, i_e, x_s, y_s, x_e, y_e, index_second_star, second_clostest_distance
def tracker(path, x, y):
"""
Continuously project the point (x, y) onto the given path (closest distance)
This is an internal function. C.f. track_projection_on_path for details and assumptions.
returns in structure tracking_results:
tracked_index - the index in the path array for the closest distance to (x, y)
Delta_index - the change of the index to the previous lookup
distance - the absolute value of the closest distance of (x, y) to the path
reached_the_end_of_currently_available_path_data
- reached the end of the path
"""
#
# define the optimization problem
#
# pack parameters
par = (x, y)
def J(path, index, par):
# unpack parameters
x, y = par
# cost function J: index -> distance
x_test, y_test = sample_path_xy(path, index)
distance = distance_between(x_test, y_test, x, y)
# cost
return distance
#
# continuous optimization
#
results = continuous_optimization_along_path(path, dy.int32(0), J, par)
#
distance = results['J_star']
minimal_distance_reached = results['reached_minimum']
#
tracking_results = dy.structure()
tracking_results['tracked_index'] = results['optimal_index']
tracking_results['Delta_index'] = results['Delta_index']
tracking_results['distance'] = distance
tracking_results['minimal_distance_reached'] = minimal_distance_reached
tracking_results[
'reached_the_end_of_currently_available_path_data'] = results[
'reached_the_end_of_currently_available_path_data']
return tracking_results
def generate_signal_PWM(period, modulator):
number_of_samples_to_stay_in_A = period * modulator
number_of_samples_to_stay_in_B = period * (dy.float64(1) - modulator)
number_of_samples_to_stay_in_A.set_name('number_of_samples_to_stay_in_A')
number_of_samples_to_stay_in_B.set_name('number_of_samples_to_stay_in_B')
with dy.sub_statemachine("statemachine1") as switch:
with switch.new_subsystem('state_on') as system:
on = dy.float64(1.0).set_name('on')
counter = dy.counter().set_name('counter')
timeout = (counter >=
number_of_samples_to_stay_in_A).set_name('timeout')
next_state = dy.conditional_overwrite(
signal=dy.int32(-1), condition=timeout,
new_value=1).set_name('next_state')
system.set_switched_outputs([on], next_state)
with switch.new_subsystem('state_off') as system:
off = dy.float64(0.0).set_name('off')
counter = dy.counter().set_name('counter')
timeout = (counter >=
number_of_samples_to_stay_in_B).set_name('timeout')
next_state = dy.conditional_overwrite(
signal=dy.int32(-1), condition=timeout,
new_value=0).set_name('next_state')
system.set_switched_outputs([off], next_state)
# define the outputs
pwm = switch.outputs[0].set_name("pwm")
state_control = switch.state.set_name('state_control')
return pwm, state_control
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
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
output_signals = [ y ]
if testname == 'test_ramp':
y1 = dy.float64(3) * dy.signal_ramp(10) - dy.float64(2) * dy.signal_ramp(70)
y2 = dy.float64(-5) * dy.signal_ramp(40)
y1.set_name('y1')
output_signals = [ y1, y2 ]
if testname == 'test_datatype_convertion':
y = dy.int32(333)
y = dy.convert(y, dy.DataTypeFloat64(1) )
output_signals = [ y ]
if testname == 'dtf_lowpass_1_order':
u = dy.float64(1.0)
y = dy.dtf_lowpass_1_order(u, z_infinity=0.95 )
output_signals = [ y ]
# estimate the travelled distance by integration of the vehicle velocity
d_hat = dy.euler_integrator(
velocity, Ts, initial_state=0) + path_distance_start_open_loop_control
# estimated travelled distance (d_hat) to path-index
open_loop_index, _, _ = tracker_distance_ahead(
path,
current_index=path_index_start_open_loop_control,
distance_ahead=d_hat)
dy.append_output(open_loop_index, 'open_loop_index')
# get the reference orientation and curvature
_, _, _, psi_rr, K_r = sample_path(path, index=open_loop_index +
dy.int32(1)) # new sampling
#
# compute an enhanced (less noise) signal for the path orientation psi_r by integrating the
# curvature profile and fusing the result with psi_rr to mitigate the integration drift.
#
psi_r, psi_r_dot = compute_path_orientation_from_curvature(Ts,
velocity,
psi_rr,
K_r,
L=1.0)
dy.append_output(psi_rr, 'psi_rr')
dy.append_output(psi_r_dot, 'psi_r_dot')
def continuous_optimization_along_path(path, current_index, J, par):
"""
Minimize the given cost function by varying the index of the path array
<----- 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
# get the highest available array index in the horizon
index_head, _ = path_horizon_head_index(path)
#
#
#
Delta_index_track = dy.signal()
# initialize J_star
J_star_0 = J(path, current_index + Delta_index_track, par)
#
# compute the direction (gradient) 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_prev_index = J(path, current_index + Delta_index_track - 1, par)
J_Delta_to_next_index = J_star_0 - J_prev_index
search_index_increment = dy.conditional_overwrite(
dy.int32(1), J_Delta_to_next_index > 0, dy.int32(-1))
# loop to find the minimum of J
with dy.sub_loop(max_iterations=1000,
subsystem_name='optim_loop') as system:
# J_star(k) - the smallest J found so far
J_star = dy.signal()
# inc- / decrease the search index
Delta_index_previous_step, Delta_index = dy.sum2(
search_index_increment, initial_state=0)
index_to_investigate = current_index + Delta_index_track + Delta_index
# sample the cost function and check if it got smaller in this step
J_to_verify = J(path, index_to_investigate, par)
step_caused_improvement = J_to_verify < J_star
# in case the step yielded a lower cost, replace the prev. minimal cost
J_star_next = dy.conditional_overwrite(J_star, step_caused_improvement,
J_to_verify)
# state for J_star
J_star << dy.delay(J_star_next, initial_state=J_star_0)
#
# loop break conditions
#
# when reaching the end of the available data, stop the loop and indicate the need for extending the horizon
reached_the_end_of_currently_available_path_data = index_to_investigate >= index_head # reached the end of the input data?
# similarly check for the begin ...
reached_the_begin_of_currently_available_path_data = index_to_investigate - 1 <= path_horizon_tail_index(
path)[0]
# in case the iteration did not reduce the cost, assume that the minimum was reached in the prev. iteration
reached_minimum = dy.logic_not(step_caused_improvement)
system.loop_until(
dy.logic_or(
dy.logic_or(reached_minimum,
reached_the_end_of_currently_available_path_data),
reached_the_begin_of_currently_available_path_data).set_name(
'loop_until'))
# assign signals names to appear in the generated source code
J_star_0.set_name('J_star_0')
search_index_increment.set_name('search_index_increment')
J_star.set_name('J_star')
Delta_index.set_name('Delta_index')
index_head.set_name('index_head')
index_to_investigate.set_name('index_to_investigate')
J_to_verify.set_name('J_to_verify')
step_caused_improvement.set_name('step_caused_improvement')
# return
outputs = dy.structure()
outputs['Delta_index'] = Delta_index_previous_step
outputs['J_star'] = J_star_next
outputs['reached_minimum'] = reached_minimum
outputs[
'reached_the_end_of_currently_available_path_data'] = reached_the_end_of_currently_available_path_data
outputs['index_head'] = index_head * 1
outputs['index_to_investigate'] = index_to_investigate
outputs['J_to_verify'] = J_to_verify
system.set_outputs(outputs.to_list())
outputs.replace_signals(system.outputs)
Delta_index = outputs['Delta_index']
J_star = outputs['J_star']
reached_minimum = outputs['reached_minimum']
reached_the_end_of_currently_available_path_data = outputs[
'reached_the_end_of_currently_available_path_data']
# Introduce dy.sink(signal) in ORTD to ensure the given signals is not optimized out and becomes visible in the debugging traces
dummy = 0 * outputs['index_head'] + 0 * outputs[
'index_to_investigate'] + 0 * outputs['J_to_verify']
Delta_index_track_next = Delta_index_track + Delta_index
Delta_index_track << dy.delay(
Delta_index_track_next, initial_state=1
) # start at 1 so that the backwards gradient can be computed at index=1
Delta_index_track.set_name('Delta_index_track')
# optimal index
optimal_index = current_index + Delta_index_track_next
results = dy.structure()
results['optimal_index'] = optimal_index
results['J_star'] = J_star + 0 * dummy
results['Delta_index'] = Delta_index
results['Delta_index_track_next'] = Delta_index_track_next
results['reached_minimum'] = reached_minimum
results[
'reached_the_end_of_currently_available_path_data'] = reached_the_end_of_currently_available_path_data
return results
if advanced_control:
Delta_index_ahead, distance_residual, Delta_index_ahead_i1 = tracker_distance_ahead(path, current_index=tracked_index, distance_ahead=distance_ahead)
dy.append_output(distance_residual, 'distance_residual')
dy.append_output(Delta_index_ahead_i1, 'Delta_index_ahead_i1')
dy.append_output(Delta_index_ahead, 'Delta_index_ahead')
# verify
if False:
index_closest_compare, distance_compare, index_start_compare = lookup_closest_point( N=path['samples'], path_distance_storage=path['D'], path_x_storage=path['X'], path_y_storage=path['Y'], x=x, y=y )
dy.append_output(index_closest_compare, 'index_closest_compare')
# get the reference
x_r, y_r, psi_rr, K_r = sample_path(path, index=tracked_index + dy.int32(1) ) # new sampling
# x_r, y_r, psi_rr = sample_path_finite_difference(path, index=tracked_index ) # old sampling
# compute the noise-reduced path orientation Psi_r from curvature
#
# NOTE: velocity: should be the projection of the vehicle velocity on the path?
#
psi_r, psi_r_dot = compute_path_orientation_from_curvature( Ts, velocity, psi_rr, K_r, L=1.0 )
dy.append_output(psi_rr, 'psi_rr')
dy.append_output(psi_r_dot, 'psi_r_dot')
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
# # lateral open-loop control to realize an 'obstacle-avoiding maneuver' # # the dynamic model for the lateral distance Delta_l is # # d/dt Delta_l = u, # # meaning u is the lateral velocity to which is used to control the lateral # distance to the path. # # generate a velocity profile u_move_left = dy.signal_step( dy.int32(50) ) - dy.signal_step( dy.int32(200) ) u_move_right = dy.signal_step( dy.int32(500) ) - dy.signal_step( dy.int32(350) ) # apply a rate limiter to limit the acceleration u = dy.rate_limit( max_lateral_velocity * (u_move_left + u_move_right), Ts, dy.float64(-1) * max_lateral_accleration, max_lateral_accleration) dy.append_output(u, 'u') # internal lateral model (to verify the lateral dynamics of the simulated vehicle) Delta_l_mdl = dy.euler_integrator(u, Ts) dy.append_output(Delta_l_mdl, 'Delta_l_mdl')
# some modification of one input
U = U2 * dy.float64(1.234)
U.set_name("stachmachine_input_U")
with dy.sub_statemachine( "statemachine1" ) as switch:
with switch.new_subsystem('state_A') as system:
# implement a dummy system the produces zero values for x and v
x = dy.float64(0.0).set_name('x_def')
v = dy.float64(0.0).set_name('v_def')
counter = dy.counter().set_name('counter')
timeout = ( counter > number_of_samples_to_stay_in_A ).set_name('timeout')
next_state = dy.conditional_overwrite(signal=dy.int32(-1), condition=timeout, new_value=1 ).set_name('next_state')
system.set_switched_outputs([ x, v, counter ], next_state)
with switch.new_subsystem('state_B') as system:
# implement a simple spring-mass oscillator:
# x is the position, v is the velocity, acc is the acceleration
# create placeholder symbols for x and v (as they are used before being defined)
x = dy.signal()
v = dy.signal()
acc = dy.add( [ U, v, x ], [ 1, -0.1, -0.1 ] ).set_blockname('acc').set_name('acc')
def tracker(path, x, y):
"""
Continuously project the point (x, y) onto the given path (closest distance)
This is an internal function. C.f. track_projection_on_path for details and assumptions.
returns in structure tracking_results:
tracked_index - the index in the path array for the closest distance to (x, y)
Delta_index - the change of the index to the previous lookup
distance - the absolute value of the closest distance of (x, y) to the path
reached_the_end_of_currently_available_path_data
- reached the end of the path
"""
index_head, _ = path_horizon_head_index(path)
index_head.set_name('index_head')
# the index that currently describes the index on the path with the closest distance to (x,y)
index_track = dy.signal().set_name('index_track')
with dy.sub_loop( max_iterations=200, subsystem_name='tracker_loop' ) 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
# the index at which to compute the distance to and see if it is the minimum
index_to_investigate = index_track + Delta_index
x_test, y_test = sample_path_xy(path, index_to_investigate)
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('distance')
minimal_distance_reached.set_name('minimal_distance_reached')
# break condition
reached_the_end_of_currently_available_path_data = index_to_investigate >= index_head # reached the end of the input data?
system.loop_until( dy.logic_or( minimal_distance_reached, reached_the_end_of_currently_available_path_data ) )
# return
system.set_outputs([
Delta_index_previous_step.set_name('Delta_index_previous_step'),
distance_previous_step.set_name('distance_previous_step'),
minimal_distance_reached.set_name('minimal_distance_reached'),
reached_the_end_of_currently_available_path_data.set_name('reached_the_end_of_currently_available_path_data')
])
Delta_index = system.outputs[0].set_name('tracker_Delta_index')
distance = system.outputs[1].set_name('distance')
minimal_distance_reached = system.outputs[2].set_name('minimal_distance_reached')
reached_the_end_of_currently_available_path_data = system.outputs[3].set_name('reached_the_end_of_currently_available_path_data')
# update current state of index_track
index_track_next = index_track + Delta_index
index_track << dy.delay(index_track_next, initial_state=1)
#
tracking_results = dy.structure()
tracking_results['tracked_index'] = index_track_next
tracking_results['Delta_index'] = Delta_index
tracking_results['distance'] = distance
tracking_results['minimal_distance_reached'] = minimal_distance_reached
tracking_results['reached_the_end_of_currently_available_path_data'] = reached_the_end_of_currently_available_path_data
# return index_track_next, Delta_index, distance, minimal_distance_reached, reached_the_end_of_currently_available_path_data
return tracking_results
