Skip to content

Commit

Permalink
Merge pull request #30 from iory/rcb4-mini
Browse files Browse the repository at this point in the history
Support RCB4 board
  • Loading branch information
iory authored Feb 7, 2024
2 parents f2896d8 + b7e46b5 commit 678b009
Show file tree
Hide file tree
Showing 4 changed files with 48 additions and 19 deletions.
28 changes: 28 additions & 0 deletions rcb4/armh7interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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 \
Expand Down Expand Up @@ -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):
Expand Down
15 changes: 9 additions & 6 deletions rcb4/rcb4interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand Down Expand Up @@ -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

Expand Down Expand Up @@ -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):
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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):
Expand Down
16 changes: 7 additions & 9 deletions ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,6 @@
import yaml

from rcb4.armh7interface import ARMH7Interface
from rcb4.armh7interface import WormmoduleStruct


np.set_printoptions(precision=0, suppress=True)
Expand Down Expand Up @@ -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()
Expand All @@ -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,
Expand Down
8 changes: 4 additions & 4 deletions ros/kxr_models/config/kxrmw4a6h2m_servo_config.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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'}
}

0 comments on commit 678b009

Please sign in to comment.