def get_tang_v_ego(v, x, y, th):
vx = -v * cs.cos(th)
vy = -v * cs.sin(th)
b_hat_x = (x - center[0]) / cs.sqrt((x - center[0])**2 +
(y - center[1])**2)
b_hat_y = (y - center[1]) / cs.sqrt((x - center[0])**2 +
(y - center[1])**2)
vb = vx * b_hat_x + vy * b_hat_y
v_tan = (v - cs.sqrt(vb**2))
return v_tan
def lane_keeping(lane_cost, ymin, ymax, y, actv_lane, yref, ACTIVATE_CONE):
passed_ymin = cs.if_else(y < ymin, 1, -1)
passed_ymax = cs.if_else(y > ymax, 1, -1)
lane_cost += cs.fmax(0.0, passed_ymin * 10000000)
lane_cost += cs.fmax(0.0, passed_ymax * 10000000)
lane_cost += cs.fmax(0.0, -1000 * cs.sqrt((ymin - y)**2) + (1000 / 2))
activate_lane_keep = cs.logic_and(ACTIVATE_CONE[0], ACTIVATE_CONE[1])
# lane_cost += cs.if_else(activate_lane_keep,(cs.sqrt((actv_lane-y)**2)**2)*Qy*0.5,0)
lane_cost += cs.if_else(activate_lane_keep, (cs.sqrt(
(yref - y)**2)**2) * Qy * 4, 0)
return lane_cost
def get_intersection_time(x, y, v, x_obs, y_obs, v_obs, lane):
v_obs = -v_obs
radius_ego = cs.sqrt((x - center[0])**2 + (y - center[1])**2)
radius_obs = road_radius_frm_lane(lane)
radius_avg = cs.if_else(radius_ego > radius_obs,
radius_obs + (radius_ego - radius_obs),
radius_obs - (radius_ego - radius_obs))
(x, y) = (x - center[0], y - center[1])
(x_obs, y_obs) = (x_obs - center[0], y_obs - center[1])
th_ego = cs.atan2(y, x)
th_obs = cs.atan2(y_obs, x_obs)
th = cs.sqrt((th_obs - th_ego)**2)
arc = radius_avg * th
t_impact = arc / (v + v_obs)
return t_impact
def cone_constraint(c, x_obs, y_obs,theta_obs, v_obs, r_cone, uk, x, y, theta, obs_dist,ego_dist,xkm1,ykm1,xref,yref):
# -------distance const
# c += cs.fmax(0.0, 1000*(r_cone -cs.sqrt((x_obs-x)**2 +(y_obs-y)**2)))
# ------- cone with activation and deactivation constraints
v_obs_x=cs.cos(theta_obs)*v_obs
v_obs_y=cs.sin(theta_obs)*v_obs
r_vec=cs.vertcat(x-x_obs,y-y_obs)
vab=cs.vertcat(cs.cos(theta)*uk[0]-v_obs_x,cs.sin(theta)*uk[0]-v_obs_y)
rterm = (cs.norm_2(r_vec))**2
lterm = (cs.norm_2(vab))**2
uterm = (cs.dot(r_vec,vab))**2
cone = (r_cone**2)*lterm-(rterm*lterm-uterm)
passed_obs=cs.if_else(obs_dist>ego_dist ,True,False)
obs_faster_than_ego=cs.if_else(uk[0]<cs.norm_2(v_obs) ,True,False)
obs_driving_towards=cs.if_else(passed_obs,False,cs.if_else(v_obs<=0,True,False))
skip_due_to_dir_and_vel=cs.if_else(obs_driving_towards,False,cs.if_else(obs_faster_than_ego,True,False))
skip_due_far_away=cs.if_else(cs.sqrt((x_obs-x)**2 - (y_obs-y)**2)<10,False,True)
skip=cs.logic_or(skip_due_to_dir_and_vel,skip_due_far_away)
deactivate_cone=cs.if_else(passed_obs,True,skip)
c += cs.if_else(deactivate_cone,0,cs.fmax(0.0, cone))
# decide what side to drive
#side_obs =cs.sign((x_obs-xkm1)*(yref-ykm1)-(y_obs-ykm1)*(xref-xkm1)) # neg if on left
# s=cs.sign((x-xkm1)*(y_obs-ykm1)-(y-ykm1)*(x_obs-xkm1)) # neg if on left
# c +=cs.if_else(deactivate_cone,cs.fmax(0.0,s*decide_side*10),0)
return c,False
def cost_function(state, ref, control, control_prev, track_weight):
cf = 0
x = state[0]
y = state[1]
x_ref = ref[0]
y_ref = ref[1]
# slope = tan[0]
# intercept = tan[1]
# Line: ax + by + c = 0 -> slope * x - y + intercept = 0
# Point: (x1, y1) -> (x, y)
# Distance = (| a*x1 + b*y1 + c |) / (sqrt(a*a + b*b))
# Contouring Error = distance from reference line
# cf += track_error_weight[0] * (cs.fabs((slope * x - y + intercept)) / (cs.sqrt(slope ** 2 + 1)))
# Tracking Error = distance from reference point
cf += track_weight * cs.sqrt((x - x_ref) ** 2 + (y - y_ref) ** 2)
# Input Weights
cf += p.in_weight[0] * control[0] ** 2
cf += p.in_weight[1] * control[1] ** 2
# Input Change Weights
cf += p.in_change_weight[0] * (control[0] - control_prev[0]) ** 2
cf += p.in_change_weight[1] * (control[1] - control_prev[1]) ** 2
return cf
def lane_keeping(lane_cost, y,x,actv_lane,yref,ACTIVATE_CONE):
global lane_border_min,lane_border_max,lane_offset_y,lane_offset_x,center_of_road,lane1,lane2
# border constraint
radius_ego=cs.sqrt((x-lane_offset_x)**2+(y+(lane_border_min-lane_offset_y))**2)
# Googla x^3 så ser du att den börjar öka vid ca 5-10.
# multiplicera dist_ymin med 10 så börjar den vid 0.5-1 ist
dist_ymin=(lane1-radius_ego)*12
dist_ymax=(radius_ego-lane2)*12
lane_cost+=cs.fmax(0.1,dist_ymin**3+10*dist_ymin)
lane_cost+=cs.fmax(0.1,dist_ymax**3+10*dist_ymax)
# active lane keeping constraint
activate_lane_keep=cs.logic_and(ACTIVATE_CONE[0],ACTIVATE_CONE[1])
lane_cost += cs.if_else(activate_lane_keep,500*(radius_ego-actv_lane)**2,0*(radius_ego-center_of_road)**2) #TODO
return lane_cost
def cone_constraint(c, x_obs, y_obs, r_cone, uk, x, y, theta, passed_obs):
v_obs_x = 0
v_obs_y = 0
r_vec = cs.vertcat(x - x_obs, y - y_obs)
vab = cs.vertcat(
cs.cos(theta) * uk[0] - v_obs_x,
cs.sin(theta) * uk[0] - v_obs_y)
rterm = (cs.norm_2(r_vec))**2
lterm = (cs.norm_2(vab))**2
uterm = (cs.dot(r_vec, vab))**2
cone = (r_cone**2) * lterm - (rterm * lterm - uterm)
skip_due_far_away = cs.if_else(
cs.sqrt((x_obs - x)**2 - (y_obs - y)**2) < 10, False, True)
deactivate_cone = cs.if_else(passed_obs, True, skip_due_far_away)
c += cs.if_else(deactivate_cone, 0, cs.fmax(0.0, cone))
return c, False
def solve_for_time(model, ratio):
vehicle = Dubins(
shapes=Circle(sqrt(2)),
options={
"safety_distance": 0.0,
"safety_weight": 10.0,
"room_constraints": True,
"ideal_prediction": True,
"ideal_update": True,
"1storder_delay": False,
"time_constant": 0.1,
"input_disturbance": None,
"stop_tol": 1.0e-2,
"substitution": False,
"exact_substitution": False,
},
bounds={
"vmax": model.imperial_v_at_current_limit(ratio=ratio),
"amax": model.max_accel(ratio=ratio),
"amin": -model.max_accel(ratio=ratio),
"wmin": -model.turn_rad_per_sec_at_current_limit(ratio=ratio),
"wmax": model.turn_rad_per_sec_at_current_limit(ratio=ratio),
"L": model.track_width,
},
)
vehicle.define_knots(knot_intervals=15)
# We provide our vehicle with a desired initial and terminal position:
# this specific impl is for Barrel Racing
path = "Bounce"
rooms = [coords_to_bounding_box(*pair) for pair in configs[path]["rooms"]]
# for i in range(len(configs[path]["rooms"]) - 1):
# print(i, configs[path]["rooms"][i], '->', configs[path]["rooms"][i+1], '@', rooms[i]['position'], rooms[i+1]['position'])
# assert(check_overlap(configs[path]["rooms"][i], configs[path]["rooms"][i+1]))
# Now, we create an environment
# An environment is determined by a room with certain shape
environment = Environment(room=rooms)
for marker in configs[path]["markers"]:
if type(marker) == str:
square = Square(2.5 / 12.0)
environment.add_obstacle(
Obstacle({"position": coord_to_pair(marker)}, shape=square))
else:
environment.add_obstacle(marker)
environment.init()
end = 1
# vehicle.set_initial_conditions(coord_to_pair(configs[path]["path"][end - 1]))
# vehicle.set_terminal_conditions(coord_to_pair(configs[path]["path"][end]))
vehicle.set_initial_conditions(
coord_to_pair(configs[path]["path"][end - 1]))
vehicle.set_terminal_conditions(coord_to_pair(configs[path]["path"][end]))
print("Environment Setup.")
# do our special bounce shit
first_problem = Point2point(
vehicle,
environment,
options={
"verbose": 2,
"solver": "ipopt",
"solver_options": {
"ipopt": {
"ipopt.tol": 1e-1,
"ipopt.warm_start_init_point": "yes",
"ipopt.print_level": 0,
"print_time": 0,
"ipopt.fixed_variable_treatment": "make_parameter",
}
},
"codegen": {
"build": "shared",
"flags": "-O0"
},
"inter_vehicle_avoidance": False,
"horizon_time": 10.0,
"hard_term_con": False,
"no_term_con_der": True,
},
freeT=True,
)
first_problem.init(path + "_p2_p1_" + str(ratio).replace(".", ""))
print("Problem Initialized.")
# simulate the problem
simulator = Simulator(first_problem)
# define what you want to plot
first_problem.plot("scene", knots=True, prediction=True)
vehicle.plot("state", knots=True,
prediction=True) # , labels=["x (m)", "y (m)"])
vehicle.plot("input", knots=True,
prediction=True) # , labels=["v_x (m/s)", "v_y (m/s)"])
# vehicle.plot(
# "dinput", knots=True, prediction=True, labels=["a_x (m/s/s)", "a_y (m/s/s)"]
# )
print("Simulator Setup.")
options = {}
options["directory"] = os.path.join(os.getcwd(), "optimization/")
options["name"] = path + "_p1_" + str(ratio) + "_p1"
simulator.run_once()
end += 1
vehicle.set_initial_conditions(
[
vehicle.signals["state"][0][-1],
vehicle.signals["state"][1][-1],
vehicle.signals["state"][2][-1],
],
input=[
vehicle.signals["splines"][0][-1],
vehicle.signals["splines"][1][-1]
],
)
vehicle.set_terminal_conditions(coord_to_pair(configs[path]["path"][end]))
second_problem = Point2point(
vehicle,
environment,
options={
"verbose": 2,
"solver": "ipopt",
"solver_options": {
"ipopt": {
"ipopt.tol": 1e-1,
"ipopt.warm_start_init_point": "yes",
"ipopt.print_level": 0,
"print_time": 0,
"ipopt.fixed_variable_treatment": "make_parameter",
}
},
"codegen": {
"build": "shared",
"flags": "-O0"
},
"inter_vehicle_avoidance": False,
"horizon_time": 10.0,
"hard_term_con": False,
"no_term_con_der": False,
},
freeT=True,
)
second_problem.init(path + "_p2_p2_" + str(ratio).replace(".", ""))
second_problem.plot("scene", knots=True, prediction=True)
simulator.set_problem(second_problem)
# simulator = Simulator(second_problem)
simulator.run_once()
options["name"] = path + "_p2_" + str(ratio) + "_p2"
# vehicle.signals["ftime"] = [first_problem.compute_objective(), second_problem.compute_objective()]
save(vehicle.traj_storage, vehicle.signals, options)
def build_opt():
u = cs.SX.sym('u', nu * N)
p = cs.SX.sym('p', nx + nref + nObs + nu_init + ntraj)
(x, y, theta) = (p[0], p[1], p[2])
ukm1 = [p[3], p[4]]
(xref, yref, thetaref) = (p[5], p[6], p[7])
(x_obs, y_obs, theta_obs, v_obs, w_obs, r_obs) = (p[8], p[9], p[10], p[11],
p[12], p[13])
for i in range(14, 14 + len_traj):
xtraj.append(p[i])
ytraj.append(p[i + len_traj])
thetatraj.append(p[i + 2 * len_traj])
cost = 0
c = 0
# -------Collission constrint
k = 0
x_tild = 0
y_tild = 0
theta_tild = 0
for j, t in enumerate(range(0, nu * N, nu)):
uk = u[t:t + 2]
# -------Reference trajectory
cost += Qx * (x_tild)**2 + Qy * (y_tild)**2 + Qtheta * (theta_tild)**2
v_tild = uk[0] - v_ref
w_tild = uk[1] - w_ref
cost += Rv * v_tild**2 + Rw * w_tild**2
x, y, theta, x_tild, y_tild, theta_tild = model_dd_tilde(
x, y, theta, v_tild, w_tild, xtraj[j], ytraj[j], thetatraj[j],
uk[0])
(x_obs, y_obs, theta_obs) = model_dd(x_obs, y_obs, theta_obs, v_obs,
w_obs)
rterm = (x - x_obs)**2 + (y - y_obs)**2
uterm = ((cs.cos(theta) * uk[0] - v_obs) * (x - x_obs) +
(cs.sin(theta) * uk[0] - v_obs) * (y - y_obs))**2
lterm = (cs.cos(theta) * uk[0] - v_obs)**2 + (cs.sin(theta) * uk[0] -
v_obs)**2
# -------distance const
# c += cs.fmax(0.0, r_obs - (x_obs-x)**2 - (y_obs-y)**2)
# -------regular cone
#c += cs.fmax(0.0, r_obs**2*lterm-(rterm*lterm-uterm**2))
# -------cone only when dist ego > dist obs
cone = r_obs**2 * lterm - (rterm * lterm - uterm)
ego_dist = cs.sqrt((x - xref)**2 + (y - yref)**2)
obs_dist = cs.sqrt((x_obs - xref)**2 + (y_obs - yref)**2)
c += cs.fmax(0.0, -(obs_dist - ego_dist)) * (cs.fmax(0.0, cone))
# -------Acceleration constraint
(dv_c, dw_c) = (0, 0)
for t in range(0, nu * N, nu):
uk = u[t:t + 2]
dv_c += cs.fmax(0.0, uk[0] - ukm1[0] - dv)
dw_c += cs.vertcat(cs.fmax(0.0, uk[1] - ukm1[1] - dw),
cs.fmax(0.0, ukm1[1] - uk[1] - dw))
ukm1 = uk
# -------Boundery constrint
umin = []
umax = []
for i in range(N):
umin.extend([vmin, wmin])
umax.extend([vmax, wmax])
set_c = og.constraints.Zero()
set_y = og.constraints.BallInf(None, 1e12)
bounds = og.constraints.Rectangle(umin, umax)
C = cs.vertcat(dv_c, dw_c)
problem = og.builder.Problem(u, p, cost).with_penalty_constraints(
C).with_constraints(bounds).with_aug_lagrangian_constraints(
c, set_c, set_y)
build_config = og.config.BuildConfiguration()\
.with_build_directory("optimizers")\
.with_build_mode("debug")\
.with_rebuild(False)\
.with_tcp_interface_config()
meta = og.config.OptimizerMeta()\
.with_optimizer_name("ref_traj_tilde")
solver_config = og.config.SolverConfiguration()\
.with_tolerance(1e-5)\
.with_initial_tolerance(1e-5)\
.with_max_outer_iterations(10)\
.with_delta_tolerance(1e-2)\
.with_penalty_weight_update_factor(5).with_initial_penalty(100).with_sufficient_decrease_coefficient(0.7)
builder = og.builder.OpEnOptimizerBuilder(problem,
meta,
build_config,
solver_config) \
.with_verbosity_level(1)
builder.build()
def build_opt():
v = cs.SX.sym('v', N)
p = cs.SX.sym('p', nObs + 2 + ntraj + 3) #TODO
(X_OBS, Y_OBS, THETA_OBS, V_OBS, W_OBS, Y_LANE_OBS,
R_CONE) = ([p[1], p[2]], [p[3], p[4]], [p[5], p[6]], [p[7], p[8]],
[p[9], p[10]], [p[11], p[12]], [p[13], p[14]])
(xref, yref, thetaref) = (p[15], p[16], p[17])
c = stage_cost = 0
v_cost = 0
for i in range(N):
x_traj.append(p[17 + i])
y_traj.append(p[17 + N + i])
theta_traj.append(p[17 + 2 * N + i])
for i in range(0, N):
vk = v[i]
stage_cost += 1e4 * ((vmax - vk) * 5)**2
for j in range(2):
ego_dist = cs.sqrt((x_traj[i] - xref)**2 + (y_traj[i] - yref)**2)
obs_dist = cs.sqrt((X_OBS[j] - xref)**2 + (Y_OBS[j] - yref)**2)
passed_obs = cs.if_else(obs_dist > ego_dist, True, False)
(X_OBS[j], Y_OBS[j], THETA_OBS[j],
W_OBS[j]) = obs_move_line(Y_LANE_OBS[j], V_OBS[j], X_OBS[j],
Y_OBS[j], THETA_OBS[j],
ts) #TOD does cone know about w_obs?
c = cone_constraint(c, X_OBS[j], Y_OBS[j], THETA_OBS[j], V_OBS[j],
R_CONE[j], vk, x_traj[i], y_traj[i],
theta_traj[i], passed_obs)
total_cost = stage_cost
vmin_lst = []
vmax_lst = []
for i in range(N):
vmin_lst.append(vmin)
vmax_lst.append(vmax)
lag_const = cs.vertcat(c)
set_c = og.constraints.Zero()
set_y = og.constraints.BallInf(None, 1e12)
bounds = og.constraints.Rectangle(vmin_lst, vmax_lst)
problem = og.builder.Problem(
v, p,
total_cost).with_constraints(bounds).with_aug_lagrangian_constraints(
lag_const, set_c, set_y)
build_config = og.config.BuildConfiguration()\
.with_build_directory("optimizers")\
.with_build_mode("debug")\
.with_rebuild(False)\
.with_tcp_interface_config()
meta = og.config.OptimizerMeta()\
.with_optimizer_name("vel_obs")
update_pen = 2
init_pen = 100
num_out_pen = 6
solver_config = og.config.SolverConfiguration()\
.with_tolerance(1e-3)\
.with_initial_tolerance(1e-3)\
.with_max_outer_iterations(num_out_pen)\
.with_delta_tolerance(1e-2)\
.with_penalty_weight_update_factor(update_pen)\
.with_initial_penalty(init_pen)#.with_sufficient_decrease_coefficient(0.7)\
# .with_max_duration_micros(200*1000)
builder = og.builder.OpEnOptimizerBuilder(problem,
meta,
build_config,
solver_config) \
.with_verbosity_level(1)
builder.build()
return update_pen, init_pen, num_out_pen
def build_opt():
u = cs.SX.sym('u', nu*N)
p = cs.SX.sym('p', nx+nref+nObs+nu_init+n_actv_lane)
(x, y, theta) = (p[0], p[1], p[2])
ukm1 = [p[3], p[4]]
(xref, yref, thetaref) = (p[5], p[6], p[7])
(X_OBS, Y_OBS, THETA_OBS, V_OBS, Y_LANE_OBS, R_CONE) = ([p[8], p[9]], [p[10], p[11]], [p[12], p[13]],[p[14], p[15]],[p[16], p[17]],[p[18], p[19]])
actv_lane = p[20]
c = stage_cost = lane_cost = jerk_cost = 0
# close_to_obs=cs.if_else(cs.sqrt((X_OBS[0]-x)**2 - (Y_OBS[0]-y)**2)<10,True,False)
# close_obs_cost += cs.if_else(close_to_obs,cs.fmax(0.0, 1000*(R_OBS[0] - cs.sqrt((X_OBS[0]-x_t)**2 + (Y_OBS[0]-y_t)**2))**2),0)
# radius_ego=cs.sqrt((x_t-lane_offset_x)**2+(y_t+(lane_border_min-lane_offset_y))**2)
# close_obs_cost += cs.if_else(close_to_obs,1000*(radius_ego-actv_lane)**2,0)
for t in range(0, nu*N, nu):
uk = u[t:t+2]
stage_cost += Qx*(x-xref)**2 + Qy*(y-yref)**2 + Qtheta*(theta-thetaref)**2
stage_cost += Rv*uk[0]**2+Rw*uk[1]**2
(xkm1,ykm1)=(x,y)
(x, y, theta) = model_dd(x, y, theta, uk[0], uk[1])
CONE_DEACTIVATED=[]
W_OBS=[0,0] # TODO
for j in range(len(X_OBS)):
# obstacles handeling
(X_OBS[j], Y_OBS[j],THETA_OBS[j],W_OBS[j])=obs_move_line(Y_LANE_OBS[j],V_OBS[j],X_OBS[j], Y_OBS[j],THETA_OBS[j],ts)
ego_dist = cs.sqrt((x-xref)**2+(y-yref)**2)
obs_dist = cs.sqrt((X_OBS[j]-xref)**2+(Y_OBS[j]-yref)**2)
c ,cone_deactivated= cone_constraint(c, X_OBS[j], Y_OBS[j],THETA_OBS[j], V_OBS[j], R_CONE[j], uk, x, y, theta, obs_dist,ego_dist,xkm1,ykm1,xref,yref)
CONE_DEACTIVATED.append(cone_deactivated)
lane_cost = lane_keeping(lane_cost, y,x,actv_lane,yref,CONE_DEACTIVATED)
terminal_cost = Qtx*(x-xref)**2 + Qty*(y-yref)**2 + Qttheta*(theta-thetaref)**2
# -------Acceleration constraint
(dv_c, dw_c) = (0, 0)
for t in range(0, nu*N, nu):
uk = u[t:t+2]
dv_c += cs.fmax(0.0, 100*(uk[0]-ukm1[0]-dv))
dw_c += cs.vertcat(cs.fmax(0.0, 100*(uk[1]-ukm1[1]-dw)),
cs.fmax(0.0, 100*(ukm1[1]-uk[1]-dw)))
jerk_cost+= Rvj*(uk[0]-ukm1[0])**2+Rwj*(uk[1]-ukm1[1])**2
ukm1 = uk
total_cost = terminal_cost+stage_cost+lane_cost+jerk_cost
# -------Boundery constrint
umin = []
umax = []
for i in range(N):
umin.extend([vmin, wmin])
umax.extend([vmax, wmax])
set_c = og.constraints.Zero()
set_y = og.constraints.BallInf(None, 1e12)
bounds = og.constraints.Rectangle(umin, umax)
lag_const=cs.vertcat(dv_c, dw_c,c)
problem = og.builder.Problem(u, p, total_cost).with_constraints(bounds).with_aug_lagrangian_constraints(lag_const, set_c, set_y)
#release
build_config = og.config.BuildConfiguration()\
.with_build_directory("optimizers")\
.with_build_mode("debug")\
.with_rebuild(False)\
.with_tcp_interface_config()
meta = og.config.OptimizerMeta()\
.with_optimizer_name("ref_point")
update_pen = 2
init_pen = 100
num_out_pen = 5
solver_config = og.config.SolverConfiguration()\
.with_tolerance(1e-9)\
.with_initial_tolerance(1e-5)\
.with_max_outer_iterations(num_out_pen)\
.with_delta_tolerance(1e-2)\
.with_penalty_weight_update_factor(update_pen)\
.with_initial_penalty(init_pen).with_sufficient_decrease_coefficient(0.7)\
# .with_max_duration_micros(200*1000)
builder = og.builder.OpEnOptimizerBuilder(problem,
meta,
build_config,
solver_config) \
.with_verbosity_level(1)
builder.build()
return update_pen, init_pen, num_out_pen
# "safety_distance": 0.0,
# "safety_weight": 1.0,
# "room_constraints": True,
# "ideal_prediction": False,
# "ideal_update": False,
# "1storder_delay": False,
# "time_constant": 0.1,
# "input_disturbance": None,
# "stop_tol": 1.0e-2,
# "syslimit": "norm_2",
# },
# bounds={"vmax": 18, "vmin": -18, "amax": 54, "amin": -54},
# )
vehicle = Dubins(
shapes=Circle(sqrt(2)),
options={
"safety_distance": 0.0,
"safety_weight": 10.0,
"room_constraints": True,
"ideal_prediction": True,
"ideal_update": True,
"1storder_delay": False,
"time_constant": 0.1,
"input_disturbance": None,
"stop_tol": 1.0e-2,
"substitution": False,
"exact_substitution": False,
},
bounds={
"vmax": 16,
def build_opt():
u = cs.SX.sym('u', nu * N)
p = cs.SX.sym('p', nx + nref + nObs + nu_init + n_actv_lane)
(x, y, theta) = (p[0], p[1], p[2])
ukm1 = [p[3], p[4]]
(xref, yref, thetaref) = (p[5], p[6], p[7])
(X_OBS, Y_OBS, THETA_OBS, V_OBS, W_OBS,
R_OBS) = ([p[8], p[9]], [p[10], p[11]], [p[12], p[13]], [p[14], p[15]],
[p[16], p[17]], [p[18], p[19]])
actv_lane = p[20]
stage_cost = lane_cost = jerk_cost = 0
c = 0
ACTIVATE_CONE = []
for t in range(0, nu * N, nu):
uk = u[t:t + 2]
stage_cost += Qx*(x-xref)**2 + Qy*(y-yref)**2 + \
Qtheta*(theta-thetaref)**2
stage_cost += Rv * uk[0]**2 + Rw * uk[1]**2
(xkm1, ykm1) = (x, y)
(x, y, theta) = model_dd(x, y, theta, uk[0], uk[1])
for j in range(len(X_OBS)):
# obstacles
(X_OBS[j], Y_OBS[j],
THETA_OBS[j]) = model_dd(X_OBS[j], Y_OBS[j], THETA_OBS[j],
V_OBS[j], W_OBS[j])
ego_dist = cs.sqrt((x - xref)**2 + (y - yref)**2)
obs_dist = cs.sqrt((X_OBS[j] - xref)**2 + (Y_OBS[j] - yref)**2)
c, activate_cone = cone_constraint(c, X_OBS[j], Y_OBS[j],
THETA_OBS[j], V_OBS[j],
R_OBS[j], uk, x, y, theta,
obs_dist, ego_dist, xkm1, ykm1,
xref, yref)
ACTIVATE_CONE.append(activate_cone)
lane_cost = lane_keeping(lane_cost, ymin, ymax, y, actv_lane, yref,
ACTIVATE_CONE)
# passed_obs_by3M = cs.if_else(obs_dist-(ego_dist+3)>0,1,-1)
# dist_actv_lane = cs.sqrt((actv_lane-y)**2)
# lane_cost += 10*cs.fmax(0.0, passed_obs_by3M*(dist_actv_lane**2)*Qy*4)
terminal_cost = Qtx * (x - xref)**2 + Qty * (y - yref)**2 + Qttheta * (
theta - thetaref)**2
# -------Acceleration constraint
(dv_c, dw_c) = (0, 0)
for t in range(0, nu * N, nu):
uk = u[t:t + 2]
dv_c += cs.fmax(0.0, 100 * (uk[0] - ukm1[0] - dv))
dw_c += cs.vertcat(cs.fmax(0.0, 100 * (uk[1] - ukm1[1] - dw)),
cs.fmax(0.0, 100 * (ukm1[1] - uk[1] - dw)))
jerk_cost += Rvj * (uk[0] - ukm1[0])**2 + Rwj * (uk[1] - ukm1[1])**2
ukm1 = uk
total_cost = terminal_cost + stage_cost + lane_cost + jerk_cost
# -------Boundery constrint
umin = []
umax = []
for i in range(N):
umin.extend([vmin, wmin])
umax.extend([vmax, wmax])
set_c = og.constraints.Zero()
set_y = og.constraints.BallInf(None, 1e12)
bounds = og.constraints.Rectangle(umin, umax)
C = cs.vertcat(dv_c, dw_c)
problem = og.builder.Problem(u, p, total_cost).with_penalty_constraints(
C).with_constraints(bounds).with_aug_lagrangian_constraints(
c, set_c, set_y)
build_config = og.config.BuildConfiguration()\
.with_build_directory("optimizers")\
.with_build_mode("debug")\
.with_rebuild(False)\
.with_tcp_interface_config()
meta = og.config.OptimizerMeta()\
.with_optimizer_name("ref_point")
update_pen = 2
init_pen = 100
num_out_pen = 4
solver_config = og.config.SolverConfiguration()\
.with_tolerance(1e-9)\
.with_initial_tolerance(1e-5)\
.with_max_outer_iterations(num_out_pen)\
.with_delta_tolerance(1e-2)\
.with_penalty_weight_update_factor(update_pen)\
.with_initial_penalty(init_pen).with_sufficient_decrease_coefficient(0.7)\
# .with_max_duration_micros(200*1000)
builder = og.builder.OpEnOptimizerBuilder(problem,
meta,
build_config,
solver_config) \
.with_verbosity_level(1)
builder.build()
return update_pen, init_pen, num_out_pen