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_second_order_cone_symbolic(self):
soc = og.constraints.SecondOrderCone(2.0)
u = cs.MX.sym('u', 3, 1)
sq_dist = soc.distance_squared(u)
u0 = [4.0, 3.0, -11.0]
sq_dist_sx_fun = cs.Function('sqd1', [u], [sq_dist])
self.assertAlmostEqual(146.0, sq_dist_sx_fun(u0), 16)
sq_dist_m2 = soc.distance_squared(u)
sq_dist_mx_fun = cs.Function('sqd2', [u], [sq_dist_m2])
self.assertAlmostEqual(146.0, sq_dist_mx_fun(u0), 16)
def with_penalty_constraints(self, penalty_constraints, penalty_function=None):
"""Constraints to for the penalty method
Specify the constraints to be treated with the penalty method (that is,
function c(u; p)) and the penalty function, g. If no penalty function
is specified, the quadratic penalty will be used.
Args:
penalty_constraints: a function <code>c(u, p)</code>, of the decision
variable <code>u</code> and the parameter vector <code>p</code>, which
corresponds to the constraints <code>c(u, p)</code>
penalty_function: a function <code>g: R -> R</code>, used to define the
penalty in the penalty method; the default is <code>g(z) = z^2</code>.
You typically will not need to change this, but if you very much want to,
you need to provide an instance of <code>casadi.casadi.Function</code>
(not a CasADi symbol such as <code>SX</code>).
Returns:
self
"""
if penalty_constraints is None:
pass
self.__penalty_constraints = penalty_constraints
if penalty_function is None:
# default penalty function: quadratic
z = cs.SX.sym("z")
self.__penalty_function = cs.Function('g_penalty_function', [z], [z ** 2])
else:
self.__penalty_function = penalty_function
return self
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 __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
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 plan_exploration_trajectory(self, t, x0, x_init=None):
# create cost function
s = cas.MX.sym("s", t + 1, self.env.o_dim)
a = cas.MX.sym("a", t, self.env.a_dim)
v = cas.MX.sym("v", t, 1)
cost = -cas.sum1(v)
fcost = cas.Function("reward", [s, a, v], [cost])
# planning
u_opt, s_opt, varx_opt, cost = self.planning.plan_multiple_shooting(
t, fcost, x0, x_init=x_init)
return u_opt, s_opt, varx_opt, cost
def test_integration_1(self):
u = cs.SX.sym('u', 3)
xi = cs.SX.sym('xi', 2)
p = cs.SX.sym('p', 2)
f = cs.sin(cs.norm_2(p)) * cs.dot(xi, p) * cs.norm_2(u)
fun = cs.Function('f', [u, xi, p], [f])
transpiler = cc.CasadiRustTranspiler(fun, 'xcst_kangaroo')
transpiler.transpile()
self.assertTrue(transpiler.compile())
(u, xi, p) = ([1, 2, 3], [4, 5], [6, 7])
expected = fun(u, xi, p)
result = transpiler.call_rust(u, xi, p)
self.assertAlmostEqual(expected, result[0], 8)
def log_det_cas(self, size):
"""
Returns a casadi function to compute the log determinant of a matrix. If the function with the specific
size was already requested, the cached function will be returned for efficiency
:param size: the dimension of the square matrix
:return: a casadi function for computing the log determinant
"""
if size in self._log_det_f:
return self._log_det_f[size]
else:
S = cas.SX.sym("s", size, size)
f = cas.Function('log_det', [S],
[cas.trace(cas.log(cas.qr(S)[1]))]).expand()
self._log_det_f[size] = f
return f
def plan_exploration_trajectory(self, t, x0, x_init=None):
# define objective
s = cas.MX.sym("s", t + 1, self.env.o_dim)
a = cas.MX.sym("a", t, self.env.a_dim)
v = cas.MX.sym("v", t, 1)
x = cas.horzcat(s[:-1, :], a)
cost = self.model.pred_ent_cas(x)
fcost = cas.Function("reward", [s, a, v], [cost])
# planning
u_opt, s_opt, varx_opt, cost = self.planning.plan_multiple_shooting(
t, fcost, x0, x_init=x_init)
return u_opt, s_opt, varx_opt, cost
def test_ball_inf_origin(self):
ball = og.constraints.BallInf(None, 1)
x = np.array([3, 2])
x_sym = cs.SX.sym("x", 2)
d_num = ball.distance_squared(x)
d_sym = float(cs.substitute(ball.distance_squared(x_sym), x_sym, x))
self.assertAlmostEqual(d_sym, d_num, 8, "computation of distance")
correct_squared_distance = 5.0
self.assertAlmostEqual(d_num, correct_squared_distance, 8,
"expected squared distance")
# verify that it works with cs.MX
x_sym_mx = cs.MX.sym("xmx", 2)
sqdist_mx = ball.distance_squared(x_sym_mx)
sqdist_mx_fun = cs.Function('sqd', [x_sym_mx], [sqdist_mx])
self.assertAlmostEqual(correct_squared_distance,
sqdist_mx_fun(x)[0], 5)
def __construct_mapping_f1_function(self) -> cs.Function:
self.__logger.info("Defining function F1(u, p)")
problem = self.__problem
u = problem.decision_variables
p = problem.parameter_variables
n1 = problem.dim_constraints_aug_lagrangian()
f1 = problem.penalty_mapping_f1
if n1 > 0:
mapping_f1 = f1
else:
mapping_f1 = 0
alm_mapping_f1_fun = cs.Function(
self.__build_config.alm_mapping_f1_function_name,
[u, p], [mapping_f1])
return alm_mapping_f1_fun
def __construct_mapping_f2_function(self) -> cs.Function:
self.__logger.info("Defining function F2(u, p)")
problem = self.__problem
u = problem.decision_variables
p = problem.parameter_variables
n2 = problem.dim_constraints_penalty()
if n2 > 0:
penalty_constraints = problem.penalty_mapping_f2
else:
penalty_constraints = 0
alm_mapping_f2_fun = cs.Function(
self.__build_config.constraint_penalty_function_name,
[u, p], [penalty_constraints])
return alm_mapping_f2_fun
def __test_simple_function(self, function, filename):
u = cs.SX.sym('u', 3)
xi = cs.SX.sym('xi', 2)
p = cs.SX.sym('p', 4)
f = cs.sum1(p) * cs.sum1(xi) * function(cs.sum1(u))
fun = cs.Function('f', [u, xi, p], [f])
transpiler = cc.CasadiRustTranspiler(casadi_function=fun,
function_alias=filename,
rust_dir='tests/rust',
c_dir='tests/c')
transpiler.transpile()
self.assertTrue(transpiler.compile())
(u, xi, p) = ([0.1, 0.2, 0.3], [4, 5], [6, 7, 8, 9])
expected = fun(u, xi, p)
result = transpiler.call_rust(u, xi, p)
self.assertAlmostEqual(expected, result[0], 12)
def test_integration_2(self):
u = cs.SX.sym('u', 3)
xi = cs.SX.sym('xi', 2)
p = cs.SX.sym('p', 2)
f = cs.sin(cs.norm_2(p)) * cs.dot(xi, p)**2 * cs.norm_2(u)
f = 1/f
df = cs.gradient(f, u)
fun = cs.Function('df', [u, xi, p], [df])
transpiler = cc.CasadiRustTranspiler(fun, 'xcst_panda')
transpiler.transpile()
self.assertTrue(transpiler.compile())
(u, xi, p) = ([1, 2, 3], [4, 5], [6, 7])
expected = fun(u, xi, p)
result = transpiler.call_rust(u, xi, p)
for i in range(3):
self.assertAlmostEqual(expected[i], result[i], 8)
def predict_casf(self, ret_var=False):
"""
Returns a casadi function for making predictions
:param ret_var: Whether the casadi function should also return the predictive variance
:return: A casadi function that takes input and
"""
if ret_var not in self._fcas:
x = cas.MX.sym("x", 1, self.x_dim)
phi = self.phi_cas(x)
mu = cas.mtimes(self.mean.T, phi.T).T
if ret_var:
sigma = 1 / self.beta + cas.sum2(
phi * cas.mtimes(self.s, phi.T).T)
res = [mu, sigma]
else:
res = [mu]
self._fcas[ret_var] = cas.Function("f_mu", [x], res)
return self._fcas[ret_var]
import crust_casadi as cc
import casadi.casadi as cs
u = cs.SX.sym('u', 3)
xi = cs.SX.sym('xi', 2)
p = cs.SX.sym('p', 4)
y = cs.sum1(u)
f = cs.sum1(p) * cs.sum1(xi) * cs.if_else(y <= 1, 2 * y, 3 * y)
fun = cs.Function('f', [u, xi, p], [f])
transpiler = cc.CasadiRustTranspiler(casadi_function=fun,
function_alias='ifelse0x100',
rust_dir='tests/rust',
c_dir='tests/c')
transpiler.transpile()
transpiler.compile()
(u, xi, p) = ([0.1, 0.2, 0.3], [4, 5], [6, 7, 8, 9])
expected = fun(u, xi, p)
result = transpiler.call_rust(u, xi, p)
def plan_multiple_shooting(self, t, cost, x0, x_lim=None, x_init=None):
"""
Optimizes for a trajectory using multiple shooting method
:param t: the horizon to plan for
:param cost: the objective function. May be a casadi function of the form (cas: [s, u, v] -> cost) or a function
of the form t -> (cas: [si, ui, vi] -> cost), so it returns a casadi function given the time step.
:param x0: the starting state
:param x_lim: The limits for states (dimension [t, s_dim, 2] or [s_dim, 2] last index 0 is lower limit,
index 1 the upper. If none, self.x_lim is taken
:param x_init: The inital guess for the trajectory. If None, uniformly random samples from [-2, 2] are taken.
:return: The optimized trajectory actions, states, variances, and the reached cost
"""
if x_lim is None:
x_lim = self.x_lim
s_dim = self.env.o_dim
a_dim = self.env.a_dim
x_dim = self.env.x_dim
a_min = self.env.a_lim[:, 0]
a_max = self.env.a_lim[:, 1]
m = self.model.predict_casf(ret_var=True)
x0 = cas.DM(np.atleast_2d(x0))
xu = cas.MX.sym("x", t,
x_dim) # states and action variables to optimize for
# full state and action variables
x = cas.vcat((x0, xu[:, :s_dim]))
u = xu[:, -a_dim:]
# define standard bounds on variables (action bounds + no state bounds)
xu_l = np.ones((t, x_dim)) * -np.inf
xu_u = np.ones((t, x_dim)) * np.inf
xu_l[:, -a_dim:] = a_min
xu_u[:, -a_dim:] = a_max
# include given task bounds if provided
if x_lim is not None:
if x_lim.ndim == 2:
x_lim = np.tile(x_lim[None, :, :], (t, 1, 1))
xu_l[:, :] = x_lim[:, :, 0]
xu_u[:, :] = x_lim[:, :, 1]
assert np.shape(x_lim) == (t, x_dim, 2)
step_based_objective = not isinstance(cost, cas.Function)
# equality constraints (model) and objective function
obj = 0
g = cas.MX()
_, var_mx = m.mx_out()
v = cas.MX()
for i in range(t):
xi = x[i, :] # current state
ui = u[i, :] # current action
pred_res = m(cas.hcat((xi, ui)))
xj = pred_res[0] # next state
vi = pred_res[1] # current predictive variance
v = cas.vcat((v, vi))
# update constraints
gi = x[i + 1, :] - xj
if self.model_diff:
gi -= xi
g = cas.horzcat(g, gi)
if step_based_objective and cost(i) is not None:
obj += cost(i)(xi, ui, vi)
# if episode-based reward, add the full cost
if not step_based_objective:
obj = cost(x, u, v)
# otherwise add cost of last time step with zero action
elif cost(t) is not None:
obj += cost(t)(x[-1, :], 0, vi)
# create NLP
xu_flat = cas.reshape(xu, -1, 1)
nlp = {'x': xu_flat, 'f': obj, 'g': g}
solver = cas.nlpsol("solver", "ipopt", nlp, self.opt_dict)
# set initial trajectory
if x_init is not None:
xopt_0 = cas.reshape(x_init, -1, 1)
else:
xopt_0 = (np.random.random_sample(xu_flat.shape[0]) - .5) * 4
# solve nlp
sol = solver(x0=xopt_0,
lbx=xu_l.T.flatten(),
ubx=xu_u.T.flatten(),
lbg=0,
ubg=0)
opt_xu = np.array(cas.reshape(sol['x'], xu.shape[0], xu.shape[1]))
opt_a = np.array(opt_xu[:, -a_dim:])
opt_x = np.array(cas.vcat((x0, opt_xu[:, :s_dim])))
cost = float(sol['f'])
# compute variance of trajectory
var_f = cas.Function("variance", [xu], [v])
opt_varx = np.array(var_f(opt_xu))
return opt_a, opt_x, opt_varx, cost
