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()