[SymForce] Add sf.clamp

Reviewers: nathan,bradley,harrison,chao,hayk
Topic: sf-clamp
Relative: sf-databuffer-unused
GitOrigin-RevId: 16ff3f5a999951e2c1f137ef9998812f9d445a72

Author: aaron-skydio

symforce/internal/symbolic.py

@@ -441,6 +441,21 @@ def acos_safe(x: Scalar, epsilon: Scalar = epsilon()) -> Scalar:
     return sympy.acos(x_safe)
 
 
+def clamp(x: sf.Scalar, min_value: sf.Scalar, max_value: sf.Scalar) -> sf.Scalar:
+    """
+    Returns min_value if x < min_value
+    Returns x if min_value < x < max_value
+    Returns max_value if x > max_value
+
+    Args:
+        x: Value to clamp between min_value and max_value
+        min_value: Scalar of same type and units as x; minimum value to return
+        max_value: Scalar of same type and units as x; maximum value to return. Must be greater
+            than min_value.
+    """
+    return sf.Min(max_value, sf.Max(min_value, x))
+
+
 def set_eval_on_sympify(eval_on_sympy: bool = True) -> None:
     """
     When using the symengine backed, set whether we should eval args when converting objects to

test/symforce_custom_methods_test.py

@@ -12,14 +12,14 @@
 
 class SymforceCustomMethodsTest(TestCase):
     """
-    Test the custom methods added by add_custom_methods in initialization.py
+    Test the custom methods added by in the "custom functions" section in symbolic.py
     """
 
     def test_arg_maxes(self) -> None:
         """
         Tests:
-            symforce.sympy.argmax_onehot
-            symforce.sympy.argmax
+            sf.argmax_onehot
+            sf.argmax
         Check that the argmax functions return the correct output
         """
 
@@ -42,8 +42,8 @@ def test_arg_maxes(self) -> None:
     def test_arg_maxes_other_sequences(self) -> None:
         """
         Tests:
-            symforce.sympy.argmax_onehot
-            symforce.sympy.argmax
+            sf.argmax_onehot
+            sf.argmax
         Check that the argmax functions work on non-list sequences
         """
         vals_range = range(5)
@@ -58,6 +58,15 @@ def test_arg_maxes_other_sequences(self) -> None:
         self.assertEqual([1, 0, 0, 0], sf.argmax_onehot(vals_arr))
         self.assertEqual(0, sf.argmax(vals_arr))
 
+    def test_clamp(self) -> None:
+        """
+        Tests:
+            sf.clamp
+        """
+        self.assertEqual(1, sf.clamp(-10, 1, 5))
+        self.assertEqual(3, sf.clamp(3, 1, 5))
+        self.assertEqual(5, sf.clamp(10, 1, 5))
+
 
 if __name__ == "__main__":
     TestCase.main()