From 679cd261f495c9dd7f09167f8f48c7c1fef755db Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Fri, 1 Nov 2024 01:27:52 +0900 Subject: [PATCH] Use ServoOnOffValues Enum instead of raw values --- rcb4/armh7interface.py | 19 +++++++++--------- rcb4/rcb4interface.py | 20 +++++++++++-------- .../node_scripts/rcb4_ros_bridge.py | 5 +++-- 3 files changed, 25 insertions(+), 19 deletions(-) diff --git a/rcb4/armh7interface.py b/rcb4/armh7interface.py index 6616693e..91263a0d 100644 --- a/rcb4/armh7interface.py +++ b/rcb4/armh7interface.py @@ -28,6 +28,7 @@ from rcb4.rcb4interface import deg_to_servovector from rcb4.rcb4interface import rcb4_dof from rcb4.rcb4interface import RCB4Interface +from rcb4.rcb4interface import ServoOnOffValues from rcb4.rcb4interface import ServoParams from rcb4.struct_header import c_vector from rcb4.struct_header import DataAddress @@ -489,7 +490,7 @@ def adjust_angle_vector(self, servo_ids=None, error_threshold=None): servo_ids = self.search_servo_ids() servo_ids = np.array(servo_ids) # Ignore free servo - servo_ids = servo_ids[self.reference_angle_vector(servo_ids=servo_ids) != 32768] + servo_ids = servo_ids[self.reference_angle_vector(servo_ids=servo_ids) != ServoOnOffValues.OFF.value] if len(servo_ids) == 0: return False @@ -722,13 +723,13 @@ def valid_servo_ids(self, servo_ids): def hold(self, servo_ids=None): if servo_ids is None: servo_ids = self.servo_sorted_ids - servo_vector = [32767] * len(servo_ids) + servo_vector = [ServoOnOffValues.ON.value] * len(servo_ids) return self.servo_angle_vector(servo_ids, servo_vector, velocity=1000) def free(self, servo_ids=None): if servo_ids is None: servo_ids = self.servo_sorted_ids - servo_vector = [32768] * len(servo_ids) + servo_vector = [ServoOnOffValues.OFF.value] * len(servo_ids) return self.servo_angle_vector(servo_ids, servo_vector, velocity=1000) def neutral(self, servo_ids=None, velocity=1000): @@ -776,11 +777,11 @@ def servo_angle_vector(self, servo_ids, servo_vector, velocity=127): if len(servo_ids) != len(servo_vector): raise ValueError("Length of servo_ids and servo_vector must be the same.") - # Update servo on/off states based on 32767 and 32768 values in servo_vector + # Update servo on/off states based on 32767(Servo ON) and 32768(Servo OFF) values in servo_vector for servo_id, angle in zip(servo_ids, servo_vector): - if angle == 32767: + if angle == ServoOnOffValues.ON.value: self.servo_on_states_dict[servo_id] = True - elif angle == 32768: + elif angle == ServoOnOffValues.OFF.value: self.servo_on_states_dict[servo_id] = False # Filter servo IDs based on their on state in servo_on_states_dict @@ -788,8 +789,8 @@ def servo_angle_vector(self, servo_ids, servo_vector, velocity=127): active_angles = [] for servo_id, angle in zip(servo_ids, servo_vector): if self.servo_on_states_dict.get(servo_id, False) or angle in ( - 32767, - 32768, + ServoOnOffValues.ON.value, + ServoOnOffValues.OFF.value, ): # Only include active servos active_ids.append(servo_id) @@ -1392,7 +1393,7 @@ def armh7_address(self): return self._armh7_address def servo_states(self): - servo_on_indices = np.where(self.reference_angle_vector() != 32768)[0] + servo_on_indices = np.where(self.reference_angle_vector() != ServoOnOffValues.OFF.value)[0] if len(servo_on_indices) > 0: servo_on_ids = self.servo_sorted_ids[servo_on_indices] # The worm module is always determined to be in the servo-off diff --git a/rcb4/rcb4interface.py b/rcb4/rcb4interface.py index 50602e5a..edbfc121 100644 --- a/rcb4/rcb4interface.py +++ b/rcb4/rcb4interface.py @@ -30,6 +30,10 @@ class CommandTypes(Enum): _None = 0xFF +class ServoOnOffValues(Enum): + ON = 32767 + OFF = 32768 + class ServoParams(Enum): Stretch = 0x01 Speed = 0x02 @@ -429,13 +433,13 @@ def valid_servo_ids(self, servo_ids): def hold(self, servo_ids=None): if servo_ids is None: servo_ids = self.servo_sorted_ids - servo_vector = [32767] * len(servo_ids) + servo_vector = [ServoOnOffValues.ON.value] * len(servo_ids) return self.servo_angle_vector(servo_ids, servo_vector, velocity=127) def free(self, servo_ids=None): if servo_ids is None: servo_ids = self.servo_sorted_ids - servo_vector = [32768] * len(servo_ids) + servo_vector = [ServoOnOffValues.OFF.value] * len(servo_ids) return self.servo_angle_vector(servo_ids, servo_vector, velocity=127) def neutral(self, servo_ids=None, velocity=127): @@ -479,11 +483,11 @@ def servo_angle_vector(self, servo_ids, servo_vector, velocity=127): if len(servo_ids) != len(servo_vector): raise ValueError("Length of servo_ids and servo_vector must be the same.") - # Update servo on/off states based on 32767 and 32768 values in servo_vector + # Update servo on/off states based on 32767 (Servo ON) and 32768 (Servo OFF) values in servo_vector for servo_id, angle in zip(servo_ids, servo_vector): - if angle == 32767: + if angle == ServoOnOffValues.ON.value: self.servo_on_states_dict[servo_id] = True - elif angle == 32768: + elif angle == ServoOnOffValues.OFF.value: self.servo_on_states_dict[servo_id] = False # Filter servo IDs based on their on state in servo_on_states_dict @@ -491,8 +495,8 @@ def servo_angle_vector(self, servo_ids, servo_vector, velocity=127): active_angles = [] for servo_id, angle in zip(servo_ids, servo_vector): if self.servo_on_states_dict.get(servo_id, False) or angle in ( - 32767, - 32768, + ServoOnOffValues.ON.value, + ServoOnOffValues.OFF.value, ): # Only include active servos active_ids.append(servo_id) @@ -643,7 +647,7 @@ def actuator_to_joint_matrix(self): return self._actuator_to_joint_matrix def servo_states(self): - servo_on_indices = np.where(self.reference_angle_vector() != 32768)[0] + servo_on_indices = np.where(self.reference_angle_vector() != ServoOnOffValues.OFF.value)[0] if len(servo_on_indices) > 0: servo_on_ids = self.servo_sorted_ids[servo_on_indices] return servo_on_ids diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py index 7c32fe33..8d561fd7 100644 --- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py +++ b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py @@ -42,6 +42,7 @@ from rcb4.armh7interface import ARMH7Interface from rcb4.rcb4interface import RCB4Interface +from rcb4.rcb4interface import ServoOnOffValues np.set_printoptions(precision=0, suppress=True) @@ -593,9 +594,9 @@ def servo_on_off_callback(self, goal): continue servo_ids.append(self.joint_name_to_id[joint_name]) if servo_on: - servo_vector.append(32767) + servo_vector.append(ServoOnOffValues.ON.value) else: - servo_vector.append(32768) + servo_vector.append(ServoOnOffValues.OFF.value) self._during_servo_off = True ret = serial_call_with_retry(