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 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 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 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_acceleration( v, v_dot, delta, delta_dot, psi_dot ):
"""
Compute the acceleration at the front axle
"""
a_lat = v_dot * dy.sin( delta ) + v * ( delta_dot + psi_dot ) * dy.cos( delta )
a_long = None
return a_lat, a_long
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 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 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 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 distance_to_line(x_s, y_s, x_e, y_e, x_test, y_test):
"""
compute the shortest distance to a line
returns a negative distance in case (x_test, y_test) is to the left of the vector pointing from
(x_s, y_s) to (x_e, y_e)
"""
Delta_x = x_e - x_s
Delta_y = y_e - y_s
x_test_ = x_test - x_s
y_test_ = y_test - y_s
psi = dy.atan2(Delta_y, Delta_x)
test_ang = dy.atan2(y_test_, x_test_)
delta_angle = dy.unwrap_angle(test_ang - psi, normalize_around_zero=True)
length_s_to_test = dy.sqrt(x_test_ * x_test_ + y_test_ * y_test_)
distance = dy.sin(delta_angle) * length_s_to_test
distance_s_to_projection = dy.cos(delta_angle) * length_s_to_test
return distance, distance_s_to_projection
# switch between IMU feedback and internal model
psi_feedback = psi_mdl
psi_feedback = dy.conditional_overwrite(psi_feedback, activate_IMU,
psi_measurement)
# path tracking
Delta_u = dy.float64(0.0)
steering = psi_r - psi_feedback + Delta_u
steering = dy.unwrap_angle(angle=steering, normalize_around_zero=True)
dy.append_output(Delta_u, 'Delta_u')
# internal model of carbody rotation (from bicycle model)
psi_mdl << dy.euler_integrator(
velocity * dy.float64(1.0 / wheelbase) * dy.sin(steering),
Ts,
initial_state=psi_measurement)
dy.append_output(psi_mdl, 'psi_mdl')
#
# The model of the vehicle including a disturbance
#
# the model of the vehicle
x_, y_, psi_real, *_ = lateral_vehicle_model(u_delta=steering,
v=velocity,
v_dot=dy.float64(0),
Ts=Ts,
wheelbase=wheelbase,
delta_disturbance=disturbance_ofs,
def compute_accelearation(v, v_dot, delta, delta_dot, psi_dot):
a_lat = v_dot * dy.sin(delta) + v * (delta_dot + psi_dot) * dy.cos(delta)
a_long = None
return a_lat, a_long
# 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()
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])
y = dy.signal().set_name('y')
psi = dy.signal().set_name('psi')
# controller error
error = reference - y
error.set_name('error')
steering = dy.float64(0.0) + k_p * error - psi
steering.set_name('steering')
sw = False
if sw:
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)
else:
def bicycle_model(delta, v):
x = dy.signal()
y = dy.signal()
psi = dy.signal()