优化求解器重试并校正AMESim机械端口

app/simulation/solvers/algebraic.py

@@ -1,5 +1,7 @@
 from __future__ import annotations
 
+from collections.abc import Callable
+
 from dataclasses import dataclass
 from math import expm1, isfinite, log, sqrt
 
@@ -32,11 +34,22 @@ class AlgebraicUnknown:
         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[tuple[AlgebraicUnknown, float], ...]
+    anchors: tuple[EffortAnchor, ...]
 
 
 @dataclass(frozen=True)
@@ -85,6 +98,11 @@ class PressureFlowSolver:
         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, ...]:
@@ -132,7 +150,7 @@ class PressureFlowSolver:
         for variable in ("p", "x", "v"):
             self._seed_equal_effort(variable)
 
-    def _effort_equality_groups(
+    def _build_effort_equality_groups(
         self,
         variable: str,
     ) -> tuple[EffortEqualityGroup, ...]:
@@ -195,10 +213,7 @@ class PressureFlowSolver:
                 effort_unknowns[endpoint]
             )
 
-        anchors_by_root: dict[
-            tuple[str, str],
-            list[tuple[AlgebraicUnknown, float]],
-        ] = {}
+        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":
@@ -215,11 +230,11 @@ class PressureFlowSolver:
                     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)
+                    EffortAnchor(
+                        unknown=unknown,
+                        evaluate=self._equation_value_reader(equation),
+                    )
                 )
 
         return tuple(
@@ -232,9 +247,17 @@ class PressureFlowSolver:
         )
 
     def _seed_equal_effort(self, variable: str) -> None:
-        for group in self._effort_equality_groups(variable):
+        for group in self._effort_groups[variable]:
             members = group.members
-            anchors = group.anchors
+            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.
@@ -490,9 +513,9 @@ class PressureFlowSolver:
 
         gap0 = float(getattr(component, "gap0", 0.0))
         if binding.algebraic_port == 1:
-            target = component.port_2.x - gap0 - penetration
+            target = component.port_2.x + gap0 + penetration
         else:
-            target = component.port_1.x + gap0 + penetration
+            target = component.port_1.x - gap0 - penetration
         for unknown in binding.algebraic_group.members:
             unknown.write(target)
         component.set_causal_contact(
@@ -515,7 +538,7 @@ class PressureFlowSolver:
 
         position_groups = {
             unknown.id: group
-            for group in self._effort_equality_groups("x")
+            for group in self._effort_groups["x"]
             for unknown in group.members
         }
         bindings: list[UnilateralContactBinding] = []
@@ -547,7 +570,7 @@ class PressureFlowSolver:
                     algebraic_group=first_group,
                     neighbor_force=first_neighbor,
                     algebraic_port=1,
-                    force_sign=1.0,
+                    force_sign=-1.0,
                 )
             elif not second_group.anchors and second_neighbor is not None:
                 binding = UnilateralContactBinding(
@@ -555,7 +578,7 @@ class PressureFlowSolver:
                     algebraic_group=second_group,
                     neighbor_force=second_neighbor,
                     algebraic_port=2,
-                    force_sign=-1.0,
+                    force_sign=1.0,
                 )
             else:
                 # With both coordinates state-owned, penetration is a dynamic
@@ -574,137 +597,135 @@ class PressureFlowSolver:
 
         return tuple(bindings)
 
-    def _solve_explicit_flow_unknowns(self) -> set[str]:
-        """Directly evaluate explicit flow variables before nonlinear closure.
+    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"
+        )
 
-        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.
+    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}."
+            )
 
-        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.
-        """
+        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()
 
-        # 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)
+        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)
 
-        # 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"
-                ]
+                flow_unknowns = self._flow_unknowns_for_equation(equation)
                 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)
+                if unknown.id not in seeded_ids:
+                    append_assignment(equation, unknown)
 
-        # 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.
+        equations = self.network.pressure_flow_equation_residuals()
         while True:
             propagated = False
-            for equation in self.network.pressure_flow_equation_residuals():
+            for equation in equations:
                 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"
-                ]
+                flow_unknowns = self._flow_unknowns_for_equation(equation)
                 if not flow_unknowns:
                     continue
-                variable_names = {unknown.variable for unknown in flow_unknowns}
-                if len(variable_names) != 1:
+                if len({unknown.variable for unknown in flow_unknowns}) != 1:
                     continue
-                unseeded = [
-                    unknown for unknown in flow_unknowns if unknown.id not in seeded_ids
-                ]
+                unseeded = tuple(
+                    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):
+                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
-                unknown.write(target_value)
-                seeded_ids.add(unknown.id)
+                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:
-                # 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 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]: