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 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()