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 compute_steering_constraints( v, v_dot, psi_dot, delta, a_l_min, a_l_max ):
"""
Compute constraints for the steering angle and its rate so that the acceleration is bounded
delta - the steering angle state of the vehicle (i.e., not the unsaturated control command)
"""
delta_min = dy.float64(-1.0)
delta_max = dy.float64(1.0)
# note: check this proper min/max
delta_dot_min = ( a_l_min - v_dot * dy.sin(delta) ) / ( v * dy.cos(delta) ) - psi_dot
delta_dot_max = ( a_l_max - v_dot * dy.sin(delta) ) / ( v * dy.cos(delta) ) - psi_dot
return delta_min, delta_max, delta_dot_min, delta_dot_max
def bicycle_model(delta, v):
x = dy.signal()
y = dy.signal()
psi = dy.signal()
# bicycle model
tmp = delta + psi
tmp.set_name('tmp')
print()
# x_dot = v * dy.cos( delta + psi )
# y_dot = v * dy.sin( delta + psi )
x_dot = v * dy.cos(tmp)
y_dot = v * dy.sin(tmp)
psi_dot = v / dy.float64(wheelbase) * dy.sin(delta)
x_dot.set_name('x_dot')
y_dot.set_name('y_dot')
psi_dot.set_name('psi_dot')
# integrators
sampling_rate = 0.01
x << dy.euler_integrator(x_dot, sampling_rate, 0.0)
y << dy.euler_integrator(y_dot, sampling_rate, 0.0)
psi << dy.euler_integrator(psi_dot, sampling_rate, 0.0)
return x, y, psi
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 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 compute_distance_from_linear_interpolation(d1, d2):
# TODO: implement
zero = dy.float64(0)
return zero
def compute_steering_constraints(v, v_dot, psi_dot, delta, a_l_min, a_l_max):
"""
delta - steering state of the vehicle (i.e., not the unsaturated control command)
"""
delta_min = dy.float64(-1.0)
delta_max = dy.float64(1.0)
# note: check this proper min/max
delta_dot_min = (a_l_min -
v_dot * dy.sin(delta)) / (v * dy.cos(delta)) - psi_dot
delta_dot_max = (a_l_max -
v_dot * dy.sin(delta)) / (v * dy.cos(delta)) - psi_dot
return delta_min, delta_max, delta_dot_min, delta_dot_max
def compute_path_orientation_from_curvature( Ts : float, velocity, psi_rr, K_r, L ):
"""
Compute the noise-reduced path orientation Psi_r from curvature
Ts - the sampling time
velocity - the driving velocity projected onto the path (d/dt d_star)
psi_rr - noisy (e.g., due to sampling) path orientation
K_r - path curvature
L - gain for fusion using internal observer
returns
psi_r_reconst - the noise-reduced path orientation (reconstructed)
psi_r_reconst_dot - the time derivative (reconstructed)
"""
eps = dy.signal()
psi_r_reconst_dot = velocity * (K_r + eps)
psi_r_reconst = dy.euler_integrator( u=psi_r_reconst_dot, Ts=Ts, initial_state=psi_rr )
# observer to compensate for integration error
eps << (psi_rr - psi_r_reconst) * dy.float64(L)
return psi_r_reconst, psi_r_reconst_dot
def discrete_time_bicycle_model(delta, v, Ts, wheelbase, x0=0.0, y0=0.0, psi0=0.0):
"""
Implement an ODE solver (Euler) for the kinematic bicycle model equations
x, y - describes the position of the front axle,
delta - the steering angle
v - the velocity measured on the front axle
wheelbase - the distance between front- and rear axle
Ts - the sampling time for the Euler integration
psi - the orientation of the carbody
(x0, y0, psi0) - the initial states of the ODEs
"""
x = dy.signal()
y = dy.signal()
psi = dy.signal()
# bicycle model
x_dot = v * dy.cos( delta + psi )
y_dot = v * dy.sin( delta + psi )
psi_dot = v / dy.float64(wheelbase) * dy.sin( delta )
# integrators
x << dy.euler_integrator(x_dot, Ts, x0)
y << dy.euler_integrator(y_dot, Ts, y0)
psi << dy.euler_integrator(psi_dot, Ts, psi0)
return x, y, psi, 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 compute_nominal_steering_from_path_heading(Ts: float, l_r: float, v,
psi_r):
psi = dy.signal()
delta = psi_r - psi
psi_dot = v / dy.float64(l_r) * dy.sin(delta)
psi << dy.euler_integrator(psi_dot, Ts)
return delta, psi, psi_dot
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 compute_nominal_steering_from_curvature( Ts : float, l_r : float, v, K_r ):
"""
compute the nominal steering angle and rate from given path heading and curvature
signals.
"""
psi_dot = dy.signal()
delta_dot = v * K_r - psi_dot
delta = dy.euler_integrator( delta_dot, Ts )
psi_dot << (v / dy.float64(l_r) * dy.sin(delta))
return delta, delta_dot, psi_dot
def project_velocity_on_path(velocity, Delta_u, Delta_l, K_r):
"""
Compute the velocity of the closest point on the path
"""
#
# This evaluates the formula
#
# v_star = d d_star / dt = v * cos( Delta_u ) / ( 1 - Delta_l * K(d_star) )
#
v_star = velocity * dy.cos( Delta_u ) / ( dy.float64(1) - Delta_l * K_r )
return v_star
def compute_nominal_steering_from_path_heading( Ts : float, l_r : float, v, psi_r ):
"""
Compute the steering angle to follow a path given the path tangent angle
Internally uses a (partial) model of the bicycle-vehicle to comput the
optimal steering angle given the path orientation-angle. Internally,
the orientation of the vehicle body (psi) is computed to determine the optimal
steering angle.
"""
psi = dy.signal()
delta = psi_r - psi
psi_dot = v / dy.float64(l_r) * dy.sin(delta)
psi << dy.euler_integrator( psi_dot, Ts )
return delta, psi, psi_dot
def tracker_distance_ahead(path, current_index, distance_ahead):
"""
Track a point on the path that is ahead to the closest point by a given distance
<----- Delta_index_track ----->
array: X X X X X X X X X X X X X X X
^
current_index
"""
#
# define the optimization problem
#
target_distance = dy.float64(distance_ahead) + sample_path_d(
path, current_index)
# pack parameters
par = (target_distance, )
def J(path, index, par):
# unpack parameters
target_distance = par[0]
# cost
d = sample_path_d(path, index)
cost = dy.abs(d - target_distance)
return cost
#
# continuous optimization
#
results = continuous_optimization_along_path(path, current_index, J, par)
# compute the residual distance
optimal_distance = sample_path_d(path, index=results['optimal_index'])
distance_residual = target_distance - optimal_distance
return results['Delta_index_track_next'], distance_residual, results[
'Delta_index']
def discrete_time_bicycle_model(delta,
v,
Ts,
wheelbase,
x0=0.0,
y0=0.0,
psi0=0.0):
x = dy.signal()
y = dy.signal()
psi = dy.signal()
# bicycle model
x_dot = v * dy.cos(delta + psi)
y_dot = v * dy.sin(delta + psi)
psi_dot = v / dy.float64(wheelbase) * dy.sin(delta)
# integrators
x << dy.euler_integrator(x_dot, Ts, x0)
y << dy.euler_integrator(y_dot, Ts, y0)
psi << dy.euler_integrator(psi_dot, Ts, psi0)
return x, y, psi, x_dot, y_dot, psi_dot
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 compile_lateral_path_transformer(wheelbase=3.0, Ts=0.01):
"""
Build OpenRTDynamics code for the lateral path transformation
"""
dy.clear()
system = dy.enter_system()
velocity = dy.system_input(dy.DataTypeFloat64(1),
name='velocity_',
default_value=1,
value_range=[0, 25],
title="vehicle velocity")
Delta_l_r = dy.system_input(dy.DataTypeFloat64(1),
name='Delta_l_r',
default_value=0.0,
value_range=[-10, 10],
title="lateral deviation to the path")
Delta_l_r_dot = dy.system_input(
dy.DataTypeFloat64(1),
name='Delta_l_r_dot',
default_value=0.0,
value_range=[-10, 10],
title="1st-order time derivative of lateral deviation to the path")
Delta_l_r_dotdot = dy.system_input(
dy.DataTypeFloat64(1),
name='Delta_l_r_dotdot',
default_value=0.0,
value_range=[-10, 10],
title="2nd-order time derivative of lateral deviation to the path")
async_input_data_valid = dy.system_input(dy.DataTypeBoolean(1),
name='async_input_data_valid')
input_sample_valid = dy.system_input(dy.DataTypeBoolean(1),
name='input_sample_valid')
path_sample = {}
path_sample['d'] = dy.system_input(dy.DataTypeFloat64(1), name='d_sample')
path_sample['x'] = dy.system_input(dy.DataTypeFloat64(1), name='x_sample')
path_sample['y'] = dy.system_input(dy.DataTypeFloat64(1), name='y_sample')
path_sample['psi'] = dy.system_input(dy.DataTypeFloat64(1),
name='psi_sample')
path_sample['K'] = dy.system_input(dy.DataTypeFloat64(1), name='K_sample')
input_signals = Ts, wheelbase, velocity, Delta_l_r, Delta_l_r_dot, Delta_l_r_dotdot
output_signals = async_path_data_handler(input_sample_valid,
async_input_data_valid,
path_sample, input_signals)
#
# outputs: these are available for visualization in the html set-up
#
dy.append_output(output_signals['output_valid'], 'output_valid')
dy.append_output(output_signals['need_more_path_input_data'],
'need_more_path_input_data')
dy.append_output(output_signals['distance_at_the_end_of_horizon'],
'distance_at_the_end_of_horizon')
dy.append_output(output_signals['distance_ahead'], 'distance_ahead')
dy.append_output(output_signals['head_index'], 'head_index')
dy.append_output(output_signals['read_position'], 'read_position')
dy.append_output(output_signals['elements_free_to_write'],
'elements_free_to_write')
dy.append_output(output_signals['tracked_index'], 'tracked_index')
dy.append_output(output_signals['d_star'], 'path_d_star')
dy.append_output(output_signals['d'], 'path_d')
dy.append_output(output_signals['x'], 'path_x')
dy.append_output(output_signals['y'], 'path_y')
dy.append_output(output_signals['psi_r'], 'path_psi')
dy.append_output(output_signals['K'], 'path_K')
dy.append_output(output_signals['delta'], 'vehicle_delta')
dy.append_output(output_signals['delta_dot'], 'vehicle_delta_dot')
dy.append_output(output_signals['psi'], 'vehicle_psi')
dy.append_output(output_signals['psi_dot'], 'vehicle_psi_dot')
dy.append_output(velocity * dy.float64(1.0), 'velocity')
# generate code for Web Assembly (wasm), requires emcc (emscripten) to build
code_gen_results = dy.generate_code(
template=dy.TargetWasm(enable_tracing=False),
folder="generated/tmp1",
build=False)
compiled_system = dyexe.CompiledCode(code_gen_results)
return code_gen_results, compiled_system
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 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
initial_velocity = dy.system_input(dy.DataTypeFloat64(1),
name='initial_velocity',
default_value=11.0,
value_range=[0, 25],
title="initial condition: vehicle velocity")
acceleration = dy.system_input(dy.DataTypeFloat64(1),
name='acceleration',
default_value=-2.5,
value_range=[-8, 0],
title="initial condition: vehicle acceleration")
disturbance_ofs = dy.system_input(
dy.DataTypeFloat64(1),
name='disturbance_ofs',
default_value=-0.7,
value_range=[-4, 4],
title="disturbance: steering offset") * dy.float64(math.pi / 180.0)
delta_factor = dy.system_input(dy.DataTypeFloat64(1),
name='delta_factor',
default_value=1.1,
value_range=[0.8, 1.2],
title="disturbance: steering factor")
velocity_factor = dy.system_input(dy.DataTypeFloat64(1),
name='velocity_factor',
default_value=1.0,
value_range=[0.8, 1.2],
title="disturbance: velocity factor")
IMU_drift = dy.system_input(
dy.DataTypeFloat64(1),
name='IMU_drift',
default_value=0.0,
value_range=[-0.5, 0.5],
def compile_lateral_path_transformer(par={},
target_template=None,
folder=None,
samples_in_buffer=10000):
"""
Build OpenRTDynamics code for the lateral path transformation
par - optional parameters for the model (unused so far)
target_template - the openrtdynamics code generation terget template to use
folder - the folder to which the generated files are written
samples_in_buffer - the size of the buffer storing the samples for the input path
"""
dy.clear()
system = dy.enter_system()
# parameters
Ts = dy.system_input(dy.DataTypeFloat64(1),
name='Ts',
default_value=0.01,
title="sampling time [s]")
wheelbase = dy.system_input(dy.DataTypeFloat64(1),
name='wheelbase',
default_value=3.0,
title="wheelbase (l_r) [m]")
# time-series for velocity, lateral distance, ...
velocity = dy.system_input(dy.DataTypeFloat64(1),
name='velocity_',
default_value=1,
value_range=[0, 25],
title="vehicle velocity [m/s]")
Delta_l_r = dy.system_input(dy.DataTypeFloat64(1),
name='Delta_l_r',
default_value=0.0,
value_range=[-10, 10],
title="lateral deviation to the path [m]")
Delta_l_r_dot = dy.system_input(
dy.DataTypeFloat64(1),
name='Delta_l_r_dot',
default_value=0.0,
value_range=[-10, 10],
title="1st-order time derivative of lateral deviation to the path [m/s]"
)
Delta_l_r_dotdot = dy.system_input(
dy.DataTypeFloat64(1),
name='Delta_l_r_dotdot',
default_value=0.0,
value_range=[-10, 10],
title=
"2nd-order time derivative of lateral deviation to the path [m/s^2]")
# initial states of the vehicle
d0 = dy.system_input(dy.DataTypeFloat64(1),
name='d0',
default_value=0,
title="initial state d0 [m]")
x0 = dy.system_input(dy.DataTypeFloat64(1),
name='x0',
default_value=0,
title="initial state x0 [m]")
y0 = dy.system_input(dy.DataTypeFloat64(1),
name='y0',
default_value=0,
title="initial state y0 [m]")
psi0 = dy.system_input(dy.DataTypeFloat64(1),
name='psi0',
default_value=0,
title="initial state psi0 [rad]")
delta0 = dy.system_input(dy.DataTypeFloat64(1),
name='delta0',
default_value=0,
title="initial state delta0 [rad]")
delta_dot0 = dy.system_input(dy.DataTypeFloat64(1),
name='delta_dot0',
default_value=0,
title="initial state delta_dot0 [rad/s]")
# control inputs
async_input_data_valid = dy.system_input(dy.DataTypeBoolean(1),
name='async_input_data_valid')
input_sample_valid = dy.system_input(dy.DataTypeBoolean(1),
name='input_sample_valid')
# inputs for asynchronously arriving path samples
path_sample = {}
path_sample['d'] = dy.system_input(dy.DataTypeFloat64(1), name='d_sample')
path_sample['x'] = dy.system_input(dy.DataTypeFloat64(1), name='x_sample')
path_sample['y'] = dy.system_input(dy.DataTypeFloat64(1), name='y_sample')
path_sample['psi'] = dy.system_input(dy.DataTypeFloat64(1),
name='psi_sample')
path_sample['K'] = dy.system_input(dy.DataTypeFloat64(1), name='K_sample')
#
# combine all input signals in a structure that serve as parameters to the
# callback function (the embedded system) vl.path_lateral_modification2
#
input_signals = {
'Ts': Ts,
'wheelbase': wheelbase,
'velocity': velocity,
'Delta_l_r': Delta_l_r,
'Delta_l_r_dot': Delta_l_r_dot,
'Delta_l_r_dotdot': Delta_l_r_dotdot,
'd0': d0,
'x0': x0,
'y0': y0,
'psi0': psi0,
'delta0': delta0,
'delta_dot0': delta_dot0
}
output_signals = async_path_data_handler(
input_sample_valid, async_input_data_valid, path_sample,
vl.path_lateral_modification2, input_signals, par, samples_in_buffer)
#
# outputs
#
dy.append_output(output_signals['output_valid'], 'output_valid')
dy.append_output(output_signals['need_more_path_input_data'],
'need_more_path_input_data')
dy.append_output(output_signals['distance_at_the_end_of_horizon'],
'distance_at_the_end_of_horizon')
dy.append_output(output_signals['distance_ahead'], 'distance_ahead')
dy.append_output(output_signals['head_index'], 'head_index')
dy.append_output(output_signals['read_position'], 'read_position')
dy.append_output(output_signals['elements_free_to_write'],
'elements_free_to_write')
dy.append_output(output_signals['tracked_index'], 'tracked_index')
dy.append_output(output_signals['d_star'], 'path_d_star')
dy.append_output(output_signals['d'], 'path_d')
dy.append_output(output_signals['x'], 'path_x')
dy.append_output(output_signals['y'], 'path_y')
dy.append_output(output_signals['psi_r'], 'path_psi')
dy.append_output(output_signals['K'], 'path_K')
dy.append_output(output_signals['delta'], 'vehicle_delta')
dy.append_output(output_signals['delta_dot'], 'vehicle_delta_dot')
dy.append_output(output_signals['psi'], 'vehicle_psi')
dy.append_output(output_signals['psi_dot'], 'vehicle_psi_dot')
dy.append_output(velocity * dy.float64(1.0), 'velocity')
# generate code
if target_template is None:
target_template = tg.TargetCppMinimal()
code_gen_results = dy.generate_code(template=target_template,
folder=folder)
compiled_system = dyexe.CompiledCode(code_gen_results)
return code_gen_results, compiled_system
# load track data
with open("track_data/simple_track.json", "r") as read_file:
track_data = json.load(read_file)
#
# Demo: a vehicle controlled to follow a given path
#
# Implemented using the code generator openrtdynamics 2 - https://pypi.org/project/openrtdynamics2/ .
# This generates c++ code for Web Assembly to be run within the browser.
#
system = dy.enter_system()
velocity = dy.system_input( dy.DataTypeFloat64(1), name='velocity', default_value=25.0, value_range=[0, 25], title="vehicle velocity [m/s]")
steering_rate = dy.system_input( dy.DataTypeFloat64(1), name='steering_rate', default_value=1.0, value_range=[-10, 10], title="steering_rate [degrees/s]") * dy.float64(math.pi / 180.0)
initial_steering = dy.system_input( dy.DataTypeFloat64(1), name='initial_steering', default_value=-10.0, value_range=[-40, 40], title="initial steering angle [degrees]") * dy.float64(math.pi / 180.0)
initial_orientation = dy.system_input( dy.DataTypeFloat64(1), name='initial_orientation', default_value=0.0, value_range=[-360, 360], title="initial orientation angle [degrees]") * dy.float64(math.pi / 180.0)
# parameters
wheelbase = 3.0
# sampling time
Ts = 0.01
# create storage for the reference path:
path = import_path_data(track_data)
# linearly increasing steering angle
delta = dy.euler_integrator( steering_rate, Ts, initial_state=initial_steering )
delta = dy.saturate(u=delta, lower_limit=-math.pi/2.0, upper_limit=math.pi/2.0)
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
if testname == 'test_comparison':
baseDatatype = dy.DataTypeFloat64(1)
u1 = dy.system_input( baseDatatype ).set_name('u1')
u2 = dy.system_input( baseDatatype ).set_name('u2')
isGreater = dy.comparison(left = u1, right = u2, operator = '>' )
# NOTE: intentionally only x is the output. v is intentionally unused in this test.
output_signals = [ isGreater ]
if testname == 'test_step':
y = dy.float64(3) * dy.signal_step(10) + dy.float64(-5) * dy.signal_step(40) + dy.float64(2) * dy.signal_step(70)
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':
advanced_control = False
#
# A vehicle controlled to follow a given path
#
system = dy.enter_system()
baseDatatype = dy.DataTypeFloat64(1)
# define simulation inputs
if not advanced_control:
velocity = dy.system_input( dy.DataTypeFloat64(1), name='velocity', default_value=7.2, value_range=[0, 25], title="vehicle velocity")
k_p = dy.system_input( dy.DataTypeFloat64(1), name='k_p', default_value=0.612, value_range=[0, 4.0], title="controller gain")
disturbance_amplitude = dy.system_input( dy.DataTypeFloat64(1), name='disturbance_amplitude', default_value=-11.0, value_range=[-45, 45], title="disturbance amplitude") * dy.float64(math.pi / 180.0)
sample_disturbance = dy.system_input( dy.DataTypeInt32(1), name='sample_disturbance', default_value=50, value_range=[0, 300], title="disturbance position")
z_inf_compensator = dy.system_input( dy.DataTypeFloat64(1), name='z_inf', default_value=0.9, value_range=[0, 1.0], title="z_inf_compensator")
if advanced_control:
velocity = dy.system_input( baseDatatype ).set_name('velocity').set_properties({ "range" : [0, 25], "unit" : "m/s", "default_value" : 23.75, "title" : "vehicle velocity" })
k_p = dy.system_input( baseDatatype ).set_name('k_p').set_properties({ "range" : [0, 4.0], "default_value" : 0.24, "title" : "controller gain" })
disturbance_amplitude = dy.system_input( baseDatatype ).set_name('disturbance_amplitude').set_properties({ "range" : [-45, 45], "unit" : "degrees", "default_value" : 0, "title" : "disturbance amplitude" }) * dy.float64(math.pi / 180.0)
sample_disturbance = dy.convert(dy.system_input( baseDatatype ).set_name('sample_disturbance').set_properties({ "range" : [0, 300], "unit" : "samples", "default_value" : 0, "title" : "disturbance position" }), target_type=dy.DataTypeInt32(1) )
distance_ahead = dy.system_input( baseDatatype ).set_name('distance_ahead').set_properties({ "range" : [0, 20.0], "default_value" : 5.0, "title" : "distance ahead" })
velocity = dy.system_input(dy.DataTypeFloat64(1),
name='velocity',
default_value=23.75,
value_range=[0, 25],
title="vehicle velocity")
k_p = dy.system_input(dy.DataTypeFloat64(1),
name='k_p',
default_value=2.0,
value_range=[0, 10.0],
title="controller gain")
disturbance_amplitude = dy.system_input(
dy.DataTypeFloat64(1),
name='disturbance_amplitude',
default_value=20.0,
value_range=[-45, 45],
title="disturbance amplitude") * dy.float64(math.pi / 180.0)
sample_disturbance = dy.system_input(dy.DataTypeInt32(1),
name='sample_disturbance',
default_value=50,
value_range=[0, 300],
title="disturbance position")
# parameters
wheelbase = 3.0
# sampling time
Ts = 0.01
# create storage for the reference path:
path = import_path_data(track_data)
# define a function that implements a discrete-time integrator
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
ofs = dy.float64(0.1).set_name('ofs')
# define system inputs
friction = dy.system_input(baseDatatype).set_name('friction') * dy.float64(
0.05) + ofs
mass = dy.system_input(baseDatatype).set_name('mass') * dy.float64(0.05) + ofs
length = dy.system_input(baseDatatype).set_name('length') * dy.float64(
0.05) + ofs
# length = dy.float64(0.3)
g = dy.float64(9.81).set_name('g')
# create placeholder for the plant output signal
angle = dy.signal()
angular_velocity = dy.signal()
