def test_second_order_cone_jacobian(self): soc = og.constraints.SecondOrderCone() # Important note: the second-order cone constraint does not work with cs.MX # An exception will be raised if the user provides an SX u = cs.MX.sym('u', 3) sq_dist = soc.distance_squared(u) sq_dist_jac = cs.jacobian(sq_dist, u) sq_dist_jac_fun = cs.Function('sq_dist_jac', [u], [sq_dist_jac]) v = sq_dist_jac_fun([0., 0., 0.]) for i in range(3): self.assertFalse(math.isnan(v[i]), "v[i] is NaN") self.assertAlmostEqual(0, cs.norm_2(v), 12)
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 __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) if n1 > 0: sq_dist_term = alm_set_c.distance_squared(f1 + xi[1:n1 + 1] / xi[0]) psi += xi[0] * sq_dist_term / 2 if n2 > 0: psi += xi[0] * cs.dot(f2, f2) / 2 jac_psi = cs.jacobian(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