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
if testname == 'rate_limit':
baseDatatype = dy.DataTypeFloat64(1)
lower_limit1 = dy.system_input( baseDatatype ).set_name('lower_limit1').set_properties({ "range" : [-5, 0], "default_value" : -1 })
upper_limit1 = dy.system_input( baseDatatype ).set_name('upper_limit1').set_properties({ "range" : [0, 5], "default_value" : 1 })
z_inf = dy.system_input( baseDatatype ).set_name('z_inf').set_properties({ "range" : [0, 0.999], "default_value" : 0.9 })
phase = dy.convert( dy.system_input( baseDatatype ).set_name('phase').set_properties({ "range" : [0, 200], "default_value" : 0 }) , target_type=dy.DataTypeInt32(1) )
period = dy.convert( dy.system_input( baseDatatype ).set_name('period').set_properties({ "range" : [0, 200], "default_value" : 50 }) , target_type=dy.DataTypeInt32(1) )
step, activate, deactivate = dy.signal_square(period=period, phase=phase)
# v1
rate_limit_1 = dy.rate_limit( u=step, Ts=0.01, lower_limit=lower_limit1, upper_limit=upper_limit1, initial_state = 0 )
# v2 - rate limiter and low pass
rate_limit_2 = dy.rate_limit( u=step, Ts=0.01, lower_limit=lower_limit1, upper_limit=upper_limit1, initial_state = 0 )
rate_limit_2 = dy.dtf_lowpass_1_order(rate_limit_2, z_inf)
# main simulation ouput
output_signals = [ step, rate_limit_1, rate_limit_2 ]
if testname == 'vanderpol':
# https://en.wikipedia.org/wiki/Van_der_Pol_oscillator
dy.append_output( delta_dot_min, 'delta_dot_min' ) dy.append_output( delta_dot_max, 'delta_dot_max' ) # estimate steering rate of the real vehicle # delta_dot_hat = dy.dtf_lowpass_1_order( dy.diff( steering, initial_state=steering ) / dy.float64(Ts), 0.1) # dy.append_output( delta_dot_hat, 'delta_dot_hat' ) delta_dot_hat_nosat = dy.diff( steering, initial_state=steering ) / dy.float64(Ts) dy.append_output( delta_dot_hat_nosat, 'delta_dot_hat_nosat' ) # # apply saturation to the steering control variable # steering = dy.saturate(steering, delta_min, delta_max) steering = dy.rate_limit( steering, Ts, delta_dot_min, delta_dot_max, initial_state=steering ) delta_dot_hat = dy.diff( steering, initial_state=steering ) / dy.float64(Ts) dy.append_output( delta_dot_hat, 'delta_dot_hat' ) # # The model of the vehicle including a disturbance # # model the disturbance disturbance_transient = np.concatenate(( cosra(50, 0, 1.0), co(10, 1.0), cosra(50, 1.0, 0) )) steering_disturbance, _ = dy.play(disturbance_transient, start_trigger=dy.counter() == sample_disturbance, auto_start=False) steering_disturbance = steering_disturbance * disturbance_amplitude
# lateral open-loop control to realize an 'obstacle-avoiding maneuver' # # the dynamic model for the lateral distance Delta_l is # # d/dt Delta_l = u, # # 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