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 make_phase_interpolation_matrix(n_intervals, tau_evaluation_points): if isinstance(tau_evaluation_points, list): tau_evaluation_points = casadi.DM(tau_evaluation_points) assert isinstance(tau_evaluation_points, casadi.DM) assert tau_evaluation_points.numel() == tau_evaluation_points.size1() assert float(casadi.mmin( casadi.diff(tau_evaluation_points) > 0)) == 1.0 # Must be ascending assert float(casadi.mmin(tau_evaluation_points)) >= 0.0 assert float(casadi.mmax(tau_evaluation_points)) <= 1.0 tau_evaluation_points_scaled = [ float(f) * n_intervals for f in casadi.vertsplit(tau_evaluation_points) ] interval_indices = [int(f) for f in tau_evaluation_points_scaled] interval_values = [f - int(f) for f in tau_evaluation_points_scaled] if interval_indices[-1] == n_intervals: interval_indices[-1] = n_intervals - 1 interval_values[-1] = 1.0 interval_interpolation_matrix = make_interval_interpolation_matrix( interval_values) phase_interpolation_matrix = casadi.DM(tau_evaluation_points.numel(), (n_intervals * (collocation.n_nodes - 1) + 1)) for i in range(len(interval_values)): column_idx = interval_indices[i] * (collocation.n_nodes - 1) phase_interpolation_matrix[i, (column_idx):( column_idx + collocation.n_nodes)] = casadi.DM( interval_interpolation_matrix[i]).T return phase_interpolation_matrix
def diff(a, n=1, axis=-1): """ Calculate the n-th discrete difference along the given axis. See syntax here: https://numpy.org/doc/stable/reference/generated/numpy.diff.html """ if not is_casadi_type(a): return _onp.diff(a, n=n, axis=axis) else: if axis != -1: raise NotImplementedError("This could be implemented, but haven't had the need yet.") result = a for i in range(n): result = _cas.diff(a) return result
* T is the local torque per unit length * G is the local shear modulus * J is the polar moment of inertia * ()' is a derivative w.r.t. x. """ import numpy as np import casadi as cas opti = cas.Opti() # Initialize a SAND environment # Define Assumptions L = 34.1376 / 2 n = 200 x = cas.linspace(0, L, n) dx = cas.diff(x) E = 228e9 # Pa, modulus of CF G = E / 2 / (1 + 0.5) # TODO fix this!!! CFRP is not isotropic! max_allowable_stress = 570e6 / 1.75 log_nominal_diameter = opti.variable(n) opti.set_initial(log_nominal_diameter, cas.log(200e-3)) nominal_diameter = cas.exp(log_nominal_diameter) thickness = 0.14e-3 * 5 opti.subject_to([ nominal_diameter > thickness, ]) # Bending loads
thetadd = casadi.cos(x[0] - math.pi / 2) * G return casadi.vertcat(x[1], u + thetadd) opti = casadi.Opti() # optimizaiton problem q_state = opti.variable(N_STATES, N + 1) # state trajectory pos = q_state[0, :] speed = q_state[1, :] u = opti.variable(U_STATES, N) # control variable: acceleration tf = opti.variable() # final time # opti.minimize(tf) # Objective is to minimize the total time # opti.minimize(casadi.mtimes(u, casadi.transpose(u))) # Minimize acceleration opti.minimize(casadi.mtimes(casadi.diff(u), casadi.transpose(casadi.diff(u)))) # Minimize jerk # opti.minimize(casadi.mtimes(casadi.diff(u), casadi.transpose(casadi.diff(u))) + tf/20) # Minimize jerk and time dt = tf / N # Each control interval for k in range(N): # RK4 method k1 = system_dynamics(q_state[:, k], u[:, k]) k2 = system_dynamics(q_state[:, k] + dt / 2 * k1, u[:, k]) k3 = system_dynamics(q_state[:, k] + dt / 2 * k2, u[:, k]) k4 = system_dynamics(q_state[:, k] + dt * k3, u[:, k]) x_next = q_state[:, k] + dt / 6.0 * (k1 + 2 * k2 + 2 * k3 + k4) opti.subject_to(q_state[:, k + 1] == x_next) # reduce the defect! # Path Conditions opti.subject_to(-1 <= u)
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, ])
load_location < 60 / 2 - 2, load_location == 18, ]) beam.add_point_load(load_location, -lift_force / 3) beam.add_uniform_load(force=lift_force / 2) beam.setup() # Tip deflection constraint opti.subject_to([ # beam.u[-1] < 2, # Source: http://web.mit.edu/drela/Public/web/hpa/hpa_structure.pdf # beam.u[-1] > -2 # Source: http://web.mit.edu/drela/Public/web/hpa/hpa_structure.pdf beam.du * 180 / cas.pi < 10, beam.du * 180 / cas.pi > -10 ]) opti.subject_to([ cas.diff(cas.diff(beam.nominal_diameter)) < 0.001, cas.diff(cas.diff(beam.nominal_diameter)) > -0.001, ]) # opti.minimize(cas.sqrt(beam.mass)) opti.minimize(beam.mass) # opti.minimize(beam.mass ** 2) # opti.minimize(beam.mass_proxy) p_opts = {} s_opts = {} s_opts["max_iter"] = 1e6 # If you need to interrupt, just use ctrl+c # s_opts["bound_frac"] = 0.5 # s_opts["bound_push"] = 0.5 # s_opts["slack_bound_frac"] = 0.5 # s_opts["slack_bound_push"] = 0.5
def falkner_skan(m, eta_edge=7, n_points=100, max_iter=100): """ Solves the Falkner-Skan equation for a given value of m. See Wikipedia for reference: https://en.wikipedia.org/wiki/Falkner–Skan_boundary_layer :param m: power-law exponent of the edge velocity (i.e. u_e(x) = U_inf * x ^ m) :return: eta, f0, f1, and f2 as a tuple of 1-dimensional ndarrays. Governing equation: f''' + f*f'' + beta*( 1 - (f')^2 ) = 0, where: beta = 2 * m / (m+1) f(0) = f'(0) = 0 f'(inf) = 1 Syntax: f0 is f f1 is f' f2 is f'' f3 is f''' """ # Assign beta beta = 2 * m / (m + 1) opti = cas.Opti() eta = cas.linspace(0, eta_edge, n_points) def trapz(x): out = (x[:-1] + x[1:]) / 2 # out[0] += x[0] / 2 # out[-1] += x[-1] / 2 return out # Vars f0 = opti.variable(n_points) f1 = opti.variable(n_points) f2 = opti.variable(n_points) # Guess (guess a quadratic velocity profile, integrate and differentiate accordingly) opti.set_initial(f0, -eta**2 * (eta - 3 * eta_edge) / (3 * eta_edge**2)) opti.set_initial(f1, 1 - (1 - eta / eta_edge)**2) opti.set_initial(f2, 2 * (eta_edge - eta) / eta_edge**2) # BCs opti.subject_to([f0[0] == 0, f1[0] == 0, f1[-1] == 1]) # ODE f3 = -f0 * f2 - beta * (1 - f1**2) # Derivative definitions (midpoint-method) df0 = cas.diff(f0) df1 = cas.diff(f1) df2 = cas.diff(f2) deta = cas.diff(eta) opti.subject_to([ df0 == trapz(f1) * deta, df1 == trapz(f2) * deta, df2 == trapz(f3) * deta ]) # Require unseparated solutions opti.subject_to([f2[0] > 0]) p_opts = {} s_opts = {} s_opts["max_iter"] = max_iter # If you need to interrupt, just use ctrl+c opti.solver('ipopt', p_opts, s_opts) try: sol = opti.solve() except: raise Exception("Solver failed for m = %f!" % m) return (sol.value(eta), sol.value(f0), sol.value(f1), sol.value(f2))
return casadi.vertcat(x[1], u) opti = casadi.Opti() # optimizaiton problem q_state = opti.variable(2, N + 1) # state trajectory pos = q_state[0, :] speed = q_state[1, :] u = opti.variable(1, N) # control variable: acceleration tf = opti.variable() # final time # opti.minimize(tf) # Objective is to minimize the total time # opti.minimize(casadi.mtimes(u, casadi.transpose(u))) # Minimize acceleration # opti.minimize(casadi.mtimes(casadi.diff(u), casadi.transpose(casadi.diff(u)))) # Minimize jerk opti.minimize(casadi.mtimes(casadi.diff(u), casadi.transpose(casadi.diff(u))) + tf/20) # Minimize jerk and time # Should result in bang bang control dt = tf / N # Each control interval for k in range(N): # RK4 method k1 = system_dynamics(q_state[:, k], u[:, k]) k2 = system_dynamics(q_state[:, k] + dt / 2 * k1, u[:, k]) k3 = system_dynamics(q_state[:, k] + dt / 2 * k2, u[:, k]) k4 = system_dynamics(q_state[:, k] + dt * k3, u[:, k]) x_next = q_state[:, k] + dt / 6.0 * (k1 + 2 * k2 + 2 * k3 + k4) opti.subject_to(q_state[:, k + 1] == x_next) # reduce the defect! # for k in range(N): # integrator = casadi.integrator('integrator', 'cvodes', dae, {'grid':ts, 'output_t0':True})
f1 = opti.variable(n_points) f2 = opti.variable(n_points) # Guess opti.set_initial(f0, -eta**2 * (eta - 3 * eta_edge) / (3 * eta_edge**2)) opti.set_initial(f1, 1 - (1 - eta / eta_edge)**2) opti.set_initial(f2, 2 * (eta_edge - eta) / eta_edge**2) # BCs opti.subject_to([f0[0] == 0, f1[0] == 0, f1[-1] == 1]) # ODE f3 = -f0 * f2 - beta * (1 - f1**2) # Derivative definitions (midpoint-method) df0 = cas.diff(f0) df1 = cas.diff(f1) df2 = cas.diff(f2) deta = cas.diff(eta) opti.subject_to([ df0 == trapz(f1) * deta, df1 == trapz(f2) * deta, df2 == trapz(f3) * deta ]) # Require barely-separating solution opti.subject_to([f2[0] == 0]) p_opts = {} s_opts = {} s_opts["max_iter"] = max_iter # If you need to interrupt, just use ctrl+c opti.solver('ipopt', p_opts, s_opts)
def __init__(self, dae, t, order, method='legendre', parallelization='serial', tdp_fun=None, expand=True, repeat_param=False, options={}): """Constructor @param t time vector of length N+1 defining N collocation intervals @param order number of collocation points per interval @param method collocation method ('legendre', 'radau') @param dae DAE model @param parallelization parallelization of the outer map. Possible set of values is the same as for casadi.Function.map(). @return Returns a dictionary with the following keys: 'X' -- state at collocation points 'Z' -- alg. state at collocation points 'x0' -- initial state 'eq' -- the expression eq == 0 defines the collocation equation. eq depends on X, Z, x0, p. 'Q' -- quadrature values at collocation points depending on x0, X, Z, p. """ # Convert whatever DAE to implicit DAE dae = dae.makeImplicit() M = order N = len(t) - 1 # # Define variables and functions corresponfing to all control intervals # K = cs.MX.sym('K', dae.nx, N * M) # State derivatives at collocation points Z = cs.MX.sym('Z', dae.nz, N * M) # Alg state at collocation points x = cs.MX.sym('x', dae.nx, N + 1) # State at the ends of collocation intervals (t) u = cs.MX.sym('u', dae.nu, N) # Input on collocation intervals U = cs.horzcat(*[cs.repmat(u[:, n], 1, M) for n in range(N)]) # Input at collocation points # Butcher tableau for the selected method butcher = butcherTableuForCollocationMethod(order, method) # Interval lengths h = np.diff(t) # Integrated state at collocation points Mx = cs.kron(cs.DM.eye(N), cs.DM.ones(1, M)) MK = cs.kron(cs.diagcat(*h), butcher.A.T) # integration matrix X = cs.mtimes(x[:, : -1], Mx) + cs.mtimes(K, MK) # Integrated state at the ends of collocation intervals xf = x[:, : -1] + cs.mtimes(K, cs.kron(cs.diagcat(*h), butcher.b)) # Points in time at which the collocation equations are calculated # TODO: this possibly can be sped up a little bit. tc = np.hstack([t[n] + h[n] * butcher.c for n in range(N)]) # Values of the time-dependent parameter if tdp_fun is not None: tdp_val = cs.horzcat(*[tdp_fun(t) for t in tc]) else: assert dae.ntdp == 0 tdp_val = np.zeros((0, tc.size)) # DAE function dae_fun = dae.createFunction('dae', ['xdot', 'x', 'z', 'u', 'p', 't', 'tdp'], ['dae', 'quad']) if expand: dae_fun = dae_fun.expand() # expand() for speed if repeat_param: reduce_in = [] p = cs.MX.sym('P', dae.np, N * M) else: reduce_in = [4] p = cs.MX.sym('P', dae.np) dae_map = dae_fun.map('dae_map', parallelization, N * M, reduce_in, [], options) dae_out = dae_map(xdot=K, x=X, z=Z, u=U, p=p, t=tc, tdp=tdp_val) eqc = ce.struct_MX([ ce.entry('collocation', expr=dae_out['dae']), ce.entry('continuity', expr=xf - x[:, 1 :]), ce.entry('param', expr=cs.diff(p, 1, 1)) ]) # Integrate the quadrature state quad = dae_out['quad'] #t0 = time.time() q = [cs.MX.zeros(dae.nq)] # Integrated quadrature at interval ends # TODO: speed up the calculation of q. for n in range(N): q.append(q[-1] + h[n] * cs.mtimes(quad[:, n * M : (n + 1) * M], butcher.b)) q = cs.horzcat(*q) Q = cs.mtimes(q[:, : -1], Mx) + cs.mtimes(quad, MK) # Integrated quadrature at collocation points #print('Creating Q took {0:.3f} s.'.format(time.time() - t0)) self._N = N self._M = M self._eq = eqc self._x = x self._X = X self._K = K self._Z = Z self._U = U self._u = u self._quad = quad self._Q = Q self._q = q self._p = p self._tc = tc self._butcher = butcher self._tdp = tdp_val self._t = t