969 lines
35 KiB
Python
969 lines
35 KiB
Python
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
|