Support RCB4 board

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):

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,18 +218,21 @@ 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):
         if servo_ids is 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):

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,

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'}
 }