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 find_algebraic_variable(self,
x,
u,
guess=None,
t=0.0,
p=None,
theta_value=None,
rootfinder_options=None):
if guess is None:
guess = [1] * self.n_y
if rootfinder_options is None:
rootfinder_options = dict(
nlpsol="ipopt",
nlpsol_options=config.SOLVER_OPTIONS["nlpsol_options"])
if p is None:
p = []
if theta_value is None:
theta_value = []
# replace known variables
alg = self.alg
known_var = vertcat(self.t, self.x, self.u, self.p, self.theta)
known_var_values = vertcat(t, x, u, p, theta_value)
alg = substitute(alg, known_var, known_var_values)
f_alg = Function("f_alg", [self.y], [alg])
rf = rootfinder("rf_algebraic_variable", "nlpsol", f_alg,
rootfinder_options)
res = rf(guess)
return res
def __init__(self, **kwargs):
super().__init__(**kwargs)
self.u = SX([])
self._parametrized_controls = []
self.u_par = vertcat(self.u)
self.u_expr = vertcat(self.u)
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 replace_variable(self, original, replacement):
if isinstance(original, list):
original = vertcat(*original)
if isinstance(replacement, list):
replacement = vertcat(*replacement)
if not original.numel() == replacement.numel():
raise ValueError(
"Original and replacement must have the same number of elements!"
"original.numel()={}, replacement.numel()={}".format(
original.numel(), replacement.numel()))
if callable(getattr(super(), 'replace_variable', None)):
super().replace_variable(original, replacement)
self.alg = substitute(self.alg, original, replacement)
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 dynamic_model(state, control, forces, calc_casadi):
# State variables
x = state[0]
y = state[1]
phi = state[2]
v_x = state[3]
v_y = state[4]
omega = state[5]
# Control variables
delta = control[1]
# Tire forces
f_fy = forces[0]
f_rx = forces[1]
f_ry = forces[2]
x_next = v_x * cs.cos(phi) - v_y * cs.sin(phi)
y_next = v_x * cs.sin(phi) + v_y * cs.cos(phi)
phi_next = omega
v_x_next = 1 / p.m * (f_rx - f_fy * cs.sin(delta) + p.m * v_y * omega)
v_y_next = 1 / p.m * (f_ry + f_fy * cs.cos(delta) - p.m * v_x * omega)
omega_next = 1 / p.I_z * (f_fy * p.l_f * cs.cos(delta) - f_ry * p.l_r)
if calc_casadi:
return cs.vertcat(x_next, y_next, phi_next, v_x_next, v_y_next, omega_next)
else:
return x_next, y_next, phi_next, v_x_next, v_y_next, omega_next
def setUpOnlyF1(cls):
u = cs.MX.sym("u", 5) # decision variable (nu = 5)
p = cs.MX.sym("p", 2) # parameter (np = 2)
f1 = cs.vertcat(1.5 * u[0] - u[1], u[2] - u[3])
set_c = og.constraints.Rectangle(xmin=[-0.01, -0.01],
xmax=[0.02, 0.03])
phi = og.functions.rosenbrock(u, p) # cost function
bounds = og.constraints.Ball2(None, 1.5) # ball centered at origin
tcp_config = og.config.TcpServerConfiguration(bind_port=3301)
meta = og.config.OptimizerMeta() \
.with_optimizer_name("only_f1")
problem = og.builder.Problem(u, p, phi) \
.with_aug_lagrangian_constraints(f1, set_c) \
.with_constraints(bounds)
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_tcp_interface_config(tcp_interface_config=tcp_config) \
.with_build_c_bindings()
og.builder.OpEnOptimizerBuilder(problem,
metadata=meta,
build_configuration=build_config,
solver_configuration=cls.solverConfig()) \
.build()
def include_state(self, var, ode=None, x_0=None):
n_x = var.numel()
self.x = vertcat(self.x, var)
if x_0 is None:
x_0 = vertcat(*[SX.sym(var_i.name()) for var_i in var.nz])
self.x_0 = vertcat(self.x_0, x_0)
# crate entry for included state
for ind, x_i in enumerate(var.nz):
if x_i in self._ode:
raise ValueError(f'State "{x_i}" already in this model')
self._ode[x_i] = None
if ode is not None:
self.include_equations(ode=ode, x=var)
return x_0
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 setUpOnlyParametricF2(cls):
u = cs.MX.sym("u", 5) # decision variable (nu = 5)
p = cs.MX.sym("p", 3) # parameter (np = 3)
f2 = u[0] - p[2]
phi = og.functions.rosenbrock(u, cs.vertcat(p[0],
p[1])) # cost function
bounds = og.constraints.Ball2(None, 1.5) # ball centered at origin
tcp_config = og.config.TcpServerConfiguration(bind_port=4599)
meta = og.config.OptimizerMeta() \
.with_optimizer_name("parametric_f2")
problem = og.builder.Problem(u, p, phi) \
.with_penalty_constraints(f2) \
.with_constraints(bounds)
build_config = og.config.BuildConfiguration() \
.with_open_version(RustBuildTestCase.OPEN_RUSTLIB_VERSION) \
.with_build_mode(og.config.BuildConfiguration.DEBUG_MODE) \
.with_build_directory(RustBuildTestCase.TEST_DIR) \
.with_tcp_interface_config(tcp_interface_config=tcp_config) \
.with_build_c_bindings()
solver_config = og.config.SolverConfiguration() \
.with_tolerance(1e-6) \
.with_initial_tolerance(1e-4) \
.with_delta_tolerance(1e-5)
og.builder.OpEnOptimizerBuilder(problem, meta, build_config,
solver_config).build()
def include_equations(self, *args, **kwargs):
if callable(getattr(super(), 'include_equations', None)):
super().include_equations(*args, **kwargs)
alg = kwargs.pop('alg', None)
if len(args) > 0 and alg is None:
alg = SX([])
# in case a list of equations `y == x + u` has been passed
for eq in args:
if is_equality(eq):
alg = vertcat(alg, eq.dep(0) - eq.dep(1))
if isinstance(alg, collections.abc.Sequence):
alg = vertcat(*alg)
if alg is not None:
self.alg = vertcat(self.alg, alg)
def __generate_casadi_code(self):
"""Generates CasADi code"""
logging.info("Defining CasADi functions and generating C code")
u = self.__problem.decision_variables
p = self.__problem.parameter_variables
ncp = self.__problem.dim_constraints_penalty()
phi = self.__problem.cost_function
# If there are penalty-type constraints, we need to define a modified
# cost function
if ncp > 0:
penalty_function = self.__problem.penalty_function
mu = cs.SX.sym("mu", self.__problem.dim_constraints_penalty())
p = cs.vertcat(p, mu)
phi += cs.dot(mu, penalty_function(self.__problem.penalty_constraints))
# Define cost and its gradient as CasADi functions
cost_fun = cs.Function(self.__build_config.cost_function_name, [u, p], [phi])
grad_cost_fun = cs.Function(self.__build_config.grad_function_name,
[u, p], [cs.jacobian(phi, u)])
# Filenames of cost and gradient (C file names)
cost_file_name = self.__build_config.cost_function_name + ".c"
grad_file_name = self.__build_config.grad_function_name + ".c"
# Code generation using CasADi (cost and gradient)
cost_fun.generate(cost_file_name)
grad_cost_fun.generate(grad_file_name)
# Move generated files to target folder
icasadi_extern_dir = os.path.join(self.__icasadi_target_dir(), "extern")
shutil.move(cost_file_name, os.path.join(icasadi_extern_dir, _AUTOGEN_COST_FNAME))
shutil.move(grad_file_name, os.path.join(icasadi_extern_dir, _AUTOGEN_GRAD_FNAME))
# Lastly, we generate code for the penalty constraints; if there aren't
# any, we generate the function c(u; p) = 0 (which will not be used)
if ncp > 0:
penalty_constraints = self.__problem.penalty_constraints
else:
penalty_constraints = 0
# Target C file name
constraints_penalty_file_name = \
self.__build_config.constraint_penalty_function_name + ".c"
# Define CasADi function for c(u; q)
constraint_penalty_fun = cs.Function(
self.__build_config.constraint_penalty_function_name,
[u, p], [penalty_constraints])
# Generate code
constraint_penalty_fun.generate(constraints_penalty_file_name)
# Move auto-generated file to target folder
shutil.move(constraints_penalty_file_name,
os.path.join(icasadi_extern_dir, _AUTOGEN_PNLT_CONSTRAINTS_FNAME))
def test_rust_build_with_penalty(self):
u = cs.SX.sym("u", 15)
p = cs.SX.sym("p", 2)
phi = og.functions.rosenbrock(u, p)
c = cs.vertcat(1.5 * u[0] - u[1], u[2] - u[3])
bounds = og.constraints.Ball2(None, 1.5)
problem = og.builder.Problem(u, p, phi) \
.with_penalty_constraints(c) \
.with_constraints(bounds)
build_config = og.config.BuildConfiguration() \
.with_build_directory(RustBuildTestCase.TEST_DIR) \
.with_build_mode("debug")
og.builder.OpEnOptimizerBuilder(problem, build_configuration=build_config) \
.with_generate_not_build_flag(False).build()
def parametrize_control(self, u, expr, u_par=None):
"""
Parametrize a control variables so it is a function of a set of parameters or other model variables.
:param list|casadi.SX u:
:param list|casadi.SX expr:
:param list|casadi.SX u_par:
"""
# input check
if isinstance(u, list):
u = vertcat(*u)
if isinstance(u_par, list):
u_par = vertcat(*u_par)
if isinstance(expr, list):
expr = vertcat(*expr)
if not u.numel() == expr.numel():
raise ValueError(
"Passed control and parametrization expression does not have same size. "
"u ({}) and expr ({})".format(u.numel(), expr.numel()))
# Check and register the control parametrization.
for u_i in u.nz:
if self.control_is_parametrized(u_i):
raise ValueError(
f'The control "{u_i}" is already parametrized.')
# to get have a new memory address
self._parametrized_controls = self._parametrized_controls + [u_i]
# Remove u from u_par if they are going to be parametrized
self.u_par = remove_variables_from_vector(u, self.u_par)
if u_par is not None:
self.u_par = vertcat(self.u_par, u_par)
# Replace u by expr into the system
self.replace_variable(u, expr)
def test_build_incompatible_dim_(self):
u = cs.SX.sym("u", 5)
p = cs.SX.sym("p", 2)
phi = og.functions.rosenbrock(u, p)
c = cs.vertcat(u[0], u[1], u[2] - u[3])
problem = og.builder.Problem(u, p, phi) \
.with_penalty_constraints(c)
build_config = og.config.BuildConfiguration()
solver_config = og.config.SolverConfiguration() \
.with_initial_penalty_weights([1.0, 2.0]) # should have dim = 3
with self.assertRaises(Exception) as __context:
og.builder.OpEnOptimizerBuilder(problem,
build_configuration=build_config,
solver_configuration=solver_config) \
.with_generate_not_build_flag(True).build()
def control_is_parametrized(self, u):
"""
Check if the control "u" is parametrized
:param casadi.SX u:
:rtype bool:
"""
u = vertcat(u)
if not u.numel() == 1:
raise ValueError(
'The parameter "u" is expected to be of size 1x1, given: {}x{}'
.format(*u.shape))
if any([
is_equal(u, parametrized_u)
for parametrized_u in self._parametrized_controls
]):
return True
return False
def include_equations(self, *args, **kwargs):
if callable(getattr(super(), 'include_equations', None)):
super().include_equations(*args, **kwargs)
ode = kwargs.pop('ode', None)
x = kwargs.pop('x', None)
if ode is None and x is not None:
raise ValueError("`ode` is None but `x` is not None")
# if is in the list form
if isinstance(ode, collections.abc.Sequence):
ode = vertcat(*ode)
if isinstance(x, collections.abc.Sequence):
x = vertcat(*x)
# if ode was passed but not x, try to guess the x
if x is None and ode is not None:
# Check if None are all sequential, ortherwise we don't know who it belongs
first_none = list(self._ode.values()).index(None)
if not all(eq is None
for eq in islice(self._ode.values(), 0, first_none)):
raise ValueError(
"ODE should be inserted on the equation form or in the list form."
"You can't mix both without explicit passing the states associated with the equation."
)
x = vertcat(*list(self._ode.keys())[first_none:first_none +
ode.numel()])
if len(args) > 0 and ode is None:
x = SX([])
ode = SX([])
# get ode and x from equality equations
for eq in args:
if isinstance(eq, EqualityEquation):
if isinstance(eq.lhs, Derivative):
ode = vertcat(ode, eq.rhs)
x = vertcat(x, eq.lhs.inner)
# actually include the equations
if ode is not None and ode.numel() > 0:
for x_i in vertcat(x).nz:
if self._ode[x_i] is not None:
raise Warning(
f'State "{x_i}" already had an ODE associated, overriding it!'
)
ode_dict = dict(self._ode)
ode_dict.update({x_i: ode[ind] for ind, x_i in enumerate(x.nz)})
self._ode = ode_dict
def setUpOnlyF2(cls):
u = cs.MX.sym("u", 5) # decision variable (nu = 5)
p = cs.MX.sym("p", 2) # parameter (np = 2)
f2 = cs.vertcat(0.2 + 1.5 * u[0] - u[1], u[2] - u[3] - 0.1)
phi = og.functions.rosenbrock(u, p)
bounds = og.constraints.Ball2(None, 1.5)
tcp_config = og.config.TcpServerConfiguration(bind_port=3302)
meta = og.config.OptimizerMeta() \
.with_optimizer_name("only_f2")
problem = og.builder.Problem(u, p, phi) \
.with_penalty_constraints(f2) \
.with_constraints(bounds)
build_config = og.config.BuildConfiguration() \
.with_build_directory(RustBuildTestCase.TEST_DIR) \
.with_build_mode("debug") \
.with_tcp_interface_config(tcp_interface_config=tcp_config) \
.with_build_c_bindings()
og.builder.OpEnOptimizerBuilder(problem,
metadata=meta,
build_configuration=build_config,
solver_configuration=cls.solverConfig()) \
.build()
def test_remove_state(model: StateMixin):
x = model.create_state('x', 5)
model.include_equations(
ode=vertcat(*[i * x_i for i, x_i in enumerate(x.nz)]), x=x)
ind_to_remove = 3
to_remove = model.x[ind_to_remove]
to_remove_eq = model.ode[ind_to_remove]
n_x_original = model.n_x
n_ode_original = model.ode.numel()
model.remove_state(to_remove)
# removed var
assert model.n_x == n_x_original - 1
for ind in range(model.n_x):
assert not is_equal(model.x[ind], to_remove)
# removed ode
assert model.ode.numel() == n_ode_original - 1
for ind in range(model.ode.numel()):
assert not is_equal(model.ode[ind], to_remove_eq)
def kinematic_model(state, control, calc_casadi):
# get states
x = state[0] # Longitudinal position
y = state[1] # Lateral Position
psi = state[2] # Heading Angle
v = state[3] # Velocity
# get inputs
a = control[0] # Acceleration
d = control[1] # Front steering angle
# compute slip angle
beta = cs.arctan2(p.l_r * cs.tan(d), (p.l_f + p.l_r))
# compute change in state
x_next = v * cs.cos(psi + beta)
y_next = v * cs.sin(psi + beta)
psi_next = v / p.l_r * cs.sin(beta)
v_next = a
if calc_casadi:
return cs.vertcat(x_next, y_next, psi_next, v_next)
else:
return x_next, y_next, psi_next, v_next
def test_fully_featured_release_mode(self):
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], u[2] - u[3])
xmin = [-2.0, -2.0, -2.0, -2.0, -2.0]
xmax = [2.0, 2.0, 2.0, 2.0, 2.0]
bounds = og.constraints.Rectangle(xmin, xmax)
problem = og.builder.Problem(u, p, phi) \
.with_penalty_constraints(c) \
.with_constraints(bounds)
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_rebuild(False) \
.with_build_mode("debug") \
.with_build_directory(RustBuildTestCase.TEST_DIR) \
.with_build_c_bindings() \
.with_tcp_interface_config()
solver_config = og.config.SolverConfiguration() \
.with_lfbgs_memory(15) \
.with_tolerance(1e-5) \
.with_max_inner_iterations(155) \
.with_constraints_tolerance(1e-4) \
.with_max_outer_iterations(15) \
.with_penalty_weight_update_factor(8.0) \
.with_initial_penalty_weights([20.0, 5.0])
builder = og.builder.OpEnOptimizerBuilder(problem,
metadata=meta,
build_configuration=build_config,
solver_configuration=solver_config) \
.with_generate_not_build_flag(False).with_verbosity_level(0)
builder.build()
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))
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()
def include_theta(self, theta):
self.theta = vertcat(self.theta, theta)
def include_parameter(self, p):
self.p = vertcat(self.p, p)
def test_link_2_c_libs(self):
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], u[2] - u[3])
xmin = [-2.0, -2.0, -2.0, -2.0, -2.0]
xmax = [2.0, 2.0, 2.0, 2.0, 2.0]
bounds = og.constraints.Rectangle(xmin, xmax)
problem = og.builder.Problem(u, p, phi) \
.with_penalty_constraints(c) \
.with_constraints(bounds)
meta1 = og.config.OptimizerMeta() \
.with_version("0.0.0") \
.with_authors(["P. Sopasakis", "E. Fresk"]) \
.with_licence("CC4.0-By") \
.with_optimizer_name("the_optimizer1")
meta2 = og.config.OptimizerMeta() \
.with_version("0.0.0") \
.with_authors(["P. Sopasakis", "E. Fresk"]) \
.with_licence("CC4.0-By") \
.with_optimizer_name("the_optimizer2")
build_config = og.config.BuildConfiguration() \
.with_rebuild(False) \
.with_build_mode("debug") \
.with_build_directory(RustBuildTestCase.TEST_DIR) \
.with_build_c_bindings()
solver_config = og.config.SolverConfiguration() \
.with_lfbgs_memory(15) \
.with_tolerance(1e-5) \
.with_max_inner_iterations(155) \
.with_constraints_tolerance(1e-4) \
.with_max_outer_iterations(15) \
.with_penalty_weight_update_factor(8.0) \
.with_initial_penalty_weights([20.0, 5.0])
builder1 = og.builder.OpEnOptimizerBuilder(problem,
metadata=meta1,
build_configuration=build_config,
solver_configuration=solver_config) \
.with_generate_not_build_flag(False).with_verbosity_level(0)
builder1.build()
builder2 = og.builder.OpEnOptimizerBuilder(problem,
metadata=meta2,
build_configuration=build_config,
solver_configuration=solver_config) \
.with_generate_not_build_flag(False).with_verbosity_level(0)
builder2.build()
p = subprocess.Popen(
[
"gcc", "test/test_2_solvers.c",
"-I" + RustBuildTestCase.TEST_DIR + "/the_optimizer1",
"-I" + RustBuildTestCase.TEST_DIR + "/the_optimizer2",
"-pthread", RustBuildTestCase.TEST_DIR +
"/the_optimizer1/target/debug/libthe_optimizer1.a",
RustBuildTestCase.TEST_DIR +
"/the_optimizer2/target/debug/libthe_optimizer2.a", "-lm",
"-ldl", "-std=c99",
"-o" + RustBuildTestCase.TEST_DIR + "/test_2_solvers"
],
#stdin=subprocess.PIPE,
stdout=subprocess.PIPE,
stderr=subprocess.PIPE)
output, err = p.communicate()
rc = p.returncode
print("gcc output: ")
print(output.decode("utf-8"))
print("gcc err: ")
print(err.decode("utf-8"))
print("gcc rc: ")
print(rc)
if rc != 0:
exit(rc)
p = subprocess.Popen(
["./" + RustBuildTestCase.TEST_DIR + "/test_2_solvers"],
#stdin=subprocess.PIPE,
stdout=subprocess.PIPE,
stderr=subprocess.PIPE)
output, err = p.communicate()
rc = p.returncode
print("test_2_solvers output: ")
print(output.decode("utf-8"))
print("test_2_solvers err: ")
print(err.decode("utf-8"))
print("test_2_solvers rc: ")
print(rc)
if rc != 0:
exit(rc)
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("potato") \
.with_version('0.1.2') \
.with_licence('LGPLv3')
ros_config = og.config.RosConfiguration() \
.with_package_name("potato_optimizer") \
.with_node_name("potato_controller") \
.with_rate(35) \
.with_description("cool ROS node")
local_open = '/home/chung/NetBeansProjects/RUST/optimization-engine/'
build_config = og.config.BuildConfiguration() \
.with_build_directory("my_optimizers") \
.with_build_mode(og.config.BuildConfiguration.RELEASE_MODE) \
.with_open_version('0.7.0-alpha.1') \
.with_tcp_interface_config() \
.with_ros(ros_config)
solver_config = og.config.SolverConfiguration() \
.with_tolerance(1e-5) \
.with_delta_tolerance(1e-4) \
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()
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:
# for j in range(0,nMAV-1):
# c = cs.vertcat(c, 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,obs_data[3] - (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))
#c = cs.vertcat(c,cs.fmax(0, obs_data[3]**2-(x[ns*j]+7)**2 - (x[ns*j+1]+1.5)**2))
#c = cs.vertcat(c,cs.fmax(0, obs_data[3]**2-(x[ns*j]+11)**2 - (x[ns*j+1])**2))
#c = cs.vertcat(c, cs.fmax(0, -0.005 + (u_n[nu*j+1] - u_old[nu*j+1])**2))
#c = cs.vertcat(c, cs.fmax(0, -0.005 + (u_n[nu*j+2] - u_old[nu*j+2])**2))
#c = cs.vertcat(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, -(obs_data[3]**2 - ((x[1]-obs_data[1])**2 + (x[2]-obs_data[2])**2)) * ( (x[0] - obs_data[0]) + 0.4) * (-(x[0] - obs_data[0]) + 0.4))
####State update MAV1
u_old = u_n
for j in range(0,nMAV):
x[ns*j] = x[ns*j] + dt * x[ns*j+3]
x[ns*j+1] = x[ns*j+1] + dt * x[ns*j+4]
x[ns*j+2] = x[ns*j+2] + dt * x[ns*j+5]
x[ns*j+3] = x[ns*j+3] + dt * (cs.sin(x[ns*j+7]) * cs.cos(x[ns*j+6]) * u_n[nu*j] - 0.1 * x[ns*j+3])
x[ns*j+4] = x[ns*j+4] + dt * (-cs.sin(x[ns*j+6]) * u_n[nu*j] - 0.1*x[ns*j+4])
