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