From d3f5db0b5415ea5b8c8f6f69e46bfdb92ddd2b2f Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Wed, 20 May 2020 08:24:10 +0530 Subject: [PATCH] [Pythonclient/types] Add utility functions to check for NaN in Pose --- PythonClient/airsim/types.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/PythonClient/airsim/types.py b/PythonClient/airsim/types.py index 25e359843c..7b7c6f5399 100644 --- a/PythonClient/airsim/types.py +++ b/PythonClient/airsim/types.py @@ -1,6 +1,7 @@ from __future__ import print_function import msgpackrpc #install as admin: pip install msgpack-rpc-python import numpy as np #pip install numpy +import math class MsgpackMixin: def __repr__(self): @@ -62,6 +63,9 @@ def __init__(self, x_val = 0.0, y_val = 0.0, z_val = 0.0): def nanVector3r(): return Vector3r(np.nan, np.nan, np.nan) + def containsNan(self): + return (math.isnan(self.x_val) or math.isnan(self.y_val) or math.isnan(self.z_val)) + def __add__(self, other): return Vector3r(self.x_val + other.x_val, self.y_val + other.y_val, self.z_val + other.z_val) @@ -122,6 +126,9 @@ def __init__(self, x_val = 0.0, y_val = 0.0, z_val = 0.0, w_val = 1.0): def nanQuaternionr(): return Quaternionr(np.nan, np.nan, np.nan, np.nan) + def containsNan(self): + return (math.isnan(self.w_val) or math.isnan(self.x_val) or math.isnan(self.y_val) or math.isnan(self.z_val)) + def __add__(self, other): if type(self) == type(other): return Quaternionr( self.x_val+other.x_val, self.y_val+other.y_val, self.z_val+other.z_val, self.w_val+other.w_val ) @@ -207,6 +214,9 @@ def __init__(self, position_val = None, orientation_val = None): def nanPose(): return Pose(Vector3r.nanVector3r(), Quaternionr.nanQuaternionr()) + def containsNan(self): + return (self.position.containsNan() or self.orientation.containsNan()) + class CollisionInfo(MsgpackMixin): has_collided = False