diff --git a/python/mujoco/sysid/_src/optimize.py b/python/mujoco/sysid/_src/optimize.py index 65227355..c36b2b4d 100644 --- a/python/mujoco/sysid/_src/optimize.py +++ b/python/mujoco/sysid/_src/optimize.py @@ -179,6 +179,24 @@ def optimize( extras={}, ) + # Warn if any non-frozen parameter component starts at (or essentially at) + # a box bound. Optimization can stall in that corner on ill-conditioned or + # rank-deficient problems; both the mujoco and scipy backends are affected. + lo, hi = bounds + rng = hi - lo + safe_rng = np.where(rng > 0, rng, 1.0) + at_bound = ((x0 - lo) <= 1e-3 * safe_rng) | ((hi - x0) <= 1e-3 * safe_rng) + at_bound &= rng > 0 + if at_bound.any(): + logging.warning( + "%d of %d non-frozen parameter components start at (or essentially " + "at) a box bound. Optimization can stall in that corner on " + "ill-conditioned problems; consider calling " + "initial_params.move_off_bounds() before optimize().", + int(at_bound.sum()), + x0.size, + ) + def optimized_residual_fn(x): residuals, _, _ = residual_fn(x, opt_params) return np.concatenate(residuals) diff --git a/python/mujoco/sysid/_src/parameter.py b/python/mujoco/sysid/_src/parameter.py index b5e7d46a..d6d0b621 100644 --- a/python/mujoco/sysid/_src/parameter.py +++ b/python/mujoco/sysid/_src/parameter.py @@ -139,6 +139,19 @@ class Parameter: rng = np.random.default_rng() return rng.uniform(self.min_value.flatten(), self.max_value.flatten()) + def move_off_bound(self, fraction: float = 0.05) -> None: + """Shift values within 0.1% of a bound to ``lo + fraction*(hi-lo)`` (or + symmetric for the upper bound). Interior components are unchanged.""" + lo, hi = self.get_bounds() + rng = hi - lo + safe_rng = np.where(rng > 0, rng, 1.0) + v = self.as_vector().copy() + at_lo = (v - lo) <= 1e-3 * safe_rng + at_hi = (hi - v) <= 1e-3 * safe_rng + v = np.where(at_lo, lo + fraction * rng, v) + v = np.where(at_hi, hi - fraction * rng, v) + self.update_from_vector(v) + def __str__(self) -> str: """Return a string representation of the parameter.""" if self.size == 1: @@ -293,6 +306,15 @@ class ParameterDict: param.update_from_vector(vector[start : start + size]) start += size + def move_off_bounds(self, fraction: float = 0.05) -> Self: + """Call :meth:`Parameter.move_off_bound` on every non-frozen parameter, + returning ``self`` so calls can be chained before + :func:`optimize`.""" + for param in self.parameters.values(): + if not param.frozen: + param.move_off_bound(fraction=fraction) + return self + def save_to_disk(self, path: str | pathlib.Path) -> None: """Save the parameter dictionary to disk (schema and data). diff --git a/python/mujoco/sysid/tests/test_parameter.py b/python/mujoco/sysid/tests/test_parameter.py index 069a771f..9274c1b2 100644 --- a/python/mujoco/sysid/tests/test_parameter.py +++ b/python/mujoco/sysid/tests/test_parameter.py @@ -146,3 +146,38 @@ def test_frozen_param_excluded(): np.testing.assert_array_equal(params["free"].value, [1.5]) # Frozen param unchanged. np.testing.assert_array_equal(params["frozen"].value, [5.0]) + + +def test_move_off_bound(): + """At-bound (or essentially at-bound) values shift inward; interior is untouched.""" + # At lower bound, at upper bound, essentially-at-lower, interior, vector mixed, + # custom fraction, and degenerate (zero-range) bounds. + cases = [ + (0.0, 0.0, 20.0, 0.05, 1.0), # at lower -> 0.05 * 20 + (1e-8, 0.0, 20.0, 0.05, 1.0), # essentially at lower + (20.0, 0.0, 20.0, 0.05, 19.0), # at upper -> 20 - 0.05 * 20 + (5.0, 0.0, 20.0, 0.05, 5.0), # interior unchanged + (0.0, 0.0, 100.0, 0.1, 10.0), # custom fraction + (3.0, 3.0, 3.0, 0.05, 3.0), # zero-range bound, pinned + ] + for nominal, lo, hi, fraction, expected in cases: + p = parameter.Parameter("d", nominal, lo, hi) + p.move_off_bound(fraction=fraction) + np.testing.assert_allclose(p.value, [expected]) + + # Vector parameter: only at-bound components are shifted. + p = parameter.Parameter( + "v", [0.0, 5.0, 10.0], [0.0, 0.0, 0.0], [10.0, 10.0, 10.0] + ) + p.move_off_bound() + np.testing.assert_allclose(p.value, [0.5, 5.0, 9.5]) + + +def test_move_off_bounds_dict_skips_frozen_and_returns_self(): + """ParameterDict shifts free params, leaves frozen alone, returns self.""" + pdict = parameter.ParameterDict() + pdict.add(parameter.Parameter("free", 0.0, 0.0, 1.0)) + pdict.add(parameter.Parameter("frozen", 0.0, 0.0, 1.0, frozen=True)) + assert pdict.move_off_bounds() is pdict + np.testing.assert_allclose(pdict["free"].value, [0.05]) + np.testing.assert_allclose(pdict["frozen"].value, [0.0])