def casadi(self, x, u, dxdt):
""" write dynamics as first order ODE: dxdt = f(x(t))
x is a 6x1 vector: [x, y, psi, vx, vy, omega]^T
u is a 2x1 vector: [acc/pwm, steer]^T
dxdt is a casadi.SX variable
"""
pwm = u[0]
steer = u[1]
psi = x[2]
vx = x[3]
vy = x[4]
omega = x[5]
vmin = 0.05
vy = cs.if_else(vx < vmin, 0, vy)
omega = cs.if_else(vx < vmin, 0, omega)
steer = cs.if_else(vx < vmin, 0, steer)
vx = cs.if_else(vx < vmin, vmin, vx)
Frx = (self.Cm1 - self.Cm2 * vx) * pwm - self.Cr0 - self.Cr2 * (vx**2)
alphaf = steer - cs.atan2((self.lf * omega + vy), vx)
alphar = cs.atan2((self.lr * omega - vy), vx)
Ffy = self.Df * cs.sin(self.Cf * cs.arctan(self.Bf * alphaf))
Fry = self.Dr * cs.sin(self.Cr * cs.arctan(self.Br * alphar))
dxdt[0] = vx * cs.cos(psi) - vy * cs.sin(psi)
dxdt[1] = vx * cs.sin(psi) + vy * cs.cos(psi)
dxdt[2] = omega
dxdt[3] = 1 / self.mass * (Frx - Ffy * cs.sin(steer)) + vy * omega
dxdt[4] = 1 / self.mass * (Fry + Ffy * cs.cos(steer)) - vx * omega
dxdt[5] = 1 / self.Iz * (Ffy * self.lf * cs.cos(steer) - Fry * self.lr)
return dxdt
def tau_actuator_constraints(ocp, nlp, t, x, u, p, minimal_tau=None):
nq = nlp.mapping["q"].reduce.len
q = [nlp.mapping["q"].expand.map(mx[:nq]) for mx in x]
q_dot = [nlp.mapping["q_dot"].expand.map(mx[nq:]) for mx in x]
min_bound = []
max_bound = []
func = biorbd.to_casadi_func("torqueMax", nlp.model.torqueMax, nlp.q,
nlp.q_dot)
for i in range(len(u)):
bound = func(q[i], q_dot[i])
if minimal_tau:
min_bound.append(nlp.mapping["tau"].reduce.map(
if_else(lt(bound[:, 1], minimal_tau), minimal_tau, bound[:,
1])))
max_bound.append(nlp.mapping["tau"].reduce.map(
if_else(lt(bound[:, 0], minimal_tau), minimal_tau, bound[:,
0])))
else:
min_bound.append(nlp.mapping["tau"].reduce.map(bound[:, 1]))
max_bound.append(nlp.mapping["tau"].reduce.map(bound[:, 0]))
obj = vertcat(*u)
min_bound = vertcat(*min_bound)
max_bound = vertcat(*max_bound)
return (
vertcat(np.zeros(min_bound.shape),
np.ones(max_bound.shape) * -np.inf),
vertcat(obj + min_bound, obj - max_bound),
vertcat(np.ones(min_bound.shape) * np.inf, np.zeros(max_bound.shape)),
)
def test_if_else(self):
with open(os.path.join(TEST_DIR, 'IfElse.mo'), 'r') as f:
txt = f.read()
ast_tree = parser.parse(txt)
casadi_model = gen_casadi.generate(ast_tree, 'IfElse')
ref_model = Model()
x = ca.MX.sym("x")
y1 = ca.MX.sym("y1")
y2 = ca.MX.sym("y2")
y3 = ca.MX.sym("y3")
y_max = ca.MX.sym("y_max")
ref_model.inputs = list(map(Variable, [x]))
ref_model.outputs = list(map(Variable, [y1, y2, y3]))
ref_model.alg_states = list(map(Variable, [y1, y2, y3]))
ref_model.parameters = list(map(Variable, [y_max]))
ref_model.parameters[0].value = 10
ref_model.equations = [
y1 - ca.if_else(x > 0, 1, 0) * y_max,
ca.if_else(
x > 1, ca.vertcat(y3 - 100, y2 - y_max),
ca.if_else(x > 2, ca.vertcat(y3 - 1000, y2 - y_max - 1),
ca.vertcat(y3 - 10000, y2)))
]
self.assert_model_equivalent_numeric(ref_model, casadi_model)
def maximal_tau(nodes: PenaltyNodes, minimal_tau):
nlp = nodes.nlp
nq = nlp.mapping["q"].to_first.len
q = [nlp.mapping["q"].to_second.map(mx[:nq]) for mx in nodes.x]
qdot = [nlp.mapping["qdot"].to_second.map(mx[nq:]) for mx in nodes.x]
min_bound = []
max_bound = []
func = biorbd.to_casadi_func("torqueMax", nlp.model.torqueMax, nlp.q,
nlp.qdot)
for n in range(len(nodes.u)):
bound = func(q[n], qdot[n])
min_bound.append(nlp.mapping["tau"].to_first.map(
if_else(lt(bound[:, 1], minimal_tau), minimal_tau, bound[:, 1])))
max_bound.append(nlp.mapping["tau"].to_first.map(
if_else(lt(bound[:, 0], minimal_tau), minimal_tau, bound[:, 0])))
obj = vertcat(*nodes.u)
min_bound = vertcat(*min_bound)
max_bound = vertcat(*max_bound)
return (
vertcat(np.zeros(min_bound.shape),
np.ones(max_bound.shape) * -np.inf),
vertcat(obj + min_bound, obj - max_bound),
vertcat(np.ones(min_bound.shape) * np.inf, np.zeros(max_bound.shape)),
)
def from_dcm(cls, R):
assert R.shape == (3, 3)
b1 = 0.5 * ca.sqrt(1 + R[0, 0] + R[1, 1] + R[2, 2])
b2 = 0.5 * ca.sqrt(1 + R[0, 0] - R[1, 1] - R[2, 2])
b3 = 0.5 * ca.sqrt(1 - R[0, 0] + R[1, 1] - R[2, 2])
b4 = 0.5 * ca.sqrt(1 - R[0, 0] - R[1, 1] - R[2, 2])
q1 = ca.SX(4, 1)
q1[0] = b1
q1[1] = (R[2, 1] - R[1, 2]) / (4 * b1)
q1[2] = (R[0, 2] - R[2, 0]) / (4 * b1)
q1[3] = (R[1, 0] - R[0, 1]) / (4 * b1)
q2 = ca.SX(4, 1)
q2[0] = (R[2, 1] - R[1, 2]) / (4 * b2)
q2[1] = b2
q2[2] = (R[0, 1] + R[1, 0]) / (4 * b2)
q2[3] = (R[0, 2] + R[2, 0]) / (4 * b2)
q3 = ca.SX(4, 1)
q3[0] = (R[0, 2] - R[2, 0]) / (4 * b3)
q3[1] = (R[0, 1] + R[1, 0]) / (4 * b3)
q3[2] = b3
q3[3] = (R[1, 2] + R[2, 1]) / (4 * b3)
q4 = ca.SX(4, 1)
q4[0] = (R[1, 0] - R[0, 1]) / (4 * b4)
q4[1] = (R[0, 2] + R[2, 0]) / (4 * b4)
q4[2] = (R[1, 2] + R[2, 1]) / (4 * b4)
q4[3] = b4
q = ca.if_else(R[0, 0] > 0, ca.if_else(R[1, 1] > 0, q1, q2),
ca.if_else(R[1, 1] > R[2, 2], q3, q4))
return q
def test_function_call(self):
with open(os.path.join(TEST_DIR, 'FunctionCall.mo'), 'r') as f:
txt = f.read()
ast_tree = parser.parse(txt)
casadi_model = gen_casadi.generate(ast_tree, 'FunctionCall')
print("FunctionCall", casadi_model)
ref_model = Model()
radius = ca.MX.sym('radius')
diameter = radius * 2
circle_properties = ca.Function('circle_properties', [radius], [
3.14159 * diameter, 3.14159 * radius**2,
ca.if_else(3.14159 * radius**2 > 10, 1, 2),
ca.if_else(3.14159 * radius**2 > 10, 10, 3.14159 * radius**2), 8,
3, 12
])
c = ca.MX.sym("c")
a = ca.MX.sym("a")
d = ca.MX.sym("d")
e = ca.MX.sym("e")
S1 = ca.MX.sym("S1")
S2 = ca.MX.sym("S2")
r = ca.MX.sym("r")
ref_model.alg_states = list(map(Variable, [c, a, d, e, S1, S2, r]))
ref_model.outputs = list(map(Variable, [c, a, d, e, S1, S2]))
ref_model.equations = [
ca.vertcat(c, a, d, e, S1, S2) -
ca.vertcat(*circle_properties.call([r])[0:-1])
]
self.assert_model_equivalent_numeric(ref_model, casadi_model)
def __call__(self, x, y):
"""
Evaluate the B-Spline at point (x, y).
The support of this function is the half-open interval [tx[0], tx[-1]) x [ty[0], ty[-1]).
:param x: The coordinate of the point at which to evaluate.
:param y: The ordinate of the point at which to evaluate.
:returns: The spline evaluated at the given point.
"""
z = 0.0
for i in range(len(self.__tx) - self.__kx - 1):
bx = if_else(
logic_and(x >= self.__tx[i],
x <= self.__tx[i + self.__kx + 1]),
self.basis(self.__tx, x, self.__kx, i), 0.0)
for j in range(len(self.__ty) - self.__ky - 1):
by = if_else(
logic_and(y >= self.__ty[j],
y <= self.__ty[j + self.__ky + 1]),
self.basis(self.__ty, y, self.__ky, j), 0.0)
z += self.__w[i *
(len(self.__ty) - self.__ky - 1) + j] * bx * by
return z
def __symbolic_setup(self):
# build composite symbolic expression
self._calc_lengths()
expr = cas.MX.nan(2, 1)
for i in range(len(self._segments) - 1, -1, -1):
s_max = self._fractions[i]
s_min = self._fractions[i - 1] if i > 0 else 0
s_loc = (self._s - s_min) / self._lengths[i]
expr = cas.if_else(self._s <= s_max,
self._segments[i].point(s_loc), expr)
self._expr = expr
self._point = cas.Function('point', [self._s], [self._expr])
dp_ds = cas.jacobian(self._expr, self._s)
# unit tangent vector
self._tangent_expr = cas.if_else(
cas.norm_2(dp_ds) > 0, dp_ds / cas.norm_2(dp_ds), cas.DM([0, 0]))
self._tangent = cas.Function('tangent', [self._s],
[self._tangent_expr])
dt_ds = cas.jacobian(self._tangent_expr, self._s)
# unit normal vector
self._normal_expr = cas.if_else(
cas.norm_2(dt_ds) > 0, dt_ds / cas.norm_2(dt_ds),
cas.vertcat(self._tangent_expr[1], -self._tangent_expr[0]))
self._normal = cas.Function('normal', [self._s], [self._normal_expr])
# curvature value
self._curvature_expr = cas.if_else(
cas.norm_2(dp_ds) > 0,
cas.norm_2(dt_ds) / cas.norm_2(dp_ds), cas.DM(0))
self._curvature = cas.Function('curvature', [self._s],
[self._curvature_expr])
def get_in_tangent_cone_function_multidim(self, cnstr):
"""Returns a casadi function for the SetConstraint instance when the
SetConstraint is multidimensional."""
if not isinstance(cnstr, SetConstraint):
raise TypeError("in_tangent_cone is only available for" +
" SetConstraint")
time_var = self.skill_spec.time_var
robot_var = self.skill_spec.robot_var
list_vars = [time_var, robot_var]
list_names = ["time_var", "robot_var"]
robot_vel_var = self.skill_spec.robot_vel_var
opt_var = [robot_vel_var]
opt_var_names = ["robot_vel_var"]
virtual_var = self.skill_spec.virtual_var
virtual_vel_var = self.skill_spec.virtual_vel_var
input_var = self.skill_spec.input_var
expr = cnstr.expression
set_min = cnstr.set_min
set_max = cnstr.set_max
dexpr = cs.jacobian(expr, time_var)
dexpr += cs.jtimes(expr, robot_var, robot_vel_var)
if virtual_var is not None:
list_vars += [virtual_var]
list_names += ["virtual_var"]
opt_var += [virtual_vel_var]
opt_var_names += ["virtual_vel_var"]
dexpr += cs.jtimes(expr, virtual_var, virtual_vel_var)
if input_var is not None:
list_vars += [input_var]
list_vars += ["input_var"]
le = expr - set_min
ue = expr - set_max
le_good = le >= 1e-12
ue_good = ue <= 1e-12
above = cs.dot(le_good - 1, le_good - 1) == 0
below = cs.dot(ue_good - 1, ue_good - 1) == 0
inside = cs.logic_and(above, below)
out_dir = (cs.sign(le) + cs.sign(ue)) / 2.0
# going_in = cs.dot(out_dir, dexpr) <= 0.0
same_signs = cs.sign(le) == cs.sign(ue)
corner = cs.dot(same_signs - 1, same_signs - 1) == 0
dists = (cs.norm_2(dexpr) + 1e-10) * cs.norm_2(out_dir)
corner_handler = cs.if_else(
cs.dot(out_dir, dexpr) < 0.0,
cs.fabs(cs.dot(-out_dir, dexpr)) / dists < cs.np.cos(cs.np.pi / 4),
False, True)
going_in = cs.if_else(corner, corner_handler,
cs.dot(out_dir, dexpr) < 0.0, True)
in_tc = cs.if_else(
inside, # Are we inside?
True, # Then true.
going_in, # If not, check if we're "going_in"
True)
return cs.Function("in_tc_" + cnstr.label.replace(" ", "_"),
list_vars + opt_var, [in_tc],
list_names + opt_var_names,
["in_tc_" + cnstr.label])
def torque_max_from_actuators(
constraint: Constraint,
all_pn: PenaltyNodeList,
min_torque=None,
):
"""
Non linear maximal values of joint torques computed from the torque-position-velocity relationship
Parameters
----------
constraint: Constraint
The actual constraint to declare
all_pn: PenaltyNodeList
The penalty node elements
min_torque: float
Minimum joint torques. This prevent from having too small torques, but introduces an if statement
"""
# TODO: Add index to select the u (control_idx)
nlp = all_pn.nlp
q = [
nlp.variable_mappings["q"].to_second.map(
mx[nlp.states["q"].index, :]) for mx in all_pn.x
]
qdot = [
nlp.variable_mappings["qdot"].to_second.map(
mx[nlp.states["qdot"].index, :]) for mx in all_pn.x
]
if min_torque and min_torque < 0:
raise ValueError(
"min_torque cannot be negative in tau_max_from_actuators")
func = biorbd.to_casadi_func("torqueMax", nlp.model.torqueMax,
nlp.states["q"].mx,
nlp.states["qdot"].mx)
constraint.min_bound = np.repeat([0, -np.inf], nlp.controls.shape)
constraint.max_bound = np.repeat([np.inf, 0], nlp.controls.shape)
for i in range(len(all_pn.u)):
bound = func(q[i], qdot[i])
if min_torque:
min_bound = nlp.variable_mappings["tau"].to_first.map(
if_else(lt(bound[:, 1], min_torque), min_torque,
bound[:, 1]))
max_bound = nlp.variable_mappings["tau"].to_first.map(
if_else(lt(bound[:, 0], min_torque), min_torque,
bound[:, 0]))
else:
min_bound = nlp.variable_mappings["tau"].to_first.map(
bound[:, 1])
max_bound = nlp.variable_mappings["tau"].to_first.map(
bound[:, 0])
ConstraintFunction.add_to_penalty(
all_pn.ocp, all_pn,
vertcat(
*[all_pn.u[i] + min_bound, all_pn.u[i] - max_bound]),
constraint)
def _get_target_load(self, var: FatigueModel, suffix: str, nlp, controls, index: int):
if self.model_type() not in nlp.controls:
raise NotImplementedError(f"Fatigue dynamics without {self.model_type()} controls is not implemented yet")
val = DynamicsFunctions.get(nlp.controls[f"{self.model_type()}_{suffix}"], controls)[index, :]
if not self.split_controls:
if var.scaling < 0:
val = if_else(lt(val, 0), val, 0)
else:
val = if_else(gt(val, 0), val, 0)
return val / var.scaling
def apply_dynamics(self, target_load, *states):
ma, mr, mf = states
# Implementation of Xia dynamics
c = if_else(
lt(ma, target_load),
if_else(gt(mr, target_load - ma), self.LD * (target_load - ma),
self.LD * mr),
self.LR * (target_load - ma),
)
ma_dot = c - self.F * ma
mr_dot = -c + self.R * mf
mf_dot = self.F * ma - self.R * mf
return vertcat(ma_dot, mr_dot, mf_dot)
def torque_max_from_q_and_qdot(constraint: Constraint,
all_pn: PenaltyNodeList,
min_torque=None):
"""
Non linear maximal values of joint torques computed from the torque-position-velocity relationship
Parameters
----------
constraint: Constraint
The actual constraint to declare
all_pn: PenaltyNodeList
The penalty node elements
min_torque: float
Minimum joint torques. This prevent from having too small torques, but introduces an if statement
"""
nlp = all_pn.nlp
if min_torque and min_torque < 0:
raise ValueError(
"min_torque cannot be negative in tau_max_from_actuators")
bound = nlp.model.torqueMax(nlp.states["q"].mx,
nlp.states["qdot"].mx)
min_bound = BiorbdInterface.mx_to_cx(
"min_bound",
nlp.controls["tau"].mapping.to_first.map(bound[1].to_mx()),
nlp.states["q"],
nlp.states["qdot"],
)
max_bound = BiorbdInterface.mx_to_cx(
"max_bound",
nlp.controls["tau"].mapping.to_first.map(bound[0].to_mx()),
nlp.states["q"],
nlp.states["qdot"],
)
if min_torque:
min_bound = if_else(lt(min_bound, min_torque), min_torque,
min_bound)
max_bound = if_else(lt(max_bound, min_torque), min_torque,
max_bound)
value = vertcat(nlp.controls["tau"].cx + min_bound,
nlp.controls["tau"].cx - max_bound)
n_rows = constraint.rows if constraint.rows else int(
value.shape[0] / 2)
constraint.min_bound = [0] * n_rows + [-np.inf] * n_rows
constraint.max_bound = [np.inf] * n_rows + [0] * n_rows
return value
def find_closest_point_to_triangle(poly, posex, posey):
point1 = find_closest_point_on_line(posex, posey, poly[0], poly[1],
poly[2], poly[3])
point2 = find_closest_point_on_line(posex, posey, poly[2], poly[3],
poly[4], poly[5])
point3 = find_closest_point_on_line(posex, posey, poly[4], poly[5],
poly[0], poly[1])
dist1 = ca.sqrt((posex - point1[0])**2 + (posey - point1[1])**2)
dist2 = ca.sqrt((posex - point2[0])**2 + (posey - point2[1])**2)
dist3 = ca.sqrt((posex - point3[0])**2 + (posey - point3[1])**2)
closest_point = ca.if_else(dist1 < dist2, point1, point2)
distance = ca.if_else(dist1 < dist2, dist1, dist2)
closest_point = ca.if_else(dist3 < distance, point3, closest_point)
return closest_point
def test_attributes(self):
with open(os.path.join(TEST_DIR, 'Attributes.mo'), 'r') as f:
txt = f.read()
ast_tree = parser.parse(txt)
casadi_model = gen_casadi.generate(ast_tree, 'Attributes')
print(casadi_model)
ref_model = CasadiSysModel()
i = ca.MX.sym("int")
b = ca.MX.sym("bool")
r = ca.MX.sym("real")
der_r = ca.MX.sym("der(real)")
i1 = ca.MX.sym("i1")
i2 = ca.MX.sym("i2")
i3 = ca.MX.sym("i3")
i4 = ca.MX.sym("i4")
cst = ca.MX.sym("cst")
prm = ca.MX.sym("prm")
protected_variable = ca.MX.sym("protected_variable")
ref_model.states = [r]
ref_model.der_states = [der_r]
ref_model.alg_states = [i, b, i1, i2, i3, i4, protected_variable]
ref_model.inputs = [i1, i2, i3]
ref_model.outputs = [i4, protected_variable]
ref_model.constants = [cst]
ref_model.constant_values = [1]
ref_model.parameters = [prm]
ref_model.equations = [
i4 - ((i1 + i2) + i3), der_r - (i1 + ca.if_else(b, 1, 0) * i),
protected_variable - (i1 + i2)
]
self.assert_model_equivalent_numeric(ref_model, casadi_model)
def exp(cls, v):
assert v.shape == (3, 1) or v.shape == (3, )
angle = ca.norm_2(v)
res = ca.SX(4, 1)
res[:3] = ca.tan(angle / 4) * v / angle
res[3] = 0
return ca.if_else(angle > eps, res, ca.SX([0, 0, 0, 0]))
def find_closest_point_on_line(posex, posey, startx, starty, endx, endy):
#A to Position = position - line_start
a2px = posex - startx
a2py = posey - starty
#A to B = line_end - line_start
a2bx = endx - startx
a2by = endy - starty
sq_a2b = ca.fmax(0.001, a2bx**2 +
a2by**2) #max, to make sure that we are not deviding by 0
a2p_dot_a2b = a2px * a2bx + a2py * a2by
diff = a2p_dot_a2b / sq_a2b
diff = ca.if_else(diff < 0, 0, diff)
diff = ca.if_else(diff > 1, 1, diff)
#diff = ca.fmax(0, ca.fmin(1, a2p_dot_a2b/sq_a2b))
closest_point = ca.vertcat(startx + a2bx * diff, starty + a2by * diff)
return closest_point
def clean(self, mass, tas, alt, path_angle=0):
"""Compute drag at clean configuration (considering compressibility).
Args:
mass (int or ndarray): Mass of the aircraft (unit: kg).
tas (int or ndarray): True airspeed (unit: kt).
alt (int or ndarray): Altitude (unit: ft).
path_angle (float or ndarray): Path angle (unit: degree). Defaults to 0.
Returns:
int: Total drag (unit: N).
"""
cd0 = self.polar["clean"]["cd0"]
k = self.polar["clean"]["k"]
if self.wave_drag:
mach_crit = self.polar["mach_crit"]
mach = aero.tas2mach(tas * aero.kts, alt * aero.ft)
dCdw = ca.if_else(mach > mach_crit, 20 * (mach - mach_crit)**4, 0)
else:
dCdw = 0
cd0 = cd0 + dCdw
D = self._calc_drag(mass, tas, alt, cd0, k, path_angle)
return D
def integrate_trap(fc,u,t1,t2,N,implementation=2):
grid = np.linspace(t1,t2,N+1)
dt = grid[1] - grid[0]
fs = [fc(u,t) for t in grid] # f on grid
if type(fs[0]) == casadi.SXMatrix:
assert fs[0].numel() == 1
hs = [fabs(f) for f in fs] # absolute values of f on grid
F=0
if implementation==1:
# This formulation theoretically allows short-circuiting, branch prediction
for i in xrange(len(fs)-1):
fa=fs[i]
fb=fs[i+1]
ha=hs(i)
hb=hs(i+1)
samesign=casadi.sign(fa)==casadi.sign(fb)
F=F+casadi.if_else(samesign,trapezoid_area(ha,hb,dt),
bowtie_area(ha,hb,dt))
if implementation==2:
# This formulation theoretically allows use of SIMD (vector) extensions
for i in xrange(len(fs)-1):
fa=fs[i]
fb=fs[i+1]
ha=hs[i]
hb=hs[i+1]
ha_plus_hb=ha+hb
F=F+ha_plus_hb + (fa*fb-ha*hb)/ha_plus_hb
F=F*dt/2
if type(F) == casadi.SXMatrix:
assert F.numel() == 1
return F,grid
def exitIfStatement(self, tree):
logger.debug('exitIfStatement')
# We assume an equal number of statements per branch.
# Furthermore, we assume that every branch assigns to the same variables.
assert (len(tree.statements) % (len(tree.conditions) + 1) == 0)
statements_per_condition = int(
len(tree.statements) / (len(tree.conditions) + 1))
all_assignments = []
for statement_index in range(statements_per_condition):
assignments = self.get_mx(tree.statements[-(statement_index + 1)])
for assignment in assignments:
src = assignment.right
for cond_index in range(len(tree.conditions)):
cond = self.get_mx(tree.conditions[-(cond_index + 1)])
src1 = None
for i in range(statements_per_condition):
other_assignments = self.get_mx(
tree.statements[-statements_per_condition *
(cond_index + 1) - (i + 1)])
for j in range(len(other_assignments)):
if ca.is_equal(assignment.left,
other_assignments[j].left):
src1 = other_assignments[j].right
break
if src1 is not None:
break
src = ca.if_else(cond, src1, src, True)
all_assignments.append(Assignment(assignment.left, src))
self.src[tree] = all_assignments
def asTwist(self):
acosarg = (casadi.trace(self.R) - 1) / 2
theta = casadi.acos(casadi.if_else(casadi.fabs(acosarg) >= 1., 1., acosarg))
w = casadi.horzcat((self.R[2, 1] - self.R[1, 2]),
(self.R[0, 2] - self.R[2, 0]),
(self.R[1, 0] - self.R[0, 1]))
return casadi.horzcat(casadi.reshape(self.t, 1, 3), theta * w / casadi.norm_2(w))
def exitIfStatement(self, tree):
logger.debug('exitIfStatement')
# We assume an equal number of statements per branch.
# Furthermore, we assume that every branch assigns to the same variables.
assert len(set((len(x) for x in tree.blocks))) == 1
# NOTE: We currently assume that we always have an else-clause. This
# is not strictly necessary, see the Modelica Spec on if statements.
assert tree.conditions[-1] == True
expanded_blocks = OrderedDict()
for b in tree.blocks:
block_assignments = []
for s in b:
assignments = self.get_mx(s)
for assignment in assignments:
expanded_blocks.setdefault(assignment.left, []).append(assignment.right)
assert len(set((len(x) for x in expanded_blocks.values()))) == 1
all_assignments = []
for lhs, values in expanded_blocks.items():
# Set default value to else block, and then loop in reverse over all branches
src = values[-1]
for cond, rhs in zip(tree.conditions[-2::-1], values[-2::-1]):
cond = self.get_mx(cond)
src = ca.if_else(cond, rhs, src, True)
all_assignments.append(Assignment(lhs, src))
self.src[tree] = all_assignments
def exitIfStatement(self, tree):
logger.debug('exitIfStatement')
# We assume an equal number of statements per branch.
# Furthermore, we assume that every branch assigns to the same variables.
assert len(set((len(x) for x in tree.blocks))) == 1
# NOTE: We currently assume that we always have an else-clause. This
# is not strictly necessary, see the Modelica Spec on if statements.
assert tree.conditions[-1] == True
expanded_blocks = OrderedDict()
for b in tree.blocks:
block_assignments = []
for s in b:
assignments = self.get_mx(s)
for assignment in assignments:
expanded_blocks.setdefault(assignment.left, []).append(assignment.right)
assert len(set((len(x) for x in expanded_blocks.values()))) == 1
all_assignments = []
for lhs, values in expanded_blocks.items():
# Set default value to else block, and then loop in reverse over all branches
src = values[-1]
for cond, rhs in zip(tree.conditions[-2::-1], values[-2::-1]):
cond = self.get_mx(cond)
src = ca.if_else(cond, rhs, src, True)
all_assignments.append(Assignment(lhs, src))
self.src[tree] = all_assignments
def exit_if(self, tree: etree._Element):
assert len(tree) == 3
cond = self.model[tree[0]]
then_eq = self.model[tree[1]]
else_eq = self.model[tree[2]]
c = self.cond(cond)
if len(then_eq) != len(else_eq):
raise SyntaxError("then and else equations must have same number of statements")
self.model[tree] = ca.if_else(c, then_eq[0], else_eq[0])
def atmosphere():
vt = ca.MX.sym('vt')
alt = ca.MX.sym('alt')
R0 = 2.377e-3
Tfac = 1 - 0.703e-5*alt
T = ca.if_else(alt > 35000, 390, 519*Tfac)
rho = R0*(Tfac**(4.14))
tables['amach'] = ca.Function('amach', [vt, alt], [vt/(ca.sqrt(1.4*1716.3*T))], ['vt', 'alt'], ['amach'])
tables['qbar'] = ca.Function('qbar', [vt, alt], [0.5*rho*vt**2], ['vt', 'alt'], ['qbar'])
tables['ps'] = ca.Function('qbar', [alt], [1715*rho*T], ['alt'], ['amach'])
def exit_if(self, tree: etree._Element):
assert len(tree) == 3
cond = self.model[tree[0]]
then_eq = self.model[tree[1]]
else_eq = self.model[tree[2]]
c = self.cond(cond)
if len(then_eq) != len(else_eq):
raise SyntaxError(
"then and else equations must have same number of statements")
self.model[tree] = ca.if_else(c, then_eq[0], else_eq[0])
def log(cls, q):
assert q.shape == (4, 1) or q.shape == (4, )
v = ca.SX(3, 1)
norm_q = ca.norm_2(q)
theta = 2 * ca.acos(q[0])
c = ca.sin(theta / 2)
v[0] = theta * q[1] / c
v[1] = theta * q[2] / c
v[2] = theta * q[3] / c
return ca.if_else(ca.fabs(c) > eps, v, ca.SX([0, 0, 0]))
def from_mrp(cls, r):
assert r.shape == (4, 1) or r.shape == (4, )
a = r[:3]
q = ca.SX(4, 1)
n_sq = ca.dot(a, a)
den = 1 + n_sq
q[0] = (1 - n_sq) / den
for i in range(3):
q[i + 1] = 2 * a[i] / den
return ca.if_else(r[3], -q, q)
def where(
condition,
value_if_true,
value_if_false,
):
if not is_casadi_type([condition, value_if_true, value_if_false],
recursive=True):
return _onp.where(condition, value_if_true, value_if_false)
else:
return _cas.if_else(condition, value_if_true, value_if_false)
def if_eq_zero(condition, if_result, else_result):
"""
A short expression which can be compiled quickly.
:type condition: Union[float, Symbol]
:type if_result: Union[float, Symbol]
:type else_result: Union[float, Symbol]
:return: if_result if condition == 0 else else_result
:rtype: Union[float, Symbol]
"""
return ca.if_else(condition, else_result, if_result)
def smooth_track(track):
N = len(track)
xs = track[:, 0]
ys = track[:, 1]
th1_f = ca.MX.sym('y1_f', N-2)
th2_f = ca.MX.sym('y2_f', N-2)
x_f = ca.MX.sym('x_f', N)
y_f = ca.MX.sym('y_f', N)
real = ca.Function('real', [th1_f, th2_f], [ca.cos(th1_f)*ca.cos(th2_f) + ca.sin(th1_f)*ca.sin(th2_f)])
im = ca.Function('im', [th1_f, th2_f], [-ca.cos(th1_f)*ca.sin(th2_f) + ca.sin(th1_f)*ca.cos(th2_f)])
sub_cmplx = ca.Function('a_cpx', [th1_f, th2_f], [ca.atan(im(th1_f, th2_f)/real(th1_f, th2_f))])
d_th = ca.Function('d_th', [x_f, y_f], [ca.if_else(ca.fabs(x_f[1:] - x_f[:-1]) < 0.01 ,ca.atan((y_f[1:] - y_f[:-1])/(x_f[1:] - x_f[:-1])), 10000)])
x = ca.MX.sym('x', N)
y = ca.MX.sym('y', N)
th = ca.MX.sym('th', N-1)
B = 5
nlp = {\
'x': ca.vertcat(x, y, th),
'f': ca.sumsqr(sub_cmplx(th[1:], th[:-1])) + B* (ca.sumsqr(x-xs) + ca.sumsqr(y-ys)),
# 'f': B* (ca.sumsqr(x-xs) + ca.sumsqr(y-ys)),
'g': ca.vertcat(\
th - d_th(x, y),
x[0] - xs[0], y[0]- ys[0],
x[-1] - xs[-1], y[-1]- ys[-1],
)\
}
S = ca.nlpsol('S', 'ipopt', nlp)
# th0 = [lib.get_bearing(track[i, 0:2], track[i+1, 0:2]) for i in range(N-1)]
th0 = d_th(xs, ys)
x0 = ca.vertcat(xs, ys, th0)
lbx = [0] *2* N + [-np.pi]*(N -1)
ubx = [100] *2 * N + [np.pi]*(N-1)
r = S(x0=x0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)
print(f"Solution found")
x_opt = r['x']
xs_new = np.array(x_opt[:N])
ys_new = np.array(x_opt[N:2*N])
track[:, 0] = xs_new[:, 0]
track[:, 1] = ys_new[:, 0]
return track
def thrust():
power = ca.MX.sym('power')
alt = ca.MX.sym('alt')
rmach = ca.MX.sym('rmach')
tidl = tables['thrust_idle'](alt, rmach)
tmil = tables['thrust_mil'](alt, rmach)
tmax = tables['thrust_max'](alt, rmach)
thrust = ca.if_else(power < 50, tidl + (tmil - tidl) * power * 0.02,
tmil + (tmax - tmil) * (power - 50) * 0.02)
return ca.Function('thrust', [power, alt, rmach], [thrust],
['power', 'alt', 'mach'], ['thrust'])
def exp(cls, v):
assert v.shape == (3, 1) or q.shape == (3, )
q = ca.SX(4, 1)
theta = ca.norm_2(v)
q[0] = ca.cos(theta / 2)
c = ca.sin(theta / 2)
n = ca.norm_2(v)
q[1] = c * v[0] / n
q[2] = c * v[1] / n
q[3] = c * v[2] / n
return ca.if_else(n > eps, q, ca.SX([1, 0, 0, 0]))
def get_h_i_t():
w_i, alpha_i, beta_i, b_i, c_i, mu_i, v_i, h_tm1_agg_i, delta_t_agg_i = casadi.SX.sym('w_i'), casadi.SX.sym('alpha_i'), casadi.SX.sym('beta_i'), casadi.SX.sym('b_i'), casadi.SX.sym('c_i'), casadi.SX.sym('mu_i'), casadi.SX.sym('v_i'), casadi.SX.sym('h_tm1_agg_i'), casadi.SX.sym('delta_t_agg_i'),
boolM = mu_i > 0
sqrthtpowmu = casadi.sqrt(h_tm1_agg_i) ** mu_i
zTrue = (w_i + alpha_i * sqrthtpowmu * f_i(delta_t_agg_i, b_i, c_i) ** v_i + beta_i * sqrthtpowmu) ** (2./mu_i)
sqrtht = casadi.sqrt(h_tm1_agg_i)
zFalse= (casadi.exp(w_i + alpha_i * f_i(delta_t_agg_i, b_i, c_i) ** v_i + beta_i * casadi.log(sqrtht)) ** (2.))
z = casadi.if_else(boolM, zTrue, zFalse)
return casadi.Function('h_i_t',
[w_i, alpha_i, beta_i, b_i, c_i, mu_i, v_i, h_tm1_agg_i, delta_t_agg_i],
[z])
def exitIfExpression(self, tree):
logger.debug('exitIfExpression')
assert (len(tree.conditions) + 1 == len(tree.expressions))
src = self.get_mx(tree.expressions[-1])
for cond_index in range(len(tree.conditions)):
cond = self.get_mx(tree.conditions[-(cond_index + 1)])
expr1 = self.get_mx(tree.expressions[-(cond_index + 2)])
src = ca.if_else(cond, expr1, src, True)
self.src[tree] = src
def exitIfEquation(self, tree):
logger.debug('exitIfEquation')
# Check if every equation block contains the same number of equations
if len(set((len(x) for x in tree.blocks))) != 1:
raise Exception("Every branch in an if-equation needs the same number of equations.")
# NOTE: We currently assume that we always have an else-clause. This
# is not strictly necessary, see the Modelica Spec on if equations.
assert tree.conditions[-1] == True
src = ca.vertcat(*[self.get_mx(e) for e in tree.blocks[-1]])
for cond_index in range(1, len(tree.conditions)):
cond = self.get_mx(tree.conditions[-(cond_index + 1)])
expr1 = ca.vertcat(*[self.get_mx(e) for e in tree.blocks[-(cond_index + 1)]])
src = ca.if_else(cond, expr1, src, True)
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
def exit_classDefinition(self, tree: etree._Element): # noqa: too-complex
dae = HybridDae()
dae.t = self.scope['time']
self.model[tree] = dae
# handle component declarations
for var_name, v in self.scope['var'].items():
variability = self.scope['prop'][var_name]['variability']
if variability == 'continuous':
if var_name in self.scope['states']:
dae.x = ca.vertcat(dae.x, v)
dae.dx = ca.vertcat(dae.dx, self.der(v))
else:
dae.y = ca.vertcat(dae.y, v)
elif variability == 'discrete':
dae.m = ca.vertcat(dae.m, v)
elif variability == 'parameter':
dae.p = ca.vertcat(dae.p, v)
elif variability == 'constant':
dae.p = ca.vertcat(dae.p, v)
else:
raise ValueError('unknown variability', variability)
for eq in self.scope['eqs']:
if isinstance(eq, self.sym):
dae.f_x = ca.vertcat(dae.f_x, eq)
# build reinit expression and discrete equations
dae.f_i = dae.x
dae.f_m = dae.m
for eq in self.scope['when_eqs']:
w = eq['cond']
for then_eq in eq['then']:
if isinstance(then_eq, tuple):
if then_eq[0] == 'reinit':
sub_var = then_eq[1]
sub_expr = ca.if_else(self.edge(w), then_eq[2], sub_var)
dae.f_i = ca.substitute(dae.f_i, sub_var, sub_expr)
elif isinstance(then_eq, self.sym):
# this is a discrete variable assignment
# so it should be a casadi subtraction y = x
assert then_eq.is_op(ca.OP_SUB) and then_eq.n_dep() == 2
sub_var = then_eq.dep(0)
sub_expr = ca.if_else(self.edge(w), then_eq.dep(1), sub_var)
dae.f_m = ca.substitute(dae.f_m, sub_var, sub_expr)
dae.t = self.scope['time']
dae.prop.update(self.scope['prop'])
c_dict = self.scope['c']
for k in c_dict.keys():
dae.c = ca.vertcat(dae.c, k)
dae.pre_c = ca.vertcat(dae.pre_c, self.pre_cond(k))
dae.f_c = ca.vertcat(dae.f_c, c_dict[k])
for l, r in [('f_c', 'c'), ('c', 'pre_c'), ('dx', 'x'), ('f_m', 'm')]:
vl = getattr(dae, l)
vr = getattr(dae, r)
if vl.shape != vr.shape:
raise ValueError(
'{:s} and {:s} must have the same shape:'
'\n{:s}: {:s}\t{:s}: {:s}'.format(
l, r, l, str(dae.f_m), r, str(dae.m)))
dae.ng = ca.vertcat(*self.scope['ng'])
dae.nu = ca.vertcat(*self.scope['nu'])
n_eq = dae.f_x.shape[0] + dae.f_m.shape[0]
n_var = dae.x.shape[0] + dae.m.shape[0] + dae.y.shape[0]
if n_eq != n_var:
raise ValueError(
'must have equal number of equations '
'{:d} and unknowns {:d}\n:{:s}'.format(
n_eq, n_var, str(dae)))
self.scope_stack.pop()
