feat: add head control and joint data decoder

tg3_local_teleop/test_head_control.py

@@ -0,0 +1,190 @@
+#!/usr/bin/env python3
+from __future__ import annotations
+
+import unittest
+
+from head_control import (
+    HeadPitchController,
+    head_pitch_command_axis,
+    left_z_pressed,
+    right_stick_vertical,
+)
+
+
+def controller() -> HeadPitchController:
+    return HeadPitchController(
+        deadzone=0.1,
+        max_speed_rad_s=0.2,
+        max_accel_rad_s2=0.5,
+        min_pitch_rad=-0.1745329252,
+        max_pitch_rad=0.872664626,
+        axis_sign=-1.0,
+        max_dt_s=0.1,
+    )
+
+
+class RightStickParserTest(unittest.TestCase):
+    def test_accepts_only_finite_two_axis_right_stick(self) -> None:
+        self.assertEqual(
+            right_stick_vertical({"joystick": {"right": [0.5, -0.2]}}),
+            0.5,
+        )
+        self.assertIsNone(right_stick_vertical({"joystick": {"right": [0.5]}}))
+        self.assertIsNone(
+            right_stick_vertical({"joystick": {"right": [float("nan"), 0.0]}})
+        )
+        self.assertIsNone(right_stick_vertical({"joystick": {"right": [2.0, 0.0]}}))
+
+    def test_left_z_strictly_gates_head_pitch_axis(self) -> None:
+        released = {
+            "button": {"left": [0, 0, 0]},
+            "joystick": {"right": [0.8, 0.1]},
+        }
+        pressed = {
+            "button": {"left": [0, 0, 1]},
+            "joystick": {"right": [0.8, 0.1]},
+        }
+        self.assertFalse(left_z_pressed(released))
+        self.assertEqual(head_pitch_command_axis(released), 0.0)
+        self.assertTrue(left_z_pressed(pressed))
+        self.assertEqual(head_pitch_command_axis(pressed), 0.8)
+        self.assertIsNone(
+            head_pitch_command_axis(
+                {
+                    "button": {"left": [0, 0, "pressed"]},
+                    "joystick": {"right": [0.8, 0.1]},
+                }
+            )
+        )
+
+
+class HeadPitchControllerTest(unittest.TestCase):
+    def test_new_session_requires_neutral_before_direct_motion(self) -> None:
+        head = controller()
+        head.new_session()
+        self.assertIsNone(
+            head.update(
+                0.0,
+                armed=True,
+                input_healthy=True,
+                feedback_healthy=True,
+                external_busy=False,
+                axis=1.0,
+                feedback_position_rad=0.1,
+            )
+        )
+        self.assertTrue(head.require_neutral)
+        head.update(
+            0.1,
+            armed=True,
+            input_healthy=True,
+            feedback_healthy=True,
+            external_busy=False,
+            axis=0.0,
+            feedback_position_rad=0.1,
+        )
+        self.assertFalse(head.require_neutral)
+        target = head.update(
+            0.2,
+            armed=True,
+            input_healthy=True,
+            feedback_healthy=True,
+            external_busy=False,
+            axis=1.0,
+            feedback_position_rad=0.1,
+        )
+        self.assertIsNotNone(target)
+        self.assertLess(target, 0.1)  # stick up -> negative pitch -> view up
+
+    def test_center_stops_and_resynchronizes_to_real_feedback(self) -> None:
+        head = controller()
+        head.new_session()
+        head.update(
+            0.0,
+            armed=True,
+            input_healthy=True,
+            feedback_healthy=True,
+            external_busy=False,
+            axis=0.0,
+            feedback_position_rad=0.2,
+        )
+        head.update(
+            0.1,
+            armed=True,
+            input_healthy=True,
+            feedback_healthy=True,
+            external_busy=False,
+            axis=-1.0,
+            feedback_position_rad=0.2,
+        )
+        self.assertTrue(head.active)
+        self.assertIsNone(
+            head.update(
+                0.2,
+                armed=True,
+                input_healthy=True,
+                feedback_healthy=True,
+                external_busy=False,
+                axis=0.0,
+                feedback_position_rad=0.25,
+            )
+        )
+        self.assertFalse(head.active)
+        self.assertEqual(head.target_rad, 0.25)
+        self.assertEqual(head.velocity_rad_s, 0.0)
+
+    def test_feedback_gap_or_external_command_fails_closed(self) -> None:
+        for feedback_healthy, external_busy in ((False, False), (True, True)):
+            head = controller()
+            head.new_session()
+            head.update(
+                0.0,
+                armed=True,
+                input_healthy=True,
+                feedback_healthy=True,
+                external_busy=False,
+                axis=0.0,
+                feedback_position_rad=0.0,
+            )
+            self.assertIsNone(
+                head.update(
+                    0.1,
+                    armed=True,
+                    input_healthy=True,
+                    feedback_healthy=feedback_healthy,
+                    external_busy=external_busy,
+                    axis=1.0,
+                    feedback_position_rad=0.0,
+                )
+            )
+            self.assertTrue(head.require_neutral)
+            self.assertFalse(head.active)
+
+    def test_target_is_clamped_to_software_working_range(self) -> None:
+        head = controller()
+        head.new_session()
+        head.update(
+            0.0,
+            armed=True,
+            input_healthy=True,
+            feedback_healthy=True,
+            external_busy=False,
+            axis=0.0,
+            feedback_position_rad=-0.17,
+        )
+        target = None
+        for index in range(1, 200):
+            target = head.update(
+                index * 0.1,
+                armed=True,
+                input_healthy=True,
+                feedback_healthy=True,
+                external_busy=False,
+                axis=1.0,
+                feedback_position_rad=-0.17,
+            )
+        self.assertEqual(target, head.min_pitch_rad)
+
+
+if __name__ == "__main__":
+    unittest.main()