fix: sixdof quat comparison + add unit tests

Address MR comments:
* Use quaternion attribute for equality checks in 2dRotational model
* Catch same bug and fix in sixdof model
* Add unit tests for equality checks between models

safe_autonomy_dynamics/cwh/rotational_model.py

@@ -192,7 +192,7 @@ def __eq__(self, other):
         if isinstance(other, CWHRotation2dSpacecraft):
             eq = (self.velocity == other.velocity).all()
             eq = eq and (self.position == other.position).all()
-            eq = eq and (self.orientation.as_quat() == other.orientation.as_quat()).all()
+            eq = eq and (self.quaternion == other.quaternion).all()
             eq = eq and (self.angular_velocity == other.angular_velocity).all()
             return eq
         return False

safe_autonomy_dynamics/cwh/sixdof_model.py

@@ -228,7 +228,7 @@ def __eq__(self, other):
         if isinstance(other, SixDOFSpacecraft):
             eq = (self.velocity == other.velocity).all()
             eq = eq and (self.position == other.position).all()
-            eq = eq and (self.orientation.as_quat == other.orientation.as_quat).all()
+            eq = eq and (self.quaternion == other.quaternion).all()
             eq = eq and (self.angular_velocity == other.angular_velocity).all()
             return eq
         return False

test/test_cases/compare_entities_test_cases.yaml

@@ -0,0 +1,288 @@
+- ID: SunEntity_equals
+  this_entity:
+    entity_class: safe_autonomy_dynamics.cwh.sun_model.SunEntity
+    init_kwargs: {}
+  other_entity:
+    entity_class: safe_autonomy_dynamics.cwh.sun_model.SunEntity
+    init_kwargs:
+      n: 0.1234
+  expect_equal: True
+- ID: SunEntity_not_equals
+  this_entity:
+    entity_class: safe_autonomy_dynamics.cwh.sun_model.SunEntity
+  other_entity:
+    entity_class: safe_autonomy_dynamics.cwh.CWHSpacecraft
+  expect_equal: False
+- ID: CWHSpacecraft_equals
+  this_entity:
+    entity_class: safe_autonomy_dynamics.cwh.CWHSpacecraft
+    init_kwargs:
+      x:
+        pint_quantity: [536.324, 'm']
+      y:
+        pint_quantity: [-738.112, 'm']
+      z:
+        pint_quantity: [995.99, 'm']
+      x_dot:
+        pint_quantity: [-3.7, 'm/s']
+      y_dot:
+        pint_quantity: [-17, 'm/s']
+      z_dot:
+        pint_quantity: [25.23, 'm/s']
+  other_entity:
+    entity_class: safe_autonomy_dynamics.cwh.CWHSpacecraft
+    init_state:
+      position: [536.324, -738.112, 995.99]
+      velocity: [-3.7, -17, 25.23]
+  expect_equal: True
+- ID: CWHSpacecraft_not_equals_position
+  this_entity:
+    entity_class: safe_autonomy_dynamics.cwh.CWHSpacecraft
+    init_state:
+      position: [536.324, -738.112, 995.99]
+      velocity: [-3.7, -17, 25.23]
+  other_entity:
+    entity_class: safe_autonomy_dynamics.cwh.CWHSpacecraft
+    init_state:
+      position: [536.324, -738.112, 0.]
+      velocity: [-3.7, -17, 25.23]
+  expect_equal: False
+- ID: CWHSpacecraft_not_equals_velocity
+  this_entity:
+    entity_class: safe_autonomy_dynamics.cwh.CWHSpacecraft
+    init_state:
+      position: [536.324, -738.112, 995.99]
+      velocity: [-3.7, -17, 25.23]
+  other_entity:
+    entity_class: safe_autonomy_dynamics.cwh.CWHSpacecraft
+    init_state:
+      position: [536.324, -738.112, 995.99]
+      velocity: [0., 0., 25.23]
+  expect_equal: False
+- ID: CWHSpacecraft_not_equals_class
+  this_entity:
+    entity_class: safe_autonomy_dynamics.cwh.CWHSpacecraft
+    init_kwargs:
+      x:
+        pint_quantity: [536.324, 'm']
+      y:
+        pint_quantity: [-738.112, 'm']
+      z:
+        pint_quantity: [995.99, 'm']
+      x_dot:
+        pint_quantity: [-3.7, 'm/s']
+      y_dot:
+        pint_quantity: [-17, 'm/s']
+      z_dot:
+        pint_quantity: [25.23, 'm/s']
+  other_entity:
+    entity_class: safe_autonomy_dynamics.cwh.CWHRotation2dSpacecraft
+  expect_equal: False
+- ID: CWHRotation2dSpacecraft_equals
+  this_entity:
+    entity_class: safe_autonomy_dynamics.cwh.CWHRotation2dSpacecraft
+    init_kwargs:
+      x:
+        pint_quantity: [536.324, 'm']
+      y:
+        pint_quantity: [-738.112, 'm']
+      x_dot:
+        pint_quantity: [-3.7, 'm/s']
+      y_dot:
+        pint_quantity: [-17, 'm/s']
+      theta:
+        pint_quantity: [1.2, 'rad']
+      wz:
+        pint_quantity: [0.03, 'rad/s']
+  other_entity:
+    entity_class: safe_autonomy_dynamics.cwh.CWHRotation2dSpacecraft
+    init_state:
+      position: [536.324, -738.112]
+      theta: 1.2
+      velocity: [-3.7, -17]
+      wz: 0.03
+  expect_equal: True
+- ID: CWHRotation2dSpacecraft_not_equals_position
+  this_entity:
+    entity_class: safe_autonomy_dynamics.cwh.CWHRotation2dSpacecraft
+    init_state:
+      position: [536.324, -738.112]
+      theta: 1.2
+      velocity: [-3.7, -17]
+      wz: 0.03
+  other_entity:
+    entity_class: safe_autonomy_dynamics.cwh.CWHRotation2dSpacecraft
+    init_state:
+      position: [0., 0.]
+      theta: 1.2
+      velocity: [-3.7, -17]
+      wz: 0.03
+  expect_equal: False
+- ID: CWHRotation2dSpacecraft_not_equals_velocity
+  this_entity:
+    entity_class: safe_autonomy_dynamics.cwh.CWHRotation2dSpacecraft
+    init_state:
+      position: [536.324, -738.112]
+      theta: 1.2
+      velocity: [0., 0.]
+      wz: 0.03
+  other_entity:
+    entity_class: safe_autonomy_dynamics.cwh.CWHRotation2dSpacecraft
+    init_state:
+      position: [536.324, -738.112]
+      theta: 1.2
+      velocity: [-3.7, -17]
+      wz: 0.03
+  expect_equal: False
+- ID: CWHRotation2dSpacecraft_not_equals_angular_velocity
+  this_entity:
+    entity_class: safe_autonomy_dynamics.cwh.CWHRotation2dSpacecraft
+    init_state:
+      position: [536.324, -738.112]
+      theta: 1.2
+      velocity: [0., 0.]
+      wz: 0.03
+  other_entity:
+    entity_class: safe_autonomy_dynamics.cwh.CWHRotation2dSpacecraft
+    init_state:
+      position: [536.324, -738.112]
+      theta: 1.2
+      velocity: [-3.7, -17]
+      wz: 0.
+  expect_equal: False
+- ID: CWHRotation2dSpacecraft_not_equals_rotation
+  this_entity:
+    entity_class: safe_autonomy_dynamics.cwh.CWHRotation2dSpacecraft
+    init_state:
+      position: [536.324, -738.112]
+      theta: 1.2
+      velocity: [0., 0.]
+      wz: 0.03
+  other_entity:
+    entity_class: safe_autonomy_dynamics.cwh.CWHRotation2dSpacecraft
+    init_state:
+      position: [536.324, -738.112]
+      theta: 0.
+      velocity: [-3.7, -17]
+      wz: 0.03
+  expect_equal: False
+- ID: CWHRotation2dSpacecraft_not_equals_class
+  this_entity:
+    entity_class: safe_autonomy_dynamics.cwh.CWHRotation2dSpacecraft
+    init_state:
+      position: [536.324, -738.112]
+      theta: 1.2
+      velocity: [-3.7, -17]
+      wz: 0.03
+  other_entity:
+    entity_class: safe_autonomy_dynamics.cwh.SixDOFSpacecraft
+  expect_equal: False
+- ID: SixDOFSpacecraft_equals
+  this_entity:
+    entity_class: safe_autonomy_dynamics.cwh.SixDOFSpacecraft
+    init_kwargs:
+      x:
+        pint_quantity: [536.324, 'm']
+      y:
+        pint_quantity: [-738.112, 'm']
+      z:
+        pint_quantity: [995.99, 'm']
+      x_dot:
+        pint_quantity: [-3.7, 'm/s']
+      y_dot:
+        pint_quantity: [-17, 'm/s']
+      z_dot:
+        pint_quantity: [25.23, 'm/s']
+      q1: 0
+      q2: 0
+      q3: -0.156
+      q4: 0.988
+      wx:
+        pint_quantity: [0.0123, 'rad/s']
+      wy:
+        pint_quantity: [-0.0065, 'rad/s']
+      wz:
+        pint_quantity: [0.03, 'rad/s']
+  other_entity:
+    entity_class: safe_autonomy_dynamics.cwh.SixDOFSpacecraft
+    init_state:
+      position: [536.324, -738.112, 995.99]
+      quaternion: [0, 0, -0.156, 0.988]
+      velocity: [-3.7, -17, 25.23]
+      angular_velocity: [0.01230, -0.0065, 0.03]
+  expect_equal: True
+- ID: SixDOFSpacecraft_not_equals_position
+  this_entity:
+    entity_class: safe_autonomy_dynamics.cwh.SixDOFSpacecraft
+    init_state:
+      position: [536.324, -738.112, 995.99]
+      quaternion: [0, 0, -0.156, 0.988]
+      velocity: [-3.7, -17, 25.23]
+      angular_velocity: [0.01230, -0.0065, 0.03]
+  other_entity:
+    entity_class: safe_autonomy_dynamics.cwh.SixDOFSpacecraft
+    init_state:
+      position: [536.324, 0., 995.99]
+      quaternion: [0, 0, -0.156, 0.988]
+      velocity: [-3.7, -17, 25.23]
+      angular_velocity: [0.01230, -0.0065, 0.03]
+  expect_equal: False
+- ID: SixDOFSpacecraft_not_equals_velocity
+  this_entity:
+    entity_class: safe_autonomy_dynamics.cwh.SixDOFSpacecraft
+    init_state:
+      position: [536.324, -738.112, 995.99]
+      quaternion: [0, 0, -0.156, 0.988]
+      velocity: [-3.7, -17, 25.23]
+      angular_velocity: [0.01230, -0.0065, 0.03]
+  other_entity:
+    entity_class: safe_autonomy_dynamics.cwh.SixDOFSpacecraft
+    init_state:
+      position: [536.324, -738.112, 995.99]
+      quaternion: [0, 0, -0.156, 0.988]
+      velocity: [-3.7, 0., 0.]
+      angular_velocity: [0.01230, -0.0065, 0.03]
+  expect_equal: False
+- ID: SixDOFSpacecraft_not_equals_angular_velocity
+  this_entity:
+    entity_class: safe_autonomy_dynamics.cwh.SixDOFSpacecraft
+    init_state:
+      position: [536.324, -738.112, 995.99]
+      quaternion: [0, 0, -0.156, 0.988]
+      velocity: [-3.7, -17, 25.23]
+      angular_velocity: [0.01230, -0.0065, 0.03]
+  other_entity:
+    entity_class: safe_autonomy_dynamics.cwh.SixDOFSpacecraft
+    init_state:
+      position: [536.324, -738.112, 995.99]
+      quaternion: [0, 0, -0.156, 0.988]
+      velocity: [-3.7, -17, 25.23]
+      angular_velocity: [0.01230, 0., 0.]
+  expect_equal: False
+- ID: SixDOFSpacecraft_not_equals_orientation
+  this_entity:
+    entity_class: safe_autonomy_dynamics.cwh.SixDOFSpacecraft
+    init_state:
+      position: [536.324, -738.112, 995.99]
+      quaternion: [0, 0, -0.156, 0.988]
+      velocity: [-3.7, -17, 25.23]
+      angular_velocity: [0.01230, -0.0065, 0.03]
+  other_entity:
+    entity_class: safe_autonomy_dynamics.cwh.SixDOFSpacecraft
+    init_state:
+      position: [536.324, -738.112, 995.99]
+      quaternion: [0, 0, 0, 1.]
+      velocity: [-3.7, -17, 25.23]
+      angular_velocity: [0.01230, -0.0065, 0.03]
+  expect_equal: False
+- ID: SixDOFSpacecraft_not_equals_class
+  this_entity:
+    entity_class: safe_autonomy_dynamics.cwh.SixDOFSpacecraft
+    init_state:
+      position: [0.0, 0.0, -4.1666300442734014]
+      quaternion: [0, 0, -0.1564348248516, 0.9876882836065]
+      velocity: [0.0, 0.0, -0.8333186843966979]
+      angular_velocity: [0, 0, -0.03500]
+  other_entity:
+    entity_class: safe_autonomy_dynamics.cwh.CWHSpacecraft
+  expect_equal: False

