diff --git a/paper_results/secVd_control_gain_optimization/README.md b/paper_results/secVd_control_gain_optimization/README.md index 1eb9cecb..32bb71ef 100644 --- a/paper_results/secVd_control_gain_optimization/README.md +++ b/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 diff --git a/paper_results/secVd_control_gain_optimization/code/control_gain_optimization_with_collocated.py b/paper_results/secVd_control_gain_optimization/code/control_gain_optimization_with_collocated.py index 18a79030..30426252 100644 --- a/paper_results/secVd_control_gain_optimization/code/control_gain_optimization_with_collocated.py +++ b/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( diff --git a/src/soromox/control/pid_control.py b/src/soromox/control/pid_control.py index 161382b1..fa44318b 100644 --- a/src/soromox/control/pid_control.py +++ b/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( diff --git a/tests/control/test_pid_control.py b/tests/control/test_pid_control.py index e1a7fe91..39803e66 100644 --- a/tests/control/test_pid_control.py +++ b/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]]),