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
