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