diff --git a/rcb4/armh7interface.py b/rcb4/armh7interface.py index 232b9580..2c485c7a 100644 --- a/rcb4/armh7interface.py +++ b/rcb4/armh7interface.py @@ -25,6 +25,7 @@ from rcb4.data import kondoh7_elf from rcb4.rcb4interface import CommandTypes from rcb4.rcb4interface import rcb4_dof +from rcb4.rcb4interface import RCB4Interface from rcb4.rcb4interface import ServoParams from rcb4.struct_header import c_vector from rcb4.struct_header import DataAddress @@ -167,6 +168,32 @@ def open(self, port='/dev/ttyUSB1', baudrate=1000000, timeout=0.01): self.all_jointbase_sensors() return True + @staticmethod + def from_port(port=None): + ports = serial.tools.list_ports.comports() + if port is not None: + for cand in ports: + if cand.device == port: + if port.vid == 0x165C and port.pid == 0x0008: + interface = RCB4Interface() + interface.open(port) + return interface + interface = ARMH7Interface() + interface.open(port) + return interface + + interface = ARMH7Interface() + ret = interface.auto_open() + print(ret) + if ret is True: + return interface + + interface = RCB4Interface() + ret = interface.auto_open() + print(ret) + if ret is True: + return interface + def auto_open(self): system_info = platform.uname() if 'amlogic' in system_info.version \ @@ -527,6 +554,7 @@ def search_servo_ids(self): if len(servo_indices): self._servo_id_to_sequentialized_servo_id[servo_indices] = \ np.arange(len(servo_indices)) + self.joint_to_actuator_matrix return servo_indices def valid_servo_ids(self, servo_ids): diff --git a/rcb4/rcb4interface.py b/rcb4/rcb4interface.py index b9c7d69d..caef1f03 100644 --- a/rcb4/rcb4interface.py +++ b/rcb4/rcb4interface.py @@ -13,7 +13,6 @@ from rcb4.asm import rcb4_checksum from rcb4.asm import rcb4_servo_svector from rcb4.asm import rcb4_velocity -from rcb4.struct_header import ServoStruct class CommandTypes(Enum): @@ -52,6 +51,7 @@ def __init__(self): self.lock = Lock() self.serial = None self.servo_sorted_ids = None + self.wheel_servo_sorted_ids = None self._joint_to_actuator_matrix = None self._actuator_to_joint_matrix = None @@ -93,8 +93,7 @@ def open(self, port='/dev/ttyUSB0', if ack is not True: return False self.check_firmware_version() - # self.copy_worm_params_from_flash() - # self.search_servo_ids() + self.search_servo_ids() return True def auto_open(self): @@ -219,7 +218,7 @@ def reference_angle_vector(self, servo_ids=None): servo_ids = self.search_servo_ids() if len(servo_ids) == 0: return np.empty(shape=0) - ref_angles = interface._angle_vector('reference')[servo_ids] + ref_angles = self._angle_vector('reference')[servo_ids] return ref_angles def servo_error(self, servo_ids=None): @@ -227,10 +226,13 @@ def servo_error(self, servo_ids=None): servo_ids = self.search_servo_ids() if len(servo_ids) == 0: return np.empty(shape=0) - error_angles = self.read_cstruct_slot_vector( - ServoStruct, slot_name='error_angle')[servo_ids] + error_angles = self._angle_vector('error')[servo_ids] return error_angles + def servo_id_to_index(self, servo_id): + if self.valid_servo_ids([servo_id]): + return self.sequentialized_servo_ids([servo_id])[0] + def sequentialized_servo_ids(self, servo_ids): if len(servo_ids) == 0: return np.empty(shape=0, dtype=np.uint8) @@ -326,6 +328,7 @@ def search_servo_ids(self): if len(servo_indices): self._servo_id_to_sequentialized_servo_id[servo_indices] = \ np.arange(len(servo_indices)) + self.joint_to_actuator_matrix return servo_indices def valid_servo_ids(self, servo_ids): diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py index a08297a6..d831d8f9 100644 --- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py +++ b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py @@ -18,7 +18,6 @@ import yaml from rcb4.armh7interface import ARMH7Interface -from rcb4.armh7interface import WormmoduleStruct np.set_printoptions(precision=0, suppress=True) @@ -130,25 +129,28 @@ def __init__(self): JointState, queue_size=1) - self.interface = ARMH7Interface() - ret = self.interface.auto_open() - if ret is not True: + self.interface = ARMH7Interface.from_port() + if self.interface is None: rospy.logerr('Could not open port!') sys.exit(1) self._prev_velocity_command = None + wheel_servo_sorted_ids = [] for _, info in servo_infos.items(): if isinstance(info, int): continue servo_id = info['id'] direction = info['direction'] + if 'type' in info and info['type'] == 'continuous': + wheel_servo_sorted_ids.append(servo_id) idx = self.interface.servo_id_to_index(servo_id) if idx is None: continue self.interface._joint_to_actuator_matrix[idx, idx] = \ direction * self.interface._joint_to_actuator_matrix[idx, idx] + if self.interface.wheel_servo_sorted_ids is None: + self.interface.wheel_servo_sorted_ids = wheel_servo_sorted_ids - self.servo_id_to_worm_id = self.interface.servo_id_to_worm_id self.set_fullbody_controller(clean_namespace) self.set_initial_positions(clean_namespace) self.check_servo_states() @@ -157,10 +159,6 @@ def __init__(self): self.proc_kxr_controller = run_kxr_controller( namespace=clean_namespace) - self.worm_servo_ids = [ - self.interface.memory_cstruct(WormmoduleStruct, idx).servo_id - for idx in self.interface.worm_sorted_ids] - self.command_joint_state_sub = rospy.Subscriber( clean_namespace + '/command_joint_state', JointState, queue_size=1, diff --git a/ros/kxr_models/config/kxrmw4a6h2m_servo_config.yaml b/ros/kxr_models/config/kxrmw4a6h2m_servo_config.yaml index 26aa85bf..4eb8dcef 100644 --- a/ros/kxr_models/config/kxrmw4a6h2m_servo_config.yaml +++ b/ros/kxr_models/config/kxrmw4a6h2m_servo_config.yaml @@ -9,8 +9,8 @@ joint_name_to_servo_id: { LARM_JOINT5: 29, LARM_JOINT6: 19, LARM_JOINT7: 19, - LLEG_JOINT0: {'id': 25, 'direction': -1}, - LLEG_JOINT1: {'id': 17, 'direction': -1}, + LLEG_JOINT0: {'id': 1, 'direction': -1, 'type': 'continuous'}, + LLEG_JOINT1: {'id': 11, 'direction': -1, 'type': 'continuous'}, RARM_JOINT0: 2, RARM_JOINT1: 4, RARM_JOINT2: 22, @@ -19,6 +19,6 @@ joint_name_to_servo_id: { RARM_JOINT5: 28, RARM_JOINT6: 18, RARM_JOINT7: 18, - RLEG_JOINT0: {'id': 24, 'direction': 1}, - RLEG_JOINT1: {'id': 16, 'direction': 1} + RLEG_JOINT0: {'id': 0, 'direction': 1, 'type': 'continuous'}, + RLEG_JOINT1: {'id': 10, 'direction': 1, 'type': 'continuous'} }