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