Merge pull request #24 from iory/ci-worm

Support worm module hardware ci

rcb4/armh7interface.py

@@ -424,6 +424,7 @@ def _send_angle_vector(self, av, servo_ids=None, velocity=127):
         if len(av) != len(servo_ids):
             raise ValueError(
                 'Length of servo_ids and angle_vector must be the same.')
+        av = np.array(av)
         worm_av = []
         worm_indices = []
         for i, (angle, servo_id) in enumerate(zip(av, servo_ids)):
@@ -445,15 +446,17 @@ def _send_angle_vector(self, av, servo_ids=None, velocity=127):
     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)
-        servo_ids = self.search_servo_ids()
-        av = np.append(self._angle_vector()[servo_ids], 1)
+        all_servo_ids = self.search_servo_ids()
+        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(servo_ids)
+        id_to_index = self.servo_id_to_index(all_servo_ids)
         worm_av = self.read_cstruct_slot_vector(
             WormmoduleStruct, slot_name='present_angle')
         for worm_idx in self.search_worm_ids():
             av[id_to_index[
                 self.worm_id_to_servo_id[worm_idx]]] = worm_av[worm_idx]
+        if servo_ids is not None:
+            av = av[self.sequentialized_servo_ids(servo_ids)]
         return av
 
     def angle_vector_to_servo_angle_vector(self, av, servo_ids=None):
@@ -975,7 +978,16 @@ def servo_states(self):
         servo_on_indices = np.where(
             self.reference_angle_vector() != 32768)[0]
         if len(servo_on_indices) > 0:
-            return self.servo_sorted_ids[servo_on_indices]
+            servo_on_ids = self.servo_sorted_ids[servo_on_indices]
+            # The worm module is always determined to be in the servo-off
+            # state because it is erroneously recognized
+            # as being in the servo-on state.
+            if self._servo_id_to_worm_id is not None \
+               and len(self._servo_id_to_worm_id) > 0:
+                mask = np.isin(servo_on_ids,
+                               list(self._servo_id_to_worm_id))
+                servo_on_ids = servo_on_ids[~mask]
+            return servo_on_ids
         return []
 
 

ros/kxr_models/urdf/kxr_test_head.urdf

@@ -71,7 +71,7 @@
     <child  link="HEAD_LINK1"/>
     <origin xyz="0 0 0.036" rpy="0 -0 0 "/>
     <axis   xyz="0 -1 0"/>
-    <limit lower="-2.0944" upper="2.0944" effort="0.656248" velocity="7.47998" />
+    <limit lower="-2.0944" upper="2.0944" effort="0.656248" velocity="0.5" />
     <dynamics damping="0.2" friction="0" />
   </joint>
 </robot>

tests/rcb4_tests/test_armh7interface.py

@@ -58,33 +58,38 @@ def test_angle_vector(self):
     def test_servo_angle_vector(self):
         self.interface.hold()
         self.interface.neutral()
-        reference = [8000, 8000]
+        reference = [8000,]
+        target_servo_ids = [32,]
 
         self.interface.servo_angle_vector(
-            [32, 34],
+            target_servo_ids,
             reference,
             velocity=1)
         time.sleep(1.0)
         testing.assert_array_almost_equal(
-            self.interface.reference_angle_vector(),
+            self.interface.reference_angle_vector(target_servo_ids),
             reference)
         time.sleep(4.0)
-        if np.any(np.abs(self.interface.angle_vector() - 16) > 2.0):
-            self.assertRaises()
+        if np.any(np.abs(self.interface.angle_vector(
+                servo_ids=target_servo_ids) - 16) > 2.0):
+            raise AssertionError(
+                "Servo angles are beyond the acceptable range.")
 
         # multi velocities
-        reference = [7500, 7500]
+        reference = [7500,]
         self.interface.servo_angle_vector(
-            [32, 34],
+            target_servo_ids,
             reference,
             velocity=[10, 1])
         time.sleep(1.0)
         testing.assert_array_almost_equal(
-            self.interface.reference_angle_vector(),
+            self.interface.reference_angle_vector(target_servo_ids),
             reference)
         time.sleep(4.0)
-        if np.any(np.abs(self.interface.angle_vector() - 0) > 2.0):
-            self.assertRaises()
+        if np.any(np.abs(self.interface.angle_vector(
+                servo_ids=target_servo_ids) - 0) > 2.0):
+            raise AssertionError(
+                "Servo angles are beyond the acceptable range.")
 
         self.interface.neutral()
         time.sleep(4.0)