From 8ce0ce43fadd7ab771e67b8d9c37058e595d69c8 Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Sat, 13 Jan 2024 18:20:05 +0900 Subject: [PATCH] Sort input id internally servo_angle_vector function --- rcb4/armh7interface.py | 51 +++++++++++++++++-- rcb4/asm.py | 27 +++++++++- .../node_scripts/rcb4_ros_bridge.py | 10 ---- 3 files changed, 74 insertions(+), 14 deletions(-) diff --git a/rcb4/armh7interface.py b/rcb4/armh7interface.py index 39bb34fa..63ce76b0 100644 --- a/rcb4/armh7interface.py +++ b/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): diff --git a/rcb4/asm.py b/rcb4/asm.py index ea391e9a..44779776 100644 --- a/rcb4/asm.py +++ b/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)))) diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py index ae48d5bd..da24e1f9 100644 --- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py +++ b/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: