def global_lookup_distance_index(path_distance_storage, path_x_storage,
path_y_storage, distance):
#
source_code = """
index = 0;
int i = 0;
for (i = 0; i < 100; ++i ) {
if ( path_distance_storage[i] < distance && path_distance_storage[i+1] > distance ) {
index = i;
break;
}
}
"""
array_type = dy.DataTypeArray(360, datatype=dy.DataTypeFloat64(1))
outputs = dy.generic_cpp_static(input_signals=[
path_distance_storage, path_x_storage, path_y_storage, distance
],
input_names=[
'path_distance_storage',
'path_x_storage', 'path_y_storage',
'distance'
],
input_types=[
array_type, array_type, array_type,
dy.DataTypeFloat64(1)
],
output_names=['index'],
output_types=[dy.DataTypeInt32(1)],
cpp_source_code=source_code)
index = outputs[0]
return index
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 import_path_data(data):
# distance on path (D), position (X/Y), path orientation (PSI), curvature (K)
path = {}
path['D'] = dy.memory(datatype=dy.DataTypeFloat64(1),
constant_array=data['D'])
path['X'] = dy.memory(datatype=dy.DataTypeFloat64(1),
constant_array=data['X'])
path['Y'] = dy.memory(datatype=dy.DataTypeFloat64(1),
constant_array=data['Y'])
path['PSI'] = dy.memory(datatype=dy.DataTypeFloat64(1),
constant_array=data['PSI'])
path['K'] = dy.memory(datatype=dy.DataTypeFloat64(1),
constant_array=data['K'])
path['samples'] = len(data['D'])
return path
def import_path_data(data):
"""
Create a data structure containing the driving path
"""
# distance on path (D), position (X/Y), path orientation (PSI), curvature (K)
path = {}
path['buffer_type'] = 'dy.memory'
path['D'] = dy.memory(datatype=dy.DataTypeFloat64(1), constant_array=data['D'] )
path['X'] = dy.memory(datatype=dy.DataTypeFloat64(1), constant_array=data['X'] )
path['Y'] = dy.memory(datatype=dy.DataTypeFloat64(1), constant_array=data['Y'] )
path['PSI'] = dy.memory(datatype=dy.DataTypeFloat64(1), constant_array=data['PSI'] )
path['K'] = dy.memory(datatype=dy.DataTypeFloat64(1), constant_array=data['K'] )
path['samples'] = len( data['D'] )
return path
from vehicle_lib.vehicle_lib import *
# 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=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)
# 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 )
# testname = 'signal_periodic_impulse' #
testname = 'inline_ifsubsystem_oscillator' # 'signal_periodic_impulse' #'loop_until' #'inline_ifsubsystem_oscillator' #
testname = 'cpp_class'
test_modification_1 = True # option should not have an influence on the result
test_modification_2 = False # shall raise an error once this is true
if testname == 'test1':
baseDatatype = dy.DataTypeFloat64(1)
U = dy.system_input( baseDatatype ).set_name('extU')
y1 = firstOrderAndGain( U, 0.2, gain=0.8, name="1")
y2 = firstOrderAndGain( y1, 0.2, gain=0.8, name="2")
y3 = firstOrderAndGain( y2, 0.2, gain=0.8, name="3")
E1 = dy.system_input( baseDatatype ).set_name('extE1')
E2 = dy.system_input( baseDatatype ).set_name('extE2')
y = dy.add( [ y3, E1, E2 ], [ 0.1, 0.2, 0.3] ).set_blockname('y (add)').set_name('y')
# define the outputs of the simulation
output_signals = [ y, y2 ]
from vehicle_lib.vehicle_lib import *
# load track data
with open("track_data/simple_track.json", "r") as read_file:
track_data = json.load(read_file)
#
# Demo: a vehicle controlled in an open-loop manner to follow a given path as long as possible
#
# 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()
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)
def lookup_closest_point(N, path_distance_storage, path_x_storage,
path_y_storage, x, y):
"""
brute force implementation for finding a clostest point
"""
#
source_code = """
// int N = 360;
int N = *(&path_distance_storage + 1) - path_distance_storage;
int i = 0;
double min_distance_to_path = 1000000;
int min_index = 0;
for (i = 0; i < N; ++i ) {
double dx = path_x_storage[i] - x_;
double dy = path_y_storage[i] - y_;
double distance_to_path = sqrt( dx * dx + dy * dy );
if ( distance_to_path < min_distance_to_path ) {
min_distance_to_path = distance_to_path;
min_index = i;
}
}
double dx_p1, dy_p1, dx_p2, dy_p2, distance_to_path_p1, distance_to_path_p2;
dx_p1 = path_x_storage[min_index + 1] - x_;
dy_p1 = path_y_storage[min_index + 1] - y_;
distance_to_path_p1 = sqrt( dx_p1 * dx_p1 + dy_p1 * dy_p1 );
dx_p2 = path_x_storage[min_index - 1] - x_;
dy_p2 = path_y_storage[min_index - 1] - y_;
distance_to_path_p2 = sqrt( dx_p2 * dx_p2 + dy_p2 * dy_p2 );
int interval_start, interval_stop;
if (distance_to_path_p1 < distance_to_path_p2) {
// minimal distance in interval [min_index, min_index + 1]
interval_start = min_index;
interval_stop = min_index + 1;
} else {
// minimal distance in interval [min_index - 1, min_index]
interval_start = min_index - 1;
interval_stop = min_index;
}
// linear interpolation (unused)
double dx = path_x_storage[interval_stop] - path_x_storage[interval_start] ;
double dy = path_y_storage[interval_stop] - path_y_storage[interval_start] ;
index_start = interval_start;
index_closest = min_index;
distance = min_distance_to_path;
"""
array_type = dy.DataTypeArray(N, datatype=dy.DataTypeFloat64(1))
outputs = dy.generic_cpp_static(
input_signals=[
path_distance_storage, path_x_storage, path_y_storage, x, y
],
input_names=[
'path_distance_storage', 'path_x_storage', 'path_y_storage', 'x_',
'y_'
],
input_types=[
array_type, array_type, array_type,
dy.DataTypeFloat64(1),
dy.DataTypeFloat64(1)
],
output_names=['index_closest', 'index_start', 'distance'],
output_types=[
dy.DataTypeInt32(1),
dy.DataTypeInt32(1),
dy.DataTypeFloat64(1)
],
cpp_source_code=source_code)
index_start = outputs[0]
index_closest = outputs[1]
distance = outputs[2]
return index_closest, distance, index_start
from vehicle_lib.vehicle_lib import *
#import vehicle_lib.example_data as example_data
# load track data
with open("track_data/simple_track.json", "r") as read_file:
track_data = json.load(read_file)
# cfg
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")
def compile_path_tracker(par={},
target_template=None,
folder=None,
samples_in_buffer=10000):
"""
Build OpenRTDynamics code for the path tracker
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()
# time-series for velocity, position, orientation, ...
input_signals = {}
input_signals['Ts'] = dy.system_input(dy.DataTypeFloat64(1),
name='Ts',
default_value=0.01,
title="sampling time [s]")
input_signals['velocity'] = dy.system_input(dy.DataTypeFloat64(1),
name='velocity_',
default_value=1,
title="vehicle velocity [m/s]")
input_signals['velocity_dot'] = dy.system_input(
dy.DataTypeFloat64(1),
name='velocity_dot',
default_value=1,
title="vehicle acceleration [m/s^2] (optional)")
input_signals['x'] = dy.system_input(dy.DataTypeFloat64(1),
name='x',
default_value=0,
title="state x [m]")
input_signals['y'] = dy.system_input(dy.DataTypeFloat64(1),
name='y',
default_value=0,
title="state y [m]")
input_signals['x_dot'] = dy.system_input(
dy.DataTypeFloat64(1),
name='x_dot',
default_value=0,
title="state d/dt x [m/s] (optional)")
input_signals['y_dot'] = dy.system_input(
dy.DataTypeFloat64(1),
name='y_dot',
default_value=0,
title="state d/dt y [m/s] (optional)")
input_signals['Delta_u'] = dy.system_input(
dy.DataTypeFloat64(1),
name='Delta_u',
default_value=0,
title="Delta_u [rad] (optional)")
input_signals['Delta_u_dot'] = dy.system_input(
dy.DataTypeFloat64(1),
name='Delta_u_dot',
default_value=0,
title="Delta_u_dot[rad/s] (optional)")
# 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
#
output_signals = async_path_data_handler(input_sample_valid,
async_input_data_valid,
path_sample, path_tracking,
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')
# data sampled at the closest point on path
dy.append_output(output_signals['tracked_index'], 'tracked_index')
dy.append_output(output_signals['d_star'], 'd_star')
dy.append_output(output_signals['v_star'], 'v_star')
dy.append_output(output_signals['x_r'], 'x_r')
dy.append_output(output_signals['y_r'], 'y_r')
dy.append_output(output_signals['psi_r'], 'psi_r')
dy.append_output(output_signals['K_r'], 'K_r')
dy.append_output(output_signals['psi_r_dot'], 'psi_r_dot')
dy.append_output(output_signals['Delta_l'], 'Delta_l')
dy.append_output(output_signals['Delta_l_dot'], 'Delta_l_dot')
# 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
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 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
import openrtdynamics2.lang as dy
import os
import json
from colorama import init, Fore, Back, Style
init(autoreset=True)
import math
#
# Enter a new system (simulation)
#
system = dy.enter_system()
baseDatatype = dy.DataTypeFloat64(1)
# 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')
# 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=6.0, value_range=[0, 25], title="vehicle velocity")
max_lateral_velocity = dy.system_input( dy.DataTypeFloat64(1), name='max_lateral_velocity', default_value=1.0, value_range=[0, 4.0], title="maximal lateral velocity")
max_lateral_accleration = dy.system_input( dy.DataTypeFloat64(1), name='max_lateral_accleration', default_value=2.0, value_range=[1.0, 4.0], title="maximal lateral acceleration")
# parameters
wheelbase = 3.0
# sampling time
Ts = 0.01
# create storage for the reference path:
path = import_path_data(track_data)
# create placeholders for the plant output signals
x = dy.signal()
y = dy.signal()
