Tune collocated integral-error saturation to tendon scale

paper_results/secVd_control_gain_optimization/README.md

@@ -4,6 +4,37 @@ This case compares gain optimization for collocated actuation-space control and
 synergistic operational-space control. Each generator writes a MAT result under
 its controller-specific directory in `data/`; the plotter combines both files.
 
+> [!WARNING]
+> The committed Section Vd MAT files and derived plots are stale. In particular,
+> the collocated results were generated with the former, non-unit-preserving
+> integral-error saturation and the former `gamma=10` setting. Do not treat the
+> current files as canonical results. Regenerate the complete Section Vd results
+> only after the saturation-scale change has been merged and the other open
+> control-gain-optimization issues (#128 and #129) have been fixed.
+
+## Collocated integral-error saturation
+
+The collocated controller integrates tendon-length errors, all expressed in
+meters. It uses the unit-preserving saturation
+
+```text
+sat(e) = tanh(gamma * e) / gamma,
+gamma = 1 / e_sat.
+```
+
+The committed legacy trajectories were inspected to select a physical error
+scale. The undeformed tendon lengths are approximately `-100 mm`, and the
+setpoints are `[-85.49, -96.41, -100.05] mm`. This produces initial errors of
+`[14.51, 3.59, -0.05] mm`. Across the saved initial and optimized trajectories,
+all other initial/transient absolute errors remain below `3.85 mm`.
+
+The default is therefore `e_sat = 10 mm`, or `gamma = 100 1/m`. At the exceptional
+`14.51 mm` reference step, saturation reduces the integral-error derivative by
+about 38%. At `3.85 mm`, the reduction is only about 4.7%, keeping normal
+closed-loop errors nearly linear while limiting integral accumulation during the
+large step. Override the physical scale, if needed, with
+`--integral-error-saturation-scale` in meters.
+
 Run the two optimizations without GUI rendering:
 
 ```bash

paper_results/secVd_control_gain_optimization/code/control_gain_optimization_with_collocated.py

@@ -1,4 +1,5 @@
 import argparse
+import math
 import pickle
 import time
 from pathlib import Path
@@ -46,6 +47,16 @@ def parse_args() -> argparse.Namespace:
         help="Directory for optional diagnostic figures.",
     )
     parser.add_argument("--num-iters", type=int, default=3)
+    parser.add_argument(
+        "--integral-error-saturation-scale",
+        type=float,
+        default=1e-2,
+        help=(
+            "Tendon-length error scale in meters for tanh integral-error "
+            "saturation. Gamma is its reciprocal (default: 0.01 m, i.e. "
+            "gamma = 100 1/m)."
+        ),
+    )
     parser.add_argument("--save-figures", action="store_true")
     parser.add_argument("--no-show", action="store_true")
     parser.add_argument("--no-render", action="store_true")
@@ -58,6 +69,13 @@ def parse_args() -> argparse.Namespace:
 OUTPUTS_DIR = ARGS.output_dir.resolve()
 if ARGS.num_iters < 1:
     raise ValueError("--num-iters must be at least 1")
+if (
+    not math.isfinite(ARGS.integral_error_saturation_scale)
+    or ARGS.integral_error_saturation_scale <= 0.0
+):
+    raise ValueError(
+        "--integral-error-saturation-scale must be finite and strictly positive"
+    )
 for output_name in ("optimization_results.mat", "animation_data.pkl"):
     output_path = RESULT_DIR / output_name
     if output_path.exists() and not ARGS.force:
@@ -238,12 +256,20 @@ def evaluate_closed_loop_system(
 Ki = 5e0 * jnp.ones((num_actuators,))
 Kd = 1e0 * jnp.ones((num_actuators,))
 
+# The committed legacy trajectories start from tendon lengths of -100 mm and
+# target [-85.49, -96.41, -100.05] mm. The largest initial error is 14.51 mm,
+# whereas all other observed initial/transient errors remain below 3.85 mm.
+# A 10 mm error scale therefore limits integral accumulation during the large
+# reference step while leaving the ordinary error regime nearly linear.
+tendon_error_saturation_scale = ARGS.integral_error_saturation_scale  # [m]
+tendon_error_gamma = 1.0 / tendon_error_saturation_scale  # [1/m]
+
 pid_control = PIDControl(
     Kp=Kp,
     Ki=Ki,
     Kd=Kd,
     saturation_fn="tanh",
-    gamma=10.0,
+    gamma=tendon_error_gamma,
 )
 
 controller = PotentialCompensationRegulator(

src/soromox/control/pid_control.py

@@ -118,10 +118,10 @@ def __init__(
                   preserves the units and small-error slope of ``e``.
                 - A callable `f(e) -> saturated_e`: Custom saturation function.
             gamma: Inverse error scale for built-in saturation functions. Can be
-                a positive scalar, positive diagonal vector, or symmetric
-                positive-definite matrix. For a scalar or vector, ``1 / gamma``
-                sets the componentwise saturation magnitude. Only used when
-                ``saturation_fn="tanh"``. Defaults to 1.0.
+                a finite positive scalar, finite positive diagonal vector, or
+                finite symmetric positive-definite matrix. For a scalar or vector,
+                ``1 / gamma`` sets the componentwise saturation magnitude. Only
+                used when ``saturation_fn="tanh"``. Defaults to 1.0.
         """
         self.Kp = jnp.atleast_1d(jnp.asarray(Kp))
         self.Ki = jnp.atleast_1d(jnp.asarray(Ki))
@@ -132,6 +132,8 @@ def __init__(
             self._saturation_fn_name = "identity"
             self._custom_saturation_fn = None
         elif saturation_fn == "tanh":
+            if not bool(jnp.all(jnp.isfinite(self.gamma))):
+                raise ValueError("Gamma must be finite for tanh saturation.")
             if self.gamma.ndim == 1:
                 if bool(jnp.any(self.gamma <= 0)):
                     raise ValueError(

tests/control/test_pid_control.py

@@ -285,6 +285,26 @@ def test_tanh_gamma_must_be_strictly_positive(self, gamma):
                 gamma=gamma,
             )
 
+    @pytest.mark.parametrize(
+        "gamma",
+        [
+            jnp.inf,
+            -jnp.inf,
+            jnp.nan,
+            jnp.array([1.0, jnp.inf]),
+            jnp.array([[1.0, 0.0], [0.0, jnp.nan]]),
+        ],
+    )
+    def test_tanh_gamma_must_be_finite(self, gamma):
+        with pytest.raises(ValueError, match="must be finite"):
+            PIDControl(
+                Kp=1.0,
+                Ki=0.5,
+                Kd=0.1,
+                saturation_fn="tanh",
+                gamma=gamma,
+            )
+
     def test_matrix_gamma_must_be_symmetric_positive_definite(self):
         for gamma in (
             jnp.array([[1.0, 1.0], [0.0, 1.0]]),