lqr infinite to zero

code/sim/ilqr/utils.py

@@ -12,7 +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"
+    # 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