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()
#
# 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(steering, 'steering')
dy.append_output(x_r, 'x_r')
dy.append_output(y_r, 'y_r')
dy.append_output(psi_r, 'psi_r')
dy.append_output(Delta_l, 'Delta_l')
dy.append_output(steering_disturbance, 'steering_disturbance')
dy.append_output(disturbed_steering, 'disturbed_steering')
dy.append_output(tracked_index, 'tracked_index')
dy.append_output(Delta_index, 'Delta_index')
# 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/path_following_control",
build=True)
#
dy.clear()
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') # 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/bicycle_model", build=True) # dy.clear()
dy.append_output(x, 'x') dy.append_output(y, 'y') dy.append_output(psi, 'psi') dy.append_output(steering, 'steering') dy.append_output(x_r, 'x_r') dy.append_output(y_r, 'y_r') dy.append_output(psi_r, 'psi_r') dy.append_output(Delta_l, 'Delta_l') dy.append_output(steering_disturbance, 'steering_disturbance') # dy.append_output(disturbed_steering, 'disturbed_steering') dy.append_output(tracked_index, 'tracked_index') dy.append_output(Delta_index, 'Delta_index') # main simulation ouput # if advanced_control: # dy.set_primary_outputs([ x, y, x_r, y_r, psi, psi_r, steering, Delta_l, distance_km1, distance_kp1, steering_disturbance, disturbed_steering, tracked_index, Delta_index, Delta_index_ahead, distance_residual, Delta_index_ahead_i1, K_r_ahead, Delta_l_r], # ['x', 'y', 'x_r', 'y_r', 'psi', 'psi_r', 'steering', 'Delta_l', 'distance_km1', 'distance_kp1', 'steering_disturbance', 'disturbed_steering', 'tracked_index', 'Delta_index', 'Delta_index_ahead', 'distance_residual', 'Delta_index_ahead_i1', 'K_r_ahead', 'Delta_l_r']) # generate code sourcecode, manifest = dy.generate_code(template=dy.TargetWasm(enable_tracing=False), folder="html/build/", build=True) # dy.clear()
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
# create placeholder for the plant output signal
angle = dy.signal()
angular_velocity = dy.signal()
angular_acceleration = dy.float64(
0) - g / length * dy.sin(angle) - (friction /
(mass * length)) * angular_velocity
angular_acceleration.set_name('angular_acceleration')
sampling_rate = 0.01
angular_velocity_ = euler_integrator(angular_acceleration, sampling_rate,
0.0).set_name('ang_vel')
angle_ = euler_integrator(angular_velocity_, sampling_rate,
30.0 / 180.0 * math.pi).set_name('ang')
angle << angle_
angular_velocity << angular_velocity_
# main simulation ouput
dy.set_primary_outputs([angle, angular_velocity])
sourcecode, manifest = dy.generate_code(template=dy.TargetWasm(),
folder="generated/",
build=True)
# print the sourcecode (main.cpp)
print(Style.DIM + sourcecode)
dy.clear()
# define the outputs
output_x = switch.outputs[0].set_name("ox")
output_v = switch.outputs[1].set_name("ov")
counter = switch.outputs[2].set_name("counter")
state_control = switch.state.set_name('state_control')
# set the outputs of the system
dy.set_primary_outputs([ output_x, output_v, state_control, counter ])
#sourcecode, manifest = dy.generate_code(template=dy.TargetWasm(), folder="generated/", build=True)
code_gen_template = dy.TargetWasm()
code_gen_results = dy.generate_code(template=code_gen_template, folder="generated/", build=True)
sourcecode, manifest = code_gen_results['sourcecode'], code_gen_results['manifest']
# print the sourcecode (main.cpp)
#print(Style.DIM + code_gen_template.get_algorithm_code())
algorithm_sourcecode = code_gen_template.get_algorithm_code()
from pygments import highlight
from pygments.style import Style
from pygments.token import Token
from pygments.lexers import get_lexer_by_name
from pygments.formatters import Terminal256Formatter, TerminalFormatter
import pygments.styles as styles
# controller error
error = reference - y
steering = dy.float64(0.0) + k_p * error - psi
x_dot = velocity * dy.cos(steering + psi)
y_dot = velocity * dy.sin(steering + psi)
psi_dot = velocity / dy.float64(wheelbase) * dy.sin(steering)
# integrators
sampling_rate = 0.01
x << euler_integrator(x_dot, sampling_rate, 0.0)
y << euler_integrator(y_dot, sampling_rate, 0.0)
psi << euler_integrator(psi_dot, sampling_rate, 0.0)
# main simulation ouput
dy.set_primary_outputs([x, y, psi, reference, steering, error],
['x', 'y', 'psi', 'refrence', 'steering', 'error'])
code_gen_results = dy.generate_code(template=dy.TargetWasm(),
folder="generated/",
build=True)
sourcecode, manifest = code_gen_results['sourcecode'], code_gen_results[
'manifest']
# print the sourcecode (main.cpp)
print(Style.DIM + sourcecode)
dy.clear()
