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])