def cone_constraint(c, x_obs, y_obs, theta_obs, v_obs, r_obs, uk, x, y, theta,
obs_dist, ego_dist, xkm1, ykm1, xref, yref):
v_obs_x = cs.cos(theta_obs) * v_obs
v_obs_y = cs.sin(theta_obs) * v_obs
rterm = (x - x_obs)**2 + (y - y_obs)**2
uterm = ((cs.cos(theta) * uk[0] - v_obs_x) * (x - x_obs) +
(cs.sin(theta) * uk[0] - v_obs_y) * (y - y_obs))
lterm = (cs.cos(theta) * uk[0] - v_obs_x)**2 + (cs.sin(theta) * uk[0] -
v_obs_y)**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**2)
passed_obs = cs.if_else((obs_dist - ego_dist) > 0, True, False)
obs_faster_than_ego = cs.if_else(uk[0] < v_obs, True, False)
obs_driving_towards = cs.if_else(
cs.norm_2(theta - theta_obs) >= (cs.pi / 2), True, False)
skip_due_todir_and_vel = cs.if_else(
obs_driving_towards, False, cs.if_else(obs_faster_than_ego, True,
False))
deactivate_activate_cone = cs.if_else(
passed_obs, True, cs.if_else(skip_due_todir_and_vel, True, False))
c += cs.fmax(0.0,
cs.if_else(deactivate_activate_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.fmax(0.0,s*5)
return c, deactivate_activate_cone
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 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,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 setUpRosPackageGeneration(cls):
u = cs.MX.sym("u", 5) # decision variable (nu = 5)
p = cs.MX.sym("p", 2) # parameter (np = 2)
phi = og.functions.rosenbrock(u, p)
c = cs.vertcat(1.5 * u[0] - u[1], cs.fmax(0.0, u[2] - u[3] + 0.1))
bounds = og.constraints.Ball2(None, 1.5)
meta = og.config.OptimizerMeta() \
.with_optimizer_name("rosenbrock_ros")
problem = og.builder.Problem(u, p, phi) \
.with_constraints(bounds) \
.with_penalty_constraints(c)
ros_config = og.config.RosConfiguration() \
.with_package_name("parametric_optimizer") \
.with_node_name("open_node") \
.with_rate(35) \
.with_description("really cool ROS node")
build_config = og.config.BuildConfiguration() \
.with_open_version(RustBuildTestCase.OPEN_RUSTLIB_VERSION) \
.with_build_directory(RustBuildTestCase.TEST_DIR) \
.with_build_mode(og.config.BuildConfiguration.DEBUG_MODE) \
.with_build_c_bindings() \
.with_ros(ros_config)
og.builder.OpEnOptimizerBuilder(problem,
metadata=meta,
build_configuration=build_config,
solver_configuration=cls.solverConfig()) \
.build()
def fmax(u, v):
if is_numeric(u) and is_numeric(v):
return np.fmax(u, v)
elif is_symbolic(u) or is_symbolic(v):
return cs.fmax(u, v)
else:
raise Exception("Illegal argument")
def cone_constraint(c, x_obs, y_obs, theta_obs, v_obs, r_cone, v, x, y, theta,
passed_obs):
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) * v - v_obs_x, cs.sin(theta) * v - 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)
# c += cs.if_else(passed_obs,0,cs.fmax(0.0, cone))
c += cs.fmax(0.0, cone)
return c
def distance_squared(self, u):
"""Computes the squared distance between a given point `u` and this
second-order cone
:param u: given point; can be a list of float, a numpy
n-dim array (`ndarray`) or a CasADi SX/MX symbol
:return: distance from set as a float or a CasADi symbol
"""
# Need to check this?:
# if isinstance(u, cs.SX):
# warnings.warn("This function does not accept casadi.SX; use casadi.MX instead")
if fn.is_symbolic(u):
nu = u.size(1)
elif (isinstance(u, list) and all(isinstance(x, (int, float)) for x in u)) \
or isinstance(u, np.ndarray):
nu = len(u)
else:
raise Exception("Illegal Argument, `u`")
# Partition `u = (x, r)`, where `r` is the last element of `u`
a = self.__a
x = u[0:nu - 1]
r = u[nu - 1]
eps = 1e-16
norm_x = fn.norm2(x) # norm of x
sq_norm_x = cs.dot(x, x) # squared norm of x
gamma = (a * norm_x + r) / (a**2 + 1)
fun1 = 0.
fun2 = sq_norm_x + r**2
fun3 = sq_norm_x * (1. - gamma * a /
(cs.fmax(eps, norm_x)))**2 + (r - gamma)**2
condition0 = norm_x + cs.fabs(r) < eps
condition1 = norm_x <= a * r
condition2 = a * norm_x <= -r
f = cs.if_else(
condition0, 0,
cs.if_else(condition1, fun1,
cs.if_else(condition2, fun2, fun3, True), True), True)
return f
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 __construct_function_psi(self):
"""
Construct function psi and its gradient
:return: cs.Function objects: psi_fun, grad_psi_fun
"""
self.__logger.info("Defining function psi(u, xi, p) and its gradient")
problem = self.__problem
bconfig = self.__build_config
u = problem.decision_variables
p = problem.parameter_variables
n2 = problem.dim_constraints_penalty()
n1 = problem.dim_constraints_aug_lagrangian()
phi = problem.cost_function
alm_set_c = problem.alm_set_c
f1 = problem.penalty_mapping_f1
f2 = problem.penalty_mapping_f2
psi = phi
if n1 + n2 > 0:
n_xi = n1 + 1
else:
n_xi = 0
xi = cs.SX.sym('xi', n_xi, 1) if isinstance(u, cs.SX) \
else cs.MX.sym('xi', n_xi, 1)
# Note: In the first term below, we divide by 'max(c, 1)', instead of
# just 'c'. The reason is that this allows to set c=0 and
# retrieve the value of the original cost function
if n1 > 0:
sq_dist_term = alm_set_c.distance_squared(f1 + xi[1:n1 + 1] /
cs.fmax(xi[0], 1))
psi += xi[0] * sq_dist_term / 2
if n2 > 0:
psi += xi[0] * cs.dot(f2, f2) / 2
jac_psi = cs.gradient(psi, u)
psi_fun = cs.Function(bconfig.cost_function_name, [u, xi, p], [psi])
grad_psi_fun = cs.Function(bconfig.grad_function_name, [u, xi, p],
[jac_psi])
return psi_fun, grad_psi_fun
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():
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
cost += Ru[0] * (u_n[0] - u_ref[0])**2 + Ru[1] * (
u_n[1] - u_ref[1])**2 + Ru[2] * (u_n[2] - u_ref[2])**2 #Input weights
cost += Rd[0] * (u_n[0] - u_old[0])**2 + Rd[1] * (
u_n[1] - u_old[1])**2 + Rd[2] * (u_n[2] -
u_old[2])**2 #Input rate weights
####Input Cost MAV2
u_n2 = u[(6 * i + 3):(6 * i + 6)]
cost += Ru[0] * (u_n2[0] - u_ref[0])**2 + Ru[1] * (
u_n2[1] - u_ref[1])**2 + Ru[2] * (u_n2[2] -
u_ref[2])**2 #Input weights
cost += Rd[0] * (u_n2[0] - u_old2[0])**2 + Rd[1] * (
u_n2[1] - u_old2[1])**2 + Rd[2] * (u_n2[2] -
u_old2[2])**2 #Input rate weights
######Penalty constraint
c = cs.vertcat(c, cs.fmax(
0,
0.49 - ((x[0] - x2[0])**2 + (x[1] - x2[1])**2))) #(x[2] - x2[2])**2))
c = cs.vertcat(c, cs.fmax(0, -0.003 + (u_n[1] - u_old[1])**2))
c = cs.vertcat(c, cs.fmax(0, -0.003 + (u_n[2] - u_old[2])**2))
c = cs.vertcat(c, cs.fmax(0, -0.003 + (u_n2[1] - u_old2[1])**2))
c = cs.vertcat(c, cs.fmax(0, -0.003 + (u_n2[2] - u_old2[2])**2))
u_old = u_n
u_old2 = u_n2
####Obstacle(hoop)
#c += cs.fmax(0, 0.49-(x[0]+0.5)**2 - (x[1]-4)**2)
#c += 0*cs.fmax(0,-(obs_data[3]**2 - ((x[1]-obs_data[1])**2 + (x[2]-obs_data[2])**2))) * cs.fmax(0, (x[0] - obs_data[0]) + 0.4) * cs.fmax(0, -(x[0] - obs_data[0]) + 0.4)
####State update MAV1
x[0] = x[0] + dt * x[3]
x[1] = x[1] + dt * x[4]
x[2] = x[2] + dt * x[5]
x[3] = x[3] + dt * (cs.sin(x[7]) * cs.cos(x[6]) * u_n[0] - 0.1 * x[3])
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
u_n = u[(3 * i):3 * i + 3]
cost += Ru[0] * (u_n[0] - u_ref[0])**2 + Ru[1] * (
u_n[1] - u_ref[1])**2 + Ru[2] * (u_n[2] - u_ref[2])**2 #Input weights
cost += Rd[0] * (u_n[0] - u_old[0])**2 + Rd[1] * (
u_n[1] - u_old[1])**2 + Rd[2] * (u_n[2] -
u_old[2])**2 #Input rate weights
u_old = u_n
#####PENALTY CONSTRAINTS
#Obstacle(hoop)
#c += cs.fmax(0, 0.49-(x[0]+0.5)**2 - (x[1]-4)**2)
#c += cs.fmax(0, -(obs_data[3]**2 - ((x[1]-obs_data[1])**2 + (x[2] - obs_data[2])**2))) * cs.fmax(0, (x[0] - obs_data[0]) + 0.4) * cs.fmax(0, -(x[0] - obs_data[0]) + 0.4)
c = cs.vertcat(
c, 10 * cs.fmax(
0, -(obs_data[3]**2 - ((x[1] - obs_data[1])**2 +
(x[2] - obs_data[2])**2))) *
cs.fmax(0, (x[0] - obs_data[0]) + 0.4) *
cs.fmax(0, -(x[0] - obs_data[0]) + 0.4))
c = cs.vertcat(c, cs.fmax(0, -0.003 + (u_n[1] - u_old[1])**2))
c = cs.vertcat(c, cs.fmax(0, -0.003 + (u_n[2] - u_old[2])**2))
#######State update
x[0] = x[0] + dt * x[3]
x[1] = x[1] + dt * x[4]
x[2] = x[2] + dt * x[5]
x[3] = x[3] + dt * (cs.sin(x[7]) * cs.cos(x[6]) * u_n[0] - 0.1 * x[3])
x[4] = x[4] + dt * (-cs.sin(x[6]) * u_n[0] - 0.1 * x[4])
x[5] = x[5] + dt * (cs.cos(x[7]) * cs.cos(x[6]) * u_n[0] - 0.2 * x[5] -
9.81)
x[6] = x[6] + dt * ((1 / 0.5) * (u_n[1] - x[6]))
x[7] = x[7] + dt * ((1 / 0.5) * (u_n[2] - x[7]))
def build(self):
# Build parametric optimizer
# ------------------------------------
u = cs.SX.sym('u', self.config.nu * self.config.N_hor)
z0 = cs.SX.sym(
'z0', self.config.nz + self.config.N_hor +
self.config.Nobs * self.config.nobs +
self.config.Ndynobs * self.config.ndynobs * self.config.N_hor +
self.config.nx * self.config.N_hor
) #init + final position + cost, obstacle params, circle radius
# params, vel_ref each step, number obstacles x num params per obs, num dynamic obstacles X num param/obs X time steps, refernce path for each time step
(x, y, theta, vel_init, omega_init) = (z0[0], z0[1], z0[2], z0[3],
z0[4])
(xref, yref, thetaref, velref, omegaref) = (z0[5], z0[6], z0[7], z0[8],
z0[9])
(q, qv, qtheta, rv, rw, qN, qthetaN, qCTE, acc_penalty,
omega_acc_penalty) = (z0[10], z0[11], z0[12], z0[13], z0[14], z0[15],
z0[16], z0[17], z0[18], z0[19])
cost = 0
obstacle_constraints = 0
# Index where reference points start
base = self.config.nz + self.config.N_hor + self.config.Nobs * self.config.nobs + self.config.Ndynobs * self.config.ndynobs * self.config.N_hor
for t in range(0, self.config.N_hor): # LOOP OVER TIME STEPS
u_t = u[t * self.config.nu:(t + 1) * self.config.nu]
cost += rv * u_t[0]**2 + rw * u_t[1]**2 # Penalize control actions
cost += qv * (
u_t[0] - z0[self.config.nz + t]
)**2 # Cost for diff between velocity and reference velocity
cost += self.cost_fn((x, y, theta), (xref, yref, thetaref), q,
qtheta)
x += self.config.ts * (u_t[0] * cs.cos(theta))
y += self.config.ts * (u_t[0] * cs.sin(theta))
theta += self.config.ts * u_t[1]
xs_static = z0[self.config.nz + self.config.N_hor:self.config.nz +
self.config.N_hor + self.config.Nobs *
self.config.nobs:self.config.nobs]
ys_static = z0[self.config.nz + self.config.N_hor +
1:self.config.nz + self.config.N_hor +
self.config.Nobs *
self.config.nobs:self.config.nobs]
rs_static = z0[self.config.nz + self.config.N_hor +
2:self.config.nz + self.config.N_hor +
self.config.Nobs *
self.config.nobs:self.config.nobs]
# ordering is x,y,x_r, y_r, angle for obstacle 0 for N_hor timesteps, then x,y,x_r, y_r, angle for obstalce 1 for N_hor timesteps etc.
end_of_static_obs_idx = self.config.nz + self.config.N_hor + self.config.Nobs * self.config.nobs
end_of_dynamic_obs_idx = end_of_static_obs_idx + self.config.Ndynobs * self.config.ndynobs * self.config.N_hor
xs_dynamic = z0[end_of_static_obs_idx +
t * self.config.ndynobs:end_of_dynamic_obs_idx:self
.config.ndynobs * self.config.N_hor]
ys_dynamic = z0[end_of_static_obs_idx + t * self.config.ndynobs +
1:end_of_dynamic_obs_idx:self.config.ndynobs *
self.config.N_hor]
x_radius = z0[end_of_static_obs_idx + t * self.config.ndynobs +
2:end_of_dynamic_obs_idx:self.config.ndynobs *
self.config.N_hor]
y_radius = z0[end_of_static_obs_idx + t * self.config.ndynobs +
3:end_of_dynamic_obs_idx:self.config.ndynobs *
self.config.N_hor]
As = z0[end_of_static_obs_idx + t * self.config.ndynobs +
4:end_of_dynamic_obs_idx:self.config.ndynobs *
self.config.N_hor]
xdiff_static = x - xs_static
ydiff_static = y - ys_static
xdiff_dynamic = x - xs_dynamic
ydiff_dynamic = y - ys_dynamic
distance_inside_circle = rs_static**2 - xdiff_static**2 - ydiff_static**2
# Ellipse parameterized according to https://math.stackexchange.com/questions/426150/what-is-the-general-equation-of-the-ellipse-that-is-not-in-the-origin-and-rotate
# xs and ys are ellipse center points, xdiff is as before
# x_radius and y_radius are radii in "x" and "y" directions
# As are angles of ellipses (positive from x axis)
distance_inside_ellipse = 1 - (
xdiff_dynamic * cs.cos(As) + ydiff_dynamic * cs.sin(As))**2 / (
x_radius**2) - (xdiff_dynamic * cs.sin(As) - ydiff_dynamic
* cs.cos(As))**2 / (y_radius)**2
obstacle_constraints += cs.fmax(
0, cs.vertcat(distance_inside_circle, distance_inside_ellipse))
# our current point
p = cs.vertcat(x, y)
# Initialize list with CTE to all line segments
# https://math.stackexchange.com/questions/330269/the-distance-from-a-point-to-a-line-segment
distances = cs.SX.ones(1)
s2 = cs.vertcat(z0[base], z0[base + 1])
for i in range(1, self.config.N_hor):
# set start point as previous end point
s1 = s2
# new end point
s2 = cs.vertcat(z0[base + i * self.config.nx],
z0[base + i * self.config.nx + 1])
# line segment
s2s1 = s2 - s1
# t_hat
t_hat = cs.dot(p - s1,
s2s1) / (s2s1[0]**2 + s2s1[1]**2 + 1e-16)
# limit t
t_star = cs.fmin(cs.fmax(t_hat, 0.0), 1.0)
# vector pointing from us to closest point
temp_vec = s1 + t_star * s2s1 - p
# append distance (is actually squared distance)
distances = cs.horzcat(distances,
temp_vec[0]**2 + temp_vec[1]**2)
cost += cs.mmin(distances[1:]) * qCTE
cost += qN * ((x - xref)**2 +
(y - yref)**2) + qthetaN * (theta - thetaref)**2
# Max speeds
umin = [self.config.lin_vel_min, -self.config.ang_vel_max
] * self.config.N_hor
umax = [self.config.lin_vel_max, self.config.ang_vel_max
] * self.config.N_hor
bounds = og.constraints.Rectangle(umin, umax)
# Acceleration bounds and cost
# Selected velocities
v = u[0::2]
omega = u[1::2]
# Accelerations
acc = (v - cs.vertcat(vel_init, v[0:-1])) / self.config.ts
omega_acc = (omega -
cs.vertcat(omega_init, omega[0:-1])) / self.config.ts
acc_constraints = cs.vertcat(acc, omega_acc)
# Acceleration bounds
acc_min = [self.config.lin_acc_min] * self.config.N_hor
omega_min = [-self.config.ang_acc_max] * self.config.N_hor
acc_max = [self.config.lin_acc_max] * self.config.N_hor
omega_max = [self.config.ang_acc_max] * self.config.N_hor
acc_bounds = og.constraints.Rectangle(acc_min + omega_min,
acc_max + omega_max)
# Accelerations cost
cost += cs.mtimes(acc.T, acc) * acc_penalty
cost += cs.mtimes(omega_acc.T, omega_acc) * omega_acc_penalty
problem = og.builder.Problem(u, z0, cost).with_penalty_constraints(obstacle_constraints) \
.with_constraints(bounds) \
.with_aug_lagrangian_constraints(acc_constraints, acc_bounds)
build_config = og.config.BuildConfiguration()\
.with_build_directory(self.config.build_directory)\
.with_build_mode(self.config.build_type)\
.with_tcp_interface_config()
meta = og.config.OptimizerMeta()\
.with_optimizer_name(self.config.optimizer_name)
solver_config = og.config.SolverConfiguration()\
.with_tolerance(1e-4)\
.with_max_duration_micros(MAX_SOVLER_TIME)
builder = og.builder.OpEnOptimizerBuilder(problem,
meta,
build_config,
solver_config) \
.with_verbosity_level(1)
builder.build()
import casadi.casadi as cs
import opengen as og
u = cs.SX.sym("u", 5) # decision variable (nu = 5)
p = cs.SX.sym("p", 2) # parameter (np = 2)
phi = og.functions.rosenbrock(u, p) # cost function
f2 = cs.fmax(0.0, u[2] - u[3] + 0.1)
f1 = cs.vertcat(1.5 * u[0] - u[1], cs.sin(u[2] + cs.pi/5) - 0.2)
C = og.constraints.Ball2(None, 1.0)
UA = og.constraints.FiniteSet([[1, 2, 3], [1, 2, 2], [1, 2, 4], [0, 5, -1]])
UB = og.constraints.Ball2(None, 1.0)
U = og.constraints.CartesianProduct(5, [2, 4], [UA, UB])
problem = og.builder.Problem(u, p, phi) \
.with_constraints(U) \
.with_aug_lagrangian_constraints(f1, C)
meta = og.config.OptimizerMeta() \
.with_version("0.0.0") \
.with_authors(["P. Sopasakis", "E. Fresk"]) \
.with_licence("CC4.0-By") \
.with_optimizer_name("the_optimizer")
build_config = og.config.BuildConfiguration() \
.with_build_directory("my_optimizers") \
.with_build_mode("debug") \
.with_tcp_interface_config()
import casadi.casadi as cs
import opengen as og
import numpy as np
x = cs.SX.sym("x", 1) # decision variable (num_x = 1)
p = cs.SX.sym("p", 2) # parameter (num_p = 2)
f = (x[0] - p[0])**2 # cost function
f2 = cs.vertcat(cs.fmax(0.0, x[0] - p[1]))
problem = og.builder.Problem(x, p, f) \
.with_penalty_constraints(f2) \
meta = og.config.OptimizerMeta() \
.with_version("0.0.0") \
.with_licence("CC4.0-By") \
.with_optimizer_name("example_02")
build_config = og.config.BuildConfiguration() \
.with_build_directory("python_build") \
.with_build_mode("debug") \
.with_tcp_interface_config()
solver_config = og.config.SolverConfiguration() \
.with_lfbgs_memory(15) \
.with_tolerance(1e-5) \
.with_max_inner_iterations(155)
builder = og.builder.OpEnOptimizerBuilder(problem,
metadata=meta,
def step(self, u: "U", ts):
self.x += ts * self.speed * cs.cos(self.theta)
self.y += ts * self.speed * cs.sin(self.theta)
self.theta += ts * self.speed / self.LENGTH * u.yaw_rate
self.speed += ts * self.MAX_ACCEL * u.accel
self.speed = cs.fmin(cs.fmax(0, self.speed), self.MAX_SPEED)
import opengen as og
import casadi.casadi as cs
u = cs.SX.sym("u", 5)
p = cs.SX.sym("p", 2)
phi = og.functions.rosenbrock(u, p)
c = cs.vertcat(1.5 * u[0] - u[1], cs.fmax(0.0, u[2] - u[3] + 0.1))
bounds = og.constraints.Ball2(None, 1.5)
problem = og.builder.Problem(u, p, phi) \
.with_penalty_constraints(c) \
.with_constraints(bounds)
meta = og.config.OptimizerMeta() \
.with_optimizer_name("rosenbrock") \
.with_version('0.1.1') \
.with_licence('LGPLv3')
ros_config = og.config.RosConfiguration() \
.with_package_name("parametric_optimizer") \
.with_node_name("open_node") \
.with_rate(35) \
.with_description("cool ROS node")
build_config = og.config.BuildConfiguration() \
.with_build_directory("my_optimizers") \
.with_build_mode("debug") \
.with_tcp_interface_config() \
.with_build_c_bindings() \
.with_ros(ros_config)
solver_config = og.config.SolverConfiguration() \
.with_tolerance(1e-5) \
.with_delta_tolerance(1e-4) \
.with_initial_penalty(890) \
.with_penalty_weight_update_factor(5)
for i in range(0, N):
###State Cost
for j in range(0,nMAV):
#print(j)
cost += Qx[0]*(x[ns*j]-x_ref[ns*j])**2 + Qx[1]*(x[ns*j+1]-x_ref[ns*j+1])**2 + Qx[2]*(x[ns*j+2]-x_ref[ns*j+2])**2 + Qx[3]*(x[ns*j+3]-x_ref[ns*j+3])**2 + Qx[4]*(x[ns*j+4]-x_ref[ns*j+4])**2 + Qx[5]*(x[ns*j+5]-x_ref[ns*j+5])**2 + Qx[6]*(x[ns*j+6]-x_ref[ns*j+6])**2 + Qx[7]*(x[ns*j+7]-x_ref[ns*j+7])**2
####Input Cost
u_n = u[(i*nMAV*nu):(i*nMAV*nu+nu*nMAV)]
for j in range(0,nMAV):
#print(j)
cost += Ru[0]*(u_n[nu*j] - u_ref[0])**2 + Ru[1]*(u_n[nu*j+1] - u_ref[1])**2 + Ru[2]*(u_n[nu*j+2] - u_ref[2])**2 #Input weights
cost += Rd[0]*(u_n[nu*j] - u_old[nu*j])**2 + Rd[1]*(u_n[nu*j+1] - u_old[nu*j+1])**2 + Rd[2]*(u_n[nu*j+2] - u_old[nu*j+2])**2 #Input rate weights
######Penalty constraint
if nMAV > 1:
c = cs.vertcat(c, 10*cs.fmax(0, 0.4**2 - ((x[0] - x[16])**2 + (x[1] - x[17])**2)))
c = cs.vertcat(c, 10*cs.fmax(0, 0.4**2 - ((x[0] - x[24])**2 + (x[1] - x[25])**2)))
c = cs.vertcat(c, 10*cs.fmax(0, 0.4**2 - ((x[8] - x[24])**2 + (x[9] - x[25])**2)))
#c = cs.vertcat(c, cs.fmax(0, 0.4**2 - ((x[0] - x[32])**2 + (x[1] - x[33])**2)))
#c = cs.vertcat(c, cs.fmax(0, 0.4**2 - ((x[8] - x[32])**2 + (x[9] - x[33])**2)))
#c = cs.vertcat(c, cs.fmax(0, 0.4**2 - ((x[16] - x[32])**2 + (x[17] - x[33])**2)))
for j in range(0,nMAV-1):
c = cs.vertcat(c, 10*cs.fmax(0, 0.4**2 - ((x[ns*j] - x[ns*j+8])**2 + (x[ns*j+1] - x[ns*j+9])**2)))
#cost += 100*cs.fmax(0, 0.3**2 - ((x[0] - x[8])**2 + (x[1] - x[9])**2))
#cost += 100*cs.fmax(0, 0.3**2 - ((x[16] - x[24])**2 + (x[17] - x[25])**2))
####Obstacle(hoop)
for j in range(0,nMAV):
#c = cs.vertcat(c,cs.fmax(0, obs_data[3]**2-(x[ns*j]-obs_data[0])**2 - (x[ns*j+1]-obs_data[1])**2))
#c = cs.vertcat(c,cs.fmax(0, obs_data[3]**2-(x[ns*j]+7)**2 - (x[ns*j+1]-1.5)**2))
#obstacle avoidance from 2D lidar
#cost+= Q_obstacle[0]*cs.fmax(d_safe-x_obst[0]+x0[3]*dt,0) #+x
#cost+= Q_obstacle[1]*cs.fmax(0.0,d_safe-x_obst[1]-x0[3]*dt) #-x
#cost+= Q_obstacle[2]*cs.fmax(d_safe-x_obst[2]+x0[4]*dt,0) #+y
#cost+= Q_obstacle[3]*cs.fmax(d_safe-x_obst[3]-x0[4]*dt,0) #-y
#print(x0[5])
if i < N - 1:
cost += cs.sumsqr(Q_u * (u0 - u[(i + 1) * nu:(i + 1) * nu + nu]))
# Constraints
#c= cs.vertcat(c,cs.fmax(0.0,Q_obstacle[0]*(d_safe-x_obst[0]-(-x0[3])*dt))) #+x vx
#c= cs.vertcat(c,cs.fmax(0.0,Q_obstacle[1]*(d_safe-(x_obst[1])-x0[3]*dt))) #-x -vx
#c= cs.vertcat(c,cs.fmax(0.0,Q_obstacle[2]*(d_safe-x_obst[2]-x0[4]*dt))) #+y vy
#c= cs.vertcat(c,cs.fmax(0.0,Q_obstacle[3]*(d_safe-(x_obst[3])-(-x0[4])*dt))) #-y -vy
c = cs.vertcat(
c,
cs.fmax(0.0,
Q_obstacle[0] * (d_safe - x_obst[0] - (-x0[1]) * dt))) #+x vx
c = cs.vertcat(c,
cs.fmax(0.0, Q_obstacle[1] * (d_safe - x_obst[1] -
(x0[1] * dt)))) #-x -vx
c = cs.vertcat(
c,
cs.fmax(0.0,
Q_obstacle[2] * (d_safe - x_obst[2] - (-x0[2] * dt)))) #+y vy
c = cs.vertcat(c,
cs.fmax(0.0, Q_obstacle[3] * (d_safe - x_obst[3] -
(x0[2]) * dt))) #-y -vy
###Cylinders
c = cs.vertcat(
c, Q_Obslist[0] * cs.fmax(
0, Q_Cylinders[2]**2 - (x0[0] - Q_Cylinders[0])**2 -
(x0[1] - Q_Cylinders[1])**2))
def build_problem(N, SV_N, WP_N, ts):
# Assumptions
assert N >= 2, f"Must generate at least 2 trajectory points, got: {N}"
assert SV_N >= 0, f"Must have non-negative # of sv's, got: {SV_N}"
assert WP_N >= 1, f"Must have at lest 1 trajectory reference"
z0_schema = [
(1, Gain),
(1, VehicleModel), # Ego
(SV_N, VehicleModel), # SV's
(WP_N, XRef), # reference trajectory
(1, Number), # impatience
(1, Number), # target_speed
]
z0_dimensions = sum(n * feature.DOF for n, feature in z0_schema)
z0 = cs.SX.sym("z0", z0_dimensions)
u_traj = UTrajectory(N)
# parse z0 into features
position = 0
parsed = []
for n, feature in z0_schema:
feature_group = []
for i in range(n):
feature_group.append(
feature(*z0[position:position + feature.DOF].elements()))
position += feature.DOF
if n > 1:
parsed.append(feature_group)
else:
assert len(feature_group) == 1
parsed += feature_group
assert position == len(z0.elements())
assert position == z0_dimensions
gain, ego, social_vehicles, xref_traj, impatience, target_speed = parsed
cost = 0
for t in range(N):
# Integrate the ego vehicle forward to the next trajectory point
ego.step(u_traj[t], ts)
# For the current pose, compute the smallest cost to any xref
cost += min_cost_by_distance(xref_traj, ego.as_xref, gain)
cost += gain.speed * (ego.speed - target_speed.value)**2 / t
for sv in social_vehicles:
# step the social vehicle assuming no change in velocity or heading
sv.step(U(accel=0, yaw_rate=0), ts)
min_dist = VehicleModel.LENGTH
cost += gain.obstacle * cs.fmax(
-1,
min_dist**2 - ((ego.x - sv.x)**2 + 9 * (ego.y - sv.y)**2),
)
# To stabilize the trajectory, we attach a higher weight to the final x_ref
cost += gain.terminal * sum(xref_traj[-1].weighted_distance_to(
ego.as_xref, gain)[:3])
cost += u_traj.integration_cost(gain)
# force acceleration when we become increasingly impatient
cost += gain.impatience * (
(u_traj[0].accel - 1.0) * impatience.value**2 * -(1.0))
# cost=0
bounds = og.constraints.Rectangle(xmin=[-1, -math.pi * 0.1] * N,
xmax=[1, math.pi * 0.1] * N)
return og.builder.Problem(u_traj.symbolic, z0,
cost).with_constraints(bounds)
cost += Ru[0] * (u_n[0] - u_ref[0])**2 + Ru[1] * (
u_n[1] - u_ref[1])**2 + Ru[2] * (u_n[2] - u_ref[2])**2 #Input weights
cost += Rd[0] * (u_n[0] - u_old[0])**2 + Rd[1] * (
u_n[1] - u_old[1])**2 + Rd[2] * (u_n[2] -
u_old[2])**2 #Input rate weights
####Input Cost MAV2
u_n2 = u[(6 * i + 3):(6 * i + 6)]
cost += Ru[0] * (u_n2[0] - u_ref[0])**2 + Ru[1] * (
u_n2[1] - u_ref[1])**2 + Ru[2] * (u_n2[2] -
u_ref[2])**2 #Input weights
cost += Rd[0] * (u_n2[0] - u_old2[0])**2 + Rd[1] * (
u_n2[1] - u_old2[1])**2 + Rd[2] * (u_n2[2] -
u_old2[2])**2 #Input rate weights
######Penalty constraint
#c = cs.vertcat(c, cs.fmax(0, 0.49 - ((x[0] - x2[0])**2 + (x[1] - x2[1])**2))) #(x[2] - x2[2])**2))
c = cs.vertcat(c, cs.fmax(0, -0.003 + (u_n[1] - u_old[1])**2))
c = cs.vertcat(c, cs.fmax(0, -0.003 + (u_n[2] - u_old[2])**2))
c = cs.vertcat(c, cs.fmax(0, -0.003 + (u_n2[1] - u_old2[1])**2))
c = cs.vertcat(c, cs.fmax(0, -0.003 + (u_n2[2] - u_old2[2])**2))
u_old = u_n
u_old2 = u_n2
####Obstacle(hoop)
#c += cs.fmax(0, 0.49-(x[0]+0.5)**2 - (x[1]-4)**2)
#c += 0*cs.fmax(0,-(obs_data[3]**2 - ((x[1]-obs_data[1])**2 + (x[2]-obs_data[2])**2))) * cs.fmax(0, (x[0] - obs_data[0]) + 0.4) * cs.fmax(0, -(x[0] - obs_data[0]) + 0.4)
####State update MAV1
x[0] = x[0] + dt * x[3]
x[1] = x[1] + dt * x[4]
x[2] = x[2] + dt * x[5]
x[3] = x[3] + dt * (cs.sin(x[7]) * cs.cos(x[6]) * u_n[0] - 0.1 * x[3])
x[4] = x[4] + dt * (-cs.sin(x[6]) * u_n[0] - 0.1 * x[4])
x[5] = x[5] + dt * (cs.cos(x[7]) * cs.cos(x[6]) * u_n[0] - 0.2 * x[5] -
