def lateral_vehicle_model(u_delta,
v,
v_dot,
Ts,
wheelbase,
x0=0.0,
y0=0.0,
psi0=0.0,
delta0=0.0,
delta_disturbance=None):
# add disturbance to steering
if delta_disturbance is not None:
delta_ = u_delta + delta_disturbance
else:
delta_ = u_delta
# saturate steering
delta = dy.saturate(u=delta_,
lower_limit=-math.pi / 2.0,
upper_limit=math.pi / 2.0)
# bicycle model
x, y, psi, x_dot, y_dot, psi_dot = discrete_time_bicycle_model(
delta, v, Ts, wheelbase, x0, y0, psi0)
#
delta_dot = dy.diff(delta, initial_state=delta) / dy.float64(Ts)
delta = dy.delay(delta, delta0)
# compute acceleration in the point (x,y) espessed in the vehicle frame
a_lat, a_long = compute_accelearation(v, v_dot, delta, delta_dot, psi_dot)
return x, y, psi, delta, delta_dot, a_lat, a_long, x_dot, y_dot, psi_dot
def test_basic_code_gen():
# create a new system
system = dy.enter_system()
# define system inputs
input1 = dy.system_input(dy.DataTypeFloat64(1),
name='input1',
default_value=5.0,
value_range=[0, 25],
title="input #1")
# the diagram
tmp = input1 * dy.float64(2.0)
tmp.set_name("some_name")
output = dy.delay(tmp)
# define output(s)
dy.append_output(output, 'outputs')
# generate code
code_gen_results = dy.generate_code(template=dy.TargetWasm(),
folder="generated/",
build=False)
sourcecode, manifest = code_gen_results['sourcecode'], code_gen_results[
'manifest']
# clear workspace
dy.clear()
def lateral_vehicle_model(u_delta, v, v_dot, Ts, wheelbase, x0=0.0, y0=0.0, psi0=0.0, delta0=0.0, delta_disturbance=None, delta_factor=None):
"""
Bicycle model and the acceleration at the front axle
"""
# add malicious factor to steering
if delta_factor is not None:
delta_ = u_delta * delta_factor
else:
delta_ = u_delta
# add disturbance to steering
if delta_disturbance is not None:
delta_ = delta_ + delta_disturbance
# saturate steering
delta = dy.saturate(u=delta_, lower_limit=-math.pi/2.0, upper_limit=math.pi/2.0)
# bicycle model
x, y, psi, x_dot, y_dot, psi_dot = discrete_time_bicycle_model(delta, v, Ts, wheelbase, x0, y0, psi0)
#
delta_dot = dy.diff( delta, initial_state=delta ) / dy.float64(Ts)
delta = dy.delay( delta, delta0 )
# compute acceleration in the point (x,y) in the vehicle frame
a_lat, a_long = compute_acceleration( v, v_dot, delta, delta_dot, psi_dot )
return x, y, psi, delta, delta_dot, a_lat, a_long, x_dot, y_dot, psi_dot
def dInt( u , name : str = ''):
yFb = dy.signal()
i = dy.add( [ yFb, u ], [ 1, 1 ] ).set_name(name + '_i')
y = dy.delay( i).set_name(name + '_y')
yFb << y
return y
def firstOrder( u, z_inf, name : str = ''):
yFb = dy.signal()
i = dy.add( [ yFb, u ], [ z_inf, 1 ] ).set_name(name + '_i')
y = dy.delay( i).set_name(name + '_y')
yFb << y
return y
def euler_integrator(u: dy.Signal, sampling_rate: float, initial_state=0.0):
yFb = dy.signal()
i = dy.add([yFb, u], [1, sampling_rate])
y = dy.delay(i, initial_state)
yFb << y
return y
def euler_integrator( u : dy.Signal, Ts : float, name : str, initial_state = None):
yFb = dy.signal()
i = dy.add( [ yFb, u ], [ 1, Ts ] ).set_name(name + '_i')
y = dy.delay( i, initial_state ).set_name(name + '_y')
yFb << y
return y
def firstOrderAndGain( u, z_inf, gain, name : str = ''):
dFb = dy.signal()
s = dy.add( [ dFb, u ], [ z_inf, 1 ] ).set_name('s'+name+'')
d = dy.delay( s).set_name('d'+name+'')
dFb << d
y = dy.gain( d, gain).set_name('y'+name+'')
return y
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 diff( u , name : str):
i = dy.delay( u ).set_name(name + '_i')
y = dy.add( [ i, u ], [ -1, 1 ] ).set_name(name + '_y')
return y
#
controlledVariableFb = dy.signal()
# control error
controlError = dy.add( [ reference, controlledVariableFb ], [ 1, -1 ] ).set_name('err')
# P
u_p = dy.operator1( [ Kp, controlError ], '*' ).set_name('u_p')
# D
d = diff( controlError, 'PID_D')
u_d = dy.operator1( [ Kd, d ], '*' ).set_name('u_d')
# I
u_i_tmp = dy.signal()
u_i = dy.delay(controlError + u_i_tmp) * Ki
u_i_tmp << u_i
u_i_tmp.set_name('u_i')
if test_modification_1:
u_i = u_i_tmp
# sum up
if not test_modification_2:
controlVar = u_p + u_d + u_i # TODO: compilation fails if u_i is removed because u_i is not connected to sth.
else:
# shall raise an error
anonymous_signal = dy.signal()
def path_lateral_modification2(
input_path,
par,
Ts,
wheelbase,
velocity,
Delta_l_r,
Delta_l_r_dot,
Delta_l_r_dotdot,
d0, x0, y0, psi0, delta0, delta_dot0
):
"""
Take an input path, modify it according to a given lateral distance profile,
and generate a new path.
Technically this combines a controller that causes an simulated vehicle to follows
the input path with defined lateral modifications.
Note: this implementation is meant as a callback routine for async_path_data_handler()
"""
# create placeholders for the plant output signals
x = dy.signal()
y = dy.signal()
psi = dy.signal()
psi_dot = dy.signal()
if 'lateral_controller' not in par:
# controller
results = path_following_controller_P(
input_path,
x, y, psi,
velocity,
Delta_l_r = Delta_l_r,
Delta_l_r_dot = Delta_l_r_dot,
Delta_l_r_dotdot = Delta_l_r_dotdot,
psi_dot = dy.delay(psi_dot),
velocity_dot = dy.float64(0),
Ts = Ts,
k_p = 1
)
else:
# path following and a user-defined linearising controller
results = path_following(
par['lateral_controller'], # callback to the implementation of P-control
par['lateral_controller_par'], # parameters to the callback
input_path,
x, y, psi,
velocity,
Delta_l_r = Delta_l_r,
Delta_l_r_dot = Delta_l_r_dot,
Delta_l_r_dotdot = Delta_l_r_dotdot,
psi_dot = dy.delay(psi_dot),
velocity_dot = dy.float64(0), # velocity_dot
Ts = Ts
)
#
# The model of the vehicle
#
with dy.sub_if( results['output_valid'], prevent_output_computation=False, subsystem_name='simulation_model') as system:
results['output_valid'].set_name('output_valid')
results['delta'].set_name('delta')
# steering angle limit
limited_steering = dy.saturate(u=results['delta'], lower_limit=-math.pi/2.0, upper_limit=math.pi/2.0)
# the model of the vehicle
x_, y_, psi_, x_dot, y_dot, psi_dot_ = discrete_time_bicycle_model(
limited_steering,
velocity,
Ts, wheelbase,
x0, y0, psi0
)
# driven distance
d = dy.euler_integrator(velocity, Ts, initial_state=d0)
# outputs
model_outputs = dy.structure(
d = d,
x = x_,
y = y_,
psi = psi_,
psi_dot = dy.delay(psi_dot_) # NOTE: delay introduced to avoid algebraic loops, wait for improved ORTD
)
system.set_outputs(model_outputs.to_list())
model_outputs.replace_signals( system.outputs )
# close the feedback loops
x << model_outputs['x']
y << model_outputs['y']
psi << model_outputs['psi']
psi_dot << model_outputs['psi_dot']
#
output_path = dy.structure({
'd' : model_outputs['d'],
'x' : model_outputs['x'],
'y' : model_outputs['y'],
'psi' : psi,
'psi_dot' : psi_dot,
'psi_r' : psi + results['delta'], # orientation angle of the path the vehicle is drawing
'K' : ( psi_dot + results['delta_dot'] ) / velocity , # curvature of the path the vehicle is drawing
'delta' : results['delta'],
'delta_dot' : results['delta_dot'],
'd_star' : results['d_star'],
'tracked_index' : results['tracked_index'],
'output_valid' : results['output_valid'],
'need_more_path_input_data' : results['need_more_path_input_data'],
'read_position' : results['read_position'],
'minimal_read_position' : results['minimal_read_position']
})
return output_path
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
def path_lateral_modification2(Ts, wheelbase, input_path, velocity, Delta_l_r, Delta_l_r_dot, Delta_l_r_dotdot):
"""
Take an input path, modify it according to a given lateral distance profile,
and generate a new path.
"""
# create placeholders for the plant output signals
x = dy.signal()
y = dy.signal()
psi = dy.signal()
psi_dot = dy.signal()
# controller
results = path_following_controller_P(
input_path,
x, y, psi,
velocity,
Delta_l_r = Delta_l_r,
Delta_l_r_dot = Delta_l_r_dot,
Delta_l_r_dotdot = Delta_l_r_dotdot,
psi_dot = dy.delay(psi_dot),
velocity_dot = dy.float64(0),
Ts = Ts,
k_p = 1
)
#
# The model of the vehicle including a disturbance
#
with dy.sub_if( results['output_valid'], prevent_output_computation=False, subsystem_name='simulation_model') as system:
results['output_valid'].set_name('output_valid')
results['delta'].set_name('delta')
# steering angle limit
limited_steering = dy.saturate(u=results['delta'], lower_limit=-math.pi/2.0, upper_limit=math.pi/2.0)
# the model of the vehicle
x_, y_, psi_, x_dot, y_dot, psi_dot_ = discrete_time_bicycle_model(limited_steering, velocity, Ts, wheelbase)
# driven distance
d = dy.euler_integrator(velocity, Ts)
# outputs
model_outputs = dy.structure(
d = d,
x = x_,
y = y_,
psi = psi_,
psi_dot = dy.delay(psi_dot_) # NOTE: delay introduced to avoid algebraic loops, wait for improved ORTD
)
system.set_outputs(model_outputs.to_list())
model_outputs.replace_signals( system.outputs )
# close the feedback loops
x << model_outputs['x']
y << model_outputs['y']
psi << model_outputs['psi']
psi_dot << model_outputs['psi_dot']
#
output_path = dy.structure({
'd' : model_outputs['d'],
'x' : model_outputs['x'],
'y' : model_outputs['y'],
'psi' : psi,
'psi_dot' : psi_dot,
'psi_r' : psi + results['delta'],
'K' : ( psi_dot + results['delta_dot'] ) / velocity ,
'delta' : results['delta'],
'delta_dot' : results['delta_dot'],
'd_star' : results['d_star'],
'tracked_index' : results['tracked_index'],
'output_valid' : results['output_valid'],
'need_more_path_input_data' : results['need_more_path_input_data'],
'read_position' : results['read_position'],
'minimal_read_position' : results['minimal_read_position']
})
return output_path
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