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 path_following(
controller,
par,
path,
x, y, psi, velocity,
Delta_l_r = 0.0,
Delta_l_r_dot = None,
psi_dot = None,
velocity_dot = None,
Delta_l_r_dotdot = None,
Ts=0.01
):
"""
Basic steering control for path tracking and user-defined lateral error compensation
Implements steering control for exact path following.
Assumed is a kinematic bicycle model.
Herein, the steering angle (delta) is the control variable. The variables
x, y, psi, and velocity are measurements taken from the controlled system.
The lateral offset Delta_l_r to the path is the reference for control.
The optional signal Delta_l_r_dot describes the time derivative of Delta_l_r.
controller - callback function that defines the error compensating controller
par - parameters that are passed to the callback
Ts - the sampling time
Return values
-------------
results = {}
results['x_r'] # the current x-position of the closest point on the reference path
results['y_r'] # the current y-position of the closest point on the reference path
results['v_star'] # the current velocity of the closest point on the reference path
results['d_star'] # the current distance parameter of the closest point on the reference path
results['psi_r'] # the current path-tangent orientation angle in the closest point on the reference path
results['psi_r_dot'] # the time derivative of psi_r
results['Delta_l_r'] # the reference to the distance to the path
results['Delta_l_r_dot'] # optional: the time derivative of Delta_l_r
results['Delta_l'] # the distance to the closest point on the reference path
results['Delta_u'] # small steering delta
results['delta'] # the requested steering angle / the control variable
in case Delta_l_r_dot, psi_dot, velocity_dot, and Delta_l_r_dotdot are given
the steering derivatives can be computed.
results['Delta_u_dot'] # the derivative of Delta_u
results['delta_dot'] # the derivative of delta_dot
"""
index_head, _ = path_horizon_head_index(path)
# structure for output signals
results = dy.structure()
# track the evolution of the closest point on the path to the vehicles position
minimal_number_of_path_samples_to_start = 5 # depends on tracker(); should be at least 2
with dy.sub_if( index_head > minimal_number_of_path_samples_to_start, subsystem_name='tracker' ) as system:
tracking_results = tracker(path, x, y)
system.set_outputs( tracking_results.to_list() )
tracking_results.replace_signals( system.outputs )
output_valid = tracking_results['minimal_distance_reached']
need_more_path_input_data = tracking_results['reached_the_end_of_currently_available_path_data']
# in case the lookup was successful, run further operations on the path
# to generate references and run the controller.
with dy.sub_if( output_valid, prevent_output_computation=False, subsystem_name='controller') as system:
# ps - path sample
ps = track_projection_on_path(path, x, y, tracking_results = tracking_results)
#
# project the vehicle velocity onto the path yielding v_star
#
# Used formula inside project_velocity_on_path:
# v_star = d d_star / dt = v * cos( Delta_u ) / ( 1 - Delta_l * K(d_star) )
#
Delta_u = dy.signal() # feedback from control
v_star = project_velocity_on_path(velocity, Delta_u, ps['Delta_l'], ps['K_r'])
#
# compute an enhanced (less noise) signal for the path orientation psi_r by integrating the
# curvature profile and fusing the result with ps['psi_r'] to mitigate the integration drift.
#
psi_r, psi_r_dot = compute_path_orientation_from_curvature( Ts, v_star, ps['psi_r'], ps['K_r'], L=1.0 )
#
# controller callback
#
references = {
'Delta_l_r' : Delta_l_r,
'Delta_l_r_dot' : Delta_l_r_dot,
'Delta_l_r_dotdot' : Delta_l_r_dotdot
}
# Delta_l_dot might be further computed which improves the accuracy of the derivatives
# in case of strong feedback control activity.
Delta_l_dot = None # TODO: implement
measurements = {
'velocity' : velocity,
'velocity_dot' : velocity_dot,
'psi' : psi,
'psi_dot' : psi_dot,
'Delta_l' : ps['Delta_l'],
'Delta_l_dot' : Delta_l_dot
}
u, u_dot = controller( references, measurements, par )
#
# path tracking
#
# resulting lateral model u --> Delta_l : 1/s
#
Delta_u << dy.asin( dy.saturate(u / velocity, -0.99, 0.99) )
delta = dy.unwrap_angle(angle=psi_r - psi + Delta_u, normalize_around_zero = True)
# compute the derivatives of the steering angle (steering rate)
if psi_dot is not None and Delta_l_r_dot is not None and velocity_dot is not None and Delta_l_r_dotdot is not None:
Delta_u_dot = dy.cos( u / velocity ) * ( velocity * u_dot - velocity_dot * u ) / ( velocity*velocity )
delta_dot = psi_r_dot - psi_dot + Delta_u_dot
results['Delta_u_dot'] = Delta_u_dot
results['delta_dot'] = delta_dot
# collect resulting signals
results['x_r'] = ps['x_r'] # the current x-position of the closest point on the reference path
results['y_r'] = ps['y_r'] # the current y-position of the closest point on the reference path
results['v_star'] = v_star # the current velocity of the closest point on the reference path
results['d_star'] = ps['d_star'] # the current distance parameter of the closest point on the reference path
results['psi_r'] = psi_r # the current path-tangent orientation angle in the closest point on the reference path
results['K_r'] = ps['K_r'] # the curvature
results['psi_r_dot'] = psi_r_dot # the time derivative of psi_r
results['Delta_u'] = Delta_u # small steering delta
results['delta'] = delta # the requested steering angle / the control variable
results['Delta_l'] = ps['Delta_l'] # the distance to the closest point on the reference path
results['Delta_l_dot'] = dy.float64(math.nan) # d/dt Delta_l TODO: implement
# results['line_tracking_internals'] = ps['internals']
# return
system.set_outputs( results.to_list() )
results.replace_signals( system.outputs )
results['tracked_index'] = tracking_results['tracked_index']
results['Delta_l_r'] = Delta_l_r # the reference to the distance to the path
results['need_more_path_input_data'] = need_more_path_input_data
results['output_valid'] = output_valid
results['read_position'] = results['tracked_index'] + 1
results['minimal_read_position'] = results['read_position'] - 100
return results
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 path_tracking(path, par, Ts, velocity, velocity_dot, x, y, x_dot, y_dot,
Delta_u, Delta_u_dot):
"""
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()
"""
index_head, _ = path_horizon_head_index(path)
# structure for output signals
results = dy.structure()
# track the evolution of the closest point on the path to the vehicles position
minimal_number_of_path_samples_to_start = 5 # depends on tracker(); should be at least 2
with dy.sub_if(index_head > minimal_number_of_path_samples_to_start,
subsystem_name='tracker') as system:
tracking_results = tracker(path, x, y)
system.set_outputs(tracking_results.to_list())
tracking_results.replace_signals(system.outputs)
output_valid = tracking_results['minimal_distance_reached']
need_more_path_input_data = tracking_results[
'reached_the_end_of_currently_available_path_data']
# in case the lookup was successful, run further operations on the path
# to generate references and run the controller.
with dy.sub_if(output_valid,
prevent_output_computation=False,
subsystem_name='controller') as system:
# ps - path sample
ps = track_projection_on_path(path,
x,
y,
tracking_results=tracking_results)
#
# project the vehicle velocity onto the path yielding v_star
#
# Used formula inside project_velocity_on_path:
# v_star = d d_star / dt = v * cos( Delta_u ) / ( 1 - Delta_l * K(d_star) )
#
v_star = project_velocity_on_path(velocity, Delta_u, ps['Delta_l'],
ps['K_r'])
#
# compute an enhanced (less noise) signal for the path orientation psi_r by integrating the
# curvature profile and fusing the result with ps['psi_r'] to mitigate the integration drift.
#
psi_r, psi_r_dot = compute_path_orientation_from_curvature(Ts,
v_star,
ps['psi_r'],
ps['K_r'],
L=1.0)
# collect resulting signals
results['x_r'] = ps[
'x_r'] # the current x-position of the closest point on the reference path
results['y_r'] = ps[
'y_r'] # the current y-position of the closest point on the reference path
results[
'v_star'] = v_star # the current velocity of the closest point on the reference path
results['d_star'] = ps[
'd_star'] # the current distance parameter of the closest point on the reference path
results[
'psi_r'] = psi_r # the current path-tangent orientation angle in the closest point on the reference path
results['psi_r_dot'] = psi_r_dot # the time derivative of psi_r
results['K_r'] = ps['K_r'] # the curvature
results['Delta_l'] = ps[
'Delta_l'] # the distance to the closest point on the reference path
results['Delta_l_dot'] = dy.float64(
math.nan) # d/dt Delta_l TODO: implement
# results['line_tracking_internals'] = ps['internals']
# return
system.set_outputs(results.to_list())
results.replace_signals(system.outputs)
#
output_path = dy.structure({
'tracked_index':
tracking_results['tracked_index'],
'd_star':
results['d_star'],
'v_star':
results['v_star'],
'x_r':
results['x_r'],
'y_r':
results['y_r'],
'psi_r':
results['psi_r'],
'psi_r_dot':
results['psi_r_dot'],
'K_r':
results['K_r'],
'Delta_l':
results['Delta_l'],
'Delta_l_dot':
results['Delta_l_dot'],
'output_valid':
output_valid,
'need_more_path_input_data':
need_more_path_input_data,
'read_position':
tracking_results['tracked_index'] + 1,
'minimal_read_position':
tracking_results['tracked_index'] - 100
})
return output_path
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 path_following_controller_P( path, x, y, psi, velocity, Delta_l_r = 0.0, Delta_l_r_dot = None, k_p=2.0, Ts=0.01, psi_dot = None, velocity_dot = None, Delta_l_r_dotdot = None, Delta_l_dot = None ):
"""
Basic steering control for path tracking using proportional lateral error compensation
Path following steering control for exact path following and P-control to control the lateral
distance to the path are combined.
Controlls a kinematic bicycle model (assumption) to follow the given path.
Herein, the steering angle delta is the control variable. The variables
x, y, psi, and velocity are measurements taken from the controlled system.
The lateral offset Delta_l_r to the path is the reference. The optional
signal Delta_l_r_dot describes the time derivative of Delta_l_r.
Ts - the sampling time
Return values
-------------
results = {}
results['x_r'] # the current x-position of the closest point on the reference path
results['y_r'] # the current y-position of the closest point on the reference path
results['v_star'] # the current velocity of the closest point on the reference path
results['d_star'] # the current distance parameter of the closest point on the reference path
results['psi_r'] # the current path-tangent orientation angle in the closest point on the reference path
results['psi_r_dot'] # the time derivative of psi_r
results['Delta_l_r'] # the reference to the distance to the path
results['Delta_l_r_dot'] # optional: the time derivative of Delta_l_r
results['Delta_l'] # the distance to the closest point on the reference path
results['Delta_u'] # small steering delta
results['delta'] # the requested steering angle / the control variable
in case Delta_l_r_dot, psi_dot, velocity_dot, and Delta_l_r_dotdot are given
the steering derivatives can be computed.
results['Delta_u_dot'] # the derivative of Delta_u
results['delta_dot'] # the derivative of delta_dot
Optionally, Delta_l_dot might be further given which improves the accuracy of the derivatives
in case of strong feedback control activity.
"""
index_head, _ = path_horizon_head_index(path)
# structure for output signals
results = dy.structure()
# track the evolution of the closest point on the path to the vehicles position
with dy.sub_if( index_head > 10 ) as system:
tracking_results = tracker(path, x, y)
system.set_outputs( tracking_results.to_list() )
tracking_results.replace_signals( system.outputs )
output_valid = tracking_results['minimal_distance_reached']
need_more_path_input_data = tracking_results['reached_the_end_of_currently_available_path_data']
# position_on_path_found = dy.boolean(True)
with dy.sub_if( output_valid, prevent_output_computation=False, subsystem_name='controller') as system:
# ps - path sample
ps = track_projection_on_path(path, x, y, tracking_results = tracking_results)
#
# project the vehicle velocity onto the path yielding v_star
#
# Used formula inside project_velocity_on_path:
# v_star = d d_star / dt = v * cos( Delta_u ) / ( 1 - Delta_l * K(d_star) )
#
Delta_u = dy.signal() # feedback from control
v_star = project_velocity_on_path(velocity, Delta_u, ps['Delta_l'], ps['K_r'])
#
# compute an enhanced (less noise) signal for the path orientation psi_r by integrating the
# curvature profile and fusing the result with ps['psi_r'] to mitigate the integration drift.
#
psi_r, psi_r_dot = compute_path_orientation_from_curvature( Ts, v_star, ps['psi_r'], ps['K_r'], L=1.0 )
# feedback control
u_fb = k_p * (Delta_l_r - ps['Delta_l'])
if Delta_l_r_dot is not None:
u = Delta_l_r_dot + u_fb
else:
u = u_fb
# path tracking
# resulting lateral model u --> Delta_l : 1/s
Delta_u << dy.asin( dy.saturate(u / velocity, -0.99, 0.99) )
delta = dy.unwrap_angle(angle=psi_r - psi + Delta_u, normalize_around_zero = True)
# compute the derivatives of the steering angle (steering rate)
if psi_dot is not None and Delta_l_r_dot is not None and velocity_dot is not None and Delta_l_r_dotdot is not None:
if Delta_l_dot is None:
u_dot = Delta_l_r_dotdot # + 0 neglect numerical random walk error compensation
else:
u_dot = Delta_l_r_dotdot + Delta_l_dot
Delta_u_dot = dy.cos( u / velocity ) * ( velocity * u_dot - velocity_dot * u ) / ( velocity*velocity )
delta_dot = psi_r_dot - psi_dot + Delta_u_dot
results['Delta_u_dot'] = Delta_u_dot
results['delta_dot'] = delta_dot
# collect resulting signals
results['x_r'] = ps['x_r'] # the current x-position of the closest point on the reference path
results['y_r'] = ps['y_r'] # the current y-position of the closest point on the reference path
results['v_star'] = v_star # the current velocity of the closest point on the reference path
results['d_star'] = ps['d_star'] # the current distance parameter of the closest point on the reference path
results['psi_r'] = psi_r # the current path-tangent orientation angle in the closest point on the reference path
results['psi_r_dot'] = psi_r_dot # the time derivative of psi_r
results['Delta_l'] = ps['Delta_l'] # the distance to the closest point on the reference path
results['Delta_u'] = Delta_u # small steering delta
results['delta'] = delta # the requested steering angle / the control variable
# results['line_tracking_internals'] = ps['internals']
# return
system.set_outputs( results.to_list() )
results.replace_signals( system.outputs )
results['tracked_index'] = tracking_results['tracked_index']
results['Delta_l_r'] = Delta_l_r # the reference to the distance to the path
results['need_more_path_input_data'] = need_more_path_input_data
results['output_valid'] = output_valid
results['read_position'] = results['tracked_index'] + 1
results['minimal_read_position'] = results['read_position'] - 100
return results
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