From 9576ae5e86b520a26e4e4e76911556b4de6928ea Mon Sep 17 00:00:00 2001
From: iory <ab.ioryz@gmail.com>
Date: Thu, 18 Jan 2024 01:47:26 +0900
Subject: [PATCH] Support empty list

---
 rcb4/armh7interface.py                  |  8 ++++++++
 tests/rcb4_tests/test_armh7interface.py | 13 ++++++++++---
 2 files changed, 18 insertions(+), 3 deletions(-)

diff --git a/rcb4/armh7interface.py b/rcb4/armh7interface.py
index c298924e..db430292 100644
--- a/rcb4/armh7interface.py
+++ b/rcb4/armh7interface.py
@@ -411,6 +411,8 @@ def servo_id_to_index(self, servo_ids=None):
                 for i, id in enumerate(servo_ids)}
 
     def sequentialized_servo_ids(self, servo_ids):
+        if len(servo_ids) == 0:
+            return np.empty(shape=0, dtype=np.uint8)
         return self._servo_id_to_sequentialized_servo_id[
             np.array(servo_ids)].astype(np.uint8)
 
@@ -447,6 +449,8 @@ def angle_vector(self, av=None, servo_ids=None, velocity=127):
         if av is not None:
             return self._send_angle_vector(av, servo_ids, velocity)
         all_servo_ids = self.search_servo_ids()
+        if len(all_servo_ids) == 0:
+            return np.empty(shape=0)
         av = np.append(self._angle_vector()[all_servo_ids], 1)
         av = np.matmul(av.T, self.actuator_to_joint_matrix.T)[:-1]
         id_to_index = self.servo_id_to_index(all_servo_ids)
@@ -456,6 +460,8 @@ def angle_vector(self, av=None, servo_ids=None, velocity=127):
             av[id_to_index[
                 self.worm_id_to_servo_id[worm_idx]]] = worm_av[worm_idx]
         if servo_ids is not None:
+            if len(servo_ids) == 0:
+                return np.empty(shape=0)
             av = av[self.sequentialized_servo_ids(servo_ids)]
         return av
 
@@ -465,6 +471,8 @@ def angle_vector_to_servo_angle_vector(self, av, servo_ids=None):
         if len(av) != len(servo_ids):
             raise ValueError(
                 'Length of servo_ids and angle_vector must be the same.')
+        if len(servo_ids) == 0:
+            return np.empty(shape=0)
         seq_indices = self.sequentialized_servo_ids(servo_ids)
         tmp_av = np.append(np.zeros(len(self.servo_sorted_ids)), 1)
         tmp_av[seq_indices] = np.array(av)
diff --git a/tests/rcb4_tests/test_armh7interface.py b/tests/rcb4_tests/test_armh7interface.py
index fd80803e..b2107edf 100644
--- a/tests/rcb4_tests/test_armh7interface.py
+++ b/tests/rcb4_tests/test_armh7interface.py
@@ -16,7 +16,12 @@ def setUpClass(cls):
         cls.interface = ARMH7Interface()
         cls.interface.auto_open()
 
+    def test_reference_angle_vector(self):
+        self.interface.reference_angle_vector([])
+
     def test_sequentialized_servo_ids(self):
+        self.interface.sequentialized_servo_ids([])
+
         testing.assert_array_equal(
             self.interface.sequentialized_servo_ids([32, 34]),
             [0, 1])
@@ -56,11 +61,13 @@ def test_angle_vector(self):
         time.sleep(4.0)
         self.interface.free()
 
+        self.interface.angle_vector([], [])
+
     def test_servo_angle_vector(self):
         self.interface.hold()
         self.interface.neutral()
-        reference = [8000,]
-        target_servo_ids = [32,]
+        reference = [8000, ]
+        target_servo_ids = [32, ]
 
         self.interface.servo_angle_vector(
             target_servo_ids,
@@ -77,7 +84,7 @@ def test_servo_angle_vector(self):
                 "Servo angles are beyond the acceptable range.")
 
         # multi velocities
-        reference = [7500,]
+        reference = [7500, ]
         self.interface.servo_angle_vector(
             target_servo_ids,
             reference,