Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Support RCB4 board #30

Merged
merged 1 commit into from
Feb 7, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
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'}
}
Loading