def loss_quadratic(m, z, v=None, W=None): """ Quadratic cost function Simple quadratic loss with W as weight matrix, ignoring variance Parameters ---------- m : dx1 ndarray[float | casadi.Sym] The mean of the input Gaussian v : dxd ndarray[float | casadi.Sym] z: dx1 ndarray[float | casadi.Sym] The target-state [optional] W: dxd ndarray[float | casadi.Sym] The weighting matrix factor for the cost-function (scaling) Returns ------- L: float The quadratic loss """ D = np.shape(m)[0] if W is None: W = SX.eye(D) l_var = 0 if not v is None: l_var = trace(mtimes(W, v)) return mtimes((m - z).T, mtimes(W, m - z)) + l_var
def matrix_norm_2_generalized(a, b_inv, x=None, n_iter=None): """ Get largest generalized eigenvalue of the pair inv_a^{-1},b get the largest eigenvalue of the generalized eigenvalue problem a x = \lambda b x <=> b x = (1/\lambda) a x Let \omega := 1/lambda We solve the problem b x = \omega a x using the inverse power iteration which converges to the smallest generalized eigenvalue \omega_\min Hence we can get \lambda_\max = 1/\omega_\min, the largest eigenvalue of a x = \lambda b x """ n, _ = a.shape if x is None: x = np.eye(n, 1) x /= norm_2(x) if n_iter is None: n_iter = 2 * n**2 y = mtimes(b_inv, mtimes(a, x)) for i in range(n_iter): x = y / norm_2(y) y = mtimes(b_inv, mtimes(a, x)) return mtimes(y.T, x)
def compute_remainder_overapproximations(q, k_fb, l_mu, l_sigma): """ Compute symbolically the (hyper-)rectangle over-approximating the lagrangians of mu and sigma Parameters ---------- q: n_s x n_s ndarray[casadi.SX.sym] The shape matrix of the current state ellipsoid k_fb: n_u x n_s ndarray[casadi.SX.sym] The linear feedback term l_mu: n x 0 numpy 1darray[float] The lipschitz constants for the gradients of the predictive mean l_sigma n x 0 numpy 1darray[float] The lipschitz constans on the predictive variance Returns ------- u_mu: n_s x 0 numpy 1darray[casadi.SX.sym] The upper bound of the over-approximation of the mean lagrangian remainder u_sigma: n_s x 0 numpy 1darray[casadi.SX.sym] The upper bound of the over-approximation of the variance lagrangian remainder """ n_u, n_s = np.shape(k_fb) s = horzcat(np.eye(n_s), k_fb.T) b = mtimes(s, s.T) qb = mtimes(q, b) evals = matrix_norm_2_generalized(b, q) r_sqr = vec_max(evals) u_mu = l_mu * r_sqr u_sigma = l_sigma * sqrt(r_sqr) return u_mu, u_sigma
def lin_ellipsoid_safety_distance(p_center, q_shape, h_mat, h_vec, c_safety=1.0): """Compute symbolically the distance between eLlipsoid and polytope in casadi. Evaluate the distance of an ellipsoid E(p_center,q_shape), to a polytopic set of the form: h_mat * x <= h_vec. Parameters ---------- p_center: n_s x 1 array The center of the state ellipsoid q_shape: n_s x n_s array The shape matrix of the state ellipsoid h_mat: m x n_s array: The shape matrix of the safe polytope (see above) h_vec: m x 1 array The additive vector of the safe polytope (see above) Returns ------- d_safety: 1darray of length m The distance of the ellipsoid to the polytope. If d < 0 (elementwise), the ellipsoid is inside the poltyope (safe), otherwise safety is not guaranteed. """ d_center = mtimes(h_mat, p_center) d_shape = c_safety * sqrt(sum1(mtimes(q_shape, h_mat.T) * h_mat.T)).T d_safety = d_center + d_shape - h_vec return d_safety
def gp_pred(x, kern, beta=None, x_train=None, k_inv_training=None, pred_var=True): """ """ n_pred, _ = np.shape(x) if beta is None: pred_mu = SX.zeros(n_pred, 1) else: k_star = kern(x, y=x_train) pred_mu = mtimes(k_star, beta) if pred_var: pred_sigm = kern(x, diag_only=True) if not beta is None: pred_sigm = pred_sigm - sum2( mtimes(k_star, k_inv_training) * k_star) return pred_mu, pred_sigm return pred_mu
def _include_statistics_of_expression(self, expr, name, exp_phi, ls_factor, model, problem): if self.variable_type == 'algebraic': raise NotImplementedError( "stochastic variables as algebraic variables are not implemented" ) if self.variable_type == 'theta': var_vector = self._get_expression_in_each_scenario(expr, model) stochastic_var_mean = problem.create_optimization_theta( name + '_mean', 1) stochastic_var_var = problem.create_optimization_theta( name + '_var', 1) stochastic_var_par = problem.create_optimization_theta( name + '_par', self.n_pol_parameters) problem.include_time_equality(stochastic_var_par - mtimes(ls_factor, var_vector), when='end') problem.include_time_equality(stochastic_var_mean - stochastic_var_par[0], when='end') problem.include_time_equality(stochastic_var_var - mtimes( (stochastic_var_par[1:]**2).T, exp_phi[1:]), when='end') return stochastic_var_mean, stochastic_var_var, stochastic_var_par
def __mul__(self, other): if isinstance(other, Pose): return Pose(casadi.mtimes(self.R, other.R), casadi.mtimes(self.R, other.t) + self.t) elif isinstance(other, casadi.casadi.MX) and other.shape == (3, 1): return casadi.mtimes(self.R, other) + self.t else: raise NotImplementedError
def test_matrixexpressions(self): with open(os.path.join(TEST_DIR, 'MatrixExpressions.mo'), 'r') as f: txt = f.read() ast_tree = parser.parse(txt) casadi_model = gen_casadi.generate(ast_tree, 'MatrixExpressions') print(casadi_model) ref_model = Model() A = ca.MX.sym("A", 3, 3) b = ca.MX.sym("b", 3) c = ca.MX.sym("c", 3) d = ca.MX.sym("d", 3) C = ca.MX.sym("C", 2, 3) D = ca.MX.sym("D", 3, 2) E = ca.MX.sym("E", 2, 3) I = ca.MX.sym("I", 5, 5) F = ca.MX.sym("F", 3, 3) ref_model.alg_states = list(map(Variable, [A, b, c, d])) ref_model.constants = list(map(Variable, [C, D, E, I, F])) constant_values = [ 1.7 * ca.DM.ones(2, 3), ca.DM.zeros(3, 2), ca.DM.ones(2, 3), ca.DM.eye(5), ca.DM.triplet([0, 1, 2], [0, 1, 2], [1, 2, 3], 3, 3) ] for const, val in zip(ref_model.constants, constant_values): const.value = val ref_model.equations = [ ca.mtimes(A, b) - c, ca.mtimes(A.T, b) - d, F[1, 2] ] self.assert_model_equivalent_numeric(ref_model, casadi_model)
def _relax_algebraic_equations(self): # get the equations to relax alg_relax = self.model.alg[self.relax_algebraic_index] n_alg_relax = alg_relax.numel() self.n_relax += n_alg_relax # create a symbolic nu nu_alg = SX.sym('AL_nu_alg', n_alg_relax) self.nu_sym = vertcat(self.nu_sym, nu_alg) # save the relaxed algebraic equations for computing the update later self.relaxed_alg = vertcat(self.relaxed_alg, alg_relax) # include the penalization term in the objective self.problem.L += (mtimes(nu_alg.T, alg_relax) + self.mu_sym / 2. * mtimes(alg_relax.T, alg_relax)) # include the relaxed y_sym as controls u_guess = self.problem.y_guess[ self. relax_algebraic_index] if self.problem.y_guess is not None else None self.problem.include_control( self.model.y[self.relax_algebraic_var_index], u_max=self.problem.y_max[self.relax_algebraic_var_index], u_min=self.problem.y_min[self.relax_algebraic_var_index], u_guess=u_guess) self.problem.remove_algebraic( self.model.y[self.relax_algebraic_var_index], alg_relax)
def generate_cost_function(self, p_0, u_0, p_all, q_all, mu_perf, sigma_perf, k_ff_safe, k_fb_safe, sigma_safe, k_fb_perf=None, k_ff_perf=None, gp_pred_sigma_perf=None, custom_cost_func=None, eps_noise=0.0): # Generate cost function if custom_cost_func is None: cost = 0 if self.n_perf > 1: n_cost_deviation = np.minimum(self.n_perf, self.n_safe) for i in range(1, n_cost_deviation): cost += mtimes(mu_perf[i, :] - p_all[i, :], mtimes(.1 * self.wx_cost, (mu_perf[i, :] - p_all[i, :]).T)) for i in range(self.n_perf): cost -= sqrt(sum2(gp_pred_sigma_perf[i, :] + eps_noise)) else: for i in range(self.n_safe): cost -= sqrt(sum2(sigma_safe[i, :] + eps_noise)) else: if self.n_perf > 1: cost = custom_cost_func(p_0, u_0, p_all, q_all, k_ff_safe, k_fb_safe, sigma_safe, mu_perf, sigma_perf, gp_pred_sigma_perf, k_fb_perf, k_ff_perf) else: cost = custom_cost_func(p_0, u_0, p_all, q_all, k_ff_safe, k_fb_safe, sigma_safe) return cost
def get_gravity_rnea(self, root, tip, gravity): """Returns the gravitational term as a casadi function.""" if self.robot_desc is None: raise ValueError('Robot description not loaded from urdf') n_joints = self.get_n_joints(root, tip) q = cs.SX.sym("q", n_joints) i_X_p, Si, Ic = self._model_calculation(root, tip, q) v = [] a = [] ag = cs.SX([0., 0., 0., gravity[0], gravity[1], gravity[2]]) f = [] tau = cs.SX.zeros(n_joints) for i in range(0, n_joints): if i == 0: a.append(cs.mtimes(i_X_p[i], -ag)) else: a.append(cs.mtimes(i_X_p[i], a[i - 1])) f.append(cs.mtimes(Ic[i], a[i])) for i in range(n_joints - 1, -1, -1): tau[i] = cs.mtimes(Si[i].T, f[i]) if i != 0: f[i - 1] = f[i - 1] + cs.mtimes(i_X_p[i].T, f[i]) tau = cs.Function("C", [q], [tau], self.func_opts) return tau
def addResidualCost(self, measurement_model, X, t_array, y_array, R, params=None): N_measurements = t_array.shape[0] if y_array is not None: p = y_array.shape[0] else: p = params["p"] # Define Casadi function x = casadi.MX.sym('x', self.n) y = casadi.MX.sym('y', p) residual = casadi.Function('l', [x, y], [y - measurement_model(x, params)]) # Measurment parameters Y = self.addParameter(N_measurements, p) # Define stage cost for (i, t) in enumerate(t_array): # Build the expression for x at time t based on phi_t = self.CPM.evaluateLagrangePolynomials(t) X_t = 0 for j in range(self.N + 1): X_t += X[j]*phi_t[j] # Then add to the cost r_t = residual(X_t, Y[i]) self.J += casadi.mtimes(r_t.T, casadi.mtimes(R, r_t)) if y_array is not None: self.setMeasurement(Y, t_array, y_array) return Y
def sqrt_correct(Rs, H, W): """ source: Fast Stable Kalman Filter Algorithms Utilising the Square Root, Steward 98 Rs: sqrt(R) H: measurement matrix W: sqrt(P) @return: Wp: sqrt(P+) = sqrt((I - KH)P) K: Kalman gain Ss: Innovation variance """ n_x = H.shape[1] n_y = H.shape[0] B = ca.sparsify(ca.blockcat(Rs, ca.mtimes(H, W), ca.SX.zeros(n_x, n_y), W)) # qr by default is upper triangular, so we transpose inputs and outputs B_Q, B_R = ca.qr(B.T) # B_Q orthogonal, B_R, lower triangular B_Q = B_Q.T B_R = B_R.T Wp = B_R[n_y:, n_y:] Ss = B_R[:n_y, :n_y] P_HT_SsInv = B_R[n_y:, :n_y] K = ca.mtimes(P_HT_SsInv, ca.inv(Ss)) return Wp, K, Ss
def eval_ws_eq(self, z, p): t_ws = p[0:2] R_ws = np.array( [ [ca.cos(p[2]), -ca.sin(p[2])], [ca.sin(p[2]), ca.cos(p[2])] ] ) obca_d = [] obca = [] j = 0 for i in range(self.n_obs): A = p[ self.n_x + j*self.d_ineq : self.n_x + (j+self.n_ineq[i])*self.d_ineq ].reshape( (self.n_ineq[i], self.d_ineq) ) b = p[ self.n_x + self.N_ineq*self.d_ineq + j : self.n_x + self.N_ineq*self.d_ineq + j + self.n_ineq[i] ] Lambda = z[ j : j + self.n_ineq[i] ] mu = z[ self.N_ineq + i*self.m_ineq : self.N_ineq + (i+1)*self.m_ineq ] d = z[ self.N_ineq + self.M_ineq + i ] obca_d.append( -ca.dot(self.g, mu) + ca.dot( ca.mtimes(A, t_ws)-b, Lambda ) - d) obca.append( ca.mtimes(self.G.T, mu) + ca.mtimes( ca.mtimes(A, R_ws).T, Lambda ) ) j += self.n_ineq[i] ws_eq = ca.vcat( [ca.vcat(obca_d), ca.vcat(obca)] ) return ws_eq
def code_generation(): x = ca.SX.sym('x', 14) x_gz = ca.SX.sym('x_gz', 14) p = ca.SX.sym('p', 16) u = ca.SX.sym('u', 4) t = ca.SX.sym('t') dt = ca.SX.sym('dt') gz_eqs = gazebo_equations() f_state = gz_eqs['state_from_gz'] eqs = rocket_equations() C_FLT_FRB = gz_eqs['C_FLT_FRB'] F_FRB, M_FRB = eqs['force_moment'](x, u, p) F_FLT = ca.mtimes(C_FLT_FRB, F_FRB) M_FLT = ca.mtimes(C_FLT_FRB, M_FRB) f_u_to_fin = ca.Function('rocket_u_to_fin', [u], [u_to_fin(u)], ['u'], ['fin']) f_force_moment = ca.Function('rocket_force_moment', [x, u, p], [F_FLT, M_FLT], ['x', 'u', 'p'], ['F_FLT', 'M_FLT']) u_control = eqs['control'](x, p, t, dt) f_control = ca.Function('rocket_control', [x, p, t, dt], [u_control], ['x', 'p', 't', 'dt'], ['u']) gen = ca.CodeGenerator('casadi_gen.c', { 'main': False, 'mex': False, 'with_header': True, 'with_mem': True }) gen.add(f_state) gen.add(f_force_moment) gen.add(f_control) gen.add(f_u_to_fin) gen.generate()
def _relax_time_equalities(self): # get the equations to relax eq_relax = self.problem.g_eq[self.relax_time_equality_index] n_eq_relax = eq_relax.numel() self.n_relax += n_eq_relax # create a symbolic nu nu_alg = SX.sym('AL_nu_eq', n_eq_relax) self.nu_sym = vertcat(self.nu_sym, nu_alg) # save the relaxed algebraic equations for computing the update later self.relaxed_eq = vertcat(self.relaxed_eq, eq_relax) # include the penalization term in the objective self.problem.L = self.problem.L + ( mtimes(nu_alg.T, eq_relax) + self.mu_sym / 2. * mtimes(eq_relax.T, eq_relax)) # Remove equality self.problem.g_eq = remove_variables_from_vector_by_indices( self.relax_time_equality_index, self.problem.g_eq) for ind in sorted(self.relax_time_equality_index, reverse=True): self.problem.time_g_eq.pop(ind)
def interpolate(ts, xs, t, equidistant, mode=0): if interp1d is not None: if mode == 0: mode_str = 'linear' elif mode == 1: mode_str = 'floor' else: mode_str = 'ceil' return interp1d(ts, xs, t, mode_str, equidistant) else: if mode == 1: xs = xs[:-1] # block-forward else: xs = xs[1:] # block-backward t = ca.MX(t) if t.size1() > 1: t_ = ca.MX.sym('t') xs_ = ca.MX.sym('xs', xs.size1()) f = ca.Function('interpolant', [t_, xs_], [ ca.mtimes(ca.transpose((t_ >= ts[:-1]) * (t_ < ts[1:])), xs_) ]) f = f.map(t.size1(), 'serial') return ca.transpose(f(ca.transpose(t), ca.repmat(xs, 1, t.size1()))) else: return ca.mtimes(ca.transpose((t >= ts[:-1]) * (t < ts[1:])), xs)
def _predict_sym(self, x_test, return_std=False, return_cov=False): """ Computes the GP posterior mean and covariance at a given a test sample using CasADi symbolics. :param x_test: vector of size mx1, where m is the number of features used for the GP prediction :return: the posterior mean (scalar) and covariance (scalar). """ k_s = self.kernel(self._x_train_cs, x_test.T) # Posterior mean value mu_s = cs.mtimes(k_s.T, self._K_inv_y_cs) + self.y_mean if not return_std and not return_cov: return {'mu': mu_s} k_ss = self.kernel(x_test, x_test) + 1e-8 * cs.MX.eye(x_test.shape[1]) # Posterior covariance cov_s = k_ss - cs.mtimes(cs.mtimes(k_s.T, self._K_inv_cs), k_s) cov_s = cs.diag(cov_s) if return_std: return {'mu': mu_s, 'std': np.sqrt(cov_s)} return {'mu': mu_s, 'cov': cov_s}
def estimate(self, t_k, y_k, u_k): if not self._checked: self._check() self._checked = True if not self._types_fixed: self._fix_types() self._types_fixed = True x_mean = self.x_mean x_cov = self.p_k (x_hat_k_minus, p_k_minus, y_hat_k_minus, p_yk_yk, k_gain) = self._priori_update(x_mean, x_cov, u=u_k, p=self.p, theta=self.theta) x_hat_k = x_hat_k_minus + mtimes(k_gain, (y_k - y_hat_k_minus)) p_k = p_k_minus - mtimes(k_gain, mtimes(p_yk_yk, k_gain.T)) self.x_mean = x_hat_k self.p_k = p_k self.dataset.insert_data('x', t_k, self.x_mean) self.dataset.insert_data('P', t_k, vec(self.p_k)) return x_hat_k, p_k
def diff(self, z, z_train): """ Computes the symbolic differentiation of the kernel function, evaluated at point z and using the training dataset z_train. This function implements equation (80) from overleaf document, without the c^{v_x} vector, and for all the partial derivatives possible (m), instead of just one. :param z: evaluation point. Symbolic vector of length m :param z_train: training dataset. Symbolic matrix of shape n x m :return: an m x n matrix, which is the derivative of the exponential kernel function evaluated at point z against the training dataset. """ if self.kernel_type != 'squared_exponential': raise NotImplementedError len_scale = self.params['l'] if len( self.params['l']) > 0 else self.params['l'] * cs.MX.ones( z_train.shape[1]) len_scale = np.atleast_2d(len_scale**2) # Broadcast z vector to have the shape of z_train (tile z to to the number of training points n) z_tile = cs.mtimes(cs.MX.ones(z_train.shape[0], 1), z.T) # Compute k_zZ. Broadcast it to shape of z_tile and z_train, i.e. by the number of variables in z. k_zZ = cs.mtimes(cs.MX.ones(z_train.shape[1], 1), self.__call__(z_train, z.T).T) return -k_zZ * (z_tile - z_train).T / cs.mtimes( cs.MX.ones(z_train.shape[0], 1), len_scale).T
def solve_K(self, robot, X, U): K = c.DM.zeros(robot.nu * robot.nx, self.T + 1) _, A, B = robot.proc_model() for i in range(self.T, 0, -1): At = A(X[:, i], U[:, i - 1]) Bt = B(X[:, i], U[:, i - 1]) #print "At:", At #print "Bt:", Bt if i == self.T: P = self.Wxf else: P = c.mtimes(c.mtimes(At.T, P), At) - c.mtimes( c.mtimes(c.mtimes(At.T, P), Bt), K_mat) + self.Wx K_mat = c.mtimes(c.inv(self.Wu + c.mtimes(c.mtimes(Bt.T, P), Bt)), c.mtimes(c.mtimes(Bt.T, P), At)) K[:, i] = c.reshape(K_mat, robot.nu * robot.nx, 1) return K
def Kalman_filter(self, robot, X_prev_est, X_prev_act, U, P_prev): #Prediction X_prior = robot.kinematics(X_prev_est, U[0], U[1], U[2], 0) Sigma_w = (self.epsilon**2) * self.Sigma_w Sigma_nu = (self.epsilon**2) * self.Sigma_nu P_prior = c.mtimes(c.mtimes(robot.A, P_prev), robot.A.T) + c.mtimes( c.mtimes(robot.G, Sigma_w), robot.G.T) #Update M = c.DM([[(X_prev_act[0] - 3)**2, 0, 0], [0, 1, 0], [0, 0, 1]]) S = P_prior + c.mtimes(c.mtimes(M, Sigma_nu), M.T) K_gain = c.mtimes(P_prior, c.inv(S)) P_post = c.mtimes(c.DM.eye(robot.nx) - K_gain, P_prior) X_act = robot.kinematics(X_prev_act, U[0], U[1], U[2], self.epsilon) X_act = np.reshape(X_act, (robot.nx, )) #to suit the receiving array nu0 = np.random.normal(0, np.sqrt(Sigma_nu[0, 0])) nu1 = np.random.normal(0, np.sqrt(Sigma_nu[1, 1])) nu2 = np.random.normal(0, np.sqrt(Sigma_nu[2, 2])) nu = c.DM([[nu0], [nu1], [nu2]]) Y = X_act + c.mtimes(M, nu) X_est = X_prior + c.mtimes(K_gain, Y - X_prior) X_est = np.reshape(X_est, (robot.nx, )) return X_est, X_act, P_post
def stage_cost(_x, _u, _x_ref=None, _u_ref=None): if _x_ref is None: _x_ref = cs.DM.zeros(_x.shape) if _u_ref is None: _u_ref = cs.DM.zeros(_u.shape) dx = _x - _x_ref du = _u - _u_ref return cs.mtimes([dx.T, Q, dx]) + cs.mtimes([du.T, R, du])
def minimize_jerk(u): xdd = u[0, :] ydd = u[1, :] xddd = casadi.diff(xdd) yddd = casadi.diff(ydd) xddd_squared = casadi.mtimes(xddd, casadi.transpose(xddd)) yddd_squared = casadi.mtimes(yddd, casadi.transpose(yddd)) return xddd_squared + yddd_squared
def get_regularised_cost_expr(self): slack_var = self.skill_spec.slack_var if slack_var is not None: slack_H = cs.diag(self.weight_shifter + self.slack_var_weights) slack_cost = cs.mtimes(cs.mtimes(slack_var.T, slack_H), slack_var) else: slack_cost = 0.0 return self.weight_shifter * self.cost_expression + slack_cost
def get_cost_integrand_function(self): """Returns a casadi function for the discretized integrand of the cost expression integrated one timestep. For the rectangle method, this just amounts to timing by the timestep. As with the other controllers, the cost is affected by the weight shifter, giving a regularised cost with the slack variables. """ # Setup new symbols needed dt = self.timestep # Setup skill_spec symbols time_var = self.skill_spec.time_var robot_var = self.skill_spec.robot_var list_vars = [time_var, robot_var] list_names = ["time_var", "robot_var"] robot_vel_var = self.skill_spec.robot_vel_var cntrl_vars = [robot_vel_var] cntrl_names = ["robot_vel_var"] virtual_var = self.skill_spec.virtual_var if virtual_var is not None: list_vars += [virtual_var] list_names += ["virtual_var"] virtual_vel_var = self.skill_spec.virtual_vel_var cntrl_vars += [virtual_vel_var] cntrl_names += ["virtual_vel_var"] slack_var = self.skill_spec.slack_var if slack_var is not None: list_vars += [slack_var] list_names += ["slack_var"] # Full symbol list same way as in other controllers list_vars += cntrl_vars list_names += cntrl_names # Expression for the cost with regularisation: if slack_var is not None: slack_H = cs.diag(self.weight_shifter + self.slack_var_weights) slack_cost = cs.mtimes(cs.mtimes(slack_var.T, slack_H), slack_var) regularised_cost = self.weight_shifter*self.cost_expression regularised_cost += slack_cost else: regularised_cost = self.cost_expression # Choose integration method if self.options["cost_integration_method"].lower() == "rectangle": cost_integrand = regularised_cost*dt elif self.options["cost_integration_method"].lower() == "trapezoidal": # Trapezoidal rule raise NotImplementedError("Trapezoidal rule integration not" + " implemented.") elif self.options["cost_integration_method"].lower() == "simpson": # Simpson rule raise NotImplementedError("Simpson rule integration not" + " implemented.") else: raise NotImplementedError(self.options["cost_integration_method"] + " is not a known integration method.") return cs.Function("fc_k", list_vars, [cost_integrand], list_names, ["cost_integrand"])
def get_constraints_expr(self): """Returns a casadi expression describing all the constraints, and expressions for their upper and lower bounds. Return: tuple: (A, Blb,Bub) for Blb<=A*x<Bub, where A*x, Blb and Bub are casadi expressions. """ cnstr_expr_list = [] lb_cnstr_expr_list = [] # lower bound ub_cnstr_expr_list = [] # upper bound time_var = self.skill_spec.time_var robot_var = self.skill_spec.robot_var virtual_var = self.skill_spec.virtual_var n_slack = self.skill_spec.n_slack_var slack_ind = 0 for cnstr in self.skill_spec.constraints: expr_size = cnstr.expression.size() # What's A*opt_var? cnstr_expr = cnstr.jacobian(robot_var) if virtual_var is not None: cnstr_expr = cs.horzcat(cnstr_expr, cnstr.jacobian(virtual_var)) # Everyone wants a feedforward lb_cnstr_expr = -cnstr.jacobian(time_var) ub_cnstr_expr = -cnstr.jacobian(time_var) # Setup bounds if isinstance(cnstr, EqualityConstraint): lb_cnstr_expr += -cs.mtimes(cnstr.gain, cnstr.expression) ub_cnstr_expr += -cs.mtimes(cnstr.gain, cnstr.expression) elif isinstance(cnstr, SetConstraint): lb_cnstr_expr += cs.mtimes(cnstr.gain, cnstr.set_min - cnstr.expression) ub_cnstr_expr += cs.mtimes(cnstr.gain, cnstr.set_max - cnstr.expression) elif isinstance(cnstr, VelocityEqualityConstraint): lb_cnstr_expr += cnstr.target ub_cnstr_expr += cnstr.target elif isinstance(cnstr, VelocitySetConstraint): lb_cnstr_expr += cnstr.set_min ub_cnstr_expr += cnstr.set_max # Soft constraints have slack if n_slack > 0: slack_mat = cs.DM.zeros((expr_size[0], n_slack)) if cnstr.constraint_type == "soft": slack_mat[:, slack_ind:slack_ind + expr_size[0]] = -cs.DM.eye(expr_size[0]) slack_ind += expr_size[0] cnstr_expr = cs.horzcat(cnstr_expr, slack_mat) # Add to lists cnstr_expr_list += [cnstr_expr] lb_cnstr_expr_list += [lb_cnstr_expr] ub_cnstr_expr_list += [ub_cnstr_expr] cnstr_expr_full = cs.vertcat(*cnstr_expr_list) lb_cnstr_expr_full = cs.vertcat(*lb_cnstr_expr_list) ub_cnstr_expr_full = cs.vertcat(*ub_cnstr_expr_list) return cnstr_expr_full, lb_cnstr_expr_full, ub_cnstr_expr_full
def buildDynamicalSystem(self): # q1 is first link q2 is second link q1 = ca.MX.sym('q1') dq1 = ca.MX.sym('dq1') q2 = ca.MX.sym('q2') dq2 = ca.MX.sym('dq2') u = ca.MX.sym('u') theta = ca.MX.sym("theta", 7, 1) if self.trainMotor: Rm = ca.MX.sym("Rm") km = ca.MX.sym("km") params = ca.vertcat(ca.MX.sym("params", 7, 1), Rm, km) else: Rm = self.Rm km = self.km params = theta states = ca.vertcat(q1, q2, dq1, dq2) controls = u # km * (u - km * dq1) / Rm; if self.hackOn: # convert actions which are given as torques to voltage to be consistent with the controller inputs of Lukas u = u * Rm / km controls_torque = km * (u - km * dq1) / Rm # build matrix for mass matrix phi_1_1 = ca.MX(2, 7) phi_1_1[0, 0] = ca.MX.ones(1) phi_1_1[0, 1] = ca.sin(q2) * ca.sin(q2) phi_1_1[1, 2] = ca.cos(q2) phi_1_2 = ca.MX(2, 7) phi_1_2[0, 2] = -ca.cos(q2) phi_1_2[1, 3] = ca.MX.ones(1) mass_matrix = ca.horzcat(ca.mtimes(phi_1_1, params), ca.mtimes(phi_1_2, params)) phi_2 = ca.MX(2, 7) phi_2[0, 1] = ca.sin(2 * q2) * dq1 * dq2 phi_2[0, 2] = ca.sin(q2) * dq2 * dq2 phi_2[0, 5] = dq1 phi_2[1, 1] = -1 / 2 * ca.sin(2 * q2) * dq1 * dq1 phi_2[1, 4] = self.g * ca.sin(q2) phi_2[1, 6] = dq2 forces = ca.mtimes(phi_2, params) actions = ca.vertcat(controls_torque, ca.MX.zeros(1)) b = actions - forces states_dd = ca.solve(mass_matrix, b) states_d = ca.vertcat(dq1, dq2, states_dd[0], states_dd[1]) return states, states_d, controls, params
def kinematics(cls, r, w): assert r.shape == (4, 1) or r.shape == (4, ) assert w.shape == (3, 1) or w.shape == (3, ) a = r[:3] n_sq = ca.dot(a, a) X = wedge(a) B = 0.25 * ((1 - n_sq) * ca.SX.eye(3) + 2 * X + 2 * ca.mtimes(a, ca.transpose(a))) return ca.vertcat(ca.mtimes(B, w), 0)
def dynamics_rear(X_prev, S, v): dt = 0.1 L = 2.33 X_new = X_prev + dt*casadi.blockcat([[casadi.mtimes(v/2.0,casadi.cos(X_prev[2] - S - 8*m.pi/180)+casadi.cos(X_prev[2] + (S + 8*m.pi/180)))], [casadi.mtimes(v/2.0,casadi.sin(X_prev[2] - S - 8*m.pi/180) + casadi.sin(X_prev[2] + (S + 8*m.pi/180)))], [casadi.mtimes(v,casadi.sin(-S - 8*m.pi/180)*1/L)]]) X_new[2] = casadi.atan2(casadi.sin(X_new[2]), casadi.cos(X_new[2])) return X_new
def construct_upd_x(self, problem=None): # construct optifather & give reference to problem self.father_updx = OptiFather(self.group.values()) self.problem.father = self.father_updx # define parameters z_i = self.define_parameter('z_i', self.q_i_struct.shape[0]) z_ji = self.define_parameter('z_ji', self.q_ji_struct.shape[0]) l_i = self.define_parameter('l_i', self.q_i_struct.shape[0]) l_ji = self.define_parameter('l_ji', self.q_ji_struct.shape[0]) rho = self.define_parameter('rho') if problem is None: # put z and l variables in the struct format z_i = self.q_i_struct(z_i) z_ji = self.q_ji_struct(z_ji) l_i = self.q_i_struct(l_i) l_ji = self.q_ji_struct(l_ji) # get time info t = self.define_symbol('t') T = self.define_symbol('T') t0 = t/T # get (part of) variables x_i = self._get_x_variables(symbolic=True) # transform spline variables: only consider future piece of spline tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0) self._transform_spline([x_i, z_i, l_i], tf, self.q_i) self._transform_spline([z_ji, l_ji], tf, self.q_ji) # construct objective obj = 0. for child, q_i in self.q_i.items(): for name in q_i.keys(): x = x_i[child.label][name] z = z_i[child.label, name] l = l_i[child.label, name] obj += mtimes(l.T, x-z) if not self.options['AMA']: obj += 0.5*rho*mtimes((x-z).T, (x-z)) for nghb in self.q_ji.keys(): z = z_ji[str(nghb), child.label, name] l = l_ji[str(nghb), child.label, name] obj += mtimes(l.T, x-z) if not self.options['AMA']: obj += 0.5*rho*mtimes((x-z).T, (x-z)) self.define_objective(obj) # construct problem prob, buildtime = self.father_updx.construct_problem( self.options, str(self._index)) else: prob, buildtime = self.father_updx.construct_problem(self.options, str(self._index), problem) self.problem_upd_x = prob self.father_updx.init_transformations(self.problem.init_primal_transform, self.problem.init_dual_transform) self.init_var_admm() return buildtime
def construct_upd_z(self, problem=None, lineq_updz=True): if problem is not None: self.problem_upd_z = problem self._lineq_updz = lineq_updz return 0. # check if we have linear equality constraints self._lineq_updz, A, b = self._check_for_lineq() if not self._lineq_updz: raise ValueError('For now, only equality constrained QP ' + 'z-updates are allowed!') x_i = struct_symMX(self.q_i_struct) x_j = struct_symMX(self.q_ij_struct) l_i = struct_symMX(self.q_i_struct) l_ij = struct_symMX(self.q_ij_struct) t = MX.sym('t') T = MX.sym('T') rho = MX.sym('rho') par = struct_symMX(self.par_global_struct) inp = [x_i.cat, l_i.cat, l_ij.cat, x_j.cat, t, T, rho, par.cat] t0 = t/T # put symbols in MX structs (necessary for transformation) x_i = self.q_i_struct(x_i) x_j = self.q_ij_struct(x_j) l_i = self.q_i_struct(l_i) l_ij = self.q_ij_struct(l_ij) # transform spline variables: only consider future piece of spline tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0) self._transform_spline([x_i, l_i], tf, self.q_i) self._transform_spline([x_j, l_ij], tf, self.q_ij) # fill in parameters A = A(par.cat) b = b(par.cat) # build KKT system and solve it via schur complement method l, x = vertcat(l_i.cat, l_ij.cat), vertcat(x_i.cat, x_j.cat) f = -(l + rho*x) G = -(1/rho)*mtimes(A, A.T) h = b + (1/rho)*mtimes(A, f) mu = solve(G, h) z = -(1/rho)*(mtimes(A.T, mu)+f) l_qi = self.q_i_struct.shape[0] l_qij = self.q_ij_struct.shape[0] z_i_new = self.q_i_struct(z[:l_qi]) z_ij_new = self.q_ij_struct(z[l_qi:l_qi+l_qij]) # transform back tf = lambda cfs, basis: shift_knot1_bwd(cfs, basis, t0) self._transform_spline(z_i_new, tf, self.q_i) self._transform_spline(z_ij_new, tf, self.q_ij) out = [z_i_new.cat, z_ij_new.cat] # create problem prob, buildtime = create_function('upd_z_'+str(self._index), inp, out, self.options) self.problem_upd_z = prob return buildtime
def evalspline(s, x): # Evaluate spline with symbolic variable # This is possible not the best way to implement this. The conditional node # from casadi should be considered Bl = s.basis coeffs = s.coeffs k = Bl.knots basis = [[]] for i in range(len(k) - 1): if i < Bl.degree + 1 and Bl.knots[0] == Bl.knots[i]: basis[-1].append((x >= Bl.knots[i])*(x <= Bl.knots[i + 1])) else: basis[-1].append((x > Bl.knots[i])*(x <= Bl.knots[i + 1])) for d in range(1, Bl.degree + 1): basis.append([]) for i in range(len(k) - d - 1): b = 0 * x bottom = k[i + d] - k[i] if bottom != 0: b = (x - k[i]) * basis[d - 1][i] / bottom bottom = k[i + d + 1] - k[i + 1] if bottom != 0: b += (k[i + d + 1] - x) * basis[d - 1][i + 1] / bottom basis[-1].append(b) result = 0. for l in range(len(Bl)): result += mtimes(coeffs[l], basis[-1][l]) return result
def _get_symbolic_flux(self, finite_element, degree): """ Get a symbolic expression for the boundary fluxes at the given finite_element and polynomial degree """ return cs.mtimes(cs.DM(self.efms_object.T.values), self.var.a_sx[finite_element, degree-1] * self.var.v_sx[self._get_stage_index(finite_element)])
def crop_spline(spline, min_value, max_value): T, knots2 = get_interval_T(spline.basis, min_value, max_value) if isinstance(spline.coeffs, (SX, MX)): coeffs2 = mtimes(T, spline.coeffs) else: coeffs2 = T.dot(spline.coeffs) basis2 = BSplineBasis(knots2, spline.basis.degree) return BSpline(basis2, coeffs2)
def get_derivative(self, s): # Case 1: s is a constant, e.g. MX(5) if ca.MX(s).is_constant(): return 0 # Case 2: s is a symbol, e.g. MX(x) elif s.is_symbolic(): if s.name() not in self.derivative: if len(self.for_loops) > 0 and s in self.for_loops[-1].indexed_symbols: # Create a new indexed symbol, referencing to the for loop index inside the vector derivative symbol. for_loop_symbol = self.for_loops[-1].indexed_symbols[s] s_without_index = self.get_mx(ast.ComponentRef(name=for_loop_symbol.tree.name)) der_s_without_index = self.get_derivative(s_without_index) if ca.MX(der_s_without_index).is_symbolic(): return self.get_indexed_symbol(ast.ComponentRef(name=der_s_without_index.name(), indices=for_loop_symbol.tree.indices), der_s_without_index) else: return 0 else: der_s = _new_mx("der({})".format(s.name()), s.size()) # If the derivative contains an expression (e.g. der(x + y)) this method is # called with MX variables that are the result of a ca.symvar call. This # ca.symvar call strips the _modelica_shape field from the MX variable, # therefore we need to find the original MX to get the modelica shape. der_s._modelica_shape = \ self.nodes[self.current_class][s.name()]._modelica_shape self.derivative[s.name()] = der_s self.nodes[self.current_class][der_s.name()] = der_s return der_s else: return self.derivative[s.name()] # Case 3: s is an already indexed symbol, e.g. MX(x[1]) elif s.is_op(ca.OP_GETNONZEROS) and s.dep().is_symbolic(): slice_info = s.info()['slice'] dep = s.dep() if dep.name() not in self.derivative: der_dep = _new_mx("der({})".format(dep.name()), dep.size()) der_dep._modelica_shape = \ self.nodes[self.current_class][dep.name()]._modelica_shape self.derivative[dep.name()] = der_dep self.nodes[self.current_class][der_dep.name()] = der_dep return der_dep[slice_info['start']:slice_info['stop']:slice_info['step']] else: return self.derivative[dep.name()][slice_info['start']:slice_info['stop']:slice_info['step']] # Case 4: s is an expression that requires differentiation, e.g. MX(x2 * x2) # Need to do this sort of expansion: der(x1 * x2) = der(x1) * x2 + x1 * der(x2) else: # Differentiate expression using CasADi orig_deps = ca.symvar(s) deps = ca.vertcat(*orig_deps) J = ca.Function('J', [deps], [ca.jacobian(s, deps)]) J_sparsity = J.sparsity_out(0) der_deps = [self.get_derivative(dep) if J_sparsity.has_nz(0, j) else ca.DM.zeros(dep.size()) for j, dep in enumerate(orig_deps)] return ca.mtimes(J(deps), ca.vertcat(*der_deps))
def shift_knot1_fwd(cfs, basis, t_shift): if isinstance(cfs, (SX, MX)): cfs_sym = MX.sym('cfs', cfs.shape) t_shift_sym = MX.sym('t_shift') T = shiftfirstknot_T(basis, t_shift_sym) cfs2_sym = mtimes(T, cfs_sym) fun = Function('fun', [cfs_sym, t_shift_sym], [cfs2_sym]).expand() return fun(cfs, t_shift) else: T = shiftfirstknot_T(basis, t_shift) return T.dot(cfs)
def shift_knot1_bwd(cfs, basis, t_shift): if isinstance(cfs, (SX, MX)): cfs_sym = SX.sym('cfs', cfs.shape) t_shift_sym = SX.sym('t_shift') _, Tinv = shiftfirstknot_T(basis, t_shift_sym, inverse=True) cfs2_sym = mtimes(Tinv, cfs_sym) fun = Function('fun', [cfs_sym, t_shift_sym], [cfs2_sym]).expand() return fun(cfs, t_shift) else: _, Tinv = shiftfirstknot_T(basis, t_shift, inverse=True) return Tinv.dot(cfs)
def construct_upd_res(self, problem=None): if problem is not None: self.problem_upd_res = problem return 0. # create parameters x_i = struct_symMX(self.q_i_struct) z_i = struct_symMX(self.q_i_struct) z_i_p = struct_symMX(self.q_i_struct) z_ij = struct_symMX(self.q_ij_struct) z_ij_p = struct_symMX(self.q_ij_struct) x_j = struct_symMX(self.q_ij_struct) t = MX.sym('t') T = MX.sym('T') t0 = t/T rho = MX.sym('rho') inp = [x_i, z_i, z_i_p, z_ij, z_ij_p, x_j, t, T, rho] # put symbols in MX structs (necessary for transformation) x_i = self.q_i_struct(x_i) z_i = self.q_i_struct(z_i) z_i_p = self.q_i_struct(z_i_p) z_ij = self.q_ij_struct(z_ij) z_ij_p = self.q_ij_struct(z_ij_p) x_j = self.q_ij_struct(x_j) # transform spline variables: only consider future piece of spline tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0) self._transform_spline([x_i, z_i, z_i_p], tf, self.q_i) self._transform_spline([x_j, z_ij, z_ij_p], tf, self.q_ij) # compute residuals pr = mtimes((x_i.cat-z_i.cat).T, (x_i.cat-z_i.cat)) pr += mtimes((x_j.cat-z_ij.cat).T, (x_j.cat-z_ij.cat)) dr = rho*mtimes((z_i.cat-z_i_p.cat).T, (z_i.cat-z_i_p.cat)) dr += rho*mtimes((z_ij.cat-z_ij_p.cat).T, (z_ij.cat-z_ij_p.cat)) cr = rho*pr + dr out = [pr, dr, cr] # create problem prob, buildtime = create_function('upd_res_'+str(self._index), inp, out, self.options) self.problem_upd_res = prob return buildtime
def _initialize_polynomial_constraints(self): """ Add constraints to the model to account for system dynamics and continuity constraints """ # All collocation time points T = np.zeros((self.nk, self.d+1), dtype=object) for k in range(self.nk): for j in range(self.d+1): T[k,j] = (self.var.h_sx[self._get_stage_index(k)] * (k + self.col_vars['tau_root'][j])) # For all finite elements for k in range(self.nk): # For all collocation points for j in range(1, self.d+1): # Get an expression for the state derivative at the collocation # point xp_jk = cs.mtimes(cs.DM(self.col_vars['C'][:,j]).T, self.var.x_sx[k]).T # Add collocation equations to the NLP. # Boundary fluxes are calculated by multiplying the EFM # coefficients in V by the efm matrix [fk] = self.dxdt.call( [T[k,j], cs.SX(self.var.x_sx[k,j]), self._get_symbolic_flux(k, j)]) self.add_constraint( self.var.h_sx[self._get_stage_index(k)] * fk - xp_jk, msg='DXDT collocation - FE {0}, degree {1}'.format(k,j)) # Add continuity equation to NLP if k+1 != self.nk: # Get an expression for the state at the end of the finite # element self.add_constraint( cs.SX(self.var.x_sx[k+1,0]) - self._get_endpoint_expr(self.var.x_sx[k]), msg='Continuity - FE {0}'.format(k)) # Get an expression for the endpoint for objective purposes xf = self._get_endpoint_expr(self.var.x_sx[-1]) self.xf = {met : x_sx for met, x_sx in zip(self.boundary_species, xf)} # Similarly, get an expression for the beginning point x0 = self.var.x_sx[0,0,:] self.x0 = {met : x_sx for met, x_sx in zip(self.boundary_species, x0)}
def dot(self, other): if isinstance(other, (cas.MX, cas.SX)): # compatible with casadi 3.0 -- added by ruben return cas.mtimes(cas.DM(csr_matrix(self)), other) # NOT COMPATIBLE WITH CASADI 2.4 # return cas.DMatrix(csr_matrix(self)).mul(other) elif get_module(other) in ['cvxpy', 'cvxopt']: return cvxopt.sparse(cvxopt.matrix(self.toarray())) * other # A = self.tocoo() # B = cvxopt.spmatrix( # A.data, A.row.tolist(), A.col.tolist(), A.shape # ) # return B * other else: try: # Scipy sparse matrix return super(csr_matrix_alt, self).dot(other) except: # Regular numpy matrix return np.dot(self.toarray(), other)
def shiftfirstknot_T(basis, t_shift, inverse=False): # Create transformation matrix that shifts the first (degree+1) knots over # t_shift. With inverse = True, the inverse transformation is also # computed. knots, deg = basis.knots, basis.degree N = len(basis) if isinstance(t_shift, SX): typ, sym = SX, True elif isinstance(t_shift, MX): typ, sym = MX, True else: typ, sym = np, False _T = typ.eye(deg+1) for k in range(deg+1): _t = typ.zeros((deg+1+k+1, deg+1+k)) for j in range(deg+1+k+1): if j >= deg+1: _t[j, j-1] = 1. elif j <= k: _t[j, j] = 1. else: _t[j, j-1] = (knots[j+deg-k]-t_shift)/(knots[j+deg-k]-knots[j]) _t[j, j] = (t_shift-knots[j])/(knots[j+deg-k]-knots[j]) _T = mtimes(_t, _T) if sym else _t.dot(_T) T = typ.eye(N) T[:deg+1, :deg+1] = _T[deg+1:, :] if inverse: # T is upper triangular: easy inverse Tinv = typ.eye(len(basis)) for i in range(deg, -1, -1): Tinv[i, i] = 1./T[i, i] for j in range(deg, i, -1): Tinv[i, j] = (-1./T[i, i])*sum([T[i, k]*Tinv[k, j] for k in range(i+1, deg+2)]) return T, Tinv else: return T
def _get_endpoint_expr(self, state_sx): """Use the variables in self.col_vars['D'] for find an expression for the end of the finite element """ return cs.mtimes(cs.DM(self.col_vars['D']).T, state_sx).T
def __init__(self,objective,*args): """ optisolve(objective) optisolve(objective,constraints) """ if len(args)>=1: constraints = args[0] else: constraints = [] options = dict() if len(args)>=2: options = args[1] if not isinstance(constraints,list): raise Exception("Constraints must be given as list: [x>=0,y<=0]") [ gl_pure, gl_equality] = sort_constraints( constraints ) symbols = OptimizationObject.get_primitives([objective]+gl_pure) # helper functions for 'x' X = C.veccat(*symbols["x"]) helper = C.Function('helper',[X],symbols["x"]) helper_inv = C.Function('helper_inv',symbols["x"],[X]) # helper functions for 'p' if applicable if 'p' in symbols: P = C.veccat(*symbols["p"]) self.Phelper_inv = C.Function('Phelper_inv',symbols["p"],[P]) else: P = C.MX.sym('p',0,1) if len(gl_pure)>0: g_helpers = []; for p in gl_pure: g_helpers.append(C.MX.sym('g',p.sparsity())) G_helpers = C.veccat(*g_helpers) self.Ghelper = C.Function('Ghelper',[G_helpers],g_helpers) self.Ghelper_inv = C.Function('Ghelper_inv',g_helpers,[G_helpers]) codegen = False; if 'codegen' in options: codegen = options["codegen"] del options["codegen"] opt = {} if codegen: options["jit"] = True opt["jit"] = True gl_pure_v = C.MX() if len(gl_pure)>0: gl_pure_v = C.veccat(*gl_pure) if objective.is_vector() and objective.numel()>1: F = C.vec(objective) objective = 0.5*C.dot(F,F) FF = C.Function('nlp',[X,P], [F]) JF = FF.jacobian() J_out = JF.call([X,P]) J = J_out[0].T; H = C.mtimes(J,J.T) sigma = C.MX.sym('sigma') Hf = C.Function('H',dict(x=X,p=P,lam_f=sigma,hess_gamma_x_x=sigma*C.triu(H)),['x', 'p', 'lam_f', 'lam_g'],['hess_gamma_x_x'],opt) if "expand" in options and options["expand"]: Hf = Hf.expand() options["hess_lag"] = Hf nlp = {"x":X,"p":P,"f":objective,"g":gl_pure_v} self.solver = C.nlpsol('solver','ipopt', nlp, options) # Save to class properties self.symbols = symbols self.helper = helper self.helper_inv = helper_inv self.gl_equality = gl_equality self.resolve()
def __init__(self, inertial_frame_id='world'): Vehicle.__init__(self, inertial_frame_id) # Declaring state variables ## Generalized position vector self.eta = casadi.SX.sym('eta', 6) ## Generalized velocity vector self.nu = casadi.SX.sym('nu', 6) # Build the Coriolis matrix self.CMatrix = casadi.SX.zeros(6, 6) S_12 = - cross_product_operator( casadi.mtimes(self._Mtotal[0:3, 0:3], self.nu[0:3]) + casadi.mtimes(self._Mtotal[0:3, 3:6], self.nu[3:6])) S_22 = - cross_product_operator( casadi.mtimes(self._Mtotal[3:6, 0:3], self.nu[0:3]) + casadi.mtimes(self._Mtotal[3:6, 3:6], self.nu[3:6])) self.CMatrix[0:3, 3:6] = S_12 self.CMatrix[3:6, 0:3] = S_12 self.CMatrix[3:6, 3:6] = S_22 # Build the damping matrix (linear and nonlinear elements) self.DMatrix = - casadi.diag(self._linear_damping) self.DMatrix -= casadi.diag(self._linear_damping_forward_speed) self.DMatrix -= casadi.diag(self._quad_damping * self.nu) # Build the restoring forces vectors wrt the BODY frame Rx = np.array([[1, 0, 0], [0, casadi.cos(self.eta[3]), -1 * casadi.sin(self.eta[3])], [0, casadi.sin(self.eta[3]), casadi.cos(self.eta[3])]]) Ry = np.array([[casadi.cos(self.eta[4]), 0, casadi.sin(self.eta[4])], [0, 1, 0], [-1 * casadi.sin(self.eta[4]), 0, casadi.cos(self.eta[4])]]) Rz = np.array([[casadi.cos(self.eta[5]), -1 * casadi.sin(self.eta[5]), 0], [casadi.sin(self.eta[5]), casadi.cos(self.eta[5]), 0], [0, 0, 1]]) R_n_to_b = casadi.transpose(casadi.mtimes(Rz, casadi.mtimes(Ry, Rx))) if inertial_frame_id == 'world_ned': Fg = casadi.SX([0, 0, -self.mass * self.gravity]) Fb = casadi.SX([0, 0, self.volume * self.gravity * self.density]) else: Fg = casadi.SX([0, 0, self.mass * self.gravity]) Fb = casadi.SX([0, 0, -self.volume * self.gravity * self.density]) self.gVec = casadi.SX.zeros(6) self.gVec[0:3] = -1 * casadi.mtimes(R_n_to_b, Fg + Fb) self.gVec[3:6] = -1 * casadi.mtimes( R_n_to_b, casadi.cross(self._cog, Fg) + casadi.cross(self._cob, Fb)) # Build Jacobian T = 1 / casadi.cos(self.eta[4]) * np.array( [[0, casadi.sin(self.eta[3]) * casadi.sin(self.eta[4]), casadi.cos(self.eta[3]) * casadi.sin(self.eta[4])], [0, casadi.cos(self.eta[3]) * casadi.cos(self.eta[4]), -casadi.cos(self.eta[4]) * casadi.sin(self.eta[3])], [0, casadi.sin(self.eta[3]), casadi.cos(self.eta[3])]]) self.eta_dot = casadi.vertcat( casadi.mtimes(casadi.transpose(R_n_to_b), self.nu[0:3]), casadi.mtimes(T, self.nu[3::])) self.u = casadi.SX.sym('u', 6) self.nu_dot = casadi.solve( self._Mtotal, self.u - casadi.mtimes(self.CMatrix, self.nu) - casadi.mtimes(self.DMatrix, self.nu) - self.gVec)
def exitExpression(self, tree): if isinstance(tree.operator, ast.ComponentRef): op = tree.operator.name else: op = tree.operator if op == '*': op = 'mtimes' # .* differs from * if op.startswith('.'): op = op[1:] logger.debug('exitExpression') n_operands = len(tree.operands) if op == 'der': v = self.get_mx(tree.operands[0]) src = self.get_derivative(v) elif op == '-' and n_operands == 1: src = -self.get_mx(tree.operands[0]) elif op == 'not' and n_operands == 1: src = ca.if_else(self.get_mx(tree.operands[0]), 0, 1, True) elif op == 'mtimes': assert n_operands >= 2 src = self.get_mx(tree.operands[0]) for i in tree.operands[1:]: src = ca.mtimes(src, self.get_mx(i)) elif op == 'transpose' and n_operands == 1: src = self.get_mx(tree.operands[0]).T elif op == 'sum' and n_operands == 1: v = self.get_mx(tree.operands[0]) src = ca.sum1(v) elif op == 'linspace' and n_operands == 3: a = self.get_mx(tree.operands[0]) b = self.get_mx(tree.operands[1]) n_steps = self.get_integer(tree.operands[2]) src = ca.linspace(a, b, n_steps) elif op == 'fill' and n_operands == 2: val = self.get_mx(tree.operands[0]) n_row = self.get_integer(tree.operands[1]) src = val * ca.DM.ones(n_row) elif op == 'fill' and n_operands == 3: val = self.get_mx(tree.operands[0]) n_row = self.get_integer(tree.operands[1]) n_col = self.get_integer(tree.operands[2]) src = val * ca.DM.ones(n_row, n_col) elif op == 'zeros' and n_operands == 1: n_row = self.get_integer(tree.operands[0]) src = ca.DM.zeros(n_row) elif op == 'zeros' and n_operands == 2: n_row = self.get_integer(tree.operands[0]) n_col = self.get_integer(tree.operands[1]) src = ca.DM.zeros(n_row, n_col) elif op == 'ones' and n_operands == 1: n_row = self.get_integer(tree.operands[0]) src = ca.DM.ones(n_row) elif op == 'ones' and n_operands == 2: n_row = self.get_integer(tree.operands[0]) n_col = self.get_integer(tree.operands[1]) src = ca.DM.ones(n_row, n_col) elif op == 'identity' and n_operands == 1: n = self.get_integer(tree.operands[0]) src = ca.DM.eye(n) elif op == 'diagonal' and n_operands == 1: diag = self.get_mx(tree.operands[0]) n = len(diag) indices = list(range(n)) src = ca.DM.triplet(indices, indices, diag, n, n) elif op == 'cat': axis = self.get_integer(tree.operands[0]) assert axis == 1, "Currently only concatenation on first axis is supported" entries = [] for sym in [self.get_mx(op) for op in tree.operands[1:]]: if isinstance(sym, list): for e in sym: entries.append(e) else: entries.append(sym) src = ca.vertcat(*entries) elif op == 'delay' and n_operands == 2: expr = self.get_mx(tree.operands[0]) duration = self.get_mx(tree.operands[1]) src = _new_mx('_pymoca_delay_{}'.format(self.delay_counter), *expr.size()) self.delay_counter += 1 for f in self.for_loops: syms = set(ca.symvar(expr)) if syms.intersection(f.indexed_symbols): f.register_indexed_symbol(src, lambda i: i, True, tree.operands[0], f.index_variable) self.model.delay_states.append(src.name()) self.model.inputs.append(Variable(src)) delay_argument = DelayArgument(expr, duration) self.model.delay_arguments.append(delay_argument) elif op == '_pymoca_interp1d' and n_operands >= 3 and n_operands <= 4: entered_class = self.entered_classes[-1] if isinstance(tree.operands[0], ast.ComponentRef): xp = self.get_mx(entered_class.symbols[tree.operands[0].name].value) else: xp = self.get_mx(tree.operands[0]) if isinstance(tree.operands[1], ast.ComponentRef): yp = self.get_mx(entered_class.symbols[tree.operands[1].name].value) else: yp = self.get_mx(tree.operands[1]) arg = self.get_mx(tree.operands[2]) if n_operands == 4: assert isinstance(tree.operands[3], ast.Primary) mode = tree.operands[3].value else: mode = 'linear' func = ca.interpolant('interpolant', mode, [xp], yp) src = func(arg) elif op == '_pymoca_interp2d' and n_operands >= 5 and n_operands <= 6: entered_class = self.entered_classes[-1] if isinstance(tree.operands[0], ast.ComponentRef): xp = self.get_mx(entered_class.symbols[tree.operands[0].name].value) else: xp = self.get_mx(tree.operands[0]) if isinstance(tree.operands[1], ast.ComponentRef): yp = self.get_mx(entered_class.symbols[tree.operands[1].name].value) else: yp = self.get_mx(tree.operands[1]) if isinstance(tree.operands[2], ast.ComponentRef): zp = self.get_mx(entered_class.symbols[tree.operands[2].name].value) else: zp = self.get_mx(tree.operands[2]) arg_1 = self.get_mx(tree.operands[3]) arg_2 = self.get_mx(tree.operands[4]) if n_operands == 6: assert isinstance(tree.operands[5], ast.Primary) mode = tree.operands[5].value else: mode = 'linear' func = ca.interpolant('interpolant', mode, [xp, yp], np.array(zp).ravel(order='F')) src = func(ca.vertcat(arg_1, arg_2)) elif op in OP_MAP and n_operands == 2: lhs = ca.MX(self.get_mx(tree.operands[0])) rhs = ca.MX(self.get_mx(tree.operands[1])) lhs_op = getattr(lhs, OP_MAP[op]) src = lhs_op(rhs) elif op in OP_MAP and n_operands == 1: lhs = ca.MX(self.get_mx(tree.operands[0])) lhs_op = getattr(lhs, OP_MAP[op]) src = lhs_op() else: src = ca.MX(self.get_mx(tree.operands[0])) # Check for built-in operations, such as the # elementary functions, first. if hasattr(src, op) and n_operands <= 2: if n_operands == 1: src = ca.MX(self.get_mx(tree.operands[0])) src = getattr(src, op)() else: lhs = ca.MX(self.get_mx(tree.operands[0])) rhs = ca.MX(self.get_mx(tree.operands[1])) lhs_op = getattr(lhs, op) src = lhs_op(rhs) else: func = self.get_function(op) src = ca.vertcat(*func.call([self.get_mx(operand) for operand in tree.operands], *self.function_mode)) self.src[tree] = src
def construct_upd_xz(self, problem=None): # construct optifather & give reference to problem self.father_updx = OptiFather(self.group.values()) self.problem.father = self.father_updx # define z_ij variables init = self.q_ij_struct(0) for nghb, q_ij in self.q_ij.items(): for child, q_j in q_ij.items(): for name, ind in q_j.items(): var = np.array(child._values[name]) v = var.T.flatten()[ind] init[nghb.label, child.label, name, ind] = v z_ij = self.define_variable( 'z_ij', self.q_ij_struct.shape[0], value=np.array(init.cat)) # define parameters l_ij = self.define_parameter('l_ij', self.q_ij_struct.shape[0]) l_ji = self.define_parameter('l_ji', self.q_ji_struct.shape[0]) # put them in the struct format z_ij = self.q_ij_struct(z_ij) l_ij = self.q_ij_struct(l_ij) l_ji = self.q_ji_struct(l_ji) # get (part of) variables x_i = self._get_x_variables(symbolic=True) # construct local copies of parameters par = {} for name, s in self.par_global.items(): par[name] = self.define_parameter(name, s.shape[0], s.shape[1]) if problem is None: # get time info t = self.define_symbol('t') T = self.define_symbol('T') t0 = t/T # transform spline variables: only consider future piece of spline tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0) self._transform_spline(x_i, tf, self.q_i) self._transform_spline([z_ij, l_ij], tf, self.q_ij) self._transform_spline(l_ji, tf, self.q_ji) # construct objective obj = 0. for child, q_i in self.q_i.items(): for name in q_i.keys(): x = x_i[child.label][name] for nghb in self.q_ji.keys(): l = l_ji[str(nghb), child.label, name] obj += mtimes(l.T, x) for nghb, q_j in self.q_ij.items(): for child in q_j.keys(): for name in q_j[child].keys(): z = z_ij[str(nghb), child.label, name] l = l_ij[str(nghb), child.label, name] obj -= mtimes(l.T, z) self.define_objective(obj) # construct constraints for con in self.global_constraints: c = con[0] for sym in symvar(c): for label, child in self.group.items(): if sym.name() in child.symbol_dict: name = child.symbol_dict[sym.name()][1] v = x_i[label][name] ind = self.q_i[child][name] sym2 = MX.zeros(sym.size()) sym2[ind] = v sym2 = reshape(sym2, sym.shape) c = substitute(c, sym, sym2) break for nghb in self.q_ij.keys(): for label, child in nghb.group.items(): if sym.name() in child.symbol_dict: name = child.symbol_dict[sym.name()][1] v = z_ij[nghb.label, label, name] ind = self.q_ij[nghb][child][name] sym2 = MX.zeros(sym.size()) sym2[ind] = v sym2 = reshape(sym2, sym.shape) c = substitute(c, sym, sym2) break for name, s in self.par_global.items(): if s.name() == sym.name(): c = substitute(c, sym, par[name]) lb, ub = con[1], con[2] self.define_constraint(c, lb, ub) # construct problem prob, buildtime = self.father_updx.construct_problem( self.options, str(self._index), problem) self.problem_upd_xz = prob self.father_updx.init_transformations(self.problem.init_primal_transform, self.problem.init_dual_transform) self.init_var_dd() return buildtime