from __future__ import annotations from collections.abc import Callable from dataclasses import dataclass from math import expm1, isfinite, log, sqrt from app.simulation.core.ports import PortState, VariableRole from app.simulation.systems.network import SimulationNetwork class AlgebraicSolveError(RuntimeError): def __init__(self, message: str, diagnostics: "AlgebraicSolveDiagnostics") -> None: super().__init__(message) self.diagnostics = diagnostics @dataclass(frozen=True) class AlgebraicUnknown: component: str port: str variable: str role: VariableRole state: PortState @property def id(self) -> str: return f"{self.component}.{self.port}.{self.variable}" def read(self) -> float: return float(getattr(self.state, self.variable)) def write(self, value: float) -> None: setattr(self.state, self.variable, float(value)) @dataclass(frozen=True) class ExplicitFlowAssignment: equation_id: str unknown: AlgebraicUnknown evaluate: Callable[[], float] @dataclass(frozen=True) class EffortAnchor: unknown: AlgebraicUnknown evaluate: Callable[[], float] @dataclass(frozen=True) class EffortEqualityGroup: variable: str members: tuple[AlgebraicUnknown, ...] anchors: tuple[EffortAnchor, ...] @dataclass(frozen=True) class UnilateralContactBinding: component: object algebraic_group: EffortEqualityGroup neighbor_force: AlgebraicUnknown algebraic_port: int force_sign: float @dataclass(frozen=True) class AlgebraicSolveDiagnostics: success: bool message: str evaluations: int pressure_scale: float flow_scale: float max_scaled_residual: float max_raw_residual: float def as_dict(self) -> dict[str, object]: return { "success": self.success, "message": self.message, "evaluations": self.evaluations, "pressureScale": self.pressure_scale, "flowScale": self.flow_scale, "maxScaledResidual": self.max_scaled_residual, "maxRawResidual": self.max_raw_residual, } class PressureFlowSolver: """Solve the acausal pressure-flow subsystem for a compiled network.""" def __init__( self, network: SimulationNetwork, *, residual_tolerance: float = 1e-7, max_evaluations: int = 500, ) -> None: self.network = network self.residual_tolerance = residual_tolerance self.max_evaluations = max_evaluations self.unknowns = self._build_unknowns() self._unknowns_by_id = {unknown.id: unknown for unknown in self.unknowns} self._effort_groups = { variable: self._build_effort_equality_groups(variable) for variable in ("p", "x", "v") } self._explicit_flow_plan = self._build_explicit_flow_plan() self.last_diagnostics: AlgebraicSolveDiagnostics | None = None def _build_unknowns(self) -> tuple[AlgebraicUnknown, ...]: unknowns: list[AlgebraicUnknown] = [] for component in self.network.components.values(): for definition in component.port_definitions: if definition.kind != "physical": continue state = component.get_port(definition.name) for variable in definition.variables: if variable.role not in {"effort", "flow"}: continue unknowns.append( AlgebraicUnknown( component=component.name, port=definition.name, variable=variable.name, role=variable.role, state=state, ) ) return tuple(unknowns) @staticmethod def _port_key(variable: str, expected_variable: str) -> tuple[str, str] | None: try: component_name, port_name, variable_name = variable.rsplit(".", 2) except ValueError: return None if variable_name != expected_variable: return None return component_name, port_name def _seed_equal_efforts(self) -> None: """Lift state-owned efforts across their complete equality groups. Dynamic components refresh their own ports before each closure, while connected algebraic ports retain values from the preceding RHS evaluation. State equations expose the current effort as ``port.variable - target``; use that target as the authoritative anchor for every connected/equal pressure, displacement, and velocity port before evaluating explicit flow laws. """ for variable in ("p", "x", "v"): self._seed_equal_effort(variable) def _build_effort_equality_groups( self, variable: str, ) -> tuple[EffortEqualityGroup, ...]: effort_unknowns = { (unknown.component, unknown.port): unknown for unknown in self.unknowns if unknown.variable == variable } if not effort_unknowns: return () parent = {key: key for key in effort_unknowns} def find(key: tuple[str, str]) -> tuple[str, str]: root = key while parent[root] != root: root = parent[root] while parent[key] != key: next_key = parent[key] parent[key] = root key = next_key return root def union(first: tuple[str, str], second: tuple[str, str]) -> None: first_root = find(first) second_root = find(second) if first_root != second_root: parent[second_root] = first_root for connection in self.network.connections: if connection.kind != "physical": continue first = connection.endpoint_a.key second = connection.endpoint_b.key if first in effort_unknowns and second in effort_unknowns: union(first, second) component_equations = { component.name: component.pressure_flow_equation_residuals() for component in self.network.components.values() } for equations in component_equations.values(): for equation in equations: if equation.relation != "equal" or equation.role != "effort": continue endpoints = [ endpoint for equation_variable in equation.variables if ( (endpoint := self._port_key(equation_variable, variable)) in effort_unknowns ) ] for endpoint in endpoints[1:]: union(endpoints[0], endpoint) members_by_root: dict[tuple[str, str], list[AlgebraicUnknown]] = {} for endpoint in effort_unknowns: members_by_root.setdefault(find(endpoint), []).append( effort_unknowns[endpoint] ) anchors_by_root: dict[tuple[str, str], list[EffortAnchor]] = {} for equations in component_equations.values(): for equation in equations: if equation.relation != "state" or equation.role != "effort": continue endpoints = [ endpoint for equation_variable in equation.variables if ( (endpoint := self._port_key(equation_variable, variable)) in effort_unknowns ) ] if len(endpoints) != 1: continue endpoint = endpoints[0] unknown = effort_unknowns[endpoint] anchors_by_root.setdefault(find(endpoint), []).append( EffortAnchor( unknown=unknown, evaluate=self._equation_value_reader(equation), ) ) return tuple( EffortEqualityGroup( variable=variable, members=tuple(members), anchors=tuple(anchors_by_root.get(root, ())), ) for root, members in members_by_root.items() ) def _seed_equal_effort(self, variable: str) -> None: for group in self._effort_groups[variable]: members = group.members anchors = tuple( (anchor.unknown, anchor.unknown.read() - anchor.evaluate()) for anchor in group.anchors ) anchors = tuple( (unknown, value) for unknown, value in anchors if isfinite(value) ) if anchors: # Keep each state-owned port current even when an invalid model # has conflicting anchors in one equality group. for unknown, target_value in anchors: unknown.write(target_value) anchor_values = [value for _unknown, value in anchors] effort_scale = max([abs(value) for value in anchor_values] + [1.0]) if max(anchor_values) - min(anchor_values) > 1.0e-9 * effort_scale: # A conflicting multi-storage group is structurally invalid; # leave it for the residual solver/preparation diagnostics. continue target_value = sum(anchor_values) / len(anchor_values) for unknown in members: unknown.write(target_value) continue if variable == "p": seed = next( ( unknown.read() for unknown in members if unknown.read() > 0.0 ), None, ) if seed is None: continue else: seed = members[0].read() for unknown in members: if variable != "p" or unknown.read() <= 0.0: unknown.write(seed) def _connected_flow_unknown( self, component_name: str, port_name: str, variable: str, ) -> AlgebraicUnknown | None: endpoint_key = (component_name, port_name) for connection in self.network.connections: if connection.kind != "physical": continue if connection.endpoint_a.key == endpoint_key: other = connection.endpoint_b elif connection.endpoint_b.key == endpoint_key: other = connection.endpoint_a else: continue return self._unknowns_by_id.get( f"{other.component}.{other.port}.{variable}" ) return None @staticmethod def _bisect_contact_root( value_at, lower: float, upper: float, target: float, ) -> float | None: lower_value = float(value_at(lower)) - target upper_value = float(value_at(upper)) - target tolerance = 1.0e-13 * max(abs(target), 1.0) if abs(lower_value) <= tolerance: return lower if abs(upper_value) <= tolerance: return upper if not isfinite(lower_value) or not isfinite(upper_value): return None if (lower_value < 0.0) == (upper_value < 0.0): return None for _iteration in range(100): middle = 0.5 * (lower + upper) middle_value = float(value_at(middle)) - target if abs(middle_value) <= tolerance: return middle if (lower_value < 0.0) == (middle_value < 0.0): lower = middle lower_value = middle_value else: upper = middle upper_value = middle_value return 0.5 * (lower + upper) def _contact_penetration_for_force( self, component, requested_force: float, current_penetration: float, ) -> float | None: """Invert one LSTP force law and select the root nearest its current state.""" if not isfinite(requested_force): return None option = int(getattr(component, "discContactOption", 2.0)) if option != 1: requested_force = max(requested_force, 0.0) stiffness = max(float(getattr(component, "kcont", 0.0)), 0.0) damping = max(float(getattr(component, "rcont", 0.0)), 0.0) damping_length = float(getattr(component, "Pdis", 0.0)) relative_velocity = float(getattr(component, "penetration_velocity")) damping_term = damping * relative_velocity current_penetration = ( max(float(current_penetration), 0.0) if isfinite(current_penetration) else 0.0 ) force_tolerance = 1.0e-12 * max(abs(requested_force), 1.0) def raw_force(penetration: float) -> float: if penetration <= 0.0: return 0.0 damping_fraction = ( -expm1(-penetration / damping_length) if damping_length > 0.0 else 1.0 ) return stiffness * penetration + damping_term * damping_fraction def contact_force(penetration: float) -> float: force = raw_force(penetration) return force if option == 1 else max(force, 0.0) candidates: list[float] = [] def add_candidate(penetration: float | None) -> None: if penetration is None or not isfinite(penetration) or penetration < 0.0: return if abs(contact_force(penetration) - requested_force) > force_tolerance: return if not any( abs(penetration - candidate) <= 1.0e-12 * max(abs(penetration), abs(candidate), 1.0e-18) for candidate in candidates ): candidates.append(penetration) add_candidate(current_penetration) add_candidate(0.0) if option != 1 and requested_force == 0.0: return min( candidates or [0.0], key=lambda penetration: abs(penetration - current_penetration), ) if damping_length <= 0.0: if stiffness > 0.0: penetration = (requested_force - damping_term) / stiffness if penetration > 0.0: add_candidate(penetration) elif abs(requested_force - damping_term) <= force_tolerance: add_candidate(max(current_penetration, 1.0e-18)) elif stiffness > 0.0: critical_penetration: float | None = None if damping_term < -stiffness * damping_length: critical_penetration = damping_length * log( -damping_term / (stiffness * damping_length) ) add_candidate(critical_penetration) upper = max( current_penetration, damping_length, abs(requested_force) / stiffness, critical_penetration or 0.0, 1.0e-18, ) for _iteration in range(100): upper_value = raw_force(upper) if isfinite(upper_value) and upper_value >= requested_force: break upper *= 2.0 else: upper = float("nan") if isfinite(upper): if critical_penetration is not None: add_candidate( self._bisect_contact_root( raw_force, 0.0, critical_penetration, requested_force, ) ) add_candidate( self._bisect_contact_root( raw_force, critical_penetration, upper, requested_force, ) ) else: add_candidate( self._bisect_contact_root( raw_force, 0.0, upper, requested_force, ) ) elif damping_term != 0.0: upper = max(current_penetration, damping_length, 1.0e-18) for _iteration in range(100): upper_value = raw_force(upper) crossed = ( upper_value >= requested_force if damping_term > 0.0 else upper_value <= requested_force ) if isfinite(upper_value) and crossed: add_candidate( self._bisect_contact_root( raw_force, 0.0, upper, requested_force, ) ) break upper *= 2.0 if not candidates: return None return min( candidates, key=lambda penetration: abs(penetration - current_penetration), ) def _apply_unilateral_contact_binding( self, binding: UnilateralContactBinding, ) -> bool: component = binding.component requested_force = binding.force_sign * binding.neighbor_force.read() if int(getattr(component, "discContactOption", 2.0)) != 1: requested_force = max(requested_force, 0.0) cached_penetration = getattr(component, "_causal_penetration", None) penetration = self._contact_penetration_for_force( component, requested_force, ( float(cached_penetration) if cached_penetration is not None else float(getattr(component, "penetration")) ), ) if penetration is None: component.clear_causal_contact() return False gap0 = float(getattr(component, "gap0", 0.0)) if binding.algebraic_port == 1: target = component.port_2.x + gap0 + penetration else: target = component.port_1.x - gap0 - penetration for unknown in binding.algebraic_group.members: unknown.write(target) component.set_causal_contact( penetration=penetration, force=requested_force, ) return True def _refresh_unilateral_contacts( self, bindings: tuple[UnilateralContactBinding, ...], ) -> None: for binding in bindings: self._apply_unilateral_contact_binding(binding) def _seed_unilateral_contacts( self, ) -> tuple[UnilateralContactBinding, ...]: """Create local eliminations for contacts with one algebraic coordinate.""" position_groups = { unknown.id: group for group in self._effort_groups["x"] for unknown in group.members } bindings: list[UnilateralContactBinding] = [] bound_group_ids: set[int] = set() for component in self.network.components.values(): if component.model_type != "amesim_lstp00a": continue first_neighbor = self._connected_flow_unknown( component.name, "port_1", "f", ) second_neighbor = self._connected_flow_unknown( component.name, "port_2", "f", ) first_group = position_groups.get(f"{component.name}.port_1.x") second_group = position_groups.get(f"{component.name}.port_2.x") if ( first_group is None or second_group is None or first_group is second_group ): continue if not first_group.anchors and first_neighbor is not None: binding = UnilateralContactBinding( component=component, algebraic_group=first_group, neighbor_force=first_neighbor, algebraic_port=1, force_sign=-1.0, ) elif not second_group.anchors and second_neighbor is not None: binding = UnilateralContactBinding( component=component, algebraic_group=second_group, neighbor_force=second_neighbor, algebraic_port=2, force_sign=1.0, ) else: # With both coordinates state-owned, penetration is a dynamic # result rather than an algebraic active-set choice. continue group_id = id(binding.algebraic_group) if group_id in bound_group_ids: # One relative contact law may eliminate a free coordinate. # Any other contact sharing that coordinate must remain in the # nonlinear system or the projections would overwrite each # other and make root selection order-dependent. continue if self._apply_unilateral_contact_binding(binding): bindings.append(binding) bound_group_ids.add(group_id) return tuple(bindings) def _flow_unknowns_for_equation(self, equation) -> tuple[AlgebraicUnknown, ...]: return tuple( self._unknowns_by_id[variable] for variable in equation.variables if variable in self._unknowns_by_id and self._unknowns_by_id[variable].role == "flow" ) def _equation_value_reader(self, equation) -> Callable[[], float]: if equation.owner == "connection": if len(equation.variables) != 2: raise ValueError( f"Connection equation {equation.id} must contain two variables." ) first = self._unknowns_by_id[equation.variables[0]] second = self._unknowns_by_id[equation.variables[1]] if equation.relation == "sumToZero": return lambda: first.read() + second.read() if equation.relation == "equal": return lambda: first.read() - second.read() raise ValueError( f"Unsupported connection equation relation: {equation.relation}." ) component = self.network.components[equation.owner_id] equation_id = equation.id def read_component_equation() -> float: for current in component.pressure_flow_equation_residuals(): if current.id == equation_id: return float(current.value) raise RuntimeError( f"Compiled algebraic equation disappeared at runtime: {equation_id}." ) return read_component_equation def _build_explicit_flow_plan(self) -> tuple[ExplicitFlowAssignment, ...]: """Compile the legacy deterministic flow assignment order once.""" assignments: list[ExplicitFlowAssignment] = [] seeded_ids: set[str] = set() def append_assignment(equation, unknown: AlgebraicUnknown) -> None: assignments.append( ExplicitFlowAssignment( equation_id=equation.id, unknown=unknown, evaluate=self._equation_value_reader(equation), ) ) seeded_ids.add(unknown.id) for component in self.network.components.values(): for equation in component.pressure_flow_equation_residuals(): if equation.relation != "constitutive" or equation.role != "flow": continue flow_unknowns = self._flow_unknowns_for_equation(equation) if len(flow_unknowns) != 1: continue unknown = flow_unknowns[0] if unknown.id not in seeded_ids: append_assignment(equation, unknown) equations = self.network.pressure_flow_equation_residuals() while True: propagated = False for equation in equations: if equation.role != "flow" or equation.relation not in { "constitutive", "sumToZero", }: continue flow_unknowns = self._flow_unknowns_for_equation(equation) if not flow_unknowns: continue if len({unknown.variable for unknown in flow_unknowns}) != 1: continue unseeded = tuple( unknown for unknown in flow_unknowns if unknown.id not in seeded_ids ) if len(unseeded) != 1: continue append_assignment(equation, unseeded[0]) propagated = True break if propagated: continue for equation in equations: if equation.role != "flow" or equation.relation not in { "constitutive", "sumToZero", }: continue flow_unknowns = self._flow_unknowns_for_equation(equation) unseeded = tuple( unknown for unknown in flow_unknowns if unknown.id not in seeded_ids ) if len(unseeded) <= 1: continue if len({unknown.variable for unknown in flow_unknowns}) != 1: continue append_assignment(equation, unseeded[-1]) propagated = True break if not propagated: break return tuple(assignments) def _solve_explicit_flow_unknowns(self) -> set[str]: """Execute the precompiled explicit flow/force causalization plan.""" for unknown in self.unknowns: if unknown.variable == "f": unknown.write(0.0) seeded_ids: set[str] = set() for assignment in self._explicit_flow_plan: target_value = assignment.unknown.read() - assignment.evaluate() if not isfinite(target_value): continue assignment.unknown.write(target_value) seeded_ids.add(assignment.unknown.id) return seeded_ids def _scales(self) -> dict[str, float]: pressure_scale = max( [ abs(unknown.read()) for unknown in self.unknowns if unknown.variable == "p" and unknown.read() > 0.0 ] + [1e5] ) estimated_flows = [ abs(float(getattr(component, "K_eff"))) * sqrt(pressure_scale) for component in self.network.components.values() if hasattr(component, "K_eff") ] mass_flow_scale = max( estimated_flows + [ abs(unknown.read()) for unknown in self.unknowns if unknown.variable == "m_flow" ] + [1e-3] ) return { "p": pressure_scale, "m_flow": mass_flow_scale, "x": max( [abs(unknown.read()) for unknown in self.unknowns if unknown.variable == "x"] + [1.0] ), "v": max( [abs(unknown.read()) for unknown in self.unknowns if unknown.variable == "v"] + [1.0] ), "f": max( [abs(unknown.read()) for unknown in self.unknowns if unknown.variable == "f"] + [1.0] ), } def solve(self) -> AlgebraicSolveDiagnostics: try: import numpy as np from scipy.optimize import least_squares except ImportError as exc: raise RuntimeError( "Topology-driven simulation requires SciPy; install requirements.txt." ) from exc for component in self.network.components.values(): clear_causal_contact = getattr(component, "clear_causal_contact", None) if clear_causal_contact is not None: clear_causal_contact() self._seed_equal_efforts() self._solve_explicit_flow_unknowns() contact_bindings = self._seed_unilateral_contacts() if contact_bindings: self._solve_explicit_flow_unknowns() self._refresh_unilateral_contacts(contact_bindings) scales = self._scales() pressure_scale = scales["p"] flow_scale = scales["m_flow"] unknown_scales = { unknown.id: ( max(abs(unknown.read()), 1.0) if unknown.variable == "f" else scales.get(unknown.variable, max(abs(unknown.read()), 1.0)) ) for unknown in self.unknowns } positive_pressures = [ unknown.read() for unknown in self.unknowns if unknown.variable == "p" and unknown.read() > 0.0 ] fallback_pressure = ( sum(positive_pressures) / len(positive_pressures) if positive_pressures else pressure_scale ) def variable_scale(unknown: AlgebraicUnknown) -> float: return unknown_scales[unknown.id] seeded_equations = self.network.pressure_flow_equation_residuals() def initial_equation_scale(equation) -> float: variable_names = [ variable.rsplit(".", 1)[-1] for variable in equation.variables ] if equation.role == "flow": force_scales = [ unknown_scales[variable] for variable in equation.variables if variable in self._unknowns_by_id and self._unknowns_by_id[variable].variable == "f" ] if force_scales: # Freeze force scaling per equation. A 1e17 N source must # not hide an unrelated 40 N piston/contact imbalance in a # different mechanical branch. return max(force_scales + [abs(float(equation.value)), 1.0]) return flow_scale if equation.role == "effort": if "x" in variable_names: return scales["x"] if "v" in variable_names: return scales["v"] return pressure_scale return max([scales.get(name, 1.0) for name in variable_names] + [1.0]) equation_scales = { equation.id: initial_equation_scale(equation) for equation in seeded_equations } def equation_scale(equation) -> float: return equation_scales.get(equation.id, initial_equation_scale(equation)) seeded_scaled = [ abs(equation.value / equation_scale(equation)) for equation in seeded_equations ] seeded_max_scaled_residual = max(seeded_scaled, default=0.0) seeded_unknown_values = [ (unknown, unknown.read()) for unknown in self.unknowns ] seeded_unknowns_are_feasible = all( isfinite(value) and (unknown.variable != "p" or value >= 1.0) for unknown, value in seeded_unknown_values ) if ( seeded_unknowns_are_feasible and all(isfinite(value) for value in seeded_scaled) and seeded_max_scaled_residual <= self.residual_tolerance ): diagnostics = AlgebraicSolveDiagnostics( success=True, message="Seeded pressure-flow state satisfies the residual tolerance.", evaluations=0, pressure_scale=pressure_scale, flow_scale=flow_scale, max_scaled_residual=seeded_max_scaled_residual, max_raw_residual=max( (abs(item.value) for item in seeded_equations), default=0.0, ), ) self.last_diagnostics = diagnostics return diagnostics # A causal contact retains its small relative penetration around the # current absolute port coordinates. Keep that local coordinate during # nonlinear fallback: the contact law remains responsive to optimizer # increments, while a sub-ULP penetration is not lost by subtracting two # large absolute displacements. x0 = np.asarray( [ ( unknown.read() if unknown.variable != "p" or unknown.read() > 0.0 else fallback_pressure ) / variable_scale(unknown) for unknown in self.unknowns ], dtype=float, ) lower = np.asarray( [ 1.0 / pressure_scale if unknown.variable == "p" else -np.inf for unknown in self.unknowns ] ) upper = np.full(len(self.unknowns), np.inf) def assign(values) -> None: for unknown, value in zip(self.unknowns, values): unknown.write(float(value) * variable_scale(unknown)) def scaled_residuals(values): assign(values) self._refresh_unilateral_contacts(contact_bindings) equations = self.network.pressure_flow_equation_residuals() return np.asarray( [ equation.value / equation_scale(equation) for equation in equations ], dtype=float, ) result = least_squares( scaled_residuals, x0, bounds=(lower, upper), x_scale="jac", ftol=1e-10, xtol=1e-10, gtol=1e-10, max_nfev=self.max_evaluations, ) assign(result.x) self._refresh_unilateral_contacts(contact_bindings) equations = self.network.pressure_flow_equation_residuals() scaled = [ abs( equation.value / equation_scale(equation) ) for equation in equations ] max_scaled_residual = max(scaled, default=0.0) residuals_converged = ( all(isfinite(value) for value in scaled) and max_scaled_residual <= self.residual_tolerance ) optimizer_status_is_acceptable = bool(result.success) or int(result.status) == 0 success = residuals_converged and optimizer_status_is_acceptable diagnostics = AlgebraicSolveDiagnostics( success=success, message=str(result.message), evaluations=int(result.nfev), pressure_scale=pressure_scale, flow_scale=flow_scale, max_scaled_residual=max_scaled_residual, max_raw_residual=max((abs(item.value) for item in equations), default=0.0), ) self.last_diagnostics = diagnostics if not success: raise AlgebraicSolveError( "Pressure-flow equations did not converge to the requested tolerance.", diagnostics, ) return diagnostics