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 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 generate_one_dimensional_motion_ORTD(acceleration_input, Ts):
# Delta_l_dotdot; the motion jerk is limited to +-1.0 m/s^3
acceleration = dy.rate_limit(acceleration_input, Ts, lower_limit = -1.0, upper_limit = 1.0)
# Delta_l_dot
velocity = dy.euler_integrator(acceleration, Ts, 0)
# Delta_l
lateral_distance = dy.euler_integrator(velocity, Ts, 0)
return lateral_distance, velocity, acceleration
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 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 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 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 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
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) # the model of the vehicle x, y, psi, x_dot, y_dot, psi_dot = discrete_time_bicycle_model(delta, velocity, Ts, wheelbase, psi0=initial_orientation) # # outputs: these are available for visualization in the html set-up # dy.append_output(x, 'x') dy.append_output(y, 'y') dy.append_output(psi, 'psi') dy.append_output(delta, 'steering')
# specify what the input signals shall be in the runtime
input_signals_mapping = {}
input_signals_mapping[ U ] = 1.0
if testname == 'test_oscillator':
baseDatatype = dy.DataTypeFloat64(1)
U = dy.system_input( baseDatatype ).set_name('extU')
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)
x << dy.euler_integrator( v, Ts=0.1)
# define the outputs of the simulation
output_signals = [ x, v ]
# specify what the input signals shall be in the runtime
input_signals_mapping = {}
input_signals_mapping[ U ] = 1.0
if testname == 'oscillator_modulation_example':
baseDatatype = dy.DataTypeFloat64(1)
#loop_iterations = dy.system_input( baseDatatype ).set_name('loop_iterations').set_properties({ "range" : [0, 100], "default_value" : 20 })
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
path, x_real, y_real)
path_index_start_open_loop_control = dy.sample_and_hold(
tracked_index, event=dy.initial_event())
path_distance_start_open_loop_control = dy.sample_and_hold(
d_star, event=dy.initial_event())
dy.append_output(path_index_start_open_loop_control,
'path_index_start_open_loop_control')
dy.append_output(Delta_l, 'Delta_l')
#
# model vehicle braking
#
velocity = dy.euler_integrator(
acceleration, Ts, initial_state=initial_velocity) * velocity_factor
velocity = dy.saturate(velocity, lower_limit=0)
dy.append_output(velocity, 'velocity')
#
# open-loop control
#
# estimate the travelled distance by integration of the vehicle velocity
d_hat = dy.euler_integrator(
velocity, Ts, initial_state=0) + path_distance_start_open_loop_control
# estimated travelled distance (d_hat) to path-index
open_loop_index, _, _ = tracker_distance_ahead(
path,
# # meaning u is the lateral velocity to which is used to control the lateral # distance to the path. # # generate a velocity profile u_move_left = dy.signal_step( dy.int32(50) ) - dy.signal_step( dy.int32(200) ) u_move_right = dy.signal_step( dy.int32(500) ) - dy.signal_step( dy.int32(350) ) # apply a rate limiter to limit the acceleration u = dy.rate_limit( max_lateral_velocity * (u_move_left + u_move_right), Ts, dy.float64(-1) * max_lateral_accleration, max_lateral_accleration) dy.append_output(u, 'u') # internal lateral model (to verify the lateral dynamics of the simulated vehicle) Delta_l_mdl = dy.euler_integrator(u, Ts) dy.append_output(Delta_l_mdl, 'Delta_l_mdl') # # path tracking control # # Control of the lateral distance to the path can be performed via the augmented control # variable u. # # Herein, a linearization yielding the resulting lateral dynamics u --> Delta_l : 1/s is applied. #
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