def setUp(self):
start = 0
end = np.pi * 4
self.x = np.linspace(start, end, 30)
self.y = np.sin(self.x)
self.num_test_points = 100
self.testpoints = linspace(start, end, self.num_test_points)
def quadrature(fcn, a, b, N=200):
# Simpson's rule
N += 1 if N % 2 != 0 else 0
fcnmap = fcn.map(N + 1)
xvals = cas.linspace(a, b, N + 1)
fvals = fcnmap(xvals)
return (b - a) / N / 3 * (fvals[0] + 4 * cas.sum2(fvals[1::2]) +
2 * cas.sum2(fvals[2:-2:2]) + fvals[-1])
def linspace(start: float = 0., stop: float = 1., num: int = 50):
"""
Returns evenly spaced numbers over a specified interval.
See syntax here: https://numpy.org/doc/stable/reference/generated/numpy.linspace.html
"""
if not is_casadi_type([start, stop, num], recursive=True):
return _onp.linspace(start, stop, num)
else:
return _cas.linspace(start, stop, num)
def transcribe(self, stage, opti):
"""
Transcription is the process of going from a continuous-time OCP to an NLP
"""
self.add_variables(stage, opti)
self.add_parameter(stage, opti)
# Create time grid (might be symbolic)
self.control_grid = linspace(MX(self.t0), self.t0 + self.T, self.N + 1)
self.integrator_grid = []
for k in range(self.N):
t_local = linspace(self.control_grid[k], self.control_grid[k+1], self.M+1)
self.integrator_grid.append(t_local[:-1] if k<self.N-1 else t_local)
self.add_constraints(stage, opti)
self.add_objective(stage, opti)
self.set_initial(stage, opti, stage._initial)
self.set_parameter(stage, opti)
placeholders = stage._bake_placeholders(self)
return placeholders
def linspace(start: float = 0., stop: float = 1., n_points: int = 50):
"""
Makes a linearly-spaced vector.
Args:
start: Value to start at.
stop: Value to end at.
n_points: Number of points in the vector.
"""
try:
return onp.linspace(start, stop, n_points)
except Exception:
return cas.linspace(start, stop, n_points)
def test_arrayexpressions(self):
with open(os.path.join(TEST_DIR, 'ArrayExpressions.mo'), 'r') as f:
txt = f.read()
ast_tree = parser.parse(txt)
casadi_model = gen_casadi.generate(ast_tree, 'ArrayExpressions')
print(casadi_model)
ref_model = Model()
a = ca.MX.sym("a", 3)
b = ca.MX.sym("b", 4)
c = ca.MX.sym("c", 3)
d = ca.MX.sym("d", 3)
e = ca.MX.sym("e", 3)
g = ca.MX.sym("g", 1)
h = ca.MX.sym("h", 1)
i = ca.MX.sym('i', 2, 3)
B = ca.MX.sym("B", 3)
C = ca.MX.sym("C", 2)
D = ca.MX.sym("D", 3)
E = ca.MX.sym("E", 2)
arx = ca.MX.sym("ar.x", 3)
arcy = ca.MX.sym("arc.y", 2)
arcw = ca.MX.sym("arc.w", 2)
nested1z = ca.MX.sym('nested1.z', 3)
nested2z = ca.MX.sym('nested2.z', 2, 3)
nested1n = ca.MX.sym('nested1.n', 1)
nested2n = ca.MX.sym('nested2.n', 2)
scalar_f = ca.MX.sym("scalar_f")
c_dim = ca.MX.sym("c_dim")
d_dim = ca.MX.sym("d_dim")
ref_model.alg_states = list(map(Variable, [arx, arcy, arcw, nested1z, nested2z, a, c, d, e, scalar_f, g, h, i]))
ref_model.alg_states[6].min = [0, 0, 0]
ref_model.parameters = list(map(Variable, [nested2n, nested1n, d_dim]))
parameter_values = [np.array([3, 3]), 3, 3]
for const, val in zip(ref_model.parameters, parameter_values):
const.value = val
ref_model.outputs = list(map(Variable, [h]))
ref_model.constants = list(map(Variable, [b, c_dim, B, C, D, E]))
constant_values = [np.array([2.7, 3.7, 4.7, 5.7]), 2, ca.linspace(1., 2., 3), 1.7 * ca.DM.ones(2),
ca.DM.zeros(3), ca.DM.ones(2)]
for const, val in zip(ref_model.constants, constant_values):
const.value = val
ref_model.equations = [c - (a + b[0:3] * e), d - (ca.sin(a / b[1:4])), e - (d + scalar_f), g - ca.sum1(c),
h - B[1], arx[1] - scalar_f, nested1z - ca.DM.ones(3), nested2z[0, :].T - np.array([4, 5, 6]),
nested2z[1, 0] - 3, nested2z[1, 1] - 2, nested2z[1, 2] - 1, i[0, :] - ca.transpose(ca.DM.ones(3)), i[1, :] - ca.transpose(ca.DM.ones(3)), arcy[0] - arcy[1],
arcw[0] + arcw[1], a - np.array([1, 2, 3]), scalar_f - 1.3]
self.assert_model_equivalent_numeric(ref_model, casadi_model)
def test_arrayexpressions(self):
with open(os.path.join(TEST_DIR, 'ArrayExpressions.mo'), 'r') as f:
txt = f.read()
ast_tree = parser.parse(txt)
casadi_model = gen_casadi.generate(ast_tree, 'ArrayExpressions')
print(casadi_model)
ref_model = CasadiSysModel()
a = ca.MX.sym("a", 3)
b = ca.MX.sym("b", 4)
c = ca.MX.sym("c", 3)
d = ca.MX.sym("d", 3)
e = ca.MX.sym("e", 3)
g = ca.MX.sym("g", 1)
h = ca.MX.sym("h", 1)
B = ca.MX.sym("B", 3)
C = ca.MX.sym("C", 2)
D = ca.MX.sym("D", 3)
E = ca.MX.sym("E", 2)
arx = ca.MX.sym("ar.x", 3)
arcy = ca.MX.sym("arc.y", 2)
arcw = ca.MX.sym("arc.w", 2)
scalar_f = ca.MX.sym("scalar_f")
c_dim = ca.MX.sym("c_dim")
d_dim = ca.MX.sym("d_dim")
ref_model.alg_states = [a, c, d, e, scalar_f, g, arx, arcy, arcw, h]
ref_model.parameters = [d_dim]
ref_model.outputs = [h]
ref_model.constants = [b, c_dim, B, C, D, E]
ref_model.constant_values = [
np.array([2.7, 3.7, 4.7, 5.7]), 2,
ca.linspace(1, 2, 3), 1.7 * ca.DM.ones(2),
ca.DM.zeros(3),
ca.DM.ones(2)
]
ref_model.equations = [
c - (a + b[0:3] * e), d - (ca.sin(a / b[1:4])), e - (d + scalar_f),
g - ca.sum1(c), h - B[1], arx[1] - scalar_f, arcy[0] - arcy[1],
arcw[0] + arcw[1]
]
self.assert_model_equivalent_numeric(ref_model, casadi_model)
"""
Suppose you have some function f(x)=exp(-x) on the range x in [0, 1].
You want to pick n points to linearly interpolate this such that the L2-error is minimized. How do you do it?
"""
import numpy as np
import casadi as cas
import matplotlib.pyplot as plt
opti = cas.Opti()
n = 101
x = opti.variable(n) # cas.linspace(0,1,n)
opti.set_initial(x, cas.linspace(0, 10, n))
opti.subject_to([
x[0] == 0,
x[-1] == 10,
])
y = cas.exp(-x)
x1 = x[:-1]
x2 = x[1:]
errors = -x1 * cas.exp(-x2) / 2 - x1 * cas.exp(-x1) / 2 + x2 * cas.exp(
-x2) / 2 + x2 * cas.exp(-x1) / 2 + cas.exp(-x2) - cas.exp(-x1)
error = cas.sum1(errors)
opti.minimize(error * 1e3)
# Minimize time and distance to target
mocp.add_objective(100 * slack_final_x)
mocp.add_objective(100 * slack_final_y)
mocp.add_objective(duration)
# Add mechanical energy output, to demonstrate output evaluation
mocp.add_path_output('energy', y + 0.5 * speed**2)
### Problem done, solve it
mocp.solve()
mocp.phases[phase_name].change_time_resolution(
8) # Refine mesh and solve again
mocp.solve()
# Interpolate result and compare with analytic solution
tau_grid = linspace(0.0, 1.0, 801)
t_grid = tau_grid * mocp.get_value(duration)
result_interpolated = mocp.phases[phase_name].interpolate(tau_grid)
analytic_solution = dict()
analytic_solution['x'] = t_grid - sin(t_grid)
analytic_solution['y'] = cos(t_grid)
analytic_solution['speed'] = sqrt(2.0 - 2.0 * cos(t_grid))
analytic_solution['path_angle'] = (t_grid - pi) / 2
analytic_solution_duration = 1.5 * pi
print('error duration: ',
mmax(fabs(mocp.get_value(duration) - analytic_solution_duration)))
print('error energy: ',
mmax(fabs(DM(result_interpolated['outputs']['energy']) - 1.0)))
for trajectory_name in analytic_solution:
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 == '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':
entries = []
for sym in [self.get_mx(op) for op in tree.operands]:
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])
delay_time = self.get_mx(tree.operands[1])
if not isinstance(expr, ca.MX) or not expr.is_symbolic():
# TODO
raise NotImplementedError(
'Currently, delay() is only supported with a variable as argument.'
)
src = ca.MX.sym('{}_delayed_{}'.format(expr.name(), delay_time),
*expr.size())
delayed_state = DelayedState(src.name(), expr.name(), delay_time)
self.model.delayed_states.append(delayed_state)
self.model.inputs.append(Variable(src))
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 = 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:
function = self.get_function(op)
src = ca.vertcat(*function.call(
[self.get_mx(operand)
for operand in tree.operands], *self.function_mode))
self.src[tree] = src
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
#### Just with slanted ground
# qp = {'h': H.sparsity()}
# S = ca.conic('hc', 'qpoases', qp)
# sol = S(h=H, g=g, lbx=lbx, ubx=ubx)
# print("Sol: \n", sol['x'])
# x_opt = sol['x']
# With A matrix
qp = {'h': H.sparsity(), 'a': A.sparsity()}
S = ca.conic('hc', 'qpoases', qp)
sol = S(h=H, g=g, a=A, lbx=lbx, ubx=ubx, lba=lba, uba=uba)
print("Sol: \n", sol['x'])
x_opt = sol['x']
Y0 = x_opt[0::2]
Z0 = x_opt[1::2]
import matplotlib.pyplot as plt
plt.plot(Y0, Z0, 'o-')
ys = ca.linspace(-2., 2., 100)
# zs = 0.5*ca.DM.ones(100,1) #+ 0.1*ys
zs = 0.5 + 0.1 * ys
plt.plot(ys, zs, '--')
plt.xlabel('y [m]')
plt.ylabel('z [m]')
plt.title('hanging chain QP')
plt.grid(True)
plt.legend(['Chain'], loc=9)
plt.legend(['Chain', 'z - 0.1y >= 0.5'], loc=9)
plt.show()
def main():
p = get_parameters()
n_phases = 4
phase_names = ['boost6', 'boost3', 'main', 'upper']
phase = [None] * n_phases
mocp = MultiPhaseOptimalControlProblem()
start = lambda a: mocp.start(a)
end = lambda a: mocp.end(a)
# Create the states, controls, constraints and dynamics for each phase
for i in range(n_phases):
phase[i] = create_rocket_stage_phase(mocp, phase_names[i], i, p)
# Initial state constraint
mocp.add_constraint(start(phase[0].rx) == p.initial_position_x)
mocp.add_constraint(start(phase[0].ry) == p.initial_position_y)
mocp.add_constraint(start(phase[0].rz) == p.initial_position_z)
mocp.add_constraint(start(phase[0].vx) == p.initial_velocity_x)
mocp.add_constraint(start(phase[0].vy) == p.initial_velocity_y)
mocp.add_constraint(start(phase[0].vz) == p.initial_velocity_z)
# Phase linkages
for i in range(n_phases - 1):
mocp.add_constraint(start(phase[i + 1].rx) == end(phase[i].rx))
mocp.add_constraint(start(phase[i + 1].ry) == end(phase[i].ry))
mocp.add_constraint(start(phase[i + 1].rz) == end(phase[i].rz))
mocp.add_constraint(start(phase[i + 1].vx) == end(phase[i].vx))
mocp.add_constraint(start(phase[i + 1].vy) == end(phase[i].vy))
mocp.add_constraint(start(phase[i + 1].vz) == end(phase[i].vz))
# Target orbit - soft constraints
# h_T - cross(r_f, v_f) == 0
# e_T + cross(h_T, v_f) + r_f / norm(r_f) == 0
r_f = casadi.vertcat(end(phase[-1].rx), end(phase[-1].ry),
end(phase[-1].rz))
v_f = casadi.vertcat(end(phase[-1].vx), end(phase[-1].vy),
end(phase[-1].vz))
h_T = casadi.SX(p.target_angular_momentum)
e_T = casadi.SX(p.target_eccentricity)
defect_angular_momentum = h_T - casadi.cross(r_f, v_f)
defect_eccentricity = e_T + casadi.cross(h_T,
v_f) + r_f / casadi.norm_2(r_f)
slack_angular_momentum = mocp.add_variable('slack_angular_momentum',
init=3.0)
slack_eccentricity = mocp.add_variable('slack_eccentricity', init=3.0)
mocp.add_constraint(slack_angular_momentum > 0)
mocp.add_constraint(slack_eccentricity > 0)
mocp.add_constraint(defect_angular_momentum[0] < slack_angular_momentum)
mocp.add_constraint(defect_angular_momentum[0] > -slack_angular_momentum)
mocp.add_constraint(defect_angular_momentum[1] < slack_angular_momentum)
mocp.add_constraint(defect_angular_momentum[1] > -slack_angular_momentum)
mocp.add_constraint(defect_angular_momentum[2] < slack_angular_momentum)
mocp.add_constraint(defect_angular_momentum[2] > -slack_angular_momentum)
mocp.add_constraint(defect_eccentricity[0] < slack_eccentricity)
mocp.add_constraint(defect_eccentricity[0] > -slack_eccentricity)
mocp.add_constraint(defect_eccentricity[1] < slack_eccentricity)
mocp.add_constraint(defect_eccentricity[1] > -slack_eccentricity)
mocp.add_constraint(defect_eccentricity[2] < slack_eccentricity)
mocp.add_constraint(defect_eccentricity[2] > -slack_eccentricity)
mocp.add_objective(100.0 * slack_angular_momentum)
mocp.add_objective(100.0 * slack_eccentricity)
# Maximize final mass
final_mass = end(phase[-1].mass)
mocp.add_objective(-1.0 * final_mass)
### Problem done, solve it
mocp.solve()
for phase_name in mocp.phases:
mocp.phases[phase_name].change_time_resolution(6)
mocp.solve()
print('solution final time:',
sum([p.duration_value for p in mocp.phases.values()]) * p.scale.time)
print('expected final time: 924.139')
# Interpolate resulting tajectory
tau_grid = casadi.linspace(0.0, 1.0, 501)
interpolated_results = dict()
for phase_name in mocp.phases:
interpolated_results[phase_name] = mocp.phases[phase_name].interpolate(
tau_grid)
# Concatenate phases into complete timeline
durations = [e['duration'] for e in interpolated_results.values()]
trajectories = [e['trajectories'] for e in interpolated_results.values()]
t_offset = casadi.vertsplit(casadi.cumsum(casadi.DM([0] + durations[:-1])))
t_grid = casadi.vertcat(
*[e[1] + e[0] * tau_grid for e in zip(durations, t_offset)])
trajectories_concatendated = dict()
for trajectory_name in trajectories[0]:
trajectories_concatendated[trajectory_name] = [
e for i in range(len(trajectories))
for e in trajectories[i][trajectory_name]
]
# Reproduce the figures from the book example
generate_figures(t_grid, trajectories_concatendated, p)
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 == '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 = 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
A = msym("A", Gf.output(2).sparsity())
b = msym("b", Ff.output(1).sparsity())
linsys = MXFunction([A, b], [-linsol.solve(A, b, False)])
linsys.init()
### LPDE -- end
Ff = SXFunction([z], [F, jacobian(F, z)])
Ff.init()
par_ = par()
par_["Xref"] = waypoints
# Physical times at control intervals
ts = vertcat([c.linspace(0, Tw, Ns + 1), c.linspace(Tw, T, N - Ns + 1)[1:]])
# Local speed of time
dts = ts[1:] - ts[:-1]
# Physical times at collocation points
tsc = []
dLm = MX(dLm)
# Collect the equality constraints
coupling = []
collocation = []
dynamics_lpde = []
scaling_K = MX(scaling_K)
:param temperature: Temperature, in Kelvin
:return: Dynamic viscosity, in kg/(m*s)
"""
# Uses Sutherland's law from the temperature
# From CFDWiki
# Sutherland constant
C1 = 1.458e-6 # kg/(m*s*sqrt(K))
S = 110.4 # K
mu = C1 * temperature ** (3 / 2) / (temperature + S)
return mu
if __name__ == "__main__":
test_altitudes = cas.linspace(0, 40000, 201)
test_pressures = get_pressure_at_altitude(test_altitudes)
test_temps = get_temperature_at_altitude(test_altitudes)
import matplotlib.pyplot as plt
import matplotlib.style as style
import plotly.express as px
import plotly.graph_objects as go
style.use("seaborn")
plt.semilogy(test_altitudes, test_pressures)
plt.xlabel("Altitude [m]")
plt.ylabel("Pressure [Pa]")
plt.show()
plt.plot(test_altitudes, test_temps)
A = msym("A",Gf.output(2).sparsity())
b = msym("b",Ff.output(1).sparsity())
linsys = MXFunction([A,b],[-linsol.solve(A,b,False)])
linsys.init()
### LPDE -- end
Ff = SXFunction([z],[F,jacobian(F,z)])
Ff.init()
par_ = par()
par_["Xref"] = waypoints
# Physical times at control intervals
ts = vertcat([c.linspace(0,Tw,Ns+1),c.linspace(Tw,T,N-Ns+1)[1:]])
# Local speed of time
dts = ts[1:]-ts[:-1]
# Physical times at collocation points
tsc = []
dLm = MX(dLm)
# Collect the equality constraints
coupling = []
collocation = []
dynamics_lpde = []
scaling_K = MX(scaling_K)
x1 = 2
dx0 = dx0
dx1 = 5
t = 0
count += 1
if t >= T / 3 and count == 1:
# delta_T = T - delta_T
x0 = x1
x1 = 0
dx0 = dx0
dx1 = 0
t = 0
count += 1
# print(f(delta_T=delta_T, t=t, x0=x0, x1=x1, dx0=dx0, dx1=dx1)['x'])
# print("x0 = " + str(x0) + ", x1 = " + str(x1) + ", dx0 = " + str(dx0) + ", dx1 = " + str(dx1) + ", y = "+ str(y[-1]))
time = ca.linspace(0, T, N)
plt.plot(time, y, label='cubic')
plt.legend()
delta_T = ca.MX.sym('delta_T', 1)
t = ca.MX.sym('t', 1)
x0 = ca.MX.sym('x0_1', 1)
x1 = ca.MX.sym('x1_1', 1)
dx0 = ca.MX.sym('dx0_1', 1)
dx1 = ca.MX.sym('dx1_1', 1)
a0 = x0
a1 = dx0
a2 = -(delta_T**(-2)) * (3 * (x0 - x1) + delta_T * (2 * dx0 + dx1))
a3 = (delta_T**(-3)) * (2 * (x0 - x1) + delta_T * (dx0 + dx1))
f0 is f f1 is f' f2 is f'' f3 is f''' """ opti = cas.Opti() m = opti.variable() opti.set_initial(m, -0.1) # Assign beta beta = 2 * m / (m + 1) eta = cas.linspace(0, eta_edge, n_points) trapz = lambda x: (x[:-1] + x[1:]) / 2 # Vars f0 = opti.variable(n_points) 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])
* phi is the local twist angle
* 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
def test_im_bugs(self): a = vertcat(1,2) self.assertTrue(isinstance(a,DM)) self.checkarray(c.linspace(1,3,10),c.linspace(1.0,3.0,10))
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))
import casadi as ca
from matplotlib import pyplot as plt
import numpy as np
import time
opti = ca.Opti()
x = 0
dx = 0
u = 0
T = 0.1
t = ca.linspace(0, T, 20)
a = {}
for i in range(5):
a.update({'a'+str(i) : opti.variable(1)})
x += a['a'+str(i)]*(t**i)
if i-1 >= 0:
dx += a['a'+str(i)]*(t**(i-1))
if i-2 >= 0:
u += a['a'+str(i)]*(t**(i-2))
# opti.subject_to(ca.jacobian(dx, t) == u)
# opti.subject_to(ca.jacobian(x, t) == dx)
opti.subject_to(x[-1] == 0)
opti.subject_to(x[0] == 5)
opti.subject_to(dx[-1] == 0)
# opti.subject_to(dx[0] == 0)
# opti.subject_to(u[-1] == 0)
# opti.subject_to(u[0] == 0)
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:
try: # Check if there is a component named as the operation. In that case we are dealing with a time access
# Should we check for symbol as well?
v = self.get_mx(ast.ComponentRef(name=op))
t = self.get_mx(tree.operands[0])
src = self.get_symbol_time_access(v, t)
except KeyError:
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 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
