Sort input id internally servo_angle_vector function

rcb4/armh7interface.py

@@ -466,13 +466,58 @@ def neutral(self, servo_ids=None, velocity=1000):
                                        servo_vector,
                                        velocity=velocity)
 
-    def servo_angle_vector(self, servo_ids, servo_vector, velocity=1000):
+    def servo_angle_vector(self, servo_ids, servo_vector, velocity=127):
+        """Sends a command to control multiple servos.
+
+        This function sorts the servo IDs and corresponding angles,
+        constructs a command byte list, and sends it.
+        The velocity parameter is clamped between 1 and 255.
+
+        Parameters
+        ----------
+        servo_ids : array_like
+            Array of servo IDs. Each ID corresponds to a specific servo.
+        servo_vector : array_like
+            Array of angles (in servo pulse) for the servos.
+            Each angle corresponds to the servo ID at the same index
+            in servo_ids.
+        velocity : int, optional
+            Velocity for the servo movement, clamped between 1 and 255.
+            Default value is 127.
+
+        Raises
+        ------
+        ValueError
+            If the length of `servo_ids` does not match the length
+            of `servo_vector`.
+
+        Notes
+        -----
+        The function internally sorts `servo_ids` and `servo_vector`
+        based on the servo IDs to maintain the correspondence
+        between each servo ID and its angle. This sorted order is
+        used for constructing the command byte list.
+        """
+        if len(servo_ids) != len(servo_vector):
+            raise ValueError(
+                'Length of servo_ids and servo_vector must be the same.')
+
+        # Sort the servo vectors based on servo IDs
+        sorted_indices = np.argsort(servo_ids)
+        sorted_servo_ids = np.array(servo_ids)[sorted_indices]
+        sorted_servo_vector = np.array(servo_vector)[sorted_indices]
+
+        # Prepare the command byte list
         byte_list = [CommandTypes.MultiServoSingleVelocity.value] \
-            + encode_servo_ids_to_5bytes_bin(servo_ids) \
+            + encode_servo_ids_to_5bytes_bin(sorted_servo_ids) \
             + [rcb4_velocity(velocity)] \
-            + encode_servo_positions_to_bytes(servo_vector)
+            + encode_servo_positions_to_bytes(sorted_servo_vector)
+
+        # Add header (length) and checksum to the byte list
         byte_list.insert(0, 2 + len(byte_list))
         byte_list.append(rcb4_checksum(byte_list))
+
+        # send the command
         return self.serial_write(byte_list)
 
     def servo_param64(self, sid, param_names=None):

rcb4/asm.py

@@ -22,7 +22,32 @@ def rcb4_checksum(byte_list: List[int]) -> int:
     return sum(b & 0xff for b in byte_list) & 0xff
 
 
-def rcb4_velocity(v):
+def rcb4_velocity(v) -> int:
+    """Adjust the velocity value to be within the range of 1 to 255.
+
+    This function takes an input velocity `v`, rounds it to the nearest
+    integer, and then clamps the value within the range 1 to 255 inclusive.
+
+    Parameters
+    ----------
+    v : float
+        The input velocity value that needs to be adjusted.
+
+    Returns
+    -------
+    int
+        The adjusted velocity value, which is an integer
+        in the range from 1 to 255 inclusive.
+
+    Examples
+    --------
+    >>> rcb4_velocity(0.5)
+    1
+    >>> rcb4_velocity(127.8)
+    128
+    >>> rcb4_velocity(300)
+    255
+    """
     return max(1, min(255, int(round(v))))
 
 

ros/kxr_controller/node_scripts/rcb4_ros_bridge.py

@@ -275,9 +275,6 @@ def velocity_command_joint_state_callback(self, msg):
             return
         svs = np.matmul(self.arm.joint_to_actuator_matrix, tmp_av)[
             np.array(valid_indices)]
-        indices = np.argsort(servo_ids)
-        svs = svs[indices]
-        servo_ids = np.array(servo_ids)[indices]
         if self._prev_velocity_command is not None and np.allclose(
                 self._prev_velocity_command, svs):
             return
@@ -330,9 +327,6 @@ def command_joint_state_callback(self, msg):
             return
         svs = np.matmul(self.arm.joint_to_actuator_matrix, tmp_av)[
             np.array(valid_indices)]
-        indices = np.argsort(servo_ids)
-        svs = svs[indices]
-        servo_ids = np.array(servo_ids)[indices]
         try:
             if len(worm_indices) > 0:
                 worm_av_tmp = np.array(self.arm.read_cstruct_slot_vector(
@@ -359,10 +353,6 @@ def servo_on_off_callback(self, goal):
             else:
                 servo_vector.append(32768)
                 self.joint_servo_on[joint_name] = False
-        servo_vector = np.array(servo_vector)
-        indices = np.argsort(servo_ids)
-        servo_vector = servo_vector[indices]
-        servo_ids = np.array(servo_ids)[indices]
         try:
             self.arm.servo_angle_vector(servo_ids, servo_vector, velocity=10)
         except RuntimeError as e: