def _enforce_governing_equations(self):
for airfoil in self.airfoils:
### Compute normal velocities at the middle of each panel
x_midpoints = np.trapz(airfoil.x())
y_midpoints = np.trapz(airfoil.y())
u_midpoints, v_midpoints = self.calculate_velocity(
x_field=x_midpoints,
y_field=y_midpoints,
)
panel_dx = np.diff(airfoil.x())
panel_dy = np.diff(airfoil.y())
panel_length = (panel_dx**2 + panel_dy**2)**0.5
xp_hat_x = panel_dx / panel_length # x-coordinate of the xp_hat vector
xp_hat_y = panel_dy / panel_length # y-coordinate of the yp_hat vector
yp_hat_x = -xp_hat_y
yp_hat_y = xp_hat_x
normal_velocities = u_midpoints * yp_hat_x + v_midpoints * yp_hat_y
### Add in flow tangency constraint
self.opti.subject_to(normal_velocities == 0)
### Add in Kutta condition
self.opti.subject_to(airfoil.gamma[0] + airfoil.gamma[-1] == 0)
def test_quadcopter_navigation():
opti = asb.Opti()
N = 300
time_final = 1
time = np.linspace(0, time_final, N)
left_thrust = opti.variable(init_guess=0.5, scale=1, n_vars=N, lower_bound=0, upper_bound=1)
right_thrust = opti.variable(init_guess=0.5, scale=1, n_vars=N, lower_bound=0, upper_bound=1)
mass = 0.1
dyn = asb.FreeBodyDynamics(
opti_to_add_constraints_to=opti,
time=time,
xe=opti.variable(init_guess=np.linspace(0, 1, N)),
ze=opti.variable(init_guess=np.linspace(0, -1, N)),
u=opti.variable(init_guess=0, n_vars=N),
w=opti.variable(init_guess=0, n_vars=N),
theta=opti.variable(init_guess=np.linspace(np.pi / 2, np.pi / 2, N)),
q=opti.variable(init_guess=0, n_vars=N),
X=left_thrust + right_thrust,
M=(right_thrust - left_thrust) * 0.1 / 2,
mass=mass,
Iyy=0.5 * mass * 0.1 ** 2,
g=9.81,
)
opti.subject_to([ # Starting state
dyn.xe[0] == 0,
dyn.ze[0] == 0,
dyn.u[0] == 0,
dyn.w[0] == 0,
dyn.theta[0] == np.radians(90),
dyn.q[0] == 0,
])
opti.subject_to([ # Final state
dyn.xe[-1] == 1,
dyn.ze[-1] == -1,
dyn.u[-1] == 0,
dyn.w[-1] == 0,
dyn.theta[-1] == np.radians(90),
dyn.q[-1] == 0,
])
effort = np.sum( # The average "effort per second", where effort is integrated as follows:
np.trapz(left_thrust ** 2 + right_thrust ** 2) * np.diff(time)
) / time_final
opti.minimize(effort)
sol = opti.solve()
dyn.substitute_solution(sol)
assert sol.value(effort) == pytest.approx(0.714563, rel=0.01)
print(sol.value(effort))
def constrain_derivative(self,
derivative: cas.MX,
variable: cas.MX,
with_respect_to: Union[np.ndarray, cas.MX],
method: str = "midpoint",
) -> None:
"""
Adds a constraint to the optimization problem such that:
d(variable) / d(with_respect_to) == derivative
Can be used directly; also called indirectly by opti.derivative_of() for implicit derivative creation.
Args:
derivative: The derivative that is to be constrained here.
variable: The variable or quantity that you are taking the derivative of. The "numerator" of the
derivative, in colloquial parlance.
with_respect_to: The variable or quantity that you are taking the derivative with respect to. The
"denominator" of the derivative, in colloquial parlance.
In a typical example case, this `with_respect_to` parameter would be time. Please make sure that the
value of this parameter is monotonically increasing, otherwise you may get nonsensical answers.
method: The type of integrator to use to define this derivative. Options are:
* "forward euler" - a first-order-accurate forward Euler method
Citation: https://en.wikipedia.org/wiki/Euler_method
* "backwards euler" - a first-order-accurate backwards Euler method
Citation: https://en.wikipedia.org/wiki/Backward_Euler_method
* "midpoint" or "trapezoid" - a second-order-accurate midpoint method
Citation: https://en.wikipedia.org/wiki/Midpoint_method
* "simpson" - Simpson's rule for integration
Citation: https://en.wikipedia.org/wiki/Simpson%27s_rule
* "runge-kutta" or "rk4" - a fourth-order-accurate Runge-Kutta method. I suppose that technically,
"forward euler", "backward euler", and "midpoint" are all (lower-order) Runge-Kutta methods...
Citation: https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods#The_Runge%E2%80%93Kutta_method
* "runge-kutta-3/8" - A modified version of the Runge-Kutta 4 proposed by Kutta in 1901. Also
fourth-order-accurate, but all of the error coefficients are smaller than they are in the standard
Runge-Kutta 4 method. The downside is that more floating point operations are required per timestep,
as the Butcher tableau is more dense (i.e. not banded).
Citation: Kutta, Martin (1901), "Beitrag zur näherungsweisen Integration totaler
Differentialgleichungen", Zeitschrift für Mathematik und Physik, 46: 435–453
Note that all methods are expressed as integrators rather than differentiators; this prevents
singularities from forming in the limit of timestep approaching zero. (For those coming from the PDE
world, this is analogous to using finite volume methods rather than finite difference methods to allow
shock capturing.)
Returns: None (adds constraint in-place).
"""
d_var = np.diff(variable)
d_time = np.diff(with_respect_to) # Calculate the timestep
# TODO scale constraints by variable scale?
# TODO make
if method == "forward euler" or method == "forward" or method == "forwards":
# raise NotImplementedError
self.subject_to(
d_var == derivative[:-1] * d_time
)
elif method == "backward euler" or method == "backward" or method == "backwards":
# raise NotImplementedError
self.subject_to(
d_var == derivative[1:] * d_time
)
elif method == "midpoint" or method == "trapezoid" or method == "trapezoidal":
self.subject_to(
d_var == np.trapz(derivative) * d_time,
)
elif method == "simpson":
raise NotImplementedError
elif method == "runge-kutta" or method == "rk4":
raise NotImplementedError
elif method == "runge-kutta-3/8":
raise NotImplementedError
else:
raise ValueError("Bad value of `method`!")
velocity = opti.derivative_of(
position,
with_respect_to=time,
derivative_init_guess=1,
)
force = opti.variable(init_guess=np.linspace(1, -1, n_timesteps),
n_vars=n_timesteps)
opti.constrain_derivative(
variable=velocity,
with_respect_to=time,
derivative=force / mass_block,
)
effort_expended = np.sum(np.trapz(force**2) * np.diff(time))
opti.minimize(effort_expended)
### Boundary conditions
opti.subject_to([
position[0] == 0,
position[-1] == 1,
velocity[0] == 0,
velocity[-1] == 0,
])
sol = opti.solve()
import matplotlib.pyplot as plt
import seaborn as sns
def test_invertability_of_diff_trapz():
a = np.sin(np.arange(10))
assert np.all(np.trapz(np.diff(a)) == pytest.approx(np.diff(np.trapz(a))))
def test_trapz():
a = np.arange(100)
assert np.diff(np.trapz(a)) == pytest.approx(1)