from __future__ import annotations 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 EffortEqualityGroup: variable: str members: tuple[AlgebraicUnknown, ...] anchors: tuple[tuple[AlgebraicUnknown, float], ...] @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.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 _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[tuple[AlgebraicUnknown, float]], ] = {} 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] target_value = unknown.read() - float(equation.value) if not isfinite(target_value): continue anchors_by_root.setdefault(find(endpoint), []).append( (unknown, target_value) ) 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_equality_groups(variable): members = group.members anchors = group.anchors 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_equality_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 _solve_explicit_flow_unknowns(self) -> set[str]: """Directly evaluate explicit flow variables before nonlinear closure. Component constitutive equations use the normalized residual form ``flow_unknown + remainder = 0`` whenever exactly one physical flow variable is present. Solve those relations by substitution first, then propagate the known values through component balances and physical connectors. This covers pneumatic ``m_flow`` variables as well as mechanical forces ``f`` such as ``FORC`` without asking the nonlinear optimizer to discover values many orders of magnitude away from zero. The remaining coupled equations still go through ``least_squares``; these assignments provide both a consistent initial guess and the nominal magnitudes used to scale that smaller nonlinear problem. """ seeded_ids: set[str] = set() # Mechanical reaction balances can contain null-space forces. Reusing # an arbitrary least-squares distribution from the preceding RHS call # makes contact activation history-dependent, so choose deterministic # zero tear values and rebuild the force chain from current signals, # states, and pressure loads on every closure. for unknown in self.unknowns: if unknown.variable == "f": unknown.write(0.0) # First evaluate constitutive relations that expose one flow unknown # with unit coefficient. Other variables in the equation (pressure, # displacement, velocity, or a signal) have already been refreshed for # the current state and time by the staged system closure. 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._unknowns_by_id[variable] for variable in equation.variables if variable in self._unknowns_by_id and self._unknowns_by_id[variable].role == "flow" ] if len(flow_unknowns) != 1: continue unknown = flow_unknowns[0] if unknown.id in seeded_ids: continue target_value = unknown.read() - float(equation.value) if not isfinite(target_value): continue unknown.write(target_value) seeded_ids.add(unknown.id) # V1/correctness-first implementation: repeatedly solve any balance that # now has exactly one unknown flow variable left. Rebuilding and # rescanning the complete residual tuple after every assignment keeps # propagation deterministic, but costs O(flow unknowns * equations) and # can dominate long, stiff simulations. A production follow-up should # compile the assignment/tear order from the static topology once and # evaluate only each owning component or connection residual here. while True: propagated = False for equation in self.network.pressure_flow_equation_residuals(): if equation.role != "flow" or equation.relation not in { "constitutive", "sumToZero", }: continue flow_unknowns = [ 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" ] if not flow_unknowns: continue variable_names = {unknown.variable for unknown in flow_unknowns} if len(variable_names) != 1: continue unseeded = [ unknown for unknown in flow_unknowns if unknown.id not in seeded_ids ] if len(unseeded) != 1: continue unknown = unseeded[0] target_value = unknown.read() - float(equation.value) if not isfinite(target_value): continue unknown.write(target_value) seeded_ids.add(unknown.id) propagated = True break if not propagated: # Causalize one remaining free flow in an otherwise normalized # linear balance. This is the algebraic equivalent of choosing # a tear variable: the other free flows retain their current # guesses and one dependent flow closes the equation exactly. # It also gives rank-deficient rigid-body reaction balances a # deterministic starting point before state reduction supplies # their common acceleration. for equation in self.network.pressure_flow_equation_residuals(): if equation.role != "flow" or equation.relation not in { "constitutive", "sumToZero", }: continue flow_unknowns = [ 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" ] unseeded = [ 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 unknown = unseeded[-1] target_value = unknown.read() - float(equation.value) if not isfinite(target_value): continue unknown.write(target_value) seeded_ids.add(unknown.id) propagated = True break if not propagated: break 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