diff --git a/safe_autonomy_simulation/sims/inspection/camera.py b/safe_autonomy_simulation/sims/inspection/camera.py index cb58ce6..b9db684 100644 --- a/safe_autonomy_simulation/sims/inspection/camera.py +++ b/safe_autonomy_simulation/sims/inspection/camera.py @@ -227,9 +227,14 @@ def capture( x = -1 y = 1 z = ( - -(image_plane_position[0] * x + image_plane_position[1] * y) - / image_plane_position[2] + ( + -(image_plane_position[0] * x + image_plane_position[1] * y) + / image_plane_position[2] + ) + if image_plane_position[2] != 0 + else -(image_plane_position[0] * x + image_plane_position[1] * y) ) + norm1 = vector.normalize([x, y, z]) # np.cross bug work-around https://github.com/microsoft/pylance-release/issues/3277 diff --git a/safe_autonomy_simulation/sims/inspection/inspection_points.py b/safe_autonomy_simulation/sims/inspection/inspection_points.py index a59a0b0..5b8c649 100644 --- a/safe_autonomy_simulation/sims/inspection/inspection_points.py +++ b/safe_autonomy_simulation/sims/inspection/inspection_points.py @@ -238,7 +238,7 @@ def __init__( dynamics: d.Dynamics = d.PassThroughDynamics(), control_queue: c.ControlQueue = c.NoControl(), material: m.Material = m.BLACK, - half_weighted: bool = False + half_weighted: bool = False, ): self.half_weighted = half_weighted self._points: typing.Dict[ @@ -273,7 +273,9 @@ def _post_step(self, step_size: float): # Updating state in post step to ensure that all points have been updated self._state = np.array([p.state for p in self.points.values()]) # Update priority vector - self._priority_vector = transform.Rotation.from_quat(self.parent.orientation).apply(self.init_priority_vector) + self._priority_vector = transform.Rotation.from_quat( + self.parent.orientation + ).apply(self.init_priority_vector) def _generate_points( self, num_points, points_algorithm: str = "cmu" @@ -319,18 +321,31 @@ def _generate_points( / (np.linalg.norm(-self.priority_vector) * np.linalg.norm(pos)) ) / np.pi + if np.linalg.norm(-self.priority_vector) != 0 + and np.linalg.norm(pos) != 0 + else np.dot(-self.priority_vector, pos) for pos in point_positions ] else: point_weights = [] for pos in point_positions: - dot_prod = np.dot(-self.priority_vector, pos) / (np.linalg.norm(-self.priority_vector) * np.linalg.norm(pos)) + if ( + np.linalg.norm(-self.priority_vector) != 0 + and np.linalg.norm(pos) != 0 + ): + dot_prod = np.dot(-self.priority_vector, pos) / ( + np.linalg.norm(-self.priority_vector) * np.linalg.norm(pos) + ) + else: + dot_prod = np.dot(-self.priority_vector, pos) if dot_prod <= 0: point_weights.append(np.arccos(dot_prod) / np.pi) else: point_weights.append(0.0) total_weight = sum(point_weights) - point_weights = [w / total_weight for w in point_weights] # normalize weights + point_weights = [ + w / total_weight if total_weight != 0 else w for w in point_weights + ] # normalize weights points = {} for i, (pos, weight) in enumerate(zip(point_positions, point_weights)): @@ -376,15 +391,19 @@ def update_points_inspection_status( # For translational motion only, camera always points towards chief (origin) # TODO: don't assume chief is at origin r_c = -cam_position - r_c = r_c / np.linalg.norm(r_c) # inspector sensor unit vector + if np.linalg.norm(r_c) != 0: + r_c = r_c / np.linalg.norm(r_c) # inspector sensor unit vector r = self.radius rt = np.linalg.norm(cam_position) - h = 2 * r * ((rt - r) / (2 * rt)) - - p_hat = cam_position / np.linalg.norm( - cam_position - ) # position unit vector (inspection zone cone axis) + if rt != 0: + h = 2 * r * ((rt - r) / (2 * rt)) + p_hat = ( + cam_position / rt + ) # position unit vector (inspection zone cone axis) + else: + h = 0 + p_hat = cam_position for ( _, diff --git a/safe_autonomy_simulation/sims/inspection/sun.py b/safe_autonomy_simulation/sims/inspection/sun.py index 975b8b0..9e1e70f 100644 --- a/safe_autonomy_simulation/sims/inspection/sun.py +++ b/safe_autonomy_simulation/sims/inspection/sun.py @@ -100,7 +100,7 @@ def theta(self) -> float: float Sun rotation angle in radians """ - return float(self.state) + return float(self.state[0]) @property def n(self) -> float: diff --git a/safe_autonomy_simulation/sims/inspection/utils/vector.py b/safe_autonomy_simulation/sims/inspection/utils/vector.py index c7edc8e..1dc1a81 100644 --- a/safe_autonomy_simulation/sims/inspection/utils/vector.py +++ b/safe_autonomy_simulation/sims/inspection/utils/vector.py @@ -19,4 +19,8 @@ def normalize(vector: np.ndarray) -> np.ndarray: np.ndarray normalized vector """ - return vector / np.linalg.norm(vector) + norm = np.linalg.norm(vector) + norm_v = vector + if norm != 0: + norm_v = vector / norm + return norm_v diff --git a/test/entities/test_entity.py b/test/entities/test_entity.py index 6680fe3..bc98b68 100644 --- a/test/entities/test_entity.py +++ b/test/entities/test_entity.py @@ -2,16 +2,16 @@ import safe_autonomy_simulation -class TestDynamics(safe_autonomy_simulation.dynamics.Dynamics): +class SimpleDynamics(safe_autonomy_simulation.dynamics.Dynamics): def _step(self, step_size, state, control): return state + control * step_size, control -class TestEntity(safe_autonomy_simulation.Entity): +class SimpleEntity(safe_autonomy_simulation.Entity): def __init__( self, name, - dynamics=TestDynamics(), + dynamics=SimpleDynamics(), control_queue=safe_autonomy_simulation.ControlQueue( default_control=np.array([1]) ), @@ -34,10 +34,10 @@ def state(self, state): def test_init(): - entity = TestEntity(name="test_entity") + entity = SimpleEntity(name="test_entity") assert entity.name == "test_entity" assert np.all(entity.state == np.array([0])) - assert entity.dynamics.__class__.__name__ == "TestDynamics" + assert entity.dynamics.__class__.__name__ == "SimpleDynamics" assert entity.control_queue.__class__.__name__ == "ControlQueue" assert entity.last_control is None assert np.all(entity.state_dot == np.zeros_like(entity.state)) @@ -47,16 +47,16 @@ def test_init(): def test_init_parent(): - parent = TestEntity(name="parent") - entity = TestEntity(name="test_entity", parent=parent) + parent = SimpleEntity(name="parent") + entity = SimpleEntity(name="test_entity", parent=parent) assert entity.parent == parent assert entity in parent.children def test_init_children(): - child1 = TestEntity(name="child1") - child2 = TestEntity(name="child2") - entity = TestEntity(name="test_entity", children={child1, child2}) + child1 = SimpleEntity(name="child1") + child2 = SimpleEntity(name="child2") + entity = SimpleEntity(name="test_entity", children={child1, child2}) assert child1 in entity.children assert child2 in entity.children assert child1.parent == entity @@ -64,9 +64,9 @@ def test_init_children(): def test_reset(): - child1 = TestEntity(name="child1") - child2 = TestEntity(name="child2") - entity = TestEntity(name="test_entity", children={child1, child2}) + child1 = SimpleEntity(name="child1") + child2 = SimpleEntity(name="child2") + entity = SimpleEntity(name="test_entity", children={child1, child2}) entity.step(step_size=1) entity.add_control(np.array([1])) entity.reset() @@ -82,9 +82,9 @@ def test_reset(): def test__pre_step(): - child1 = TestEntity(name="child1") - child2 = TestEntity(name="child2") - entity = TestEntity(name="test_entity", children={child1, child2}) + child1 = SimpleEntity(name="child1") + child2 = SimpleEntity(name="child2") + entity = SimpleEntity(name="test_entity", children={child1, child2}) entity._pre_step(step_size=1) assert np.all(entity.state == np.array([0])) assert entity.last_control is None @@ -98,9 +98,9 @@ def test__pre_step(): def test__post_step(): - child1 = TestEntity(name="child1") - child2 = TestEntity(name="child2") - entity = TestEntity(name="test_entity", children={child1, child2}) + child1 = SimpleEntity(name="child1") + child2 = SimpleEntity(name="child2") + entity = SimpleEntity(name="test_entity", children={child1, child2}) entity._post_step(step_size=1) assert np.all(entity.state == np.array([0])) assert entity.last_control is None @@ -114,9 +114,9 @@ def test__post_step(): def test_step(): - child1 = TestEntity(name="child1") - child2 = TestEntity(name="child2") - entity = TestEntity(name="test_entity", children={child1, child2}) + child1 = SimpleEntity(name="child1") + child2 = SimpleEntity(name="child2") + entity = SimpleEntity(name="test_entity", children={child1, child2}) entity.step(step_size=2) assert np.all(entity.state == np.array([2])) assert np.all(entity.last_control == np.array([1])) @@ -128,15 +128,15 @@ def test_step(): def test_add_control(): - entity = TestEntity(name="test_entity") + entity = SimpleEntity(name="test_entity") entity.add_control(np.array([2])) assert np.all(entity.control_queue.next_control() == np.array([2])) def test__is_descendant(): - parent = TestEntity(name="parent") - child = TestEntity(name="child", parent=parent) - grandchild = TestEntity(name="grandchild", parent=child) + parent = SimpleEntity(name="parent") + child = SimpleEntity(name="child", parent=parent) + grandchild = SimpleEntity(name="grandchild", parent=child) assert parent._is_descendant(parent) assert not parent._is_descendant(child) assert not parent._is_descendant(grandchild) @@ -149,16 +149,16 @@ def test__is_descendant(): def test_add_child(): - parent = TestEntity(name="parent") - child = TestEntity(name="child") + parent = SimpleEntity(name="parent") + child = SimpleEntity(name="child") parent.add_child(child) assert child in parent.children assert child.parent == parent def test_remove_child(): - parent = TestEntity(name="parent") - child = TestEntity(name="child") + parent = SimpleEntity(name="parent") + child = SimpleEntity(name="child") parent.add_child(child) parent.remove_child(child) assert child not in parent.children diff --git a/test/sims/inspection/test_inspection_points/test_inspection_point_set.py b/test/sims/inspection/test_inspection_points/test_inspection_point_set.py index 4b4bc95..d51b528 100644 --- a/test/sims/inspection/test_inspection_points/test_inspection_point_set.py +++ b/test/sims/inspection/test_inspection_points/test_inspection_point_set.py @@ -294,14 +294,20 @@ def test_update_points_inspection_status(camera, sun, binary_ray): radius=1, priority_vector=np.array([1, 0, 0]), ) - h = ( - 2 - * point_set.radius - * (np.linalg.norm(camera.position) / (2 * np.linalg.norm(camera.position))) - ) - p_hat = camera.position / np.linalg.norm(camera.position) + norm_pos = np.linalg.norm(camera.position) + if norm_pos == 0: + h = 0 + p_hat = camera.position + else: + h = ( + 2 + * point_set.radius + * (norm_pos / (2 * norm_pos)) + ) + p_hat = camera.position / norm_pos r_c = transform.Rotation.from_quat(camera.orientation).as_euler("xyz") - r_c = r_c / np.linalg.norm(r_c) + if np.linalg.norm(r_c) != 0: + r_c = r_c / np.linalg.norm(r_c) point_set.update_points_inspection_status(camera, sun, binary_ray) for _, point in point_set.points.items(): expected_inspected = False diff --git a/test/sims/inspection/test_inspection_simulator.py b/test/sims/inspection/test_inspection_simulator.py index 0d880f4..974d8ca 100644 --- a/test/sims/inspection/test_inspection_simulator.py +++ b/test/sims/inspection/test_inspection_simulator.py @@ -285,7 +285,7 @@ def test_step(frame_rate, inspectors, targets): priority_vector=np.array([1, 0, 0]), ) ], - ) + ), ], ) def test_run_sim(frame_rate, inspectors, targets): @@ -303,7 +303,11 @@ def test_run_sim(frame_rate, inspectors, targets): prev_inspected = {target.name: 0 for target in inspection_sim.targets} for i in range(100): for inspector in inspection_sim.inspectors: - control = rng.uniform(-1, 1, size=inspector.control_queue.default_control.shape) + control = rng.uniform( + inspector.control_queue.control_min, + inspector.control_queue.control_max, + size=inspector.control_queue.default_control.shape, + ) inspector.add_control(control) inspection_sim.step() for target in inspection_sim.targets: diff --git a/test/sims/spacecraft/test_defaults.py b/test/sims/spacecraft/test_defaults.py index 191a28f..7d354e8 100644 --- a/test/sims/spacecraft/test_defaults.py +++ b/test/sims/spacecraft/test_defaults.py @@ -8,7 +8,7 @@ def test_defaults(): assert safe_autonomy_simulation.sims.spacecraft.defaults.INERTIA_DEFAULT == 0.0573 assert np.all( safe_autonomy_simulation.sims.spacecraft.defaults.INERTIA_MATRIX_DEFAULT - == np.matrix( + == np.array( [ [ safe_autonomy_simulation.sims.spacecraft.defaults.INERTIA_DEFAULT, diff --git a/test/test_simulator.py b/test/test_simulator.py index c20032a..58f0a75 100644 --- a/test/test_simulator.py +++ b/test/test_simulator.py @@ -5,7 +5,7 @@ import numpy as np -class TestDynamics(safe_autonomy_simulation.Dynamics): +class SimpleDynamics(safe_autonomy_simulation.Dynamics): def _step( self, step_size: float, state: ndarray, control: ndarray ) -> Tuple[ndarray, ndarray]: @@ -26,11 +26,11 @@ def _step( ) -class TestEntity(safe_autonomy_simulation.Entity): +class SimpleEntity(safe_autonomy_simulation.Entity): def __init__(self, name): super().__init__( name, - dynamics=TestDynamics(), + dynamics=SimpleDynamics(), control_queue=TEST_CONTROL_QUEUE, material=TEST_MATERIAL, ) @@ -50,7 +50,7 @@ def state(self, value): @pytest.fixture def entities(): - return [TestEntity(name=i) for i in range(3)] + return [SimpleEntity(name=i) for i in range(3)] @pytest.fixture