remove prev modification

code/sim/ilqr/utils.py

@@ -12,8 +12,8 @@ def principal_value(angle, min_=-np.pi):
     :param min_: minimum domain for angle (rad)
     :return angle wrapped to [min_, min_ + 2 pi).
     """
-    # assert np.all(np.isfinite(angle)), "angle is not finite"
-    angle = np.nan_to_num(angle, nan=0.0, posinf=0.0, neginf=0.0)
+    assert np.all(np.isfinite(angle)), "angle is not finite"
+    # angle = np.nan_to_num(angle, nan=0.0, posinf=0.0, neginf=0.0)
     lhs = (angle - min_) % (2 * np.pi) + min_
     return lhs