diff --git a/rcb4/armh7interface.py b/rcb4/armh7interface.py
index 12c2febf..3ae2c7cb 100644
--- a/rcb4/armh7interface.py
+++ b/rcb4/armh7interface.py
@@ -375,9 +375,14 @@ def read_jointbase_sensor_ids(self):
             self.id_vector = list(sorted(ret))
         return self.id_vector
 
-    def reference_angle_vector(self):
-        return self.read_cstruct_slot_vector(
-            ServoStruct, slot_name='ref_angle')
+    def reference_angle_vector(self, servo_ids=None):
+        if servo_ids is None:
+            servo_ids = self.search_servo_ids()
+        if len(servo_ids) == 0:
+            return np.empty(shape=0)
+        ref_angles = self.read_cstruct_slot_vector(
+            ServoStruct, slot_name='ref_angle')[servo_ids]
+        return ref_angles
 
     def servo_id_to_index(self, servo_ids=None):
         if servo_ids is None:
@@ -409,6 +414,7 @@ def search_servo_ids(self):
             servo = self.memory_cstruct(ServoStruct, idx)
             if servo.flag > 0:
                 indices.append(idx)
+        indices = np.array(indices)
         self.servo_sorted_ids = indices
         return indices
 
diff --git a/tests/rcb4_tests/test_armh7interface.py b/tests/rcb4_tests/test_armh7interface.py
index 36ddcbbe..40b49d49 100644
--- a/tests/rcb4_tests/test_armh7interface.py
+++ b/tests/rcb4_tests/test_armh7interface.py
@@ -2,6 +2,7 @@
 import unittest
 
 import numpy as np
+from numpy import testing
 
 from rcb4.armh7interface import ARMH7Interface
 
@@ -19,10 +20,14 @@ def setUpClass(cls):
     def test_servo_angle_vector(self):
         self.interface.hold()
         self.interface.neutral()
+        reference = [8000, 8000]
         self.interface.servo_angle_vector(
             [32, 34],
-            [8000, 8000],
+            reference,
             velocity=0)
+        testing.assert_array_almost_equal(
+            self.interface.reference_angle_vector(),
+            reference)
         time.sleep(4.0)
         if np.any(np.abs(self.interface.angle_vector() - 16) > 2.0):
             self.assertRaises()