[SymForce] Fixed default epsilon

With this, we add two methods to `symforce.sympy`:
- `sm.default_epsilon`, and
- `sm.set_default_epsilon`.

With these, the user can set the value returned by
`sm.default_epsilon` to be any value they'd like using
`sm.set_default_epsilon`. Currently, if the user doesn't
set the default epsilon, I default it to `0.0` (this could
be trivially changed though).

`sm.set_default_epsilon` can only be called if the `sm.default_epsilon`
hasn't yet been called (raises an `AlreadyUsedDefaultEpsilon`
exception). This is to ensure that the default epsilons are consistent
between different functions (as I imagine it would lead to confusing
behavior if that weren't enforced).

Topic: fixed_default_epsilon
Relative: use_default_epsilon
GitOrigin-RevId: 73757807f84f4e9430d84740b3fdfe8a6a479421

Author: bradley-solliday-skydio

symforce/initialization.py

@@ -37,6 +37,7 @@ def modify_symbolic_api(sympy_module: T.Any) -> None:
     override_count_ops(sympy_module)
     override_matrix_symbol(sympy_module)
     add_derivatives(sympy_module)
+    add_epsilon_functions(sympy_module)
 
     setattr(sympy_module, "_MODIFIED_BY_SYMFORCE", True)
 
@@ -300,38 +301,77 @@ def override_count_ops(sympy_module: T.Type) -> None:
         sympy_module.count_ops = _sympy_count_ops.count_ops
 
 
-def add_custom_methods(sympy_module: T.Type) -> None:
+class AlreadyUsedEpsilon(Exception):
+    pass
+
+
+def add_epsilon_functions(sympy_module: T.Type) -> None:
     """
-    Add safe helper methods to the symbolic API.
+    Add sympy_module.epsilon() and associated functions.
+
+    Precondition: This function can be called on a given module only once.
     """
 
-    def register(func: T.Callable) -> T.Callable:
-        setattr(sympy_module, func.__name__, func)
-        return func
-
-    # Should match C++ default epsilon in epsilon.h
-    sympy_module.numeric_epsilon = 10 * sys.float_info.epsilon
+    EPSILON = 0.0
+    HAVE_USED_EPSILON = False
 
     # Save original functions to reference in wrappers
     original_atan2 = sympy_module.atan2
 
-    @register
+    def add_custom_methods_with_epsilon(sympy_module: T.Type, default_epsilon: T.Scalar) -> None:
+        def atan2(y: T.Scalar, x: T.Scalar, epsilon: T.Scalar = default_epsilon) -> T.Scalar:
+            return original_atan2(y, x + (sympy_module.sign(x) + 0.5) * epsilon)
+
+        sympy_module.atan2 = atan2
+
+        def asin_safe(x: T.Scalar, epsilon: T.Scalar = default_epsilon) -> T.Scalar:
+            x_safe = sympy_module.Max(-1 + epsilon, sympy_module.Min(1 - epsilon, x))
+            return sympy_module.asin(x_safe)
+
+        sympy_module.asin_safe = asin_safe
+
+        def acos_safe(x: T.Scalar, epsilon: T.Scalar = default_epsilon) -> T.Scalar:
+            x_safe = sympy_module.Max(-1 + epsilon, sympy_module.Min(1 - epsilon, x))
+            return sympy_module.acos(x_safe)
+
+        sympy_module.acos_safe = acos_safe
+
+    add_custom_methods_with_epsilon(sympy_module, EPSILON)
+
     def epsilon() -> T.Scalar:
-        return 0
+        nonlocal HAVE_USED_EPSILON
+        HAVE_USED_EPSILON = True
+        return EPSILON
 
-    @register
-    def atan2(y: T.Scalar, x: T.Scalar, epsilon: T.Scalar = epsilon()) -> T.Scalar:
-        return original_atan2(y, x + (sympy_module.sign(x) + 0.5) * epsilon)
+    sympy_module.epsilon = epsilon
 
-    @register
-    def asin_safe(x: T.Scalar, epsilon: T.Scalar = epsilon()) -> T.Scalar:
-        x_safe = sympy_module.Max(-1 + epsilon, sympy_module.Min(1 - epsilon, x))
-        return sympy_module.asin(x_safe)
+    def set_epsilon(new_epsilon: T.Scalar) -> None:
+        nonlocal HAVE_USED_EPSILON
+        if HAVE_USED_EPSILON:
+            raise AlreadyUsedEpsilon(
+                "Cannot set return value of epsilon after it has already been called."
+            )
 
-    @register
-    def acos_safe(x: T.Scalar, epsilon: T.Scalar = epsilon()) -> T.Scalar:
-        x_safe = sympy_module.Max(-1 + epsilon, sympy_module.Min(1 - epsilon, x))
-        return sympy_module.acos(x_safe)
+        # NOTE(brad): These must be redefined so they use new_epsilon
+        add_custom_methods_with_epsilon(sympy_module, default_epsilon=new_epsilon)
+
+        nonlocal EPSILON
+        EPSILON = new_epsilon
+
+    sympy_module.set_epsilon = set_epsilon
+
+
+def add_custom_methods(sympy_module: T.Type) -> None:
+    """
+    Add safe helper methods to the symbolic API.
+    """
+
+    def register(func: T.Callable) -> T.Callable:
+        setattr(sympy_module, func.__name__, func)
+        return func
+
+    # Should match C++ default epsilon in epsilon.h
+    sympy_module.numeric_epsilon = 10 * sys.float_info.epsilon
 
     @register
     def wrap_angle(x: T.Scalar) -> T.Scalar: