From 024387b02ae5f7ff78028a45a625df0d825d6ca8 Mon Sep 17 00:00:00 2001
From: iwishiwasaneagle <jh.ewers@gmail.com>
Date: Thu, 2 Mar 2023 09:56:12 +0000
Subject: [PATCH] fix: Add condition within PositionDroneEnv to truncate sim if
 any value is nan

---
 src/jdrones/envs/position.py | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/jdrones/envs/position.py b/src/jdrones/envs/position.py
index ccc3b76..86a46ef 100644
--- a/src/jdrones/envs/position.py
+++ b/src/jdrones/envs/position.py
@@ -32,7 +32,6 @@ def __init__(
     @property
     def action_space(self) -> ActType:
         bounds = np.array([[0, 10], [0, 10], [1, 10]])
-
         return spaces.Box(low=bounds[:, 0], high=bounds[:, 1])
 
     def reset(
@@ -64,10 +63,12 @@ def step(
 
             e = self.env.controllers["lqr"].e
             dist = np.linalg.norm(np.concatenate([e.pos, e.rpy]))
+            if np.any(np.isnan(dist)):
+                trunc = True
+
             if dist < 0.01:
                 term = True
                 info["error"] = dist
-                break
 
         states = States(observations)
         return states, self.get_reward(states), term, trunc, info