Corrected problem with proper tracking of initial conditions when setting goals in Control

Author: eisDNV

examples/DrivingForce.fmu

@@ -59,7 +59,7 @@ def __init__(
                     l_rng[i] = val  # type: ignore[reportArgumentType] ## l_rng is not empty # fixed display value
                 else:
                     assert isinstance(r, (str, int, bool, float, Enum)), f"Found type {type(r)}"
-                    check, q = unit.compatible(r, no_unit=False, strict=True)  # q in base units
+                    check, q = unit.compatible(r, no_unit=False, strict=False)  # q in base units
                     if not check:
                         raise ValueError(f"Provided range {rng}[{i}] is not conformant with unit {unit}") from None
                     assert isinstance(q, (int, bool, float)), "Unexpected type {type(q)} in {rng}[{i}]"

examples/DrivingForce6D.fmu

@@ -19,62 +19,6 @@ class RW(Protocol):
     def __call__(self, val: float | None = None, /) -> float: ...
 
 
-class Controls(object):
-    """Keep track of float variable changes.
-
-    limit_err: Determines how limit errors are dealt with.
-      Anything below critical sets the value to the limit and provides a logger message.
-      Critical leads to a program run error.
-    """
-
-    limit_err: int = logging.WARNING
-
-    def __init__(
-        self,
-        limit_err: int = logging.WARNING,
-    ):
-        Controls.limit_err = limit_err
-        self.controls: list["Control"] = []
-
-    @property
-    def nogoals(self):
-        for crl in self.controls:
-            if len(crl.goal):
-                return False
-        return True
-
-    def extend(self, crls: tuple["Control", ...]):
-        for crl in crls:
-            self.append(crl)
-
-    def append(self, crl: "Control"):
-        """Append one or several Control object(s)."""
-        for c in self.controls:
-            if c.name == crl.name:
-                raise KeyError(f"Control with name {c.name} already exists. Choose a unique name.") from None
-        self.controls.append(crl)
-
-    def __getitem__(self, ident: int | str):
-        """Get the control object identified by ident (index within .controls or valid name)."""
-        if isinstance(ident, str):
-            for crl in self.controls:
-                if crl.name == ident:
-                    return crl
-            raise KeyError(f"Control with name {ident} not found.") from None
-        elif isinstance(ident, int):
-            if ident < 0 or ident >= len(self.controls):
-                raise ValueError(f"Control {ident} does not exist within set of controls.") from None
-            return self.controls[ident]
-        else:
-            raise TypeError(f"Integer expected as subscript. Found {ident}") from None
-
-    def step(self, time: float, dt: float):
-        """Step towards the goals (if goals are set)."""
-        if not self.nogoals:
-            for crl in self.controls:
-                crl.step(time, dt)
-
-
 class Control(object):
     """Keep track of the changes of a single float variable, avoiding discontinuities and abrupt changes.
 
@@ -194,7 +138,7 @@ def check_limit(self, order: int, value: float) -> float:
                 return lim
         return value
 
-    def setgoal(self, order: int, value: float | None, speed: float | None = 0.0, acc: float | None = 0.0):
+    def setgoal(self, order: int, value: float | None, speed: float | None = None, acc: float | None = None):
         """Set a new goal for the control, i.e. set the required time-acceleration sequence
         to reach value with all derivatives = 0.0.
 
@@ -285,7 +229,6 @@ def setgoal(self, order: int, value: float | None, speed: float | None = 0.0, ac
                             (t0 + dt1 + dt2 + dt3, acc2),
                             (float("inf"), 0.0),
                         ]
-        logger.info(f"New goal({self.name}): {self.goal}")
         self.started = False
 
     @property
@@ -299,6 +242,7 @@ def step(self, time: float, dt: float):
             if not self.started:  # not yet started. Need to add current time.
                 for i, (t, a) in enumerate(self.goal):
                     self.goal[i] = (t + time, a)
+                logger.info(f"@{time}. New goal({self.name}): {self.goal}")
                 self.started = True
             _t, self.acc = self.goal[0]
             if time > _t:  # move to correct goal entry
@@ -317,4 +261,61 @@ def step(self, time: float, dt: float):
                     _t, self.acc = self.goal[0]
 
             if np.isinf(_t) and abs(self.acc) < 1e-12 and abs(self.speed) < 1e-12:
+                logger.info(f"@{time}. Goal {self.name} finalized.")
                 self.goal = []
+
+
+class Controls(object):
+    """Keep track of float variable changes.
+
+    limit_err: Determines how limit errors are dealt with.
+      Anything below critical sets the value to the limit and provides a logger message.
+      Critical leads to a program run error.
+    """
+
+    limit_err: int = logging.WARNING
+
+    def __init__(
+        self,
+        limit_err: int = logging.WARNING,
+    ):
+        Controls.limit_err = limit_err
+        self.controls: list[Control] = []
+
+    @property
+    def nogoals(self):
+        for crl in self.controls:
+            if len(crl.goal):
+                return False
+        return True
+
+    def append(self, crl: "Control"):
+        """Append one or several Control object(s)."""
+        for c in self.controls:
+            if c.name == crl.name:
+                raise KeyError(f"Control with name {c.name} already exists. Choose a unique name.") from None
+        self.controls.append(crl)
+
+    def extend(self, crls: tuple[Control, ...]):
+        for crl in crls:
+            self.append(crl)
+
+    def __getitem__(self, ident: int | str):
+        """Get the control object identified by ident (index within .controls or valid name)."""
+        if isinstance(ident, str):
+            for crl in self.controls:
+                if crl.name == ident:
+                    return crl
+            raise KeyError(f"Control with name {ident} not found.") from None
+        elif isinstance(ident, int):
+            if ident < 0 or ident >= len(self.controls):
+                raise ValueError(f"Control {ident} does not exist within set of controls.") from None
+            return self.controls[ident]
+        else:
+            raise TypeError(f"Integer expected as subscript. Found {ident}") from None
+
+    def step(self, time: float, dt: float):
+        """Step towards the goals (if goals are set)."""
+        if not self.nogoals:
+            for crl in self.controls:
+                crl.step(time, dt)

examples/HarmonicOscillator.fmu

@@ -1,5 +1,6 @@
 import logging
 from functools import partial
+from typing import Sequence
 
 import matplotlib.pyplot as plt
 import numpy as np
@@ -71,7 +72,17 @@ def __call__(self, v: float | None = None) -> float:
 
 
 def test_goal(show: bool = False):
-    def do_goal(order: int, value: float, current: np.ndarray | None = None, t_end: float = 10.0):
+    def do_goal(
+        order: int,
+        value: float,
+        current: np.ndarray | None = None,
+        t_end: float = 10.0,
+        change: tuple[float, int, float]|None = None,
+    ):
+        if change is not None:
+            t1, order1, val1 = change
+        else:
+            t1 = float("inf")
         time = 0.0
         if current is not None:
             _b[0].rw(current[0])
@@ -83,6 +94,8 @@ def do_goal(order: int, value: float, current: np.ndarray | None = None, t_end:
         assert len(_b[0].goal)
         res.append((time, _b[0].rw(), _b[0].speed, _b[0].acc))
         while time + dt < t_end:
+            if change is not None and abs(time - t1) < dt / 2:
+                _b["len"].setgoal(order1, val1)
             _b.step(time, dt)
             time += dt
             res.append((time, *_b[0].current))
@@ -99,6 +112,12 @@ def boom_setter(newval: float | None = None, idx: int = 0):
             boom[idx] = newval
             return newval
 
+    def i_time(res: Sequence[tuple[float, ...]], time: float, eps: float = 0.0001):
+        for i in range(len(res)):
+            if abs(res[i][0] - time) < eps:
+                return i
+        raise KeyError(f"Time {time} not found in table")
+
     _b = Controls(limit_err=logging.CRITICAL)
     _b.append(Control("len", ((-100.0, 90.0), None, (-1.0, 0.5)), partial(boom_setter, idx=0)))
     # fixed velocity and the acceleration limits are not needed:
@@ -131,7 +150,15 @@ def boom_setter(newval: float | None = None, idx: int = 0):
     # set an acceleration (non-zero position and velocity)
     res = do_goal(2, -0.1, current=np.array((10.0, 1.0, 0.0), float), t_end=2.01)
     expected = (10 + 1.0 * 2.0 - 0.5 * 0.1 * 2.0**2, 1.0 - 0.1 * 2.0, -0.1)
-    assert np.allclose(_b[0].current, expected), f"{_b[0].current} != {expected} "
+    assert np.allclose(_b[0].current, expected), f"{_b[0].current} != {expected}."
+
+    # set a speed and overwrite it after a time with a new value
+    res = do_goal(1, 0.45, current=np.array((10.0, 0, 0), float), t_end=10.0, change=(5.0, 1, 0.0))
+    i_acc = i_time(res, 0.45 / 0.5)
+    assert np.allclose(res[i_acc], (0.45 / 0.5, 10 + 0.5 * 0.5 * (0.45 / 0.5) ** 2, 0.45, 0.5)), (
+        f"After acc.: {res[i_acc]}"
+    )
+    assert np.allclose(res[-1][1:], (12.14875, 0, 0)), f"Found end state {res[-1]}"
 
     Controls.limit_err = logging.WARNING  # allow corrections from now on