def predict_state(state, v, delta, state_type):
"""
Function: predict the next state based on control input
Input: state: current state (class); v, delta: control input; state_type: "class" or "list"
Output: return state represented in list
"""
Model_class = vehicle_model.load_model("RC_Car")
vehicle = Model_class()
beta = math.atan(math.tan(delta)*vehicle.L_r/vehicle.L)
if state_type == "class":
x_dot = v * math.cos(beta+state.yaw)
x = state.x + x_dot* DT
y_dot = v * math.sin(beta+state.yaw)
y = state.y + y_dot* DT
vx = float(v*cos(beta))
vy = float(v*sin(beta))
yaw_dot = v*math.tan(delta)*math.cos(beta) / vehicle.L
yaw = state.yaw + yaw_dot * DT
return [x, y, 0, yaw, vx, vy, yaw_dot] #self.state
else:
x_dot = float(v * math.cos(beta+state[3]))
x = state[0] + x_dot* DT
y_dot = float(v * math.sin(beta+state[3]))
y = state[1] + y_dot* DT
vx = float(v*cos(beta))
vy = float(v*sin(beta))
yaw_dot = float(v*math.tan(delta)*math.cos(beta) / vehicle.L)
yaw = state[3] + yaw_dot * DT
return [x, y, 0, yaw, vx, vy, yaw_dot]
def update_state_ge(state, v_cmd, delta):
"""
Function: state update based on numerically stable dynamic bicycle model (Ref: https://arxiv.org/abs/2011.09612)
Input: state and control action
Output: return updated state
"""
Model_class = vehicle_model.load_model("RC_Car")
vehicle = Model_class()
state.x = state.x + DT * (state.vx * np.cos(state.yaw) -
state.vy * np.sin(state.yaw))
state.y = state.y + DT * (state.vy * np.cos(state.yaw) +
state.vx * np.sin(state.yaw))
state.yaw = state.yaw + state.yaw_dot * DT
state.vx = v_cmd
state.vy = (vehicle.m * state.vx * state.vy + DT *
(vehicle.L_f * vehicle.C_alpha - vehicle.L_r * vehicle.C_alpha)
* state.yaw_dot - DT * vehicle.C_alpha * delta * state.vx -
DT * vehicle.m * state.vx**2 * state.yaw_dot) / (
vehicle.m * state.vx - DT * 2 * vehicle.C_alpha)
state.yaw_dot = (
vehicle.I_z * state.vx * state.yaw_dot + DT *
(vehicle.L_f * vehicle.C_alpha - vehicle.L_r * vehicle.C_alpha) *
state.vy - DT * vehicle.L_f * vehicle.C_alpha * delta * state.vx
) / (vehicle.I_z * state.vx - DT *
(vehicle.L_f**2 * vehicle.C_alpha + vehicle.L_r**2 * vehicle.C_alpha))
state.v = math.sqrt(state.vx**2 + state.vy**2)
beta = math.atan2(state.vy, state.vx)
state.x_dot = state.v * math.cos(beta + state.yaw)
state.y_dot = state.v * math.sin(beta + state.yaw)
return state
def update_state_slip(state, v, delta):
"""
Function: state update based on kinematic bicycle model considering slip
Input: state and control action
Output: return updated state
"""
Model_class = vehicle_model.load_model("RC_Car")
vehicle = Model_class()
beta = math.atan(math.tan(delta)*vehicle.L_r/vehicle.L)
state.x_dot = v * math.cos(beta+state.yaw)
state.x = state.x + state.x_dot* DT
state.y_dot = v * math.sin(beta+state.yaw)
state.y = state.y + state.y_dot* DT
state.vx = float(v*cos(beta))
state.vy = float(v*sin(beta))
state.v = np.sqrt(state.vx**2+state.vy**2)
state.yaw_dot = v*math.tan(delta)*math.cos(beta) / vehicle.L
state.yaw = state.yaw + state.yaw_dot * DT
return state
def compute_curvature(delta):
"""
Function: compute curvature based on proposed steering angle
Input: steering angle
Output: curvature
"""
Model_class = vehicle_model.load_model("RC_Car")
vehicle = Model_class()
beta = math.atan(math.tan(delta)*vehicle.L_r/vehicle.L)
curvature = math.tan(delta)*math.cos(beta)/vehicle.L
return curvature
def tire_dyn_r(v_x, wheel_vx, alpha):
"""
function: tire force calculate
"""
Model_class = vehicle_model.load_model("RC_Car")
vehicle = Model_class()
##find longitudinal wheel slip K (kappa)
if np.abs(wheel_vx - v_x) < 0.01 or (np.abs(wheel_vx) < 0.01
and np.abs(v_x) < 0.01):
K = 0
elif np.abs(v_x) < 0.01: #infinite slip, longitudinal saturation
K = math.inf
Fx = np.sign(wheel_vx) * vehicle.mu * vehicle.load_r
Fy = 0
return Fx, Fy
else:
K = (wheel_vx - v_x) / np.abs(v_x)
###instead of avoiding -1, now look for positive equivalent
if K < 0:
spin_dir = -1
K = np.abs(K)
else:
spin_dir = 1
#alpha > pi/2 cannot be adapted to this formula
#because of the use of tan(). Use the equivalent angle instead.
#alpha > pi/2 means vehicle moving backwards
#Fy sign has to be reversed, but the *sign(alpha) will take care of it
if np.abs(alpha) > np.pi / 2:
alpha = (np.pi - np.abs(alpha)) * np.sign(alpha)
gamma = np.sqrt(vehicle.C_x**2 * (K / (1 + K))**2 + vehicle.C_alpha**2 *
(np.tan(alpha) / (1 + K))**2)
if gamma <= 3 * vehicle.mu * vehicle.load_r:
F = gamma - 1 / (3 * vehicle.mu * vehicle.load_r) * gamma**2 + 1 / (
27 * vehicle.mu**2 * vehicle.load_r**2) * gamma**3
else:
#more accurate modeling with peak friction value
F = vehicle.mu_s * vehicle.load_r
if gamma == 0:
Fx = 0
Fy = 0
else:
Fx = vehicle.C_x / gamma * (K / (1 + K)) * F * spin_dir
Fy = -vehicle.C_alpha / gamma * (np.tan(alpha) / (1 + K)) * F
return Fx, Fy
def update_state_ffast(state, vc, delta):
"""
Function: state update based on dynamic bicycle model used in ffast
Input: state and control action
Output: return updated state
"""
Model_class = vehicle_model.load_model("RC_Car")
vehicle = Model_class()
if np.abs(state.vx) < 0.01 and np.abs(state.vy) < 0.01:
alpha_f = 0
alpha_r = 0
else:
alpha_f = math.atan2(
(state.vy + vehicle.L_f * state.yaw_dot), state.vx) - delta
alpha_r = math.atan2((state.vy - vehicle.L_r * state.yaw_dot),
state.vx)
F_yf = tire_dyn_f(alpha_f)
F_xr, F_yr = tire_dyn_r(state.vx, vc, alpha_r)
T_z = vehicle.L_f * F_yf * np.cos(delta) - vehicle.L_r * F_yr
ma_x = F_xr - F_yf * np.sin(delta)
ma_y = F_yf * np.cos(delta) + F_yr
####without damping
yawdot_dot = T_z / vehicle.I_z
vx_dot = ma_x / vehicle.m + state.yaw_dot * state.vy
vy_dot = ma_y / vehicle.m - state.yaw_dot * state.vx
####with damping
# yawdot_dot = T_z/vehicle.I_z -0.02*state.yaw_dot
# vx_dot = ma_x/vehicle.m + state.yaw_dot*state.vy -0.025*state.vx
# vy_dot = ma_y/vehicle.m - state.yaw_dot*state.vx -0.025*state.vy
###translate to inertial frame
state.v = math.sqrt(state.vx**2 + state.vy**2)
beta = math.atan2(state.vy, state.vx)
state.x_dot = state.v * math.cos(beta + state.yaw)
state.y_dot = state.v * math.sin(beta + state.yaw)
state.x = state.x + state.x_dot * DT
state.y = state.y + state.y_dot * DT
state.yaw = state.yaw + state.yaw_dot * DT
state.vx = state.vx + vx_dot * DT
state.vy = state.vy + vy_dot * DT
state.yaw_dot = state.yaw_dot + yawdot_dot * DT
return state
def update_state_linear_tire(state, v_cmd, delta):
"""
Function: state update based on dynamic bicycle model with linear tire model
Input: state and control action
Output: return updated state
"""
Model_class = vehicle_model.load_model("RC_Car")
vehicle = Model_class()
if np.abs(state.vy) <= 0.01 or np.abs(state.vx) <= 0.01:
state = update_state_slip(state, v_cmd, delta)
return state
else:
theta_f = (state.vy + vehicle.L_f * state.yaw_dot) / state.vx
theta_r = (state.vy - vehicle.L_r * state.yaw_dot) / state.vx
a22 = -4.0 * vehicle.C_alpha / (vehicle.m * state.vx)
a24 = -state.vx - (2.0 * vehicle.C_alpha * vehicle.L_f -
2.0 * vehicle.C_alpha * vehicle.L_r) / (vehicle.m *
state.vx)
a42 = (-2.0 * vehicle.L_f * vehicle.C_alpha +
2.0 * vehicle.L_r * vehicle.C_alpha) / (vehicle.I_z * state.vx)
a44 = (-2.0 * vehicle.L_f * vehicle.L_f * vehicle.C_alpha -
2.0 * vehicle.L_r * vehicle.L_r * vehicle.C_alpha) / (
vehicle.I_z * state.vx)
b2 = (2.0 * vehicle.C_alpha) / vehicle.m
b4 = (2.0 * vehicle.L_f * vehicle.C_alpha) / (vehicle.I_z)
vy_dot = state.vy * a22 + state.yaw_dot * a24 + delta * b2
yaw_dotdot = state.vy * a42 + state.yaw_dot * a44 + delta * b4
#print("linear tire parameters", a22, a24, a42, a44, b2, b4)
state.vx = v_cmd
beta = math.atan2(state.vy, state.vx)
state.v = math.sqrt(state.vx**2 + state.vy**2)
state.x_dot = state.v * math.cos(beta + state.yaw)
state.y_dot = state.v * math.sin(beta + state.yaw)
state.x = state.x + state.x_dot * DT
state.y = state.y + state.y_dot * DT
state.yaw = state.yaw + state.yaw_dot * DT
state.vy = state.vy + vy_dot * DT
state.yaw_dot = state.yaw_dot + yaw_dotdot * DT
return state
def tire_dyn_f(alpha):
"""
function: tire force calculate
"""
Model_class = vehicle_model.load_model("RC_Car")
vehicle = Model_class()
#alpha > pi/2 cannot be adapted to this formula
#because of the use of tan(). Use the equivalent angle instead.
#alpha > pi/2 means vehicle moving backwards
#Fy sign has to be reversed, but the *sign(alpha) will take care of it
if np.abs(alpha) > pi/2:
alpha = (np.pi-np.abs(alpha))*np.sign(alpha)
alpha_sl = math.atan(3*vehicle.mu*vehicle.load_f/vehicle.C_alpha)
if np.abs(alpha) <= alpha_sl:
Fy = -vehicle.C_alpha*np.tan(alpha) + vehicle.C_alpha**2/(3*vehicle.mu*vehicle.load_f)*np.abs(np.tan(alpha))*np.tan(alpha) - vehicle.C_alpha**3/(27*vehicle.mu**2*vehicle.load_f**2)*np.tan(alpha)**3
else:
Fy = -vehicle.mu*vehicle.load_f*np.sign(alpha)
return Fy
def simulate(vehicle_type, old_state, command, ODE, DT):
'''
Function: model-based state update
vehicle_type: "RC_Car"/"MRZR"
old_state: [pos_x, pos_y, pos_z, yaw, vx, vy, yaw_dot]
command: [delta, v]
ODE: options "nonlinear_without_beta", "linear", "nonlinear_with_beta", "linear_tire"
DT: sampling time
'''
state_predict = list()
delta = command[1]
v = command[0]
Model_class = vehicle_model.load_model(vehicle_type)
model = Model_class()
if ODE == "nonlinear_without_beta":
x_dot = v * math.cos(old_state[3])
x = old_state[0] + x_dot * DT
y_dot = v * math.sin(old_state[3])
y = old_state[1] + y_dot * DT
rot = np.array([[np.cos(old_state[3]), -np.sin(old_state[3])],
[np.sin(old_state[3]),
np.cos(old_state[3])]])
rot_inv = np.linalg.inv(rot)
vx = rot_inv.dot(np.array([[x_dot], [y_dot]]))[0, 0]
vy = rot_inv.dot(np.array([[x_dot], [y_dot]]))[1, 0]
yaw_dot = v / model.L * math.tan(delta)
yaw = old_state[3] + yaw_dot * DT
state_predict = [x, y, 0, yaw, vx, vy, yaw_dot]
if ODE == "linear_tire":
vx = old_state[4]
vy = old_state[5]
phi = old_state[3]
alpha_f = math.atan2(
(vy + model.L_f * old_state[6]) / vx, 1) - delta if vx != 0 else 0
alpha_r = math.atan2(
(vy - model.L_f * old_state[6]) / vx, 1) if vx != 0 else 0
k = 0
F_xf = model.C_x * k
F_xr = model.C_x * k
F_yf = -1 * model.C_alpha * alpha_f
F_yr = -1 * model.C_alpha * alpha_r
vx_dot = old_state[6] * vy + (F_xr - F_yf * math.sin(delta)) / model.m
vy_dot = -1 * old_state[6] * vx + (F_xr * math.cos(delta) +
F_yf) / model.m
yaw_dotdot = (model.L_f * F_yf - model.L_r * F_yr) / model.I_z
# v = math.sqrt(v_x*v_x+v_y*v_y)
# beta = math.atan2(v_y/v_x, 1)
# x_dot = v*math.cos(beta+old_state[3])# or x_dot*cos(phi)-y_dot*sin(phi)
# y_dot = v*math.sin(beta+old_state[3])# or x_dot*sin(phi)+y_dot*cos(phi)
#yaw_dot = v / model.L * math.tan(delta) #should update it?
vx = old_state[4] + vx_dot * DT
vy = old_state[5] + vy_dot * DT
yaw_dot = old_state[6] + yaw_dotdot * DT
x_dot = vx * np.cos(phi) - vy * np.sin(phi)
y_dot = vx * np.sin(phi) + vy * np.cos(phi)
x = old_state[0] + x_dot * DT
y = old_state[1] + y_dot * DT
yaw = old_state[3] + yaw_dot * DT
state_predict = [x, y, 0, yaw, vx, vy, yaw_dot]
return state_predict
def do_simulation(initial_state):
"""
Simulation
"""
Model_class = vehicle_model.load_model("RC_Car")
vehicle = Model_class()
state = initial_state
uncertainty_estimation_method = 'gp' #option['gp', 'sampling']SUE-GP and sampling distribution approach
mode = "rounded_square" #["straight", "circle", "rounded_square"],
clock = 0.0
x_list = [state.x]
x_dot_list = [state.x_dot]
y_list = [state.y]
y_dot_list = [state.y_dot]
z_list = [0.0]
yaw_list = [state.yaw]
yaw_dot_list = [state.yaw_dot]
v_list = [state.v]
t_list = []
d_list = []
a_list = []
wp_x_list = []
wp_y_list = []
wp_cur_list = []
timestamp_list = []
vc_list = []
xg_list = []
yg_list = []
pl = Policy(planner_mode="rounded_square")#straight, rounded_square, circle
ffi = FFI()
monitor = ffi.dlopen(os.getcwd()+"/utility/heading_monitor.so")
ffi.cdef("""
typedef struct parameters {
long double dt;
long double ep;
long double maxCurv;
long double maxV;
} parameters;
typedef struct state {
long double v;
long double curv;
long double lx;
long double ly;
long double rx;
long double ry;
long double t;
} state;
typedef struct input input;
typedef struct verdict { int id; long double val; } verdict;
verdict boundaryDist(state pre, state curr, const parameters* const params);
""")
params = ffi.new("struct parameters*")
curr = ffi.new("struct state*")
next = ffi.new("struct state*") #future state from model-based prediction
post = ffi.new("struct state*") #future state for keymera approach
last_state_timestamp = 1
ifplot = False #False
total_t = 0 #total running time, what unit?
counter = 0 #interation number
max_t = 0 #max runtime in each iteration
min_t = 100 #min runtime in each iteration
vr_reach = 0
vr_monitor = 0
vr_reach_list = []
vr_monitor_plant_list = [[], []] #value&ID
vr_monitor_control_list = [[], []] #value&ID
ctrl_verdict_value = 0
###############initialization#############
params.dt = constant.DT #?? TODO
params.ep = 0.1#0.4
params.maxCurv = 10#0.2
params.maxV = 6 #TODO
curr.v = 0.0
curr.curv = 0.0
curr.lx = 0.0
curr.ly = 0.0
curr.rx = 0.0
curr.ry = 0.0
curr.t = 0.0
counter_timeout = 0
bad_position_x = []
bad_position_y = []
counter_inside = 0 #how many future state is actually included by reachability
safety_violated = False #set true if unsafe
uncertainty_estimation_method = 'gp' #option['gp', 'sampling']SUE-GP and sampling distribution approach
x_list_buffer, y_list_buffer = [], []
counter_eg_x, counter_eg_y = [], []
counter_flow_x, counter_flow_y = [], []
x_all, y_all, ind_all, flow_x_all, flow_y_all = [], [], [], [], []
rec_x_all, rec_y_all, rec_yaw_all = [], [], []
delta_t_all = []
reach_verdict_value_all = []
verify_result = [[], []]
frames = []
start_sec = tm.time()
bad_cnt = 0
data_f = open ("datalog.txt", "w")
pre_ctrl_verdict_value = 1
prev_action = [0, 0]
state_buffer = []
control_buffer = []
cnt = 0
while MAX_TIME >= clock:
current_state_timestamp = clock
x0 = [state.x, state.y, state.yaw, state.vx, state.vy, state.yaw_dot] # current state
action, waypoint, curvature, wp_heading = pl.get_action1(x0) #get from aa_planner, [velocity, angle]
current_state = [state.x, state.y, 0, state.yaw, state.vx, state.vy, state.yaw_dot] #self.state
new_state = modelplex.rotate_state(modelplex.translate_state(x0, (-waypoint[0], -waypoint[1])), -wp_heading)
x_new, y_new, yaw_new, vx_new, vy_new, yawdot_new = new_state[0:6]
c = rmin * np.cos(yaw_new)
s = rmin * np.sin(yaw_new)
rx = y_new + c
lx = y_new - c
ry = x_new - s
ly = x_new + s
proposed_action = action[:]
current_waypoint = [waypoint[0], waypoint[1], curvature]
data_f.write(str(state.x)+" "+str(state.y)+" "+str(0)+" "+str(state.yaw)+" "+str(state.vx)+" "+str(state.vy)+" "+str(state.yaw_dot)+" ")
data_f.write(str(current_waypoint[0])+" "+str(current_waypoint[1])+" "+str(current_waypoint[2])+" ")
data_f.write(str(proposed_action[0])+" "+str(proposed_action[1]))
data_f.write("\n")
########################simulation of bad control
if inject_bad_control == True:
#bad_ctr_sequence = [[0.7, -0.7], [0.7, -0.7], [0.7, -0.7]] #for kinematic model reachability experiment, DT=0.1
bad_ctr_sequence = [[0.7, -0.7] for i in range(6)] #for kinematic model reachability experiment, DT=0.05
#bad_ctr_sequence = [[0.7, -0.7] for i in range(15)] #for dynamic model reachability experiment, DT=0.02
if(state.x > 1.0 and cnt < len(bad_ctr_sequence)):
proposed_action[0] = bad_ctr_sequence[cnt][0]
proposed_action[1] = bad_ctr_sequence[cnt][1]
print("clock %5.2f:, inject bad control (%5.2f, %5.2f)" %(clock, bad_ctr_sequence[cnt][0], bad_ctr_sequence[cnt][1]))
print("action got from CPO: (%5.2f, %5.2f)" %(action[0], action[1]))
print("x-y position: (%5.2f, %5.2f); waypoint: (%5.2f, %5.2f)\n" %(current_state[0], current_state[1], current_waypoint[0], current_waypoint[1]))
cnt += 1
# ##################control verification using monitor#################
curr.v = float(post.v)
curr.curv = float(post.curv)
curr.t = float(post.t)
post.v = proposed_action[0]#np.sqrt(state.vx**2+state.vy**2)
post.curv = compute_curvature(proposed_action[1])#curvature
post.lx = lx
post.ly = ly
post.rx = rx
post.ry = ry
post.t = 0 ##TODO
curr.lx = float(post.lx)
curr.ly = float(post.ly)
curr.rx = float(post.rx)
curr.ry = float(post.ry)
ctrl_monitor_verdict = monitor.boundaryDist(curr[0], post[0], params)
ctrl_verdict_value = float(ctrl_monitor_verdict.val)
ctrl_verdict_id = float(ctrl_monitor_verdict.id)
# ##################control verification using reachability##############
speed_bound = 0.01
delta_bound = 0.01
uncertainty_control = [speed_bound, delta_bound]
uncertainty_state = [0.01, 0.01, 0, 0.01, 0.01, 0.01, 0.01] #(x, y, z, yaw, vx, vy, yaw_dot)
start = datetime.datetime.now()
x_bound, y_bound, yaw_bound, vx_bound, vy_bound, yawdot_bound = flowstar.execute_flowstar(DT, vehicle_type, ODE, horizon, current_state,
proposed_action, waypoint[0], waypoint[1], uncertainty_state, uncertainty_control, flowstar_stepsize, vehicle.L, vehicle.width, full_brake=False)
#####apply full brake
last2_state_x = 0.5*(x_bound[-1][0]+x_bound[-1][1])
last2_state_y = 0.5*(y_bound[-1][0]+y_bound[-1][1])
last2_state_yaw = 0.5*(yaw_bound[-1][0]+yaw_bound[-1][1])
last2_state_vx = 0.5*(vx_bound[-1][0]+vx_bound[-1][1])
last2_state_vy = 0.5*(vy_bound[-1][0]+vy_bound[-1][1])
last2_state_yawdot = 0.5*(yawdot_bound[-1][0]+yawdot_bound[-1][1])
last2_state = [last2_state_x, last2_state_y, 0, last2_state_yaw, last2_state_vx, last2_state_vy, last2_state_yawdot]
last2_state_x_uc = 0.5*np.abs(x_bound[-1][1]-x_bound[-1][0]-vehicle.L)
last2_state_y_uc = 0.5*np.abs(y_bound[-1][1]-y_bound[-1][0]-vehicle.width)
last2_state_yaw_uc = 0.5*np.abs(yaw_bound[-1][1]-yaw_bound[-1][0])
last2_state_vx_uc = 0.5*np.abs(vx_bound[-1][1]-vx_bound[-1][0])
last2_state_vy_uc = 0.5*np.abs(vy_bound[-1][1]-vy_bound[-1][0])
last2_state_yawdot_uc = 0.5*np.abs(yawdot_bound[-1][1]-yawdot_bound[-1][0])
last2_state_uc = [last2_state_x_uc, last2_state_y_uc, [], last2_state_yaw_uc, last2_state_vx_uc, last2_state_vy_uc, last2_state_yawdot_uc]
last_proposed_action = [proposed_action[0], 0]
uncertainty_control = [0.01, 0.01]
x_bound_last, y_bound_last, yaw_bound_last, vx_bound_last, vy_bound_last, yawdot_bound_last = flowstar.execute_flowstar(DT, vehicle_type, ODE, horizon_b, last2_state,
last_proposed_action, waypoint[0], waypoint[1], last2_state_uc, uncertainty_control, flowstar_stepsize, vehicle.L, vehicle.width, full_brake=True)
end = datetime.datetime.now()
dt = (end-start).total_seconds()*1000
print("Reachability runtime %5.2f" %dt)
x_segs = []
y_segs = []
for i in range(len(x_bound)):
x_segs.append([x_bound[i][0], x_bound[i][1], x_bound[i][1], x_bound[i][0]])
y_segs.append([y_bound[i][0], y_bound[i][0], y_bound[i][1], y_bound[i][1]])
#append last reachable set of full brake
for i in range(len(x_bound_last)):
x_segs.append([x_bound_last[i][0], x_bound_last[i][1], x_bound_last[i][1], x_bound_last[i][0]])
y_segs.append([y_bound_last[i][0], y_bound_last[i][0], y_bound_last[i][1], y_bound_last[i][1]])
veri_para = verification.Verify_para()
vr_reach_segs = list() #reachability result for each flow segment
vr_reach = verification.verify_reach(x_segs, y_segs, current_state, current_waypoint) #liveness-like verification using reachability
for i in range(len(x_segs)):
vr_collision = verification.verify_dubin(veri_para, mode, x_segs[i], y_segs[i], current_state, current_waypoint)
#vr_reach_segs.append(vr_collision or vr_reach) #reachability verifies "reach-and-avoid"
vr_reach_segs.append(vr_collision) #reachability verifies only "avoid"
if True in vr_reach_segs:
reach_verdict_value = -1
else:
reach_verdict_value = 1
rec_x, rec_y, rec_yaw = [], [], []
if ctrl_verdict_value < 0:
print("DETECT: x-y (%5.2f, %5.2f), waypoint (%5.2f, %5.2f)" %(state.x, state.y, current_waypoint[0], current_waypoint[1]))
print ("clock: %5.2f, reach verdict %d, control (%5.2f, %d) \n" %(clock, reach_verdict_value, ctrl_verdict_value, ctrl_verdict_id))
if fallback == True:
option = "robust"#"nonrobust"#"robust"#
if pre_ctrl_verdict_value > 0 and ctrl_verdict_value < 0: #modelPlex violation just starts
op_control, op_state, rec_x, rec_y, rec_yaw = sample_control1(monitor, wp_heading, current_state, current_state, prev_action, current_waypoint, curr, next, params, clock, option)
control_buffer.append(op_control)
state_buffer.append(op_state)
if pre_ctrl_verdict_value<0 and ctrl_verdict_value < 0 and reach_verdict_value>0: #small modelPlex violation continues
op_control, op_state, rec_x, rec_y, rec_yaw = sample_control1(monitor, wp_heading, current_state, state_buffer[-1], control_buffer[-1], current_waypoint, curr, next, params, clock, option)
control_buffer.append(op_control)
state_buffer.append(op_state)
if ctrl_verdict_value < 0 and reach_verdict_value<0: #large modelPlex violation
op_control, op_state, rec_x, rec_y, rec_yaw = sample_control1(monitor, wp_heading, current_state, state_buffer[-1], control_buffer[-1], current_waypoint, curr, next, params, clock, option)
control_buffer.append(op_control)
state_buffer.append(op_state)
bk_state = state_buffer[-1]
bk_control = control_buffer[-1]
tgt_state = predict_state(bk_state, state_buffer[-1][0], state_buffer[-1][1], "list")
tgt_x = tgt_state[0]
tgt_y = tgt_state[1]
tgt_v = 0.8#TODO
tgt_yaw = tgt_state[3]
start = datetime.datetime.now()
# code for new mpc
odelta, oa = None, None
X_int = [state.x, state.y, state.v, state.yaw]
X_ref = np.zeros((4, 2))
dref = np.zeros((1, 2))
X_ref[0,0] = tgt_x
X_ref[1,0] = tgt_y
X_ref[2,0] = tgt_v
X_ref[3,0] = tgt_yaw
X_ref[0,1] = tgt_x
X_ref[1,1] = tgt_y
X_ref[2,1] = tgt_v
X_ref[3,1] = tgt_yaw
dref[0,0] = 0 #steering angle reference
dref[0,1] = 0
oa, odelta, ox, oy, oyaw, ov = tubempcCVX.mpc(
X_ref, X_int, dref, oa, odelta)
if odelta is not None:
di, ai = odelta[0], oa[0]
#####################
proposed_action[0] = state.v+ai
proposed_action[1] = di
print("clock: %5.2f, mpc control!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! (acc: %5.2f, angle: %5.2f)" %(clock, proposed_action[0], proposed_action[1]))
end = datetime.datetime.now()
dt = (end-start).total_seconds()*1000
print("Tube MPC runtime %5.2f" %dt)
prev_action = [proposed_action[0], proposed_action[1]]
pre_ctrl_verdict_value = ctrl_verdict_value
di = proposed_action[1]
ai = proposed_action[0]-state.v
reach_verdict_value_all.append(reach_verdict_value)
rec_x_all.append(rec_x)
rec_y_all.append(rec_y)
rec_yaw_all.append(rec_yaw)
x_list.append(state.x)
x_dot_list.append(state.x_dot)
y_list.append(state.y)
y_dot_list.append(state.y_dot)
z_list.append(0.0)
yaw_list.append(state.yaw)
yaw_dot_list.append(state.yaw_dot)
v_list.append(state.v)
t_list.append(clock)
d_list.append(di)
a_list.append(ai)
vc_list.append(action[0])
wp_x_list.append(waypoint[0])
wp_y_list.append(waypoint[1])
wp_cur_list.append(curvature)
timestamp_list.append(start_sec+clock)
vr_monitor_control_list[0].append(ctrl_verdict_value)
vr_monitor_control_list[1].append(ctrl_verdict_id)
if save_gif:
frame = plot(t_list, wp_x_list, wp_y_list, wp_cur_list, state, clock, di, current_state, veri_para, x_segs, y_segs, vr_reach_segs, reach_verdict_value_all,
mode, vr_monitor_plant_list, vr_monitor_control_list, xg_list, yg_list, rec_x, rec_y, rec_yaw)
frames.append(frame)
#state = update_state(state, ai, di)
state = update_state_slip(state, proposed_action[0], proposed_action[1])
#state = update_state_ffast(state, proposed_action[0], proposed_action[1])
#state = update_state_linear_tire(state, proposed_action[0], proposed_action[1])
#state = update_state_ge(state, proposed_action[0], proposed_action[1])
clock = clock + DT
if save_gif:
gif.save(frames, "simulation.gif", duration=100)
return t_list, x_list, y_list, yaw_list, v_list, d_list, a_list, reach_verdict_value_all, vr_monitor_control_list
def execute_flowstar(DT, vehicle_type, ODE, horizon, state, command,
waypoint_x, waypoint_y, uncertainty_state,
uncertainty_control, stepsize, length, width, full_brake):
'''
Function: External execution of reachability computation by calling ./RC_bicycle
Input: type of dynamic model, state (x, y, z, yaw, vx, vy, yaw_dot),
command, waypoint coordinate, control bounds
Output: flowpipe points for visulization, safety indicator
'''
uncertainty_x = uncertainty_state[0]
uncertainty_y = uncertainty_state[1]
uncertainty_yaw = uncertainty_state[3]
uncertainty_vx = uncertainty_state[4]
uncertainty_vy = uncertainty_state[5]
uncertainty_yawdot = uncertainty_state[6]
uncertainty_v = uncertainty_control[0]
uncertainty_delta = uncertainty_control[1]
horizon = stepsize * horizon
#uncertainty_angle = delta_bound #control steering angle uncertainty, SUE-GP
#uncertainty_speed = speed_bound #control velocity uncertainty, SUE-GP
rot = np.array([[np.cos(state[3]), -np.sin(state[3])],
[np.sin(state[3]), np.cos(state[3])]])
x_dot = rot.dot(np.array([[state[3]], [state[4]]]))[0, 0]
y_dot = rot.dot(np.array([[state[3]], [state[4]]]))[1, 0]
pos_x = [-uncertainty_x, uncertainty_x]
pos_x_I = interval(pos_x[0], pos_x[1])
pos_y = [-uncertainty_y, uncertainty_y]
pos_y_I = interval(pos_y[0], pos_y[1])
yaw = [state[3] - uncertainty_yaw, state[3] + uncertainty_yaw]
yaw_I = interval(yaw[0], yaw[1])
vx = [state[4] - uncertainty_vx, state[4] + uncertainty_vx]
vx_I = interval(vx[0], vx[1])
vy = [state[5] - uncertainty_vy, state[5] + uncertainty_vy]
vy_I = interval(vy[0], vy[1])
yaw_dot = [state[6] - uncertainty_yawdot, state[6] + uncertainty_yawdot]
yaw_dot_I = interval(yaw_dot[0], yaw_dot[1])
v = [command[0] - uncertainty_v, command[0] + uncertainty_v]
v_I = interval(v[0], v[1])
delta = [command[1] - uncertainty_delta, command[1] + uncertainty_delta]
delta_I = interval(delta[0], delta[1])
if full_brake == False:
min_a = min((v[0] - state[4]) / DT, (v[1] - state[4]) / DT)
max_a = max((v[0] - state[4]) / DT, (v[1] - state[4]) / DT)
a_I = interval(min_a, max_a)
else:
a_I = interval(
-8.01, -7.99
) #interval(-10.01, -9.99) #TODO: maximum acc/dec set to 2 in rc-car
model_class = vehicle_model.load_model(vehicle_type)
vehicle = model_class()
####################for model without beta#########################
exec_file = os.path.join(ODE, './RC_bicycle')
if ODE == 'nonlinear_without_beta':
args = [
exec_file,
str(pos_x_I[0][0]),
str(pos_x_I[1][0]),
str(pos_y_I[0][0]),
str(pos_y_I[1][0]),
str(yaw_I[0][0]),
str(yaw_I[1][0]),
str(delta_I[0][0]),
str(delta_I[1][0]),
str(v_I[0][0]),
str(v_I[1][0]),
str(a_I[0][0]),
str(a_I[1][0]),
str(horizon),
str(stepsize),
str(vehicle.L)
]
####################for model with beta############################
elif ODE == 'nonlinear_with_beta':
beta_I = interval_estimation.initial_beta_interval(vehicle, delta_I)
co = './RC_bicycle '+str(pos_x_I[0][0]) + ' '+ str(pos_x_I[0][1]) +' ' + str(pos_y_I[0][0]) + ' '+ str(pos_y_I[0][1]) +\
' ' + str(yaw_I[0][0]) +' '+ str(yaw_I[0][1]) + ' ' + str(beta_I[0][0]) + ' ' +str(beta_I[0][1])+\
' '+str(v_I[0][0]) + ' '+str(v_I[0][1])
os.system('cd ' + ODE + ';' + co)
elif ODE == 'linear':
a13 = -1 * command[0] * np.sin(state[3])
a23 = command[0] * np.cos(state[3])
b11 = np.cos(state[3])
b21 = np.sin(state[3])
b31 = np.tan(command[1]) / 0.12
b32 = command[0] / (0.12 * np.cos(command[1]) * np.cos(command[1]))
co = './RC_bicycle '+ str(a13) +' '+str(a23) + ' ' + str(b11) + ' '+str(b21)+' '+str(b31)+' '+ str(b32) +\
' '+str(pos_x_I[0][0]) + ' '+ str(pos_x_I[0][1]) +' ' + str(pos_y_I[0][0]) + ' '+ str(pos_y_I[0][1]) + ' ' + str(yaw_I[0][0]) +\
' '+ str(yaw_I[0][1]) + ' ' +str(v_I[0][0]) + ' '+str(v_I[0][1]) + ' '+ str(delta_I[0][0]) +\
' '+ str(delta_I[0][1]) + ' '+str(-1*speed_bound)+' '+str(speed_bound) + ' '+str(-1*delta_bound) +' '+str(delta_bound)
os.system('cd ' + ODE + ';' + co)
else: #'linear_tire'
a22 = -4.0 * vehicle.C_alpha / (vehicle.m * command[0])
a24 = -command[0] - (2.0 * vehicle.C_alpha * vehicle.L_f -
2.0 * vehicle.C_alpha * vehicle.L_r) / (
vehicle.m * command[0])
a42 = (-2.0 * vehicle.L_f * vehicle.C_alpha + 2.0 * vehicle.L_r *
vehicle.C_alpha) / (vehicle.I_z * command[0])
a44 = (-2.0 * vehicle.L_f * vehicle.L_f * vehicle.C_alpha -
2.0 * vehicle.L_r * vehicle.L_r * vehicle.C_alpha) / (
vehicle.I_z * command[0])
b2 = (2.0 * vehicle.C_alpha) / vehicle.m
b4 = (2.0 * vehicle.L_f * vehicle.C_alpha) / (vehicle.I_z)
a24_1 = add_sign(a24)
a44_1 = add_sign(a44)
b2_1 = add_sign(b2)
b4_1 = add_sign(b4)
# print('x', str(pos_x_I[0][0]), str(pos_x_I[1][0]))
# print('y', str(pos_y_I[0][0]), str(pos_y_I[1][0]))
# print('vx', str(vx_I[0][0]), str(vx_I[1][0]))
# print('vy', str(vy_I[0][0]), str(vy_I[1][0]))
# print('yaw', str(yaw_I[0][0]), str(yaw_I[1][0]))
# print(state[6]-uncertainty_yawdot, state[6]+uncertainty_yawdot)
# print(yaw_dot_I)
# print(delta_I)
# print('dyaw', str(yaw_dot_I[0][0]), str(yaw_dot_I[1][0]))
# print('delta', str(delta_I[0][0]), str(delta_I[1][0]))
args = [
exec_file,
str(pos_x_I[0][0]),
str(pos_x_I[1][0]),
str(pos_y_I[0][0]),
str(pos_y_I[1][0]),
str(vx_I[0][0]),
str(vx_I[1][0]),
str(vy_I[0][0]),
str(vy_I[1][0]),
str(yaw_I[0][0]),
str(yaw_I[1][0]),
str(yaw_dot_I[0][0]),
str(yaw_dot_I[1][0]),
str(a_I[0][0]),
str(a_I[1][0]),
str(delta_I[0][0]),
str(delta_I[1][0]),
str(a22),
add_sign(a24),
str(a42),
add_sign(a44),
add_sign(b2),
add_sign(b4),
str(horizon),
str(stepsize)
]
#print(args[1:])
#print(str(a22)+"*vy+"+a24_1+"*dpsi+"+b2_1+"*delta")
#print(str(a42)+"*vy+"+a44_1+"*dpsi+"+b4_1+"*delta")
# beta = interval_estimation.initial_beta_interval(model, delta_I)
# alpha_f = interval_estimation.initial_alpha_f_interval(model, y_dot_I, yaw_dot_I, x_dot_I, delta_I)
# alpha_r = interval_estimation.initial_alpha_r_interval(model, y_dot_I, yaw_dot_I, x_dot_I)
# Fxf = [0, 0] #fxi = C*kappa, 0 if no skid
# Fxr = [0, 0]
# Fyf = [min(-1*model.C_alpha*interval(alpha_f)[0][0], -1*model.C_alpha*interval(alpha_f)[0][1]), max(-1*model.C_alpha*interval(alpha_f)[0][0], -1*model.C_alpha*interval(alpha_f)[0][1])]
# Fyr = [min(-1*model.C_alpha*interval(alpha_r)[0][0], -1*model.C_alpha*interval(alpha_r)[0][1]), max(-1*model.C_alpha*interval(alpha_r)[0][0], -1*model.C_alpha*interval(alpha_r)[0][1])]
# args = [exec_file, str(pos_x[0]), str(pos_x[1]), str(pos_y[0]), str(pos_y[1]), str(yaw[0]), str(yaw[1]), str(yaw_dot[0]), str(yaw_dot[1]),
# str(Fxf[0]), str(Fxf[1]), str(Fxr[0]), str(Fxr[1]), str(Fyf[0]), str(Fyf[1]), str(Fyr[0]), str(Fyr[1]),
# str(beta[0]), str(beta[1]), str(command_speed[0]), str(command_speed[1]), str(command_delta[0]), str(command_delta[1]),
# str(vx[0]), str(vx[1]), str(vy[0]), str(vy[1]), str(y_dot[0]), str(y_dot[1]),
# str(model.I_z), str(model.L_f), str(model.L_r), str(model.m), str(horizon), str(stepsize)]
process = subprocess.Popen(args, stdout=subprocess.PIPE)
timer = Timer(FLOWSTAR_TIMEOUT, process.kill)
try:
timer.start()
(output, err) = process.communicate()
finally:
timer.cancel()
if not timer.is_alive():
print(
"Flowstar timed out at %.3f! Now running backup reachflow prediction"
)
return
output = output.decode(encoding='utf8')
line = output.strip().split(';')
#print("line", line)
x_bound = list()
y_bound = list()
yaw_bound = list()
vx_bound = list()
vy_bound = list()
yawdot_bound = list()
seg_num = len(line) - 1 if line[-1] == '' else len(line)
#print("seg", seg_num)
if ODE == 'nonlinear_without_beta':
for i in range(seg_num):
seg = line[0].split(',')
x_bound.append(
(float(seg[0]) + state[0] - 0.5 * length,
float(seg[1]) + state[0] +
0.5 * length)) #add up x global position by shifting
y_bound.append(
(float(seg[2]) + state[1] - 0.5 * length,
float(seg[3]) + state[1] +
0.5 * length)) #add up y global position by shifting
yaw_bound.append((float(seg[4]), float(seg[5])))
# kinematic bicycle model doesn't compute vx, vy, yawdot
vx_bound.append((vx_I[0][0], vx_I[0][1]))
vy_bound.append((vy_I[0][0], vy_I[0][1]))
yawdot_bound.append((yaw_dot_I[0][0], yaw_dot_I[0][1]))
if ODE == 'linear_tire':
for i in range(seg_num):
seg = line[i].split(',')
#print(len(seg))
x_bound.append(
(float(seg[0]) + state[0] - 0.5 * length,
float(seg[1]) + state[0] +
0.5 * length)) #add up x global position by shifting
y_bound.append(
(float(seg[2]) + state[1] - 0.5 * length,
float(seg[3]) + state[1] +
0.5 * length)) #add up y global position by shifting
vx_bound.append((float(seg[4]), float(seg[5])))
vy_bound.append((float(seg[6]), float(seg[7])))
yaw_bound.append((float(seg[8]), float(seg[9])))
yawdot_bound.append((float(seg[10]), float(seg[11])))
#print('xxbb', x_bound)
return x_bound, y_bound, yaw_bound, vx_bound, vy_bound, yawdot_bound