test/test_simulators/test_entities/conftest.py

@@ -128,4 +128,4 @@ def wrap_angles(x, angle_wrap):
     return x
 
 def wrap_angle_quantity(x):
-    return (x + np.pi) % (2*np.pi) - np.pi
+    return (x + np.pi) % (2*np.pi) - np.pi

test/test_simulators/test_entities/test_compare_entities.py

@@ -0,0 +1,58 @@
+"""
+# -------------------------------------------------------------------------------
+# Air Force Research Laboratory (AFRL) Autonomous Capabilities Team (ACT3)
+# Reinforcement Learning Core (CoRL) Runtime Assurance Extensions
+#
+# This is a US Government Work not subject to copyright protection in the US.
+#
+# The use, dissemination or disclosure of data in this file is subject to
+# limitation or restriction. See accompanying README and LICENSE for details.
+# -------------------------------------------------------------------------------
+
+This module defines tests for the CWHRotationSpacecraft entity.
+
+Author: John McCarroll, Andy Barth
+"""
+
+import importlib
+import numpy as np
+import os
+import pytest
+
+from test.conftest import read_test_cases, delimiter
+
+
+# Define test assay
+test_cases_file_path = os.path.join(os.path.split(__file__)[0], "../../test_cases/compare_entities_test_cases.yaml")
+parameterized_fixture_keywords = ["this_entity",
+                                  "other_entity",
+                                  "expect_equal"]
+test_configs, IDs = read_test_cases(test_cases_file_path, parameterized_fixture_keywords)
+
+def build_entity(entity_name, entity_config):
+    mod_name, _, obj_name = entity_config['entity_class'].rpartition('.')
+    entity_cls = getattr(importlib.import_module(mod_name), obj_name)
+    entity = entity_cls(name=entity_name, **entity_config.get('init_kwargs', {}))
+    if 'init_state' in entity_config and isinstance(entity_config['init_state'], dict):
+        initial_state = []
+        for value in entity_config['init_state'].values():
+            initial_state += value if type(value) in [list, np.ndarray] else [value]
+        entity.state = np.array(initial_state, dtype=entity.state.dtype)
+    return entity
+
+@pytest.fixture
+def this_entity(request):
+    return build_entity('this', request.param)
+
+@pytest.fixture
+def other_entity(request):
+    return build_entity('other', request.param)
+
+@pytest.fixture
+def expect_equal(request):
+    return request.param
+
+@pytest.mark.parametrize(delimiter.join(parameterized_fixture_keywords), test_configs, indirect=True, ids=IDs)
+def test_compare_entities(this_entity, other_entity, expect_equal):
+    assert (this_entity == other_entity) == expect_equal
+