def _initialize_variables(self, pvars=None): core_variables = { 'x' : (self.nk, self.d+1, self.nx), 'v' : (self.nf, self.nv), 'a' : (self.nk, self.d), 'h' : (self.nf), } self.var = VariableHandler(core_variables) # Initialize default variable bounds self.var.x_lb[:] = 0. self.var.x_ub[:] = 100. self.var.x_in[:] = 1. # Initialize EFM bounds. EFMs are nonnegative. self.var.v_lb[:] = 0. self.var.v_ub[:] = 1. self.var.v_in[:] = 0. # Activity polynomial. self.var.a_lb[:] = 0. self.var.a_ub[:] = np.inf self.var.a_in[:] = 1. # Stage stepsize control (in hours) self.var.h_lb[:] = .1 self.var.h_ub[:] = 10. self.var.h_in[:] = 1. # Maintain compatibility with codes using a symbolic final time self.var.tf_sx = sum([self.var.h_sx[i] * self.stage_breakdown[i] for i in range(self.nf)]) # We also want the v_sx variable to represent a fraction of the overall # efm, so we'll add a constraint saying the sum of the variable must # equal 1. if self.nf > 1: self.add_constraint(cs.sum2(self.var.v_sx[:]), np.ones(self.nf), np.ones(self.nf), 'Sum(v_sx) == 1') elif self.nf == 1: self.add_constraint(cs.sum1(self.var.v_sx[:]), np.ones(self.nf), np.ones(self.nf), 'Sum(v_sx) == 1') if pvars is None: pvars = {} self.pvar = VariableHandler(pvars)
def __setPhysics__(self): # Generalized Coordinates, q = [x_B, z_B, theta_H, theta_K] q = ca.SX.sym('q', self.n_generalized, 1) dq = ca.SX.sym('dq', self.n_generalized, 1) # ddq = ca.SX.sym('ddq', self.n_generalized, 1) # Inputs and external force, u = [u_H, u_K] u = ca.SX.sym('u', self.dof, 1) lam = ca.SX.sym('lambda', 3, self.n_contact) """Hopper Dynamics""" "Joint positions from Base to end effector" p = ca.SX.zeros(2, self.n_joints) p[0, 0], p[1, 0] = q[0] + (self.length[1, 0]) * ca.sin( q[2]), q[1] - (self.length[1, 0]) * ca.cos(q[2]) p[0, 1], p[1, 1] = p[0, 0] + self.length[2, 0] * ca.sin(q[2] + q[3]), p[ 1, 0] - self.length[2, 0] * ca.cos(q[2] + q[3]) # COG g = ca.SX.zeros(2, self.n_joints + 1) g[0, 0], g[1, 0] = q[0], q[1] g[0, 1], g[1, 1] = p[0, 0] / 2, p[1, 0] / 2 g[0, 2], g[ 1, 2] = p[0, 0] + (self.length[2, 0] / 2) * ca.sin(q[2] + q[3]), p[ 1, 0] - (self.length[2, 0] / 2) * ca.cos(q[2] + q[3]) J_ang = ca.DM([[0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 1., 0], [0, 0, 1., 1.]]) a = (J_ang @ q).T J_ee = ca.jacobian(p[:, -1], q) "For inertia matrix" H = ca.SX.zeros(self.n_generalized, self.n_generalized) for i in range(self.dof): J_l = ca.jacobian(p[:, i], q) J_a = ca.jacobian(a[:, i], q) I = (1 / 12) * self.mass[i] * (self.length[i, 0]**2) # Rod M = self.mass[i] H += M * J_l.T @ J_l + I * J_a.T @ J_a "For coriolis + centrifugal matrix" C = ca.SX.zeros(self.n_generalized, self.n_generalized) for i in range(self.n_generalized): for j in range(self.n_generalized): sum_ = 0 for k in range(self.n_generalized): c_ijk = ca.jacobian(H[i, j], q[k]) + ca.jacobian( H[i, j], q[j]) - ca.jacobian(H[k, j], q[i]) sum_ += c_ijk @ dq[k] C[i, j] = (1 / 2) * sum_ # sum_ = 0 # for k in range(self.dof): # c_ijk = ca.jacobian(H[i, j], q[k]) - (1/2)*ca.jacobian(H[j, k], q[i]) # sum_ += c_ijk @ dq[j] @ dq[k] # C[i, j] = sum_ "For G matrix" V = self.gravity * ca.sum1(self.mass * g[1, :].T) G = ca.jacobian(V, q).T "For B matrix" B = ca.DM([[0., 0.], [0., 0.], [1., 0.], [0., 1.]]) "For external force" pe_terrain = ca.SX.zeros(2, 1) pe_terrain[0, 0] = p[0, -1] pe_terrain[1, 0] = self.terrain.heightMap(p[0, -1]) phi = p[:, -1] - pe_terrain B_J_C = ca.jacobian(phi, q) theta_contact = ca.acos( ca.dot(self.terrain.heightMapNormalVector(p[0, -1]), ca.DM([0, 1]))) # rotate lambda force represented in contact frame to world frame B_Rot_C = ca.SX.zeros(2, 2) B_Rot_C[0, 0], B_Rot_C[0, 1] = ca.cos(theta_contact), ca.sin(theta_contact) B_Rot_C[1, 0], B_Rot_C[1, 1] = -ca.sin(theta_contact), ca.cos(theta_contact) C_lam = ca.SX.zeros(2, 1) C_lam[0, 0], C_lam[1, 0] = lam[0, 0] - lam[1, 0], lam[2, 0] B_lam = B_Rot_C @ C_lam dp = J_ee @ dq # print(dp.shape) psi = ca.dot(self.terrain.heightMapTangentVector(pe_terrain[0, 0]), dp) # psi = dp[0, 0] self.kinematics = ca.Function('Kinematics', [q], [p, g], ['q'], ['p', 'g']) self.dynamics = ca.Function('Dynamics', [q, dq, u, lam], [ H, C, B, G, phi, J_ee, B_J_C, C_lam, B_lam, psi ], ['q', 'dq', 'u', 'lam'], [ 'H', 'C', 'B', 'G', 'phi', 'J_ee', 'B_J_C', 'C_lam', 'B_lam', 'psi' ])
def _initialize_mav_objective(self): """ Initialize the objective function to minimize the absolute value of the parameter vector """ self.objective_sx += (self.pvar.alpha_sx[:] * cs.sum1(cs.fabs(self.var.p_sx[:])))
def Max_velocity(pts, conf, show=False): mu = conf.mu m = conf.m g = conf.g l_f = conf.l_f l_r = conf.l_r safety_f = conf.force_f f_max = mu * m * g * safety_f f_long_max = l_f / (l_r + l_f) * f_max max_v = conf.max_v max_a = conf.max_a s_i, th_i = convert_pts_s_th(pts) th_i_1 = th_i[:-1] s_i_1 = s_i[:-1] N = len(s_i) N1 = len(s_i) - 1 # setup possible casadi functions d_x = ca.MX.sym('d_x', N - 1) d_y = ca.MX.sym('d_y', N - 1) vel = ca.Function('vel', [d_x, d_y], [ca.sqrt(ca.power(d_x, 2) + ca.power(d_y, 2))]) dx = ca.MX.sym('dx', N) dy = ca.MX.sym('dy', N) dt = ca.MX.sym('t', N - 1) f_long = ca.MX.sym('f_long', N - 1) f_lat = ca.MX.sym('f_lat', N - 1) nlp = {\ 'x': ca.vertcat(dx, dy, dt, f_long, f_lat), 'f': ca.sum1(dt), 'g': ca.vertcat( # dynamic constraints dt - s_i_1 / ((vel(dx[:-1], dy[:-1]) + vel(dx[1:], dy[1:])) / 2 ), # ca.arctan2(dy, dx) - th_i, dx/dy - ca.tan(th_i), dx[1:] - (dx[:-1] + (ca.sin(th_i_1) * f_long + ca.cos(th_i_1) * f_lat) * dt / m), dy[1:] - (dy[:-1] + (ca.cos(th_i_1) * f_long - ca.sin(th_i_1) * f_lat) * dt / m), # path constraints ca.sqrt(ca.power(f_long, 2) + ca.power(f_lat, 2)), # boundary constraints # dx[0], dy[0] ) \ } # S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt':{'print_level':0}}) S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt': {'print_level': 5}}) # make init sol v0 = np.ones(N) * max_v / 2 dx0 = v0 * np.sin(th_i) dy0 = v0 * np.cos(th_i) dt0 = s_i_1 / ca.sqrt(ca.power(dx0[:-1], 2) + ca.power(dy0[:-1], 2)) f_long0 = np.zeros(N - 1) ddx0 = dx0[1:] - dx0[:-1] ddy0 = dy0[1:] - dy0[:-1] a0 = (ddx0**2 + ddy0**2)**0.5 f_lat0 = a0 * m x0 = ca.vertcat(dx0, dy0, dt0, f_long0, f_lat0) # make lbx, ubx # lbx = [-max_v] * N + [-max_v] * N + [0] * N1 + [-f_long_max] * N1 + [-f_max] * N1 # lbx = [-max_v] * N + [0] * N + [0] * N1 + [-f_long_max] * N1 + [-f_max] * N1 # ubx = [max_v] * N + [max_v] * N + [10] * N1 + [f_long_max] * N1 + [f_max] * N1 lbx = [-max_v] * N + [0] * N + [0] * N1 + [-ca.inf] * N1 + [-f_max] * N1 ubx = [max_v] * N + [max_v] * N + [10] * N1 + [ca.inf] * N1 + [f_max] * N1 #make lbg, ubg lbg = [0] * N1 + [0] * N + [0] * 2 * N1 + [0] * N1 #+ [0] * 2 # ubg = [0] * N1 + [0] * N + [0] * 2 * N1 + [f_max] * N1 #+ [0] * 2 ubg = [0] * N1 + [0] * N + [0] * 2 * N1 + [ca.inf] * N1 #+ [0] * 2 r = S(x0=x0, lbg=lbg, ubg=ubg, lbx=lbx, ubx=ubx) x_opt = r['x'] dx = np.array(x_opt[:N]) dy = np.array(x_opt[N:N * 2]) dt = np.array(x_opt[2 * N:N * 2 + N1]) f_long = np.array(x_opt[2 * N + N1:2 * N + N1 * 2]) f_lat = np.array(x_opt[-N1:]) f_t = (f_long**2 + f_lat**2)**0.5 # print(f"Dt: {dt.T}") # print(f"DT0: {dt[0]}") t = np.cumsum(dt) t = np.insert(t, 0, 0) # print(f"Dt: {dt.T}") # print(f"Dx: {dx.T}") # print(f"Dy: {dy.T}") vs = (dx**2 + dy**2)**0.5 if show: plt.figure(1) plt.clf() plt.title("Velocity vs dt") plt.plot(t, vs) plt.plot(t, th_i) plt.legend(['vs', 'ths']) # plt.plot(t, dx) # plt.plot(t, dy) # plt.legend(['v', 'dx', 'dy']) plt.plot(t, np.ones_like(t) * max_v, '--') plt.figure(3) plt.clf() plt.title("F_long, F_lat vs t") plt.plot(t[:-1], f_long) plt.plot(t[:-1], f_lat) plt.plot(t[:-1], f_t, linewidth=3) plt.plot(t, np.ones_like(t) * f_max, '--') plt.plot(t, np.ones_like(t) * -f_max, '--') plt.plot(t, np.ones_like(t) * f_long_max, '--') plt.plot(t, np.ones_like(t) * -f_long_max, '--') plt.ylim([-25, 25]) plt.legend(['Flong', "f_lat", "f_t"]) # plt.show() plt.pause(0.001) # plt.figure(9) # plt.clf() # plt.title("F_long, F_lat vs t") # plt.plot(t[:-1], f_long) # plt.plot(t[:-1], f_lat) # plt.plot(t[:-1], f_t, linewidth=3) # plt.plot(t, np.ones_like(t) * f_max, '--') # plt.plot(t, np.ones_like(t) * -f_max, '--') # plt.plot(t, np.ones_like(t) * f_long_max, '--') # plt.plot(t, np.ones_like(t) * -f_long_max, '--') # plt.legend(['Flong', "f_lat", "f_t"]) return vs
def compute_penalty_values(t, x, u, p, penalty, dt): """ Compute the penalty value for the given time, state, control, parameters, penalty and time step Parameters ---------- t: int Time index x: ndarray State vector with intermediate states u: ndarray Control vector with starting control (and sometimes final control) p: ndarray Parameters vector penalty: Penalty The penalty object containing details on how to compute it dt: float Time step for the whole interval Returns ------- Values computed for the given time, state, control, parameters, penalty and time step """ if len(x.shape) < 2: x = x.reshape((-1, 1)) # if time is parameter of the ocp, we need to evaluate with current parameters if isinstance(dt, Function): dt = dt(p) # The division is to account for the steps in the integration. The else is for Mayer term dt = dt / (x.shape[1] - 1) if x.shape[1] > 1 else dt if not isinstance(penalty.dt, (float, int)): if dt.shape[0] > 1: dt = dt[penalty.phase] _target = (np.hstack([ p[..., penalty.node_idx.index(t)] for p in penalty.target ]) if penalty.target is not None and isinstance(t, int) else []) out = [] if penalty.transition or penalty.multinode_constraint: out.append( penalty.weighted_function_non_threaded( x.reshape((-1, 1)), u.reshape((-1, 1)), p, penalty.weight, _target, dt)) elif penalty.derivative or penalty.explicit_derivative: out.append( penalty.weighted_function_non_threaded( x[:, [0, -1]], u, p, penalty.weight, _target, dt)) elif (penalty.integration_rule == IntegralApproximation.TRAPEZOIDAL or penalty.integration_rule == IntegralApproximation.TRUE_TRAPEZOIDAL): out = [ penalty.weighted_function_non_threaded( x[:, [i, i + 1]], u[:, i], p, penalty.weight, _target, dt) for i in range(x.shape[1] - 1) ] else: out.append( penalty.weighted_function_non_threaded( x, u, p, penalty.weight, _target, dt)) return sum1(horzcat(*out))
nominal_diameter = cas.exp(log_nominal_diameter) thickness = 0.14e-3 * 5 opti.subject_to([ nominal_diameter > thickness, ]) def trapz(x): out = (x[:-1] + x[1:]) / 2 out[0] += x[0] / 2 out[-1] += x[-1] / 2 return out # Mass volume = cas.sum1(cas.pi / 4 * trapz((nominal_diameter + thickness)**2 - (nominal_diameter - thickness)**2) * dx) mass = volume * 1600 # Bending loads I = cas.pi / 64 * ((nominal_diameter + thickness)**4 - (nominal_diameter - thickness)**4) EI = E * I total_lift_force = 9.81 * (mass_total - mass) / 2 # 9.81 * 292 / 2 lift_distribution = "elliptical" if lift_distribution == "rectangular": force_per_unit_length = total_lift_force * cas.GenDM_ones(n) / L elif lift_distribution == "elliptical": force_per_unit_length = total_lift_force * cas.sqrt(1 - (x / L)**2) * ( 4 / cas.pi) / L
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 calc_NLL(hyper, X, Y, squaredist, meanFunc='zero', prior=None): """ Objective function Calculate the negative log likelihood function using Casadi SX symbols. # Arguments: hyper: Array with hyperparameters [ell_1 .. ell_Nx sf sn], where Nx is the number of inputs to the GP. X: Training data matrix with inputs of size (N x Nx). Y: Training data matrix with outpyts of size (N x Ny), with Ny number of outputs. # Returns: NLL: The negative log likelihood function (scalar) """ N, Nx = ca.MX.size(X) ell = hyper[:Nx] sf2 = hyper[Nx]**2 sn2 = hyper[Nx + 1]**2 m = get_mean_function(hyper, X.T, func=meanFunc) # Calculate covariance matrix K_s = ca.SX.sym('K_s', N, N) sqdist = ca.SX.sym('sqd', N, N) elli = ca.SX.sym('elli') ki = ca.Function('ki', [sqdist, elli, K_s], [sqdist / elli**2 + K_s]) K1 = ca.MX(N, N) for i in range(Nx): K1 = ki(squaredist[:, (i * N):(i + 1) * N], ell[i], K1) sf2_s = ca.SX.sym('sf2') exponent = ca.SX.sym('exp', N, N) K_exp = ca.Function('K', [exponent, sf2_s], [sf2_s * ca.SX.exp(-.5 * exponent)]) K2 = K_exp(K1, sf2) K = K2 + sn2 * ca.MX.eye(N) K = (K + K.T) * 0.5 # Make sure matrix is symmentric A = ca.SX.sym('A', ca.MX.size(K)) cholesky = ca.Function('cholesky', [A], [ca.chol(A).T]) L = cholesky(K) B = 2 * ca.sum1(ca.SX.log(ca.diag(A))) log_determinant = ca.Function('log_det', [A], [B]) log_detK = log_determinant(L) Y_s = ca.SX.sym('Y', ca.MX.size(Y)) L_s = ca.SX.sym('L', ca.Sparsity.lower(N)) sol = ca.Function('sol', [L_s, Y_s], [ca.solve(L_s, Y_s)]) invLy = sol(L, Y - m(X.T)) invLy_s = ca.SX.sym('invLy', ca.MX.size(invLy)) sol2 = ca.Function('sol2', [L_s, invLy_s], [ca.solve(L_s.T, invLy_s)]) alpha = sol2(L, invLy) alph = ca.SX.sym('alph', ca.MX.size(alpha)) detK = ca.SX.sym('det') # Calculate hyperpriors theta = ca.SX.sym('theta') mu = ca.SX.sym('mu') s2 = ca.SX.sym('s2') prior_gauss = ca.Function( 'hyp_prior', [theta, mu, s2], [-(theta - mu)**2 / (2 * s2) - 0.5 * ca.log(2 * ca.pi * s2)]) log_prior = 0 if prior is not None: for i in range(Nx): log_prior += prior_gauss(ell[i], prior['ell_mean'], prior['ell_std']**2) log_prior += prior_gauss(sf2, prior['sf_mean'], prior['sf_std']**2) log_prior += prior_gauss(sn2, prior['sn_mean'], prior['sn_std']**2) NLL = ca.Function('NLL', [Y_s, alph, detK], [0.5 * ca.mtimes(Y_s.T, alph) + 0.5 * detK]) return NLL(Y - m(X.T), alpha, log_detK) + log_prior
def test_sum(self): self.message("sum") D = DM([[1, 2, 3], [4, 5, 6], [7, 8, 9]]) self.checkarray(c.sum1(D), array([[12, 15, 18]]), 'sum()') self.checkarray(c.sum2(D), array([[6, 15, 24]]).T, 'sum()')
def opt_mintime(reftrack: np.ndarray, coeffs_x: np.ndarray, coeffs_y: np.ndarray, normvectors: np.ndarray, pars: dict, tpamap_path: str, tpadata_path: str, export_path: str, print_debug: bool = False, plot_debug: bool = False) -> tuple: """ Created by: Fabian Christ Documentation: The minimum lap time problem is described as an optimal control problem, converted to a nonlinear program using direct orthogonal Gauss-Legendre collocation and then solved by the interior-point method IPOPT. Reduced computing times are achieved using a curvilinear abscissa approach for track description, algorithmic differentiation using the software framework CasADi, and a smoothing of the track input data by approximate spline regression. The vehicles behavior is approximated as a double track model with quasi-steady state tire load simplification and nonlinear tire model. Please refer to our paper for further information: Christ, Wischnewski, Heilmeier, Lohmann Time-Optimal Trajectory Planning for a Race Car Considering Variable Tire-Road Friction Coefficients Inputs: reftrack: track [x_m, y_m, w_tr_right_m, w_tr_left_m] coeffs_x: coefficient matrix of the x splines with size (no_splines x 4) coeffs_y: coefficient matrix of the y splines with size (no_splines x 4) normvectors: array containing normalized normal vectors for every traj. point [x_component, y_component] pars: parameters dictionary tpamap_path: file path to tpa map (required for friction map loading) tpadata_path: file path to tpa data (required for friction map loading) export_path: path to output folder for warm start files and solution files print_debug: determines if debug messages are printed plot_debug: determines if debug plots are shown Outputs: alpha_opt: solution vector of the optimization problem containing the lateral shift in m for every point v_opt: velocity profile for the raceline """ # ------------------------------------------------------------------------------------------------------------------ # PREPARE TRACK INFORMATION ---------------------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ # spline lengths spline_lengths_refline = tph.calc_spline_lengths.calc_spline_lengths(coeffs_x=coeffs_x, coeffs_y=coeffs_y) # calculate heading and curvature (numerically) kappa_refline = tph.calc_head_curv_num. \ calc_head_curv_num(path=reftrack[:, :2], el_lengths=spline_lengths_refline, is_closed=True, stepsize_curv_preview=pars["curv_calc_opts"]["d_preview_curv"], stepsize_curv_review=pars["curv_calc_opts"]["d_review_curv"], stepsize_psi_preview=pars["curv_calc_opts"]["d_preview_head"], stepsize_psi_review=pars["curv_calc_opts"]["d_review_head"])[1] # close track kappa_refline_cl = np.append(kappa_refline, kappa_refline[0]) w_tr_left_cl = np.append(reftrack[:, 3], reftrack[0, 3]) w_tr_right_cl = np.append(reftrack[:, 2], reftrack[0, 2]) # step size along the reference line h = pars["stepsize_opts"]["stepsize_reg"] # optimization steps (0, 1, 2 ... end point/start point) steps = [i for i in range(kappa_refline_cl.size)] # number of control intervals N = steps[-1] # station along the reference line s_opt = np.linspace(0.0, N * h, N + 1) # interpolate curvature of reference line in terms of steps kappa_interp = ca.interpolant('kappa_interp', 'linear', [steps], kappa_refline_cl) # interpolate track width (left and right to reference line) in terms of steps w_tr_left_interp = ca.interpolant('w_tr_left_interp', 'linear', [steps], w_tr_left_cl) w_tr_right_interp = ca.interpolant('w_tr_right_interp', 'linear', [steps], w_tr_right_cl) # describe friction coefficients from friction map with linear equations or gaussian basis functions if pars["optim_opts"]["var_friction"] is not None: w_mue_fl, w_mue_fr, w_mue_rl, w_mue_rr, center_dist = opt_mintime_traj.src. \ approx_friction_map.approx_friction_map(reftrack=reftrack, normvectors=normvectors, tpamap_path=tpamap_path, tpadata_path=tpadata_path, pars=pars, dn=pars["optim_opts"]["dn"], n_gauss=pars["optim_opts"]["n_gauss"], print_debug=print_debug, plot_debug=plot_debug) # ------------------------------------------------------------------------------------------------------------------ # DIRECT GAUSS-LEGENDRE COLLOCATION -------------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ # degree of interpolating polynomial d = 3 # legendre collocation points tau = np.append(0, ca.collocation_points(d, 'legendre')) # coefficient matrix for formulating the collocation equation C = np.zeros((d + 1, d + 1)) # coefficient matrix for formulating the collocation equation D = np.zeros(d + 1) # coefficient matrix for formulating the collocation equation B = np.zeros(d + 1) # construct polynomial basis for j in range(d + 1): # construct Lagrange polynomials to get the polynomial basis at the collocation point p = np.poly1d([1]) for r in range(d + 1): if r != j: p *= np.poly1d([1, -tau[r]]) / (tau[j] - tau[r]) # evaluate polynomial at the final time to get the coefficients of the continuity equation D[j] = p(1.0) # evaluate time derivative of polynomial at collocation points to get the coefficients of continuity equation p_der = np.polyder(p) for r in range(d + 1): C[j, r] = p_der(tau[r]) # evaluate integral of the polynomial to get the coefficients of the quadrature function pint = np.polyint(p) B[j] = pint(1.0) # ------------------------------------------------------------------------------------------------------------------ # STATE VARIABLES -------------------------------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ # number of state variables nx = 5 # velocity [m/s] v_n = ca.SX.sym('v_n') v_s = 50 v = v_s * v_n # side slip angle [rad] beta_n = ca.SX.sym('beta_n') beta_s = 0.5 beta = beta_s * beta_n # yaw rate [rad/s] omega_z_n = ca.SX.sym('omega_z_n') omega_z_s = 1 omega_z = omega_z_s * omega_z_n # lateral distance to reference line (positive = left) [m] n_n = ca.SX.sym('n_n') n_s = 5.0 n = n_s * n_n # relative angle to tangent on reference line [rad] xi_n = ca.SX.sym('xi_n') xi_s = 1.0 xi = xi_s * xi_n # scaling factors for state variables x_s = np.array([v_s, beta_s, omega_z_s, n_s, xi_s]) # put all states together x = ca.vertcat(v_n, beta_n, omega_z_n, n_n, xi_n) # ------------------------------------------------------------------------------------------------------------------ # CONTROL VARIABLES ------------------------------------------------------------------------------------------------ # ------------------------------------------------------------------------------------------------------------------ # number of control variables nu = 4 # steer angle [rad] delta_n = ca.SX.sym('delta_n') delta_s = 0.5 delta = delta_s * delta_n # positive longitudinal force (drive) [N] f_drive_n = ca.SX.sym('f_drive_n') f_drive_s = 7500.0 f_drive = f_drive_s * f_drive_n # negative longitudinal force (brake) [N] f_brake_n = ca.SX.sym('f_brake_n') f_brake_s = 20000.0 f_brake = f_brake_s * f_brake_n # lateral wheel load transfer [N] gamma_y_n = ca.SX.sym('gamma_y_n') gamma_y_s = 5000.0 gamma_y = gamma_y_s * gamma_y_n # scaling factors for control variables u_s = np.array([delta_s, f_drive_s, f_brake_s, gamma_y_s]) # put all controls together u = ca.vertcat(delta_n, f_drive_n, f_brake_n, gamma_y_n) # ------------------------------------------------------------------------------------------------------------------ # MODEL EQUATIONS -------------------------------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ # extract vehicle and tire parameters veh = pars["vehicle_params_mintime"] tire = pars["tire_params_mintime"] # general constants g = pars["veh_params"]["g"] mass = pars["veh_params"]["mass"] # curvature of reference line [rad/m] kappa = ca.SX.sym('kappa') # drag force [N] f_xdrag = pars["veh_params"]["dragcoeff"] * v ** 2 # rolling resistance forces [N] f_xroll_fl = 0.5 * tire["c_roll"] * mass * g * veh["wheelbase_rear"] / veh["wheelbase"] f_xroll_fr = 0.5 * tire["c_roll"] * mass * g * veh["wheelbase_rear"] / veh["wheelbase"] f_xroll_rl = 0.5 * tire["c_roll"] * mass * g * veh["wheelbase_front"] / veh["wheelbase"] f_xroll_rr = 0.5 * tire["c_roll"] * mass * g * veh["wheelbase_front"] / veh["wheelbase"] f_xroll = tire["c_roll"] * mass * g # static normal tire forces [N] f_zstat_fl = 0.5 * mass * g * veh["wheelbase_rear"] / veh["wheelbase"] f_zstat_fr = 0.5 * mass * g * veh["wheelbase_rear"] / veh["wheelbase"] f_zstat_rl = 0.5 * mass * g * veh["wheelbase_front"] / veh["wheelbase"] f_zstat_rr = 0.5 * mass * g * veh["wheelbase_front"] / veh["wheelbase"] # dynamic normal tire forces (aerodynamic downforces) [N] f_zlift_fl = 0.5 * veh["liftcoeff_front"] * v ** 2 f_zlift_fr = 0.5 * veh["liftcoeff_front"] * v ** 2 f_zlift_rl = 0.5 * veh["liftcoeff_rear"] * v ** 2 f_zlift_rr = 0.5 * veh["liftcoeff_rear"] * v ** 2 # dynamic normal tire forces (load transfers) [N] f_zdyn_fl = (-0.5 * veh["cog_z"] / veh["wheelbase"] * (f_drive + f_brake - f_xdrag - f_xroll) - veh["k_roll"] * gamma_y) f_zdyn_fr = (-0.5 * veh["cog_z"] / veh["wheelbase"] * (f_drive + f_brake - f_xdrag - f_xroll) + veh["k_roll"] * gamma_y) f_zdyn_rl = (0.5 * veh["cog_z"] / veh["wheelbase"] * (f_drive + f_brake - f_xdrag - f_xroll) - (1.0 - veh["k_roll"]) * gamma_y) f_zdyn_rr = (0.5 * veh["cog_z"] / veh["wheelbase"] * (f_drive + f_brake - f_xdrag - f_xroll) + (1.0 - veh["k_roll"]) * gamma_y) # sum of all normal tire forces [N] f_z_fl = f_zstat_fl + f_zlift_fl + f_zdyn_fl f_z_fr = f_zstat_fr + f_zlift_fr + f_zdyn_fr f_z_rl = f_zstat_rl + f_zlift_rl + f_zdyn_rl f_z_rr = f_zstat_rr + f_zlift_rr + f_zdyn_rr # slip angles [rad] alpha_fl = delta - ca.atan((v * ca.sin(beta) + veh["wheelbase_front"] * omega_z) / (v * ca.cos(beta) - 0.5 * veh["track_width_front"] * omega_z)) alpha_fr = delta - ca.atan((v * ca.sin(beta) + veh["wheelbase_front"] * omega_z) / (v * ca.cos(beta) + 0.5 * veh["track_width_front"] * omega_z)) alpha_rl = ca.atan((-v * ca.sin(beta) + veh["wheelbase_rear"] * omega_z) / (v * ca.cos(beta) - 0.5 * veh["track_width_rear"] * omega_z)) alpha_rr = ca.atan((-v * ca.sin(beta) + veh["wheelbase_rear"] * omega_z) / (v * ca.cos(beta) + 0.5 * veh["track_width_rear"] * omega_z)) # lateral tire forces [N] f_y_fl = (pars["optim_opts"]["mue"] * f_z_fl * (1 + tire["eps_front"] * f_z_fl / tire["f_z0"]) * ca.sin(tire["C_front"] * ca.atan(tire["B_front"] * alpha_fl - tire["E_front"] * (tire["B_front"] * alpha_fl - ca.atan(tire["B_front"] * alpha_fl))))) f_y_fr = (pars["optim_opts"]["mue"] * f_z_fr * (1 + tire["eps_front"] * f_z_fr / tire["f_z0"]) * ca.sin(tire["C_front"] * ca.atan(tire["B_front"] * alpha_fr - tire["E_front"] * (tire["B_front"] * alpha_fr - ca.atan(tire["B_front"] * alpha_fr))))) f_y_rl = (pars["optim_opts"]["mue"] * f_z_rl * (1 + tire["eps_rear"] * f_z_rl / tire["f_z0"]) * ca.sin(tire["C_rear"] * ca.atan(tire["B_rear"] * alpha_rl - tire["E_rear"] * (tire["B_rear"] * alpha_rl - ca.atan(tire["B_rear"] * alpha_rl))))) f_y_rr = (pars["optim_opts"]["mue"] * f_z_rr * (1 + tire["eps_rear"] * f_z_rr / tire["f_z0"]) * ca.sin(tire["C_rear"] * ca.atan(tire["B_rear"] * alpha_rr - tire["E_rear"] * (tire["B_rear"] * alpha_rr - ca.atan(tire["B_rear"] * alpha_rr))))) # longitudinal tire forces [N] f_x_fl = 0.5 * f_drive * veh["k_drive_front"] + 0.5 * f_brake * veh["k_brake_front"] - f_xroll_fl f_x_fr = 0.5 * f_drive * veh["k_drive_front"] + 0.5 * f_brake * veh["k_brake_front"] - f_xroll_fr f_x_rl = 0.5 * f_drive * (1 - veh["k_drive_front"]) + 0.5 * f_brake * (1 - veh["k_brake_front"]) - f_xroll_rl f_x_rr = 0.5 * f_drive * (1 - veh["k_drive_front"]) + 0.5 * f_brake * (1 - veh["k_brake_front"]) - f_xroll_rr # longitudinal acceleration [m/s²] ax = (f_x_rl + f_x_rr + (f_x_fl + f_x_fr) * ca.cos(delta) - (f_y_fl + f_y_fr) * ca.sin(delta) - pars["veh_params"]["dragcoeff"] * v ** 2) / mass # lateral acceleration [m/s²] ay = ((f_x_fl + f_x_fr) * ca.sin(delta) + f_y_rl + f_y_rr + (f_y_fl + f_y_fr) * ca.cos(delta)) / mass # time-distance scaling factor (dt/ds) sf = (1.0 - n * kappa) / (v * (ca.cos(xi + beta))) # model equations for two track model (ordinary differential equations) dv = (sf / mass) * ((f_x_rl + f_x_rr) * ca.cos(beta) + (f_x_fl + f_x_fr) * ca.cos(delta - beta) + (f_y_rl + f_y_rr) * ca.sin(beta) - (f_y_fl + f_y_fr) * ca.sin(delta - beta) - f_xdrag * ca.cos(beta)) dbeta = sf * (-omega_z + (-(f_x_rl + f_x_rr) * ca.sin(beta) + (f_x_fl + f_x_fr) * ca.sin(delta - beta) + (f_y_rl + f_y_rr) * ca.cos(beta) + (f_y_fl + f_y_fr) * ca.cos(delta - beta) + f_xdrag * ca.sin(beta)) / (mass * v)) domega_z = (sf / veh["I_z"]) * ((f_x_rr - f_x_rl) * veh["track_width_rear"] / 2 - (f_y_rl + f_y_rr) * veh["wheelbase_rear"] + ((f_x_fr - f_x_fl) * ca.cos(delta) + (f_y_fl - f_y_fr) * ca.sin(delta)) * veh["track_width_front"] / 2 + ((f_y_fl + f_y_fr) * ca.cos(delta) + (f_x_fl + f_x_fr) * ca.sin(delta)) * veh["track_width_front"]) dn = sf * v * ca.sin(xi + beta) dxi = sf * omega_z - kappa # put all ODEs together dx = ca.vertcat(dv, dbeta, domega_z, dn, dxi) / x_s # ------------------------------------------------------------------------------------------------------------------ # CONTROL BOUNDARIES ----------------------------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ delta_min = -veh["delta_max"] / delta_s # min. steer angle [rad] delta_max = veh["delta_max"] / delta_s # max. steer angle [rad] f_drive_min = 0.0 # min. longitudinal drive force [N] f_drive_max = veh["f_drive_max"] / f_drive_s # max. longitudinal drive force [N] f_brake_min = -veh["f_brake_max"] / f_brake_s # min. longitudinal brake force [N] f_brake_max = 0.0 # max. longitudinal brake force [N] gamma_y_min = -np.inf # min. lateral wheel load transfer [N] gamma_y_max = np.inf # max. lateral wheel load transfer [N] # ------------------------------------------------------------------------------------------------------------------ # STATE BOUNDARIES ------------------------------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ v_min = 1.0 / v_s # min. velocity [m/s] v_max = pars["veh_params"]["v_max"] / v_s # max. velocity [m/s] beta_min = -0.5 * np.pi / beta_s # min. side slip angle [rad] beta_max = 0.5 * np.pi / beta_s # max. side slip angle [rad] omega_z_min = - 0.5 * np.pi / omega_z_s # min. yaw rate [rad/s] omega_z_max = 0.5 * np.pi / omega_z_s # max. yaw rate [rad/s] xi_min = - 0.5 * np.pi / xi_s # min. relative angle to tangent on reference line [rad] xi_max = 0.5 * np.pi / xi_s # max. relative angle to tangent on reference line [rad] # ------------------------------------------------------------------------------------------------------------------ # INITIAL GUESS FOR DECISION VARIABLES ----------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ v_guess = 20.0 / v_s # ------------------------------------------------------------------------------------------------------------------ # HELPER FUNCTIONS ------------------------------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ # continuous time dynamics f_dyn = ca.Function('f_dyn', [x, u, kappa], [dx, sf], ['x', 'u', 'kappa'], ['dx', 'sf']) # longitudinal tire forces [N] f_fx = ca.Function('f_fx', [x, u], [f_x_fl, f_x_fr, f_x_rl, f_x_rr], ['x', 'u'], ['f_x_fl', 'f_x_fr', 'f_x_rl', 'f_x_rr']) # lateral tire forces [N] f_fy = ca.Function('f_fy', [x, u], [f_y_fl, f_y_fr, f_y_rl, f_y_rr], ['x', 'u'], ['f_y_fl', 'f_y_fr', 'f_y_rl', 'f_y_rr']) # vertical tire forces [N] f_fz = ca.Function('f_fz', [x, u], [f_z_fl, f_z_fr, f_z_rl, f_z_rr], ['x', 'u'], ['f_z_fl', 'f_z_fr', 'f_z_rl', 'f_z_rr']) # longitudinal and lateral acceleration [m/s²] f_a = ca.Function('f_a', [x, u], [ax, ay], ['x', 'u'], ['ax', 'ay']) # ------------------------------------------------------------------------------------------------------------------ # FORMULATE NONLINEAR PROGRAM -------------------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ # initialize NLP vectors w = [] w0 = [] lbw = [] ubw = [] J = 0 g = [] lbg = [] ubg = [] # initialize ouput vectors x_opt = [] u_opt = [] dt_opt = [] tf_opt = [] ax_opt = [] ay_opt = [] ec_opt = [] # initialize control vectors (for regularization) delta_p = [] F_p = [] # boundary constraint: lift initial conditions Xk = ca.MX.sym('X0', nx) w.append(Xk) n_min = (-w_tr_right_interp(0) + pars["optim_opts"]["width_opt"] / 2) / n_s n_max = (w_tr_left_interp(0) - pars["optim_opts"]["width_opt"] / 2) / n_s lbw.append([v_min, beta_min, omega_z_min, n_min, xi_min]) ubw.append([v_max, beta_max, omega_z_max, n_max, xi_max]) w0.append([v_guess, 0.0, 0.0, 0.0, 0.0]) x_opt.append(Xk * x_s) # loop along the racetrack and formulate path constraints & system dynamic for k in range(N): # add decision variables for the control Uk = ca.MX.sym('U_' + str(k), nu) w.append(Uk) lbw.append([delta_min, f_drive_min, f_brake_min, gamma_y_min]) ubw.append([delta_max, f_drive_max, f_brake_max, gamma_y_max]) w0.append([0.0] * nu) # add decision variables for the state at collocation points Xc = [] for j in range(d): Xkj = ca.MX.sym('X_' + str(k) + '_' + str(j), nx) Xc.append(Xkj) w.append(Xkj) lbw.append([-np.inf] * nx) ubw.append([np.inf] * nx) w0.append([v_guess, 0.0, 0.0, 0.0, 0.0]) # loop over all collocation points Xk_end = D[0] * Xk sf_opt = [] for j in range(1, d + 1): # calculate the state derivative at the collocation point xp = C[0, j] * Xk for r in range(d): xp = xp + C[r + 1, j] * Xc[r] # interpolate kappa at the collocation point kappa_col = kappa_interp(k + tau[j]) # append collocation equations (system dynamic) fj, qj = f_dyn(Xc[j - 1], Uk, kappa_col) g.append(h * fj - xp) lbg.append([0.0] * nx) ubg.append([0.0] * nx) # add contribution to the end state Xk_end = Xk_end + D[j] * Xc[j - 1] # add contribution to quadrature function J = J + B[j] * qj * h # add contribution to scaling factor (for calculating lap time) sf_opt.append(B[j] * qj * h) # calculate used energy dt_opt.append(sf_opt[0] + sf_opt[1] + sf_opt[2]) ec_opt.append(Xk[0] * v_s * Uk[1] * f_drive_s * dt_opt[-1]) # add new decision variables for state at end of the collocation interval Xk = ca.MX.sym('X_' + str(k + 1), nx) w.append(Xk) n_min = (-w_tr_right_interp(k + 1) + pars["optim_opts"]["width_opt"] / 2.0) / n_s n_max = (w_tr_left_interp(k + 1) - pars["optim_opts"]["width_opt"] / 2.0) / n_s lbw.append([v_min, beta_min, omega_z_min, n_min, xi_min]) ubw.append([v_max, beta_max, omega_z_max, n_max, xi_max]) w0.append([v_guess, 0.0, 0.0, 0.0, 0.0]) # add equality constraint g.append(Xk_end - Xk) lbg.append([0.0] * nx) ubg.append([0.0] * nx) # get tire forces f_x_flk, f_x_frk, f_x_rlk, f_x_rrk = f_fx(Xk, Uk) f_y_flk, f_y_frk, f_y_rlk, f_y_rrk = f_fy(Xk, Uk) f_z_flk, f_z_frk, f_z_rlk, f_z_rrk = f_fz(Xk, Uk) # get accelerations (longitudinal + lateral) axk, ayk = f_a(Xk, Uk) # path constraint: limitied engine power g.append(Xk[0] * Uk[1]) lbg.append([-np.inf]) ubg.append([veh["power_max"] / (f_drive_s * v_s)]) # get constant friction coefficient if pars["optim_opts"]["var_friction"] is None: mue_fl = pars["optim_opts"]["mue"] mue_fr = pars["optim_opts"]["mue"] mue_rl = pars["optim_opts"]["mue"] mue_rr = pars["optim_opts"]["mue"] # calculate variable friction coefficients along the reference line (regression with linear equations) elif pars["optim_opts"]["var_friction"] == "linear": # friction coefficient for each tire mue_fl = w_mue_fl[k + 1, 0] * Xk[3] * n_s + w_mue_fl[k + 1, 1] mue_fr = w_mue_fr[k + 1, 0] * Xk[3] * n_s + w_mue_fr[k + 1, 1] mue_rl = w_mue_rl[k + 1, 0] * Xk[3] * n_s + w_mue_rl[k + 1, 1] mue_rr = w_mue_rr[k + 1, 0] * Xk[3] * n_s + w_mue_rr[k + 1, 1] # calculate variable friction coefficients along the reference line (regression with gaussian basis functions) elif pars["optim_opts"]["var_friction"] == "gauss": # gaussian basis functions sigma = 2.0 * center_dist[k + 1, 0] n_gauss = pars["optim_opts"]["n_gauss"] n_q = np.linspace(-n_gauss, n_gauss, 2 * n_gauss + 1) * center_dist[k + 1, 0] gauss_basis = [] for i in range(2 * n_gauss + 1): gauss_basis.append(ca.exp(-(Xk[3] * n_s - n_q[i]) ** 2 / (2 * (sigma ** 2)))) gauss_basis = ca.vertcat(*gauss_basis) mue_fl = ca.dot(w_mue_fl[k + 1, :-1], gauss_basis) + w_mue_fl[k + 1, -1] mue_fr = ca.dot(w_mue_fr[k + 1, :-1], gauss_basis) + w_mue_fr[k + 1, -1] mue_rl = ca.dot(w_mue_rl[k + 1, :-1], gauss_basis) + w_mue_rl[k + 1, -1] mue_rr = ca.dot(w_mue_rr[k + 1, :-1], gauss_basis) + w_mue_rr[k + 1, -1] else: raise ValueError("No friction coefficients are available!") # path constraint: Kamm's Circle for each wheel g.append(((f_x_flk / (mue_fl * f_z_flk)) ** 2 + (f_y_flk / (mue_fl * f_z_flk)) ** 2)) g.append(((f_x_frk / (mue_fr * f_z_frk)) ** 2 + (f_y_frk / (mue_fr * f_z_frk)) ** 2)) g.append(((f_x_rlk / (mue_rl * f_z_rlk)) ** 2 + (f_y_rlk / (mue_rl * f_z_rlk)) ** 2)) g.append(((f_x_rrk / (mue_rr * f_z_rrk)) ** 2 + (f_y_rrk / (mue_rr * f_z_rrk)) ** 2)) lbg.append([0.0] * 4) ubg.append([1.0] * 4) # path constraint: lateral wheel load transfer g.append(((f_y_flk + f_y_frk) * ca.cos(Uk[0] * delta_s) + f_y_rlk + f_y_rrk + (f_x_flk + f_x_frk) * ca.sin(Uk[0] * delta_s)) * veh["cog_z"] / ((veh["track_width_front"] + veh["track_width_rear"]) / 2) - Uk[3] * gamma_y_s) lbg.append([0.0]) ubg.append([0.0]) # path constraint: f_drive * f_brake == 0 (no simultaneous operation of brake and accelerator pedal) g.append(Uk[1] * Uk[2]) lbg.append([-20000.0 / (f_drive_s * f_brake_s)]) ubg.append([0.0]) # path constraint: actor dynamic if k > 0: sigma = (1 - kappa_interp(k) * Xk[3] * n_s) / (Xk[0] * v_s) g.append((Uk - w[1 + (k - 1) * nx]) / (pars["stepsize_opts"]["stepsize_reg"] * sigma)) lbg.append([delta_min / (veh["t_delta"]), -np.inf, f_brake_min / (veh["t_brake"]), -np.inf]) ubg.append([delta_max / (veh["t_delta"]), f_drive_max / (veh["t_drive"]), np.inf, np.inf]) # path constraint: safe trajectories with acceleration ellipse if pars["optim_opts"]["safe_traj"]: g.append((ca.fmax(axk, 0) / pars["optim_opts"]["ax_pos_safe"]) ** 2 + (ayk / pars["optim_opts"]["ay_safe"]) ** 2) g.append((ca.fmin(axk, 0) / pars["optim_opts"]["ax_neg_safe"]) ** 2 + (ayk / pars["optim_opts"]["ay_safe"]) ** 2) lbg.append([0.0] * 2) ubg.append([1.0] * 2) # append controls (for regularization) delta_p.append(Uk[0] * delta_s) F_p.append(Uk[1] * f_drive_s / 10000.0 + Uk[2] * f_brake_s / 10000.0) # append outputs x_opt.append(Xk * x_s) u_opt.append(Uk * u_s) tf_opt.extend([f_x_flk, f_y_flk, f_z_flk, f_x_frk, f_y_frk, f_z_frk]) tf_opt.extend([f_x_rlk, f_y_rlk, f_z_rlk, f_x_rrk, f_y_rrk, f_z_rrk]) ax_opt.append(axk) ay_opt.append(ayk) # boundary constraint: start states = final states g.append(w[0] - Xk) lbg.append([0.0] * nx) ubg.append([0.0] * nx) # path constraint: limited energy consumption if pars["optim_opts"]["limit_energy"]: g.append(ca.sum1(ca.vertcat(*ec_opt)) / 3600000.0) lbg.append([0]) ubg.append([pars["optim_opts"]["energy_limit"]]) # formulate differentiation matrix (for regularization) diff_matrix = np.eye(N) for i in range(N - 1): diff_matrix[i, i + 1] = -1.0 diff_matrix[N - 1, 0] = -1.0 # regularization (delta) delta_p = ca.vertcat(*delta_p) Jp_delta = ca.mtimes(ca.MX(diff_matrix), delta_p) Jp_delta = ca.dot(Jp_delta, Jp_delta) # regularization (f_drive + f_brake) F_p = ca.vertcat(*F_p) Jp_f = ca.mtimes(ca.MX(diff_matrix), F_p) Jp_f = ca.dot(Jp_f, Jp_f) # formulate objective J = J + pars["optim_opts"]["penalty_F"] * Jp_f + pars["optim_opts"]["penalty_delta"] * Jp_delta # concatenate NLP vectors w = ca.vertcat(*w) g = ca.vertcat(*g) w0 = np.concatenate(w0) lbw = np.concatenate(lbw) ubw = np.concatenate(ubw) lbg = np.concatenate(lbg) ubg = np.concatenate(ubg) # concatenate output vectors x_opt = ca.vertcat(*x_opt) u_opt = ca.vertcat(*u_opt) tf_opt = ca.vertcat(*tf_opt) dt_opt = ca.vertcat(*dt_opt) ax_opt = ca.vertcat(*ax_opt) ay_opt = ca.vertcat(*ay_opt) ec_opt = ca.vertcat(*ec_opt) # ------------------------------------------------------------------------------------------------------------------ # CREATE NLP SOLVER ------------------------------------------------------------------------------------------------ # ------------------------------------------------------------------------------------------------------------------ # fill nlp structure nlp = {'f': J, 'x': w, 'g': g} # solver options opts = {"expand": True, "verbose": print_debug, "ipopt.max_iter": 2000, "ipopt.tol": 1e-7} # solver options for warm start if pars["optim_opts"]["warm_start"]: opts_warm_start = {"ipopt.warm_start_init_point": "yes", "ipopt.warm_start_bound_push": 1e-9, "ipopt.warm_start_mult_bound_push": 1e-9, "ipopt.warm_start_slack_bound_push": 1e-9, "ipopt.mu_init": 1e-9} opts.update(opts_warm_start) # load warm start files if pars["optim_opts"]["warm_start"]: try: w0 = np.loadtxt(os.path.join(export_path, 'w0.csv')) lam_x0 = np.loadtxt(os.path.join(export_path, 'lam_x0.csv')) lam_g0 = np.loadtxt(os.path.join(export_path, 'lam_g0.csv')) except IOError: print('\033[91m' + 'WARNING: Failed to load warm start files!' + '\033[0m') sys.exit(1) # check warm start files if pars["optim_opts"]["warm_start"] and not len(w0) == len(lbw): print('\033[91m' + 'WARNING: Warm start files do not fit to the dimension of the NLP!' + '\033[0m') sys.exit(1) # create solver instance solver = ca.nlpsol("solver", "ipopt", nlp, opts) # ------------------------------------------------------------------------------------------------------------------ # SOLVE NLP -------------------------------------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ # start time measure t0 = time.perf_counter() # solve NLP if pars["optim_opts"]["warm_start"]: sol = solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg, lam_x0=lam_x0, lam_g0=lam_g0) else: sol = solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg) # end time measure tend = time.perf_counter() # ------------------------------------------------------------------------------------------------------------------ # EXTRACT SOLUTION ------------------------------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ # helper function to extract solution for state variables, control variables, tire forces, time f_sol = ca.Function('f_sol', [w], [x_opt, u_opt, tf_opt, dt_opt, ax_opt, ay_opt, ec_opt], ['w'], ['x_opt', 'u_opt', 'tf_opt', 'dt_opt', 'ax_opt', 'ay_opt', 'ec_opt']) # extract solution x_opt, u_opt, tf_opt, dt_opt, ax_opt, ay_opt, ec_opt = f_sol(sol['x']) # solution for state variables x_opt = np.reshape(x_opt, (N + 1, nx)) # solution for control variables u_opt = np.reshape(u_opt, (N, nu)) # solution for tire forces tf_opt = np.append(tf_opt[-12:], tf_opt[:]) tf_opt = np.reshape(tf_opt, (N + 1, 12)) # solution for time t_opt = np.hstack((0.0, np.cumsum(dt_opt))) # solution for acceleration ax_opt = np.append(ax_opt[-1], ax_opt) ay_opt = np.append(ay_opt[-1], ay_opt) atot_opt = np.sqrt(np.power(ax_opt, 2) + np.power(ay_opt, 2)) # solution for energy consumption ec_opt_cum = np.hstack((0.0, np.cumsum(ec_opt))) / 3600.0 # ------------------------------------------------------------------------------------------------------------------ # EXPORT SOLUTION -------------------------------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ # export data to CSVs opt_mintime_traj.src.export_mintime_solution.export_mintime_solution(file_path=export_path, s=s_opt, t=t_opt, x=x_opt, u=u_opt, tf=tf_opt, ax=ax_opt, ay=ay_opt, atot=atot_opt, w0=sol["x"], lam_x0=sol["lam_x"], lam_g0=sol["lam_g"]) # ------------------------------------------------------------------------------------------------------------------ # PLOT & PRINT RESULTS --------------------------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ if plot_debug: opt_mintime_traj.src.result_plots_mintime.result_plots_mintime(pars=pars, reftrack=reftrack, s=s_opt, t=t_opt, x=x_opt, u=u_opt, ax=ax_opt, ay=ay_opt, atot=atot_opt, tf=tf_opt, ec=ec_opt_cum) if print_debug: print("INFO: Laptime: %.3fs" % t_opt[-1]) print("INFO: NLP solving time: %.3fs" % (tend - t0)) print("INFO: Maximum abs(ay): %.2fm/s2" % np.amax(ay_opt)) print("INFO: Maximum ax: %.2fm/s2" % np.amax(ax_opt)) print("INFO: Minimum ax: %.2fm/s2" % np.amin(ax_opt)) print("INFO: Maximum total acc: %.2fm/s2" % np.amax(atot_opt)) print('INFO: Energy consumption: %.3fWh' % ec_opt_cum[-1]) return -x_opt[:-1, 3], x_opt[:-1, 0]
def __init__(self, model, options=dict()): # declare object fields self.model = model self.input_vals = None self.options = {} self.model_info = {} self.stats = {} self.problem_functions = None self.status = "new" self.clib_id = -1 # default options self.options['printLevel'] = 0 self.options['hbbSettings'] = {'numProcesses': 1} self.options['integration_opts'] = None self.options['max_iteration_time'] = cs.inf self.options['relaxed_tail_length'] = 0 self.options['prediction_horizon_length'] = 1 self.options['variable_parameters_exp'] = {} # load user options if not options is None: for k in options.keys(): if k in self.options: self.options[k] = options[k] else: warn('Option not recognized: ' + k) # check the model syms_ = [v.sym for v in model.x] + [v.sym for v in model.y] + [ v.sym for v in model.a ] + [v.sym for v in model.u + model.v] if len(model.p) > 0: raise NameError( 'MPCcontroller is not suited to optimize parameters, please fix them or give them a state dependent expression using the \'varing_parameters_exp\' option' ) if len(model.icns) > 0: raise NameError('MPCcontroller: initial constraints not supported') obj_ = 0.0 if not model.lag is None: obj_ += model.lag if not model.ipn is None: obj_ += model.ipn if not model.may is None: obj_ += model.may if oc.get_order(obj_, syms_) > 2: raise NameError( "Objectives with order higher than 2 are not currently supported. Please, perform an epigraph reformulation of the objective." ) # prepare collection of performace statistics self.stats['times'] = { 'total': 0., 'preprocessing': 0., 'iterations': { 'total': 0., 'pre/post-processing': 0., 'minlp_solver': { 'total': 0., 'nlp': 0., 'mip': 0., 'linearizations_generation': 0. } } } self.stats['num_solves'] = {'nlp': 0, 'qp': 0} ############################################################################################## ####################################### Preprocessing ####################################### ############################################################################################## # start timing start_time = time() # collect info PHL = self.options['prediction_horizon_length'] RTL = self.options['relaxed_tail_length'] self.model_info['num_states'] = num_states = len(model.x) + len( model.y) self.model_info['num_algebraic'] = num_algebraic = len(model.a) self.model_info['num_controls'] = num_controls = len(model.u + model.v) self.model_info['num_path_constraints'] = len(model.pcns) num_vars_per_step = num_states + num_algebraic + num_controls ############## construct the parametric mpc model ############### mpc_time_vector = cs.MX.sym('t', PHL + RTL + 1, 1) mpc_model = deepcopy(model) # impose the initial state with a set of initial constraints mpc_model.icns.extend([ oc.eq(v.sym, 0, '<initial_state>') for v in mpc_model.x + mpc_model.y ]) # we do not support non-linear terminal costs in the shift yet (use a slack to represent the terminal cost as a terminal constraint) if not model.may is None and oc.get_order(model.may, syms_) > 1: mpc_model.a.append( oc.variable('terminal_cost_slack', -cs.DM.inf(), cs.DM.inf(), 0)) mpc_model.pcns = [ oc.eq(mpc_model.a[-1].sym, 0.0, '<terminal_cost_reformulation>') ] + mpc_model.pcns mpc_model.fcns.append( oc.leq(mpc_model.may, mpc_model.a[-1].sym, '<terminal_cost_reformulation>')) mpc_model.may = mpc_model.a[-1].sym self.model_info['num_algebraic'] += 1 # assign variables to the inputs for later definition for k in range(len(model.i)): mpc_model.i[k].val = cs.MX.sym(model.i[k].nme, PHL + RTL + 1, 1) param_symbols = [mpc_time_vector] + [i.val for i in mpc_model.i] # construct a parametric optimization problem describing the mpc short time horizon self.mpc_problem = oc.Problem() oc.multiple_shooting( mpc_model, self.mpc_problem, mpc_time_vector, {'integration_opts': self.options['integration_opts']}, True) # relax the tail (if required) #TODO check what happens to the sos1 groups for k in range(PHL * num_vars_per_step + num_states + num_algebraic, len(self.mpc_problem.var['dsc'])): self.mpc_problem.var['dsc'][k] = False # store the parametric version of the constraint set and variable bounds self.problem_functions = { 'cns': oc.CvxFuncForJulia('cns', [self.mpc_problem.var['sym']] + param_symbols, self.mpc_problem.cns['exp']), 'obj': oc.LinQuadForJulia('obj', [self.mpc_problem.var['sym']] + param_symbols, oc.sum1(self.mpc_problem.obj['exp'])) } # compile the parametric function created self.problem_functions['cns'].compile(oc.home_dir + '/temp_jit_files') # store the parametric version of the terminal part of the constraint set and the objective (for shift) num_constraints_per_step = num_states + len(mpc_model.pcns) num_steps = PHL + RTL self.problem_terminals = { 'cns': oc.CvxFuncForJulia( 'cns', [self.mpc_problem.var['sym']] + param_symbols, self.mpc_problem.cns['exp'][num_constraints_per_step * (num_steps - 1):]), 'obj': oc.LinQuadForJulia( 'obj', [self.mpc_problem.var['sym']] + param_symbols, cs.sum1(self.mpc_problem.obj['exp'][num_steps - 1:])) } # load the OpenBB interface self.openbb = oc.openbb.OpenBBinterface() # load the python interface of the mpc addon self.mpc_addon = oc.mpc_addon.MPC_addon(self.openbb) # load some additional helper functions in OpenBB self.openbb.eval_string('include(\"' + oc.home_dir + '/src/openbb_interface/helper_functions.jl\")') # check for multiprocessing if 'numProcesses' in self.options['hbbSettings'] and self.options[ 'hbbSettings']['numProcesses'] > 1: # prepare for multi-processing current_num_procs = self.openbb.eval_string('nprocs()') if current_num_procs < self.options['hbbSettings']['numProcesses']: self.openbb.eval_string( 'addprocs(' + str(self.options['hbbSettings']['numProcesses'] - current_num_procs) + ')') self.openbb.eval_string( '@sync for k in workers() @async remotecall_fetch(Main.eval,k,:(using OpenBB)) end' ) # collect timing statistics self.stats['times']['preprocessing'] = time() - start_time self.stats['times']['total'] += self.stats['times']['preprocessing']
def solve(self, solver="ipopt", show_online_optim=False, options_ipopt={}): """ Gives to CasADi states, controls, constraints, sum of all objective functions and theirs bounds. Gives others parameters to control how solver works. """ all_J = MX() for j_nodes in self.J: for j in j_nodes: all_J = vertcat(all_J, j) for nlp in self.nlp: for obj_nodes in nlp["J"]: for obj in obj_nodes: all_J = vertcat(all_J, obj) all_g = MX() all_g_bounds = Bounds(interpolation_type=InterpolationType.CONSTANT) for i in range(len(self.g)): for j in range(len(self.g[i])): all_g = vertcat(all_g, self.g[i][j]) all_g_bounds.concatenate(self.g_bounds[i][j]) for nlp in self.nlp: for i in range(len(nlp["g"])): for j in range(len(nlp["g"][i])): all_g = vertcat(all_g, nlp["g"][i][j]) all_g_bounds.concatenate(nlp["g_bounds"][i][j]) nlp = {"x": self.V, "f": sum1(all_J), "g": all_g} options_common = {} if show_online_optim: options_common["iteration_callback"] = OnlineCallback(self) if solver == "ipopt": options = { "ipopt.tol": 1e-6, "ipopt.max_iter": 1000, "ipopt.hessian_approximation": "exact", # "exact", "limited-memory" "ipopt.limited_memory_max_history": 50, "ipopt.linear_solver": "mumps", # "ma57", "ma86", "mumps" } for key in options_ipopt: ipopt_key = key if key[:6] != "ipopt.": ipopt_key = "ipopt." + key options[ipopt_key] = options_ipopt[key] opts = {**options, **options_common} else: raise RuntimeError("Available solvers are: 'ipopt'") solver = casadi.nlpsol("nlpsol", solver, nlp, opts) # Bounds and initial guess arg = { "lbx": self.V_bounds.min, "ubx": self.V_bounds.max, "lbg": all_g_bounds.min, "ubg": all_g_bounds.max, "x0": self.V_init.init, } # Solve the problem return solver.call(arg)
def run(): alignment = SplineLeicaAlignment() alignment.deserialize() ground_truth = min_bags.readGroundTruthFromImuGtoBag() t_L_Ps, p_L_Ps = leica.parseTextFile( os.path.join(flags.rawDataPath(), 'leica.txt')) while True: aligned_gt = GroundTruthInLeicaFrame(ground_truth, alignment) t_L_PGTs = aligned_gt.times() t_G0_PGTs = [(i - t_L_PGTs[0]).to_sec() for i in t_L_PGTs] t_G0_Ps = [(i - t_L_PGTs[0]).to_sec() for i in t_L_Ps] # Calculate errors T_L_I_of_t_G0 = aligned_gt.getLinearInterpolation() num_errors = numericalErrors(T_L_I_of_t_G0, t_G0_Ps, p_L_Ps, aligned_gt.alignment.p_I_P) print('Mean square error is %f' % np.mean(np.square(num_errors))) # Empirically, optimization only made stuff worse after this (?) # break opti = casadi.Opti() t_corr_G0 = opti.variable() T_corr_L_twist = opti.variable(6) p_I_P = opti.variable(3) opti.set_initial(p_I_P, aligned_gt.alignment.p_I_P) errvec = symbolicalErrors(T_L_I_of_t_G0, t_G0_Ps, p_L_Ps, p_I_P, t_corr_G0, T_corr_L_twist) opti.minimize(casadi.sum1(errvec)) opti.solver('ipopt') sol = opti.solve() upd_coeffs = np.hstack( (sol.value(t_corr_G0), sol.value(T_corr_L_twist), sol.value(p_I_P) - alignment.p_I_P.ravel())) if np.max(np.abs(upd_coeffs)) < 1e-5: break alignment.t_G_L = alignment.t_G_L + sol.value(t_corr_G0) alignment.T_L_G = pose.fromTwist( sol.value(T_corr_L_twist)) * alignment.T_L_G alignment.p_I_P = sol.value(p_I_P).reshape((3, 1)) alignment.serialize('gt_leica_alignment') out_files = flags.SplineLeicaAlignmentFiles('gt_leica_alignment') np.savetxt(out_files.errors, np.array(num_errors)) if FLAGS.gui: plt.figure(0) plots.xyPlot(aligned_gt.imuPositions(), aligned_gt.prismPositions(), p_L_Ps) plt.title('x,y trajectory of %s' % flags.sequenceSensorString()) plt.figure(1) plots.xyztPlot(t_G0_PGTs, aligned_gt.imuPositions(), aligned_gt.prismPositions(), t_G0_Ps, p_L_Ps) plt.title('x,y,z trajectory of %s' % flags.sequenceSensorString()) plt.figure(2) plt.semilogx(sorted(num_errors), np.arange(len(num_errors), dtype=float) / len(num_errors)) plt.grid() plt.xlabel('Leica error [m]') plt.ylabel('Fraction of measurements with smaller error') plt.title('Cumulative distribution of Leica error for %s' % flags.sequenceSensorString()) plt.show()
def BaseOptiX(): pathpoints = 30 ref_path = {} ref_path['x'] = 5 * np.sin(np.linspace(0, 2 * np.pi, pathpoints + 1)) ref_path['y'] = np.linspace(1, 2, pathpoints + 1)**2 * 10 wp = ca.horzcat(ref_path['x'], ref_path['y']).T N = 5 N1 = N - 1 wpts = np.array(wp[:, 0:N]) x = ca.MX.sym('x', N) x_dot = ca.MX.sym('xd', N - 1) dt = ca.MX.sym('dt', N - 1) nlp = {\ 'x': ca.vertcat(x, x_dot, dt), 'f': ca.sumsqr(x - wpts[0, :]) *100 + ca.sum1(dt), 'g': ca.vertcat(\ x[1:] - (x[:-1] + x_dot * dt), x[0] - wpts[0, 0], ), } S = ca.nlpsol('S', 'ipopt', nlp) # x0 x00 = wpts[0, :] T = 5 dt00 = [T / N1] * N1 xd00 = (x00[1:] - x00[:-1]) / dt00 x0 = ca.vertcat(x00, xd00, dt00) max_speed = 1 lbx = [0] * N + [-max_speed] * N1 + [0] * N1 ubx = [ca.inf] * N + [max_speed] * N1 + [10] * N1 lbg = [0] * N ubg = [0] * N # solve r = S(x0=x0, lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg) x_opt = r['x'] x = np.array(x_opt[:N]) xds = np.array(x_opt[N:N + N1]) times = np.array(x_opt[N + N1:]) total_time = np.sum(times) print(f"Times: {times}") print(f"Total Time: {total_time}") print(f"X dots: {xds.T}") print(f"xs: {x.T}") print(f"----------------") print(f"{xds * times}") plt.figure(1) plt.plot(wpts[0, :], np.ones_like(wpts[0, :]), 'o', markersize=12) plt.plot(x, np.ones_like(x), '+', markersize=20) plt.show()
def setup(self, bending_BC_type="cantilevered"): """ Sets up the problem. Run this last. :return: None (in-place) """ ### Discretize and assign loads # Discretize point_load_locations = [load["location"] for load in self.point_loads] point_load_locations.insert(0, 0) point_load_locations.append(self.length) self.x = cas.vertcat(*[ cas.linspace(point_load_locations[i], point_load_locations[i + 1], self.points_per_point_load) for i in range(len(point_load_locations) - 1) ]) # Post-process the discretization self.n = self.x.shape[0] dx = cas.diff(self.x) # Add point forces self.point_forces = cas.GenMX_zeros(self.n - 1) for i in range(len(self.point_loads)): load = self.point_loads[i] self.point_forces[self.points_per_point_load * (i + 1) - 1] = load["force"] # Add distributed loads self.force_per_unit_length = cas.GenMX_zeros(self.n) self.moment_per_unit_length = cas.GenMX_zeros(self.n) for load in self.distributed_loads: if load["type"] == "uniform": self.force_per_unit_length += load["force"] / self.length elif load["type"] == "elliptical": load_to_add = load["force"] / self.length * ( 4 / cas.pi * cas.sqrt(1 - (self.x / self.length)**2)) self.force_per_unit_length += load_to_add else: raise ValueError( "Bad value of \"type\" for a load within beam.distributed_loads!" ) # Initialize optimization variables log_nominal_diameter = self.opti.variable(self.n) self.opti.set_initial(log_nominal_diameter, cas.log(self.diameter_guess)) self.nominal_diameter = cas.exp(log_nominal_diameter) self.opti.subject_to([log_nominal_diameter > cas.log(self.thickness)]) def trapz(x): out = (x[:-1] + x[1:]) / 2 # out[0] += x[0] / 2 # out[-1] += x[-1] / 2 return out # Mass self.volume = cas.sum1( cas.pi / 4 * trapz((self.nominal_diameter + self.thickness)**2 - (self.nominal_diameter - self.thickness)**2) * dx) self.mass = self.volume * self.density # Mass proxy self.volume_proxy = cas.sum1(cas.pi * trapz(self.nominal_diameter) * dx * self.thickness) self.mass_proxy = self.volume_proxy * self.density # Find moments of inertia self.I = cas.pi / 64 * ( # bending (self.nominal_diameter + self.thickness)**4 - (self.nominal_diameter - self.thickness)**4) self.J = cas.pi / 32 * ( # torsion (self.nominal_diameter + self.thickness)**4 - (self.nominal_diameter - self.thickness)**4) if self.bending: # Set up derivatives self.u = 1 * self.opti.variable(self.n) self.du = 0.1 * self.opti.variable(self.n) self.ddu = 0.01 * self.opti.variable(self.n) self.dEIddu = 1 * self.opti.variable(self.n) self.opti.set_initial(self.u, 0) self.opti.set_initial(self.du, 0) self.opti.set_initial(self.ddu, 0) self.opti.set_initial(self.dEIddu, 0) # Define derivatives self.opti.subject_to([ cas.diff(self.u) == trapz(self.du) * dx, cas.diff(self.du) == trapz(self.ddu) * dx, cas.diff(self.E * self.I * self.ddu) == trapz(self.dEIddu) * dx, cas.diff( self.dEIddu) == trapz(self.force_per_unit_length) * dx + self.point_forces, ]) # Add BCs if bending_BC_type == "cantilevered": self.opti.subject_to([ self.u[0] == 0, self.du[0] == 0, self.ddu[-1] == 0, # No tip moment self.dEIddu[-1] == 0, # No tip higher order stuff ]) else: raise ValueError("Bad value of bending_BC_type!") # Stress self.stress_axial = (self.nominal_diameter + self.thickness) / 2 * self.E * self.ddu if self.torsion: # Set up derivatives phi = 0.1 * self.opti.variable(self.n) dphi = 0.01 * self.opti.variable(self.n) # Add forcing term ddphi = -self.moment_per_unit_length / (self.G * self.J) self.stress = self.stress_axial self.opti.subject_to([ self.stress / self.max_allowable_stress < 1, self.stress / self.max_allowable_stress > -1, ])
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:] n_operands = len(tree.operands) if op == 'der': orig = self.get_mx(tree.operands[0]) if orig in self.derivative: src = self.derivative[orig] else: s = ca.MX.sym("der({})".format(orig.name()), orig.sparsity()) self.derivative[orig] = s self.nodes[s] = s src = s elif op == '-' and n_operands == 1: src = -self.get_mx(tree.operands[0]) elif op == 'mtimes': 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 == 'delay' and n_operands == 2: expr = self.get_mx(tree.operands[0]) delay_time = self.get_mx(tree.operands[1]) assert isinstance(expr, MX) src = ca.MX.sym('{}_delayed_{}'.format(expr.name, delay_time), expr.size1(), expr.size2()) elif op in OP_MAP and n_operands == 2: lhs = self.get_mx(tree.operands[0]) rhs = 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 = self.get_mx(tree.operands[0]) lhs_op = getattr(lhs, OP_MAP[op]) src = lhs_op() elif n_operands == 1: src = self.get_mx(tree.operands[0]) src = getattr(src, tree.operator.name)() elif n_operands == 2: lhs = self.get_mx(tree.operands[0]) rhs = self.get_mx(tree.operands[1]) lhs_op = getattr(lhs, tree.operator.name) src = lhs_op(rhs) else: raise Exception("Unknown operator {}({})".format( op, ','.join(n_operands * ['.']))) self.src[tree] = src
def update_controls(self, target_belief, x_obs, x): ts = time.time() self.x = x nx, ny = target_belief.shape xmin, xmax, ymin, ymax = self.bounds L1 = xmax - xmin L2 = ymax - ymin if self.mode == 'mi': mu = convolve2d(target_belief, self.footprint_mask, mode='same') pos = self.tp_rate * mu + self.fp_rate * (1 - mu) mi = (-self.tp_rate * mu * np.log(pos / self.tp_rate) - (1 - self.tp_rate) * mu * np.log( (1 - pos) / (1 - self.tp_rate)) - self.fp_rate * (1 - mu) * np.log(pos / self.fp_rate) - (1 - self.fp_rate) * (1 - mu) * np.log((1 - pos) / (1 - self.fp_rate))) info = mi / np.sum(mi) elif self.mode == 'p': mu = convolve2d(target_belief, self.footprint_mask, mode='same') info = mu / np.sum(mu) elif self.mode == 'b': info = target_belief / np.sum(target_belief) elif self.mode == 'alpha': mu = convolve2d(target_belief, self.footprint_mask, mode='same') pos = self.tp_rate * mu + self.fp_rate * (1 - mu) alpha = .5 ami = (1 / (1 - alpha)) * (pos * np.log(mu * (self.tp_rate / pos)**alpha + (1 - mu) * (self.fp_rate / pos)**alpha) + (1 - pos) * np.log(mu * ((1 - self.tp_rate) / (1 - pos))**alpha + (1 - mu) * ((1 - self.fp_rate) / (1 - pos))**alpha)) info = ami / np.sum(ami) muk = dctn(info * np.sqrt(nx) * np.sqrt(ny), type=2, norm='ortho') muk = muk[0:self.nfourier, 0:self.nfourier].flatten() x1_comp = np.cos(np.outer((x_obs[:, 0] - xmin) / L1, self.k1)) x2_comp = np.cos(np.outer((x_obs[:, 1] - ymin) / L2, self.k2)) ck_prev = (1 / self.hk) * np.sum(x1_comp * x2_comp, axis=0) if len(self.ck_history_cum) == 0: self.ck_history_cum.append(ck_prev) ck_init = np.zeros(self.k1.shape[0]) t_total = self.t_horizon else: prev_cum = self.ck_history_cum[-1] self.ck_history_cum.append(prev_cum + ck_prev) if len(self.ck_history_cum) > self.t_history: ck_init = self.ck_history_cum[-1] - self.ck_history_cum[ -self.t_history - 1] t_total = self.t_history + self.t_horizon else: ck_init = self.ck_history_cum[-1] t_total = len(self.ck_history_cum) + self.t_horizon opti = casadi.Opti() v_u = [opti.variable(2, self.t_horizon) for i in range(self.nagents)] v_x = [casadi.cumsum(v_u[i], 2) + x[i, :] for i in range(self.nagents)] v_ck = (ck_init + (1 / self.hk) * sum([ casadi.sum2( casadi.cos(self.k1 @ (v_x[i][0, :] - xmin) / L1) * casadi.cos(self.k2 @ (v_x[i][1, :] - ymin) / L2)) for i in range(self.nagents) ])) / (self.nagents * t_total) erg_metric = casadi.sum1(self.Lambdak * (v_ck - muk)**2) change_penalties = [] for i in range(self.nagents): opti.subject_to(casadi.sum2(v_u[i]**2) < self.u_max**2) opti.subject_to(v_x[i][0, :] > xmin) opti.subject_to(v_x[i][0, :] < xmax) opti.subject_to(v_x[i][1, :] > ymin) opti.subject_to(v_x[i][1, :] < ymax) for j in range(i + 1, self.nagents): opti.subject_to( casadi.sum1((v_x[i] - v_x[j])**2) > self.min_dist**2) u_init = np.zeros((2, self.t_horizon)) u_init[:, 0:-1] = self.u[i][:, 1:] opti.set_initial(v_u[i], u_init) change_penalties.append( self.change_paramt0 * casadi.sum1(casadi.sum2((v_u[i][:, 0] - self.u[i][:, 0])**2))) change_penalties.append( self.change_param * casadi.sum1(casadi.sum2((v_u[i][:, 1:] - v_u[i][:, 0:-1])**2))) change_cost = sum(change_penalties) opti.minimize(erg_metric + change_cost) p_opts = {} # s_opts = {'max_cpu_time': .09, 'print_level': 0} s_opts = {'print_level': 0} opti.solver('ipopt', p_opts, s_opts) sol = opti.solve() self.u = [sol.value(v_u[i]) for i in range(self.nagents)] xs = [sol.value(v_x[i]) for i in range(self.nagents)] # try: # sol = opti.solve() # self.u = [sol.value(v_u[i]) for i in range(self.nagents)] # xs = [sol.value(v_x[i]) for i in range(self.nagents)] # except: # self.u = [opti.debug.value(v_u[i]) for i in range(self.nagents)] # xs = [opti.debug.value(v_x[i]) for i in range(self.nagents)] action = np.zeros((self.nagents, 2)) for i in range(self.nagents): action[i, :] = xs[i][:, 0] t_comp = time.time() - ts self.x = x if self.gui: xs_plot = [ np.insert(xs[i], 0, self.x[i, :], axis=1) for i in range(self.nagents) ] # display information map, planned trajectories, and if self.image1 is None: fig, (ax1, ax2) = plt.subplots(2, 1, num='erg', figsize=(2, 4)) self.image1 = ax1.imshow(info.T / np.max(info), extent=self.bounds, origin='lower', cmap='gray') self.image2 = ax2.imshow(target_belief.T / np.max(target_belief), extent=self.bounds, origin='lower', cmap='gray') ax1.set_title('Information Map') ax2.set_title('Target Belief') ax1.axis('off') ax2.axis('off') self.lines = [] self.inits = [] for i in range(self.nagents): line, = ax1.plot(xs[i][0, :], xs[i][1, :], 'r.-') #, linestyle=':') self.lines.append(line) init, = ax1.plot(self.x[i, 0], self.x[i, 1], 'g.') self.inits.append(init) else: plt.figure('erg') self.image1.set_data(info.T / np.max(info)) self.image2.set_data(target_belief.T / np.max(target_belief)) for i in range(self.nagents): self.lines[i].set_xdata(xs[i][0, :]) self.lines[i].set_ydata(xs[i][1, :]) self.inits[i].set_xdata(self.x[i, 0]) self.inits[i].set_ydata(self.x[i, 1]) plt.draw() plt.pause(.001) return action, t_comp
opti.subject_to( casadi.vec(x) >= 0 ) # elements nonnegative opti.subject_to( casadi.vec(x) <= 1 ) # elements bounded by 1 opti.subject_to( x@o == 1 ) # rows sum to 1 # opti.subject_to( casadi.eig_symbolic(A) <= np.sqrt(k) ) # minimize eigenvalue - eigenvalue vs sparsity # https://math.stackexchange.com/questions/199268/eigenvalues-vs-matrix-sparsity # opti.subject_to( casadi.sum1(casadi.sum2(x)) <= 0.3*9 ) # opti.subject_to( (x.nnz() / x.numel()) <= 0.5 ) # calculating sparsity directly # some reason it defaults to dense matrix opti.set_initial(x, np.diag(np.ones(len(c)))) rowsum = x@o csum = casadi.sum1(c)*o avmetric = casadi.sum1(((x@c - csum)**2)) # |Xc - average(c)| - this shows for one c, but there will be more c_k # sparsemetric = x.nnz() / x.numel() # some reason it defaults to dense matrix sparsemetric = casadi.sum1(casadi.sum2(elt_metric(x)))/x.numel() # sparsity metric - average penalization over elements opti.minimize(avmetric + sparsemetric) p_opts = {} s_opts = {'print_level': 0} opti.solver('ipopt', p_opts, s_opts) sol = opti.solve() print("Solution \n", sol.value(x))
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 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 optimize_weights2(agents, agentidx, server, N, cur_iter, num_steps, elt_metric=(lambda x: oneminusoneover_metric(x, 100))): # only optimizes agent's controls so that final c_k ergodicity is low """ Casadi Optimization -- Optimal Adjacency Matrix """ opti = casadi.Opti() A_opt = opti.variable(1, N) opti.subject_to(casadi.vec(A_opt) >= 0) # elements nonnegative opti.subject_to(casadi.vec(A_opt) <= 1) # elements bounded by 1 opti.subject_to(A_opt @ np.ones(N) == 1) # rows sum to 1 opti.set_initial(A_opt, np.array([int(j == agentidx) for j in range(N)])) ck_new = {} for k in np.ndindex(*[K] * n): ck = np.array([server[j][k] for j in range(N)]) ck_new[k] = A_opt @ ck """ Unrolling of planning horizon """ u_opt_plan = opti.variable(num_steps, agents[agentidx].m) init = np.array([[ agents[agentidx].umax * int(agents[agentidx].m - 1 == x) for x in range(agents[agentidx].m) ]]) opti.set_initial(u_opt_plan, np.repeat(init, [num_steps], axis=0)) curr_x_plan = [agents[agentidx].x_log[-1]] curr_v_plan = [np.zeros(agents[agentidx].m)] curr_c_k_plan = [ck_new] curr_agent_c_k_plan = [agents[agentidx].agent_c_k] for plan_i in range(0, num_steps): i = cur_iter + plan_i t = i * delta_t # [time, time+time_step] u = u_opt_plan[plan_i, :].T opti.subject_to( casadi.sum1(u_opt_plan[plan_i, :]**2) <= agents[agentidx].umax**2) x, v = agents[agentidx].move(u, t, delta_t, is_pred=True, prev_x=curr_x_plan[-1], prev_v=curr_v_plan[-1]) curr_x_plan.append(x) curr_v_plan.append(v) opti.subject_to(curr_x_plan[-1][:] >= 0) opti.subject_to( curr_x_plan[-1][:] <= np.array(agents[agentidx].U_shape)) curr_c_k, curr_agent_c_k = agents[agentidx].recalculate_c_k( t, delta_t, prev_x=curr_x_plan[-2], curr_x=curr_x_plan[-1], old_c_k=curr_c_k_plan[-1], old_agent_c_k=curr_agent_c_k_plan[-1]) curr_c_k_plan.append(curr_c_k) curr_agent_c_k_plan.append(curr_agent_c_k) # Sparse Metric -- Average element penalization sparsemetric = casadi.sum1(casadi.sum2(elt_metric(A_opt))) / A_opt.numel() # opti.subject_to(sparsemetric < 0.25) e_plan = 0 for k in np.ndindex(*[K] * n): ave_c_k = np.array([server[j][k] for j in range(N)]) ave_c_k[agentidx] = curr_c_k_plan[-1][k] ave_c_k = sum(ave_c_k) / N e_plan += lambd[k] * (ave_c_k - mu[k])**2 # readjust sparsemetric range to be [0, e_plan] opti.minimize(e_plan * (1 + sparsemetric)) p_opts = {} s_opts = {'print_level': 0} opti.solver('ipopt', p_opts, s_opts) sol = opti.solve() r = 2 A = sol.value(A_opt).round(r) u_plan = sol.value(u_opt_plan) x_plan = [sol.value(curr_x_plan[plan_i]) for plan_i in range(num_steps)] print("e_plan: ", sol.value(e_plan)) if np.count_nonzero(A) == 1 and A[agentidx] != 0: # only communicating with itself A = np.zeros(N) return A, x_plan, u_plan
def test_sum(self): self.message("sum") D=DM([[1,2,3],[4,5,6],[7,8,9]]) self.checkarray(c.sum1(D),array([[12,15,18]]),'sum()') self.checkarray(c.sum2(D),array([[6,15,24]]).T,'sum()')
curr_v_plan = [[np.zeros(agents[j].m)] for j in range(N)] curr_c_k_plan = [[agents[j].c_k] for j in range(N)] curr_agent_c_k_plan = [[agents[j].agent_c_k] for j in range(N)] u_opt_plan = [] for j in range(N): u_j = opti.variable(num_plan, agents[j].m) init = np.array([[ agents[j].umax * int(agents[j].m - 1 == x) for x in range(agents[j].m) ]]) opti.set_initial(u_j, np.repeat(init, [num_plan], axis=0)) u_opt_plan.append(u_j) for j in range(N): for plan_i in range(num_plan): opti.subject_to( casadi.sum1(u_opt_plan[j][plan_i, :]**2) <= agents[j].umax**2) for plan_i in range(0, num_plan): # i = big_i*num_plan + plan_i i = plan_i t = i * delta_t # [time, time+time_step] for j in range(N): x, v = agents[j].move(u_opt_plan[j][plan_i, :].T, t, delta_t, is_pred=True, prev_x=curr_x_plan[j][-1], prev_v=curr_v_plan[j][-1]) curr_x_plan[j].append(x) curr_v_plan[j].append(v)