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 _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 distance_to_Delta_l(distance, psi_r, x_r, y_r, x, y):
psi_tmp = dy.atan2(y - y_r, x - x_r)
delta_angle = dy.unwrap_angle(psi_r - psi_tmp, normalize_around_zero=True)
sign = dy.conditional_overwrite(dy.float64(1.0),
delta_angle > dy.float64(0), -1.0)
Delta_l = distance * sign
return Delta_l
def _distance_to_Delta_l( distance, psi_r, x_r, y_r, x, y ):
"""
Add sign information to a closest distance measurement
"""
psi_tmp = dy.atan2(y - y_r, x - x_r)
delta_angle = dy.unwrap_angle( psi_r - psi_tmp, normalize_around_zero=True )
sign = dy.conditional_overwrite(dy.float64(1.0), delta_angle > dy.float64(0) , -1.0 )
Delta_l = distance * sign
return Delta_l
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
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: # NOTE: do not put c++ keywords as system names
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 > dy.int32(10) ).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:
x = dy.signal()
v = dy.signal()
acc = dy.add( [ U, v, x ], [ 1, -0.1, -0.1 ] ).set_blockname('acc').set_name('acc')
v << dy.euler_integrator( acc, Ts=0.1, initial_state=-1.0 )
x << dy.euler_integrator( v, Ts=0.1 )
counter = dy.counter().set_name('counter')
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')
# feedback of internal model
psi_mdl = dy.signal()
# switch between IMU feedback and internal model
psi_feedback = psi_mdl
psi_feedback = dy.conditional_overwrite(psi_feedback, activate_IMU,
psi_measurement)
# path tracking
Delta_u = dy.float64(0.0)
steering = psi_r - psi_feedback + Delta_u
steering = dy.unwrap_angle(angle=steering, normalize_around_zero=True)
dy.append_output(Delta_u, 'Delta_u')
# internal model of carbody rotation (from bicycle model)
psi_mdl << dy.euler_integrator(
velocity * dy.float64(1.0 / wheelbase) * dy.sin(steering),
Ts,
initial_state=psi_measurement)
dy.append_output(psi_mdl, 'psi_mdl')
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
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
