From 1c6b831201a2f6ac4642623bf80889c80e464681 Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Tue, 12 Nov 2024 14:49:43 +0900 Subject: [PATCH 01/28] Add ics class to manipulate servo config --- rcb4/ics.py | 506 ++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 506 insertions(+) create mode 100644 rcb4/ics.py diff --git a/rcb4/ics.py b/rcb4/ics.py new file mode 100644 index 00000000..a85f91bc --- /dev/null +++ b/rcb4/ics.py @@ -0,0 +1,506 @@ +import sys +import time + +from colorama import Fore +from colorama import Style +import readchar +import serial +import serial.tools.list_ports +import yaml + + +def load_and_process_yaml(yaml_path): + with open(yaml_path) as f: + joint_data = yaml.safe_load(f) + servo_candidates = [] + servo_id_to_rotation = {} + + for _joint_name, properties in joint_data.get("joint_name_to_servo_id", {}).items(): + candidate_servo_id = properties["id"] // 2 + rotation = False + if properties.get("type") == "continuous": + rotation = True + servo_id_to_rotation[candidate_servo_id] = rotation + servo_candidates.append(candidate_servo_id) + servo_candidates = sorted(set(servo_candidates)) + return servo_candidates, servo_id_to_rotation + + +def format_baud(baud): + """Convert baud rate to a more readable format.""" + if baud >= 1_000_000: + return f"{baud / 1_000_000:.2f}M" + elif baud >= 1_000: + return f"{baud / 1_000:.0f}k" + else: + return str(baud) + + +class ICSServoController: + def __init__(self, baudrate=1250000, yaml_path=None): + if baudrate not in [1250000, 625000, 115200]: + print(f";; baud={baudrate} is wrong.") + print(";; baud should be one of 1250000, 625000, 115200") + print("Use default baudrate 1250000") + baudrate = 1250000 + if yaml_path is not None: + self.servo_candidates, self.servo_id_to_rotation = load_and_process_yaml( + yaml_path + ) + else: + self.servo_id_to_rotation = None + self.servo_candidates = list(range(18)) + self.servo_id_index = 0 + self.servo_id = 0 + self.selected_index = 0 + self.baudrate = baudrate + self.timeout = 0.1 + self.ics = None + self.servo_eeprom_params64 = [ + ("fix-header", [1, 2]), + ("stretch-gain", [3, 4]), + ("speed", [5, 6]), + ("punch", [7, 8]), + ("dead-band", [9, 10]), + ("dumping", [11, 12]), + ("safe-timer", [13, 14]), + ("mode-flag-b7slave-b4rotation-b3pwm-b1free-b0reverse", [15, 16]), + ("pulse-max-limit", [17, 18, 19, 20]), + ("pulse-min-limit", [21, 22, 23, 24]), + ("fix-dont-change-25", [25, 26]), + ("ics-baud-rate-10-115200-00-1250000", [27, 28]), + ("temperature-limit", [29, 30]), + ("current-limit", [31, 32]), + ("fix-dont-change-33", [33, 34]), + ("fix-dont-change-35", [35, 36]), + ("fix-dont-change-37", [37, 38]), + ("fix-dont-change-39", [39, 40]), + ("fix-dont-change-41", [41, 42]), + ("fix-dont-change-43", [43, 44]), + ("fix-dont-change-45", [45, 46]), + ("fix-dont-change-47", [47, 48]), + ("fix-dont-change-49", [49, 50]), + ("response", [51, 52]), + ("user-offset", [53, 54]), + ("fix-dont-change-55", [55, 56]), + ("servo-id", [57, 58]), + ("stretch-1", [59, 60]), + ("stretch-2", [61, 62]), + ("stretch-3", [63, 64]), + ] + + def open_connection(self): + ports = serial.tools.list_ports.comports() + for p in ports: + if p.vid == 0x165C and p.pid == 0x08: + for baudrate in [115200, 625000, 1250000]: + try: + self.ics = serial.Serial( + f"/dev/{p.name}", + baudrate, + timeout=self.timeout, + parity=serial.PARITY_EVEN, + ) + if baudrate != self.baudrate: + self.baud(self.baudrate) + return True + except IndexError: + continue + return False + + def read_baud(self): + _, result = self.read_param() + return result["baud"] + + def baud(self, baud=None, servo_id=None): + if baud is None: + return self.read_baud() + if baud not in [1250000, 625000, 115200]: + print(f";; baud={baud} is wrong.") + print(";; baud should be one of 1250000, 625000, 115200") + return None + + if servo_id is None: + servo_id = self.get_servo_id() + + ics_param64, _ = self.read_param() + if baud == 1250000: + ics_param64[27] = 0 + elif baud == 625000: + ics_param64[27] = 1 + elif baud == 115200: + ics_param64[27] = 10 + + # Send updated parameters to the device + self.set_param(ics_param64, servo_id) + # Re-open the connection with the updated baud rate + self.open_connection() + return self.read_baud() + + def get_servo_id(self): + self.ics.write(bytes([0xFF, 0x00, 0x00, 0x00])) + time.sleep(0.1) + ret = self.ics.read(5) + return ret[4] & 0x1F + + def set_servo_id(self, servo_id): + self.ics.write(bytes([0xE0 | (0x1F & servo_id), 0x01, 0x01, 0x01])) + time.sleep(0.1) + ret = self.ics.read(5) + return 0x1F & ret[4] + + def reset_servo_position(self): + self.set_angle(7500) + print(f"{Fore.YELLOW}Servo reset to zero position.{Fore.RESET}") + + def toggle_rotation_mode(self): + rotation_mode = self.read_rotation() + self.set_rotation(not rotation_mode) + rotation_mode = self.read_rotation() + mode_text = "Enabled" if rotation_mode else "Disabled" + print(f"{Fore.CYAN}Rotation mode set to {mode_text}{Fore.RESET}") + + def set_free_mode(self): + free_mode = self.read_free() + self.set_free(not free_mode) + time.sleep(0.1) + free_mode = self.read_free() + mode_text = "Enabled" if free_mode else "Disabled" + print(f"{Fore.MAGENTA}Free mode set to {mode_text}{Fore.RESET}") + + def increase_angle(self): + angle = self.read_angle() + angle = min(10000, angle + 500) + self.set_angle(angle) + print(f"{Fore.BLUE}Angle increased to {angle}{Fore.RESET}") + + def decrease_angle(self): + angle = self.read_angle() + angle = max(0, angle - 500) + self.set_angle(angle) + print(f"{Fore.RED}Angle decreased to {angle}{Fore.RESET}") + + def parse_param64_key_value(self, v): + alist = {} + for param in self.servo_eeprom_params64: + param_name, indices = param[0], param[1] + alist[param_name] = self._4bit2num(indices, v) + + baud_value = alist.get("ics-baud-rate-10-115200-00-1250000", 0) + baud_rate = {10: 115200, 1: 625000, 0: 1250000}.get(baud_value, None) + mode_flag_value = alist.get( + "mode-flag-b7slave-b4rotation-b3pwm-b1free-b0reverse", 0 + ) + alist.update(self.ics_flag_dict(mode_flag_value)) + alist.update({"servo-id": alist.get("servo-id", 0), "baud": baud_rate}) + return alist + + def _4bit2num(self, lst, v): + sum_val = 0 + for i in lst: + sum_val = (sum_val << 4) + (v[i - 1] & 0x0F) + return sum_val + + def ics_flag_dict(self, v): + return { + "slave": (v & 0xF0) >> 4 & 0x8 == 0x8, + "rotation": (v & 0xF0) >> 4 & 0x1 == 0x1, + "free": v & 0xF & 0x2 == 0x2, + "reverse": v & 0xF & 0x1 == 0x1, + "serial": v & 0xF & 0x8 == 0x8, + } + + def set_flag(self, flag_name, value, servo_id=None): + if servo_id is None: + servo_id = self.get_servo_id() + ics_param64, _ = self.read_param(servo_id=servo_id) + + # Calculate byte and bit for manipulation + byte_idx = 14 if flag_name in ["slave", "rotation"] else 15 + bit_position = 3 if flag_name in ["slave", "serial"] else 0 + mask = 1 << bit_position + + # Set or clear the bit based on the `value` argument + if value: + ics_param64[byte_idx] |= mask # Set the bit + else: + ics_param64[byte_idx] &= ~mask + # Clear the bit + # Set updated parameters to the servo + self.set_param(ics_param64, servo_id=servo_id) + + def set_slave(self, slave=None, servo_id=None): + return self.set_flag("slave", slave, servo_id=servo_id) + + def set_rotation(self, rotation=None, servo_id=None): + return self.set_flag("rotation", rotation, servo_id=servo_id) + + def set_serial(self, serial=None, servo_id=None): + return self.set_flag("serial", serial, servo_id=servo_id) + + def set_reverse(self, reverse=None, servo_id=None): + return self.set_flag("reverse", reverse, servo_id=servo_id) + + def set_free(self, free, servo_id=None): + if servo_id is None: + servo_id = self.get_servo_id() + + ics_param64, _ = self.read_param() + if free is None or free == 0: + ics_param64[15] = ics_param64[15] & 0xD + else: + ics_param64[15] = ics_param64[15] | 0x2 + self.set_param(ics_param64, servo_id) + if servo_id != self.read_param()[1].get("servo-id"): + return self.read_free(servo_id) + + def read_free(self, servo_id=None): + if servo_id is None: + servo_id = self.get_servo_id() + _, result = self.read_param() + return result.get("free", None) + + def read_rotation(self, servo_id=None): + _, result = self.read_param(servo_id=servo_id) + return result["rotation"] + + def set_param(self, ics_param64, servo_id=None): + if servo_id is None: + servo_id = self.get_servo_id() + self.ics.write(bytes([0xC0 | servo_id, 0x00] + ics_param64)) + time.sleep(0.5) + ret = self.ics.read(68) + ret_ics_param64 = ret[4:] + self.parse_param64_key_value(ret_ics_param64) + + def close_connection(self): + if self.ics and self.ics.is_open: + self.ics.close() + + def display_status(self): + options = [ + "Servo ID", + "Angle", + "Baud Rate", + "Rotation Mode", + "Slave Mode", + "Reverse Mode", + "Serial Mode", + "Free", + ] + selectable_options = ["Servo ID", "Angle"] + try: + use_previous_result = False + while True: + if not self.ics or not self.ics.is_open: + # Clear the previous output at the start of each loop + sys.stdout.write("\033[H\033[J") + sys.stdout.flush() + print(f"{Fore.RED}Connection is not open.{Style.RESET_ALL}") + print( + f"{Fore.RED}Are you using dual USB? Is it set to ICS mode? Have you connected the servo?{Style.RESET_ALL}" + ) + print( + f"{Fore.RED}To establish the connection, please execute the following commands in Linux:{Style.RESET_ALL}" + ) + print() + print(f"{Fore.RED} $ sudo su{Style.RESET_ALL}") + print(f"{Fore.RED} modprobe ftdi-sio{Style.RESET_ALL}") + print( + f"{Fore.RED} echo 165C 0008 > /sys/bus/usb-serial/drivers/ftdi_sio/new_id{Style.RESET_ALL}" + ) + print(f"{Fore.RED} exit{Style.RESET_ALL}") + try: + ret = self.open_connection() + except Exception: + continue + if ret is False: + time.sleep(1.0) + continue + sys.stdout.write("\033[H\033[J") + sys.stdout.flush() + + # Print servo status + print("--- Servo Status ---") + if use_previous_result is False: + _, result = self.read_param() + for i, option in enumerate(options): + if i == self.selected_index: + print( + f"{Fore.YELLOW}>> {option}: {self.get_status(option, result)}{Style.RESET_ALL}" + ) + else: + print(f" {option}: {self.get_status(option, result)}") + print("----------------------\n") + print( + "Use ↑↓ to navigate, ←→ to adjust Servo ID or Servo Angles, 'q' to quit." + ) + print("Press 'z' to reset servo position") + print( + "Press 'r' to toggle rotation mode (enables continuous wheel-like rotation)" + ) + print("Press 'f' to set free mode\n") + + use_previous_result = False + + # Get key input without Enter + key = readchar.readkey() + + # Perform actions based on key + if key == "z": + self.reset_servo_position() + elif key == "r": + self.toggle_rotation_mode() + elif key == "f": + self.set_free_mode() + elif key == "q": + print("Exiting...") + break + elif key == readchar.key.UP: + self.selected_index = (self.selected_index - 1) % len( + selectable_options + ) + use_previous_result = True + elif key == readchar.key.DOWN: + self.selected_index = (self.selected_index + 1) % len( + selectable_options + ) + use_previous_result = True + elif ( + key == readchar.key.ENTER + and selectable_options[self.selected_index] == "Servo ID" + ): + self.set_servo_id(self.servo_id) + time.sleep(0.3) + if self.servo_id_to_rotation is not None: + self.set_rotation(self.servo_id_to_rotation[self.servo_id]) + time.sleep(0.3) + elif ( + key == readchar.key.LEFT + and selectable_options[self.selected_index] == "Servo ID" + ): + use_previous_result = True + if self.servo_id_index == 0: + self.servo_id_index = len(self.servo_candidates) - 1 + else: + self.servo_id_index = max(0, self.servo_id_index - 1) + self.servo_id = self.servo_candidates[self.servo_id_index] + elif ( + key == readchar.key.RIGHT + and selectable_options[self.selected_index] == "Servo ID" + ): + use_previous_result = True + if self.servo_id_index == len(self.servo_candidates) - 1: + self.servo_id_index = 0 + else: + self.servo_id_index = min( + len(self.servo_candidates) - 1, self.servo_id_index + 1 + ) + self.servo_id = self.servo_candidates[self.servo_id_index] + elif ( + key == readchar.key.LEFT + and selectable_options[self.selected_index] == "Angle" + ): + self.decrease_angle() + elif ( + key == readchar.key.RIGHT + and selectable_options[self.selected_index] == "Angle" + ): + self.increase_angle() + # Flush the output to ensure it displays correctly + sys.stdout.flush() + + # Wait for a short period before updating again + # time.sleep(0.1) + + except KeyboardInterrupt: + print("\nDisplay stopped by user.") + + def get_status(self, option, param=None): + """Return the status based on the selected option.""" + if option == "Servo ID": + if param is not None: + current_servo_id = param["servo-id"] + else: + current_servo_id = self.get_servo_id() + + str = f"{Style.RESET_ALL}[" + for i, servo_id in enumerate(self.servo_candidates): + if i == self.servo_id_index: + str += f"{Fore.GREEN}{servo_id}{Style.RESET_ALL} " + else: + str += f"{servo_id} " + str += "]" + return f"{current_servo_id} -> {str}" + elif option == "Angle": + return f"{self.read_angle()}" + elif option == "Baud Rate": + if param is not None: + baudrate = param["baud"] + else: + baudrate = self.read_baud() + return f"{format_baud(baudrate)}bps" + elif option == "Rotation Mode": + if param is not None: + rotation = param["rotation"] + else: + rotation = self.read_rotation() + return f"{Fore.GREEN if rotation else Fore.RED}{'Enabled' if rotation else 'Disabled'}{Style.RESET_ALL}" + elif option == "Slave Mode": + if param is not None: + slave = param["slave"] + else: + slave = self.read_slave() + return f"{Fore.GREEN if slave else Fore.RED}{'Enabled' if slave else 'Disabled'}{Style.RESET_ALL}" + elif option == "Reverse Mode": + if param is not None: + reverse = param["reverse"] + else: + reverse = self.read_reverse() + return f"{Fore.GREEN if reverse else Fore.RED}{'Enabled' if reverse else 'Disabled'}{Style.RESET_ALL}" + elif option == "Serial Mode": + if param is not None: + serial = param["serial"] + else: + serial = self.read_serial() + return f"{Fore.GREEN if serial else Fore.RED}{'Enabled' if serial else 'Disabled'}{Style.RESET_ALL}" + elif option == "Free": + if param is not None: + free = param["free"] + else: + free = self.read_free() + return f"{Fore.GREEN if free else Fore.RED}{'Enabled' if free else 'Disabled'}{Style.RESET_ALL}" + + def read_angle(self, sid=None): + if sid is None: + sid = self.get_servo_id() + self.ics.write(bytes([0xA0 | (sid & 0x1F), 5])) + time.sleep(0.1) + v = self.ics.read(6) + angle = ((v[4] & 0x7F) << 7) | (v[5] & 0x7F) + return angle + + def set_angle(self, v=7500, servo_id=None): + if servo_id is None: + servo_id = self.get_servo_id() + self.ics.write(bytes([0x80 | (servo_id & 0x1F), (v >> 7) & 0x7F, v & 0x7F])) + time.sleep(0.1) + v = self.ics.read(6) + angle = ((v[4] & 0x7F) << 7) | (v[5] & 0x7F) + return angle + + def read_param(self, servo_id=None): + if servo_id is None: + servo_id = self.get_servo_id() + self.ics.write(bytes([0xA0 | servo_id, 0x00])) + time.sleep(0.1) + ret = self.ics.read(68) + ics_param64 = ret[4:] + result = self.parse_param64_key_value(list(ics_param64)) + return list(ics_param64), result + + def _4bit2num(self, lst, v): + sum_val = 0 + for i in lst: + sum_val = (sum_val << 4) + (v[i - 1] & 0x0F) + return sum_val From 0c1e1c4231db3567bac1a80feee422ad84750e0f Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Tue, 12 Nov 2024 14:55:32 +0900 Subject: [PATCH 02/28] Add ics manager command line app --- rcb4/apps/ics_manager.py | 78 ++++++++++++++++++++++++++++++++++++++++ setup.py | 1 + 2 files changed, 79 insertions(+) create mode 100644 rcb4/apps/ics_manager.py diff --git a/rcb4/apps/ics_manager.py b/rcb4/apps/ics_manager.py new file mode 100644 index 00000000..7c9b38e8 --- /dev/null +++ b/rcb4/apps/ics_manager.py @@ -0,0 +1,78 @@ +#!/usr/bin/env python + +import argparse +import sys + +import yaml + +from rcb4.ics import ICSServoController + + +def load_yaml(yaml_path): + try: + with open(yaml_path) as file: + data = yaml.safe_load(file) + print("YAML configuration loaded successfully.") + return data + except FileNotFoundError: + print(f"Error: YAML file not found at path: {yaml_path}") + sys.exit(1) + except yaml.YAMLError as exc: + print(f"Error parsing YAML file: {exc}") + sys.exit(1) + + +def parse_args(): + """Parse command-line arguments.""" + parser = argparse.ArgumentParser(description="ICS Servo Controller CLI Tool") + parser.add_argument( + "--yaml_path", + default=None, + help="Path to YAML configuration file for servo settings" + ) + parser.add_argument( + "--baudrate", + type=int, + default=1250000, + choices=[1250000, 625000, 115200], + help="Baud rate for servo connection" + ) + parser.add_argument( + "--verbose", + action="store_true", + help="Enable verbose output" + ) + return parser.parse_args() + + +def main(): + args = parse_args() + + # Verbose output if enabled + if args.verbose: + print(f"Using baud rate: {args.baudrate}") + if args.yaml_path: + print(f"Loading configuration from: {args.yaml_path}") + else: + print("No YAML configuration file specified.") + + if args.yaml_path: + load_yaml(args.yaml_path) + + # Initialize the ICS Servo Controller + servo_controller = ICSServoController( + baudrate=args.baudrate, + yaml_path=args.yaml_path + ) + + try: + servo_controller.display_status() + except Exception as e: + print(f"An error occurred while displaying status: {e}") + finally: + servo_controller.close_connection() + print("Connection closed.") + + +if __name__ == "__main__": + main() diff --git a/setup.py b/setup.py index b12952b3..9b1309db 100644 --- a/setup.py +++ b/setup.py @@ -59,6 +59,7 @@ "console_scripts": [ "rcb4-write-firmware=rcb4.apps.write_firmware:main", "armh7-tools=rcb4.apps.armh7_tool:main", + "ics-manager=rcb4.apps.ics_manager:main", ], }, ) From d575beb89c02f2407035a843935ab5ad19d0c849 Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Tue, 12 Nov 2024 15:03:25 +0900 Subject: [PATCH 03/28] Add readchar for ics --- requirements.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/requirements.txt b/requirements.txt index cec765f6..3cddcac5 100644 --- a/requirements.txt +++ b/requirements.txt @@ -6,4 +6,5 @@ numpy pandas pyelftools pyserial +readchar tabulate From a2c4f976621890367b1b339c3ed3160c45709ebe Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Tue, 12 Nov 2024 15:18:37 +0900 Subject: [PATCH 04/28] Fixed Warning print --- rcb4/ics.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/rcb4/ics.py b/rcb4/ics.py index a85f91bc..4c9a2f2b 100644 --- a/rcb4/ics.py +++ b/rcb4/ics.py @@ -297,9 +297,11 @@ def display_status(self): sys.stdout.write("\033[H\033[J") sys.stdout.flush() print(f"{Fore.RED}Connection is not open.{Style.RESET_ALL}") - print( - f"{Fore.RED}Are you using dual USB? Is it set to ICS mode? Have you connected the servo?{Style.RESET_ALL}" - ) + print(f"{Fore.RED}Please check the following:") + print("1. Use Dual USB Adapter (https://kondo-robot.com/product/02116)") + print("2. Set the device to ICS mode.") + print(f"3. Connect only one servo correctly.{Style.RESET_ALL}") + print( f"{Fore.RED}To establish the connection, please execute the following commands in Linux:{Style.RESET_ALL}" ) From cc1ac3b4d723a187b518b4403fc5ae03740464dc Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Tue, 12 Nov 2024 15:37:38 +0900 Subject: [PATCH 05/28] Modified ics-manager description --- rcb4/ics.py | 41 +++++++++++++++++++++++------------------ 1 file changed, 23 insertions(+), 18 deletions(-) diff --git a/rcb4/ics.py b/rcb4/ics.py index 4c9a2f2b..d5920a24 100644 --- a/rcb4/ics.py +++ b/rcb4/ics.py @@ -279,7 +279,7 @@ def close_connection(self): def display_status(self): options = [ - "Servo ID", + "Current Servo ID", "Angle", "Baud Rate", "Rotation Mode", @@ -288,7 +288,7 @@ def display_status(self): "Serial Mode", "Free", ] - selectable_options = ["Servo ID", "Angle"] + selectable_options = ["Current Servo ID", "Angle"] try: use_previous_result = False while True: @@ -329,19 +329,21 @@ def display_status(self): for i, option in enumerate(options): if i == self.selected_index: print( - f"{Fore.YELLOW}>> {option}: {self.get_status(option, result)}{Style.RESET_ALL}" + f">> {option}: {self.get_status(option, result, selected=True)}" ) else: - print(f" {option}: {self.get_status(option, result)}") + print(f" {option}: {self.get_status(option, result, selected=False)}") print("----------------------\n") print( - "Use ↑↓ to navigate, ←→ to adjust Servo ID or Servo Angles, 'q' to quit." + "Use ↑↓ to navigate, ←→ to adjust Current Servo ID or Servo Angles" ) + print(f"Press 'Enter' when Current Servo ID is selected to save the currently highlighted ID in {Fore.GREEN}green{Style.RESET_ALL}.") print("Press 'z' to reset servo position") print( "Press 'r' to toggle rotation mode (enables continuous wheel-like rotation)" ) print("Press 'f' to set free mode\n") + print("'q' to quit.") use_previous_result = False @@ -370,7 +372,7 @@ def display_status(self): use_previous_result = True elif ( key == readchar.key.ENTER - and selectable_options[self.selected_index] == "Servo ID" + and selectable_options[self.selected_index] == "Current Servo ID" ): self.set_servo_id(self.servo_id) time.sleep(0.3) @@ -379,7 +381,7 @@ def display_status(self): time.sleep(0.3) elif ( key == readchar.key.LEFT - and selectable_options[self.selected_index] == "Servo ID" + and selectable_options[self.selected_index] == "Current Servo ID" ): use_previous_result = True if self.servo_id_index == 0: @@ -389,7 +391,7 @@ def display_status(self): self.servo_id = self.servo_candidates[self.servo_id_index] elif ( key == readchar.key.RIGHT - and selectable_options[self.selected_index] == "Servo ID" + and selectable_options[self.selected_index] == "Current Servo ID" ): use_previous_result = True if self.servo_id_index == len(self.servo_candidates) - 1: @@ -418,22 +420,25 @@ def display_status(self): except KeyboardInterrupt: print("\nDisplay stopped by user.") - def get_status(self, option, param=None): + def get_status(self, option, param=None, selected=False): """Return the status based on the selected option.""" - if option == "Servo ID": + if option == "Current Servo ID": if param is not None: current_servo_id = param["servo-id"] else: current_servo_id = self.get_servo_id() - str = f"{Style.RESET_ALL}[" - for i, servo_id in enumerate(self.servo_candidates): - if i == self.servo_id_index: - str += f"{Fore.GREEN}{servo_id}{Style.RESET_ALL} " - else: - str += f"{servo_id} " - str += "]" - return f"{current_servo_id} -> {str}" + s = f'{current_servo_id}' + if selected: + str = f"{Style.RESET_ALL}[" + for i, servo_id in enumerate(self.servo_candidates): + if i == self.servo_id_index: + str += f"{Fore.GREEN}{servo_id}{Style.RESET_ALL} " + else: + str += f"{servo_id} " + str += "]" + s += f' -> Next Servo ID: {str}' + return s elif option == "Angle": return f"{self.read_angle()}" elif option == "Baud Rate": From d4ae94f0a883bd2a9e0e59e1487b45633d2423be Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Tue, 12 Nov 2024 15:38:28 +0900 Subject: [PATCH 06/28] Modified --- README.md | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/README.md b/README.md index 01343f1c..88999daf 100644 --- a/README.md +++ b/README.md @@ -13,6 +13,14 @@ Connect st-link and run the following command. rcb4-write-firmware ``` +## Change Servo ID + +You can use ics-manager command to change servo id. + +```bash +ics-manager +``` + ## Contributing ### Automatic Formatting From e712d0264c8bc0c02c7ebb29dc194054a318108b Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Tue, 12 Nov 2024 15:38:28 +0900 Subject: [PATCH 07/28] Modified --- rcb4/apps/ics_manager.py | 2 +- rcb4/ics.py | 194 ++++++++++++++++++++++----------------- 2 files changed, 109 insertions(+), 87 deletions(-) diff --git a/rcb4/apps/ics_manager.py b/rcb4/apps/ics_manager.py index 7c9b38e8..b1c6709c 100644 --- a/rcb4/apps/ics_manager.py +++ b/rcb4/apps/ics_manager.py @@ -26,7 +26,7 @@ def parse_args(): """Parse command-line arguments.""" parser = argparse.ArgumentParser(description="ICS Servo Controller CLI Tool") parser.add_argument( - "--yaml_path", + "--yaml-path", default=None, help="Path to YAML configuration file for servo settings" ) diff --git a/rcb4/ics.py b/rcb4/ics.py index d5920a24..ff008de7 100644 --- a/rcb4/ics.py +++ b/rcb4/ics.py @@ -91,6 +91,9 @@ def __init__(self, baudrate=1250000, yaml_path=None): def open_connection(self): ports = serial.tools.list_ports.comports() + if len(ports) == 0: + print(f"{Fore.RED}No USB Found.{Style.RESET_ALL}") + print(f"{Fore.RED}May Dual USB Adapter is wrong.{Style.RESET_ALL}") for p in ports: if p.vid == 0x165C and p.pid == 0x08: for baudrate in [115200, 625000, 1250000]: @@ -141,7 +144,8 @@ def get_servo_id(self): self.ics.write(bytes([0xFF, 0x00, 0x00, 0x00])) time.sleep(0.1) ret = self.ics.read(5) - return ret[4] & 0x1F + servo_id = ret[4] & 0x1F + return servo_id def set_servo_id(self, servo_id): self.ics.write(bytes([0xE0 | (0x1F & servo_id), 0x01, 0x01, 0x01])) @@ -276,6 +280,7 @@ def set_param(self, ics_param64, servo_id=None): def close_connection(self): if self.ics and self.ics.is_open: self.ics.close() + self.ics = None def display_status(self): options = [ @@ -291,6 +296,7 @@ def display_status(self): selectable_options = ["Current Servo ID", "Angle"] try: use_previous_result = False + previous_servo_id = None while True: if not self.ics or not self.ics.is_open: # Clear the previous output at the start of each loop @@ -323,94 +329,105 @@ def display_status(self): sys.stdout.flush() # Print servo status - print("--- Servo Status ---") - if use_previous_result is False: - _, result = self.read_param() - for i, option in enumerate(options): - if i == self.selected_index: - print( - f">> {option}: {self.get_status(option, result, selected=True)}" - ) - else: - print(f" {option}: {self.get_status(option, result, selected=False)}") - print("----------------------\n") - print( - "Use ↑↓ to navigate, ←→ to adjust Current Servo ID or Servo Angles" - ) - print(f"Press 'Enter' when Current Servo ID is selected to save the currently highlighted ID in {Fore.GREEN}green{Style.RESET_ALL}.") - print("Press 'z' to reset servo position") - print( - "Press 'r' to toggle rotation mode (enables continuous wheel-like rotation)" - ) - print("Press 'f' to set free mode\n") - print("'q' to quit.") - - use_previous_result = False - - # Get key input without Enter - key = readchar.readkey() - - # Perform actions based on key - if key == "z": - self.reset_servo_position() - elif key == "r": - self.toggle_rotation_mode() - elif key == "f": - self.set_free_mode() - elif key == "q": - print("Exiting...") - break - elif key == readchar.key.UP: - self.selected_index = (self.selected_index - 1) % len( - selectable_options + try: + servo_id = self.get_servo_id() + if servo_id != previous_servo_id: + use_previous_result = False + previous_servo_id = servo_id + print("--- Servo Status ---") + if use_previous_result is False: + _, result = self.read_param() + for i, option in enumerate(options): + if i == self.selected_index: + print( + f">> {option}: {self.get_status(option, result, selected=True)}" + ) + else: + print(f" {option}: {self.get_status(option, result, selected=False)}") + + print("----------------------\n") + print( + "Use ↑↓ to navigate, ←→ to adjust Current Servo ID or Servo Angles" ) - use_previous_result = True - elif key == readchar.key.DOWN: - self.selected_index = (self.selected_index + 1) % len( - selectable_options + print(f"Press 'Enter' when Current Servo ID is selected to save the currently highlighted ID in {Fore.GREEN}green{Style.RESET_ALL}.") + print("Press 'z' to reset servo position") + print( + "Press 'r' to toggle rotation mode (enables continuous wheel-like rotation)" ) - use_previous_result = True - elif ( - key == readchar.key.ENTER - and selectable_options[self.selected_index] == "Current Servo ID" - ): - self.set_servo_id(self.servo_id) - time.sleep(0.3) - if self.servo_id_to_rotation is not None: - self.set_rotation(self.servo_id_to_rotation[self.servo_id]) - time.sleep(0.3) - elif ( - key == readchar.key.LEFT - and selectable_options[self.selected_index] == "Current Servo ID" - ): - use_previous_result = True - if self.servo_id_index == 0: - self.servo_id_index = len(self.servo_candidates) - 1 - else: - self.servo_id_index = max(0, self.servo_id_index - 1) - self.servo_id = self.servo_candidates[self.servo_id_index] - elif ( - key == readchar.key.RIGHT - and selectable_options[self.selected_index] == "Current Servo ID" - ): - use_previous_result = True - if self.servo_id_index == len(self.servo_candidates) - 1: - self.servo_id_index = 0 - else: - self.servo_id_index = min( - len(self.servo_candidates) - 1, self.servo_id_index + 1 + print("Press 'f' to set free mode\n") + print("'q' to quit.") + + use_previous_result = False + + # Get key input without Enter + key = readchar.readkey() + + # Perform actions based on key + if key == "z": + self.reset_servo_position() + elif key == "r": + self.toggle_rotation_mode() + elif key == "f": + self.set_free_mode() + elif key == "q": + print("Exiting...") + break + elif key == readchar.key.UP: + self.selected_index = (self.selected_index - 1) % len( + selectable_options + ) + use_previous_result = True + elif key == readchar.key.DOWN: + self.selected_index = (self.selected_index + 1) % len( + selectable_options ) - self.servo_id = self.servo_candidates[self.servo_id_index] - elif ( - key == readchar.key.LEFT - and selectable_options[self.selected_index] == "Angle" - ): - self.decrease_angle() - elif ( - key == readchar.key.RIGHT - and selectable_options[self.selected_index] == "Angle" - ): - self.increase_angle() + use_previous_result = True + elif ( + key == readchar.key.ENTER + and selectable_options[self.selected_index] == "Current Servo ID" + ): + self.set_servo_id(self.servo_id) + time.sleep(0.3) + if self.servo_id_to_rotation is not None: + self.set_rotation(self.servo_id_to_rotation[self.servo_id]) + time.sleep(0.3) + elif ( + key == readchar.key.LEFT + and selectable_options[self.selected_index] == "Current Servo ID" + ): + use_previous_result = True + if self.servo_id_index == 0: + self.servo_id_index = len(self.servo_candidates) - 1 + else: + self.servo_id_index = max(0, self.servo_id_index - 1) + self.servo_id = self.servo_candidates[self.servo_id_index] + elif ( + key == readchar.key.RIGHT + and selectable_options[self.selected_index] == "Current Servo ID" + ): + use_previous_result = True + if self.servo_id_index == len(self.servo_candidates) - 1: + self.servo_id_index = 0 + else: + self.servo_id_index = min( + len(self.servo_candidates) - 1, self.servo_id_index + 1 + ) + self.servo_id = self.servo_candidates[self.servo_id_index] + elif ( + key == readchar.key.LEFT + and selectable_options[self.selected_index] == "Angle" + ): + self.decrease_angle() + elif ( + key == readchar.key.RIGHT + and selectable_options[self.selected_index] == "Angle" + ): + self.increase_angle() + except Exception as e: + print(f'[ERROR] {e}') + use_previous_result = False + self.close_connection() + continue # Flush the output to ensure it displays correctly sys.stdout.flush() @@ -511,3 +528,8 @@ def _4bit2num(self, lst, v): for i in lst: sum_val = (sum_val << 4) + (v[i - 1] & 0x0F) return sum_val + + +if __name__ == '__main__': + servo_controller = ICSServoController() + servo_controller.open_connection() From 1ab9d8da2cd34a0a44b544310b84e9d84c4ea84e Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Tue, 12 Nov 2024 16:17:44 +0900 Subject: [PATCH 08/28] 0.0.4 --- ros/kxr_controller/package.xml | 2 +- ros/kxr_models/package.xml | 2 +- ros/kxreus/package.xml | 2 +- setup.py | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ros/kxr_controller/package.xml b/ros/kxr_controller/package.xml index a3f671ff..e72eeb22 100644 --- a/ros/kxr_controller/package.xml +++ b/ros/kxr_controller/package.xml @@ -1,7 +1,7 @@ kxr_controller - 0.0.3 + 0.0.4 The kxr_controller package Iori Yanokura diff --git a/ros/kxr_models/package.xml b/ros/kxr_models/package.xml index fb958d2a..7ae80c24 100644 --- a/ros/kxr_models/package.xml +++ b/ros/kxr_models/package.xml @@ -1,6 +1,6 @@ kxr_models - 0.0.3 + 0.0.4

URDF Description package for kxr_models

This package contains configuration data, 3D models and launch files for kxr robot

diff --git a/ros/kxreus/package.xml b/ros/kxreus/package.xml index 22411cad..4a8c02ef 100644 --- a/ros/kxreus/package.xml +++ b/ros/kxreus/package.xml @@ -1,7 +1,7 @@ kxreus - 0.0.3 + 0.0.4 roseus interface for kxr robots. diff --git a/setup.py b/setup.py index 9b1309db..e1edcc33 100644 --- a/setup.py +++ b/setup.py @@ -6,7 +6,7 @@ from setuptools import find_packages from setuptools import setup -version = "0.0.3" +version = "0.0.4" if sys.argv[-1] == "release": From 1d9e95a5511c464ae87cf83a4f74aa73119d5735 Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Wed, 13 Nov 2024 00:58:00 +0900 Subject: [PATCH 09/28] Seperate keyboard listener --- rcb4/ics.py | 94 +++++++++++++++++++++++++++++++++++------------------ 1 file changed, 63 insertions(+), 31 deletions(-) diff --git a/rcb4/ics.py b/rcb4/ics.py index ff008de7..5618dd63 100644 --- a/rcb4/ics.py +++ b/rcb4/ics.py @@ -1,4 +1,5 @@ import sys +import threading import time from colorama import Fore @@ -9,6 +10,36 @@ import yaml +class KeyListener(threading.Thread): + def __init__(self): + super().__init__() + self.key = None + self.lock = threading.Lock() + self.running = True # Add a running flag + + def run(self): + """Continuously read keys and store the latest key in self.key while running is True.""" + while self.running: + try: + key = readchar.readkey() + except KeyboardInterrupt: + self.stop() + break + with self.lock: + self.key = key + + def get_key(self): + """Return the latest key and reset it.""" + with self.lock: + key = self.key + self.key = None + return key + + def stop(self): + """Stop the listener thread.""" + self.running = False + + def load_and_process_yaml(yaml_path): with open(yaml_path) as f: joint_data = yaml.safe_load(f) @@ -294,10 +325,14 @@ def display_status(self): "Free", ] selectable_options = ["Current Servo ID", "Angle"] + + key_listener = KeyListener() + key_listener.daemon = True + key_listener.start() try: use_previous_result = False previous_servo_id = None - while True: + while key_listener.running: if not self.ics or not self.ics.is_open: # Clear the previous output at the start of each loop sys.stdout.write("\033[H\033[J") @@ -325,43 +360,41 @@ def display_status(self): if ret is False: time.sleep(1.0) continue - sys.stdout.write("\033[H\033[J") - sys.stdout.flush() - # Print servo status try: servo_id = self.get_servo_id() if servo_id != previous_servo_id: use_previous_result = False previous_servo_id = servo_id - print("--- Servo Status ---") if use_previous_result is False: _, result = self.read_param() - for i, option in enumerate(options): - if i == self.selected_index: - print( - f">> {option}: {self.get_status(option, result, selected=True)}" - ) - else: - print(f" {option}: {self.get_status(option, result, selected=False)}") - - print("----------------------\n") - print( - "Use ↑↓ to navigate, ←→ to adjust Current Servo ID or Servo Angles" - ) - print(f"Press 'Enter' when Current Servo ID is selected to save the currently highlighted ID in {Fore.GREEN}green{Style.RESET_ALL}.") - print("Press 'z' to reset servo position") - print( - "Press 'r' to toggle rotation mode (enables continuous wheel-like rotation)" - ) - print("Press 'f' to set free mode\n") - print("'q' to quit.") + print('======================================') + sys.stdout.write("\033[H\033[J") + sys.stdout.flush() + print("--- Servo Status ---") + for i, option in enumerate(options): + if i == self.selected_index: + print( + f">> {option}: {self.get_status(option, result, selected=True)}" + ) + else: + print(f" {option}: {self.get_status(option, result, selected=False)}") + + print("----------------------\n") + print( + "Use ↑↓ to navigate, ←→ to adjust Current Servo ID or Servo Angles" + ) + print(f"Press 'Enter' when Current Servo ID is selected to save the currently highlighted ID in {Fore.GREEN}green{Style.RESET_ALL}.") + print("Press 'z' to reset servo position") + print( + "Press 'r' to toggle rotation mode (enables continuous wheel-like rotation)" + ) + print("Press 'f' to set free mode\n") + print("'q' to quit.") + key = key_listener.get_key() use_previous_result = False - # Get key input without Enter - key = readchar.readkey() - # Perform actions based on key if key == "z": self.reset_servo_position() @@ -376,12 +409,10 @@ def display_status(self): self.selected_index = (self.selected_index - 1) % len( selectable_options ) - use_previous_result = True elif key == readchar.key.DOWN: self.selected_index = (self.selected_index + 1) % len( selectable_options ) - use_previous_result = True elif ( key == readchar.key.ENTER and selectable_options[self.selected_index] == "Current Servo ID" @@ -395,7 +426,6 @@ def display_status(self): key == readchar.key.LEFT and selectable_options[self.selected_index] == "Current Servo ID" ): - use_previous_result = True if self.servo_id_index == 0: self.servo_id_index = len(self.servo_candidates) - 1 else: @@ -405,7 +435,6 @@ def display_status(self): key == readchar.key.RIGHT and selectable_options[self.selected_index] == "Current Servo ID" ): - use_previous_result = True if self.servo_id_index == len(self.servo_candidates) - 1: self.servo_id_index = 0 else: @@ -423,6 +452,8 @@ def display_status(self): and selectable_options[self.selected_index] == "Angle" ): self.increase_angle() + else: + use_previous_result = True except Exception as e: print(f'[ERROR] {e}') use_previous_result = False @@ -436,6 +467,7 @@ def display_status(self): except KeyboardInterrupt: print("\nDisplay stopped by user.") + key_listener.stop() def get_status(self, option, param=None, selected=False): """Return the status based on the selected option.""" From f2173b774032d0c10a4ded0ecd00e853e860e14d Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Wed, 13 Nov 2024 00:56:18 +0900 Subject: [PATCH 10/28] Use config's wheel servo ids --- ros/kxr_controller/node_scripts/rcb4_ros_bridge.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py index 1421204e..ff3e101a 100644 --- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py +++ b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py @@ -401,7 +401,9 @@ def log_error_and_close_interface(function_name): if ret is None: return log_error_and_close_interface("set trim_vector") if self.interface.wheel_servo_sorted_ids is None: - self.interface.wheel_servo_sorted_ids = wheel_servo_sorted_ids + self.interface.wheel_servo_sorted_ids = [] + self.interface.wheel_servo_sorted_ids = list( + set(self.interface.wheel_servo_sorted_ids + wheel_servo_sorted_ids)) # set servo ids to rosparam servo_ids = self.get_ids(type="servo") From bd3281dbf118c0ac05cc43b546e5cdac0bd24ccf Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Wed, 13 Nov 2024 00:59:44 +0900 Subject: [PATCH 11/28] 0.0.5 --- ros/kxr_controller/package.xml | 2 +- ros/kxr_models/package.xml | 2 +- ros/kxreus/package.xml | 2 +- setup.py | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ros/kxr_controller/package.xml b/ros/kxr_controller/package.xml index e72eeb22..3d3fc270 100644 --- a/ros/kxr_controller/package.xml +++ b/ros/kxr_controller/package.xml @@ -1,7 +1,7 @@ kxr_controller - 0.0.4 + 0.0.5 The kxr_controller package Iori Yanokura diff --git a/ros/kxr_models/package.xml b/ros/kxr_models/package.xml index 7ae80c24..a72fb48a 100644 --- a/ros/kxr_models/package.xml +++ b/ros/kxr_models/package.xml @@ -1,6 +1,6 @@ kxr_models - 0.0.4 + 0.0.5

URDF Description package for kxr_models

This package contains configuration data, 3D models and launch files for kxr robot

diff --git a/ros/kxreus/package.xml b/ros/kxreus/package.xml index 4a8c02ef..2bc27a17 100644 --- a/ros/kxreus/package.xml +++ b/ros/kxreus/package.xml @@ -1,7 +1,7 @@ kxreus - 0.0.4 + 0.0.5 roseus interface for kxr robots. diff --git a/setup.py b/setup.py index e1edcc33..0b6ce5dc 100644 --- a/setup.py +++ b/setup.py @@ -6,7 +6,7 @@ from setuptools import find_packages from setuptools import setup -version = "0.0.4" +version = "0.0.5" if sys.argv[-1] == "release": From 8dc9e75c95940bea8b6f6bb8df83d522803daa48 Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Sat, 16 Nov 2024 23:32:23 +0900 Subject: [PATCH 12/28] [kxreus] Don't wait action server when use sim time is True --- ros/kxreus/euslisp/kxr-interface.l | 60 +++++++++++++++++++----------- 1 file changed, 38 insertions(+), 22 deletions(-) diff --git a/ros/kxreus/euslisp/kxr-interface.l b/ros/kxreus/euslisp/kxr-interface.l index ebd299ab..21f58ed7 100644 --- a/ros/kxreus/euslisp/kxr-interface.l +++ b/ros/kxreus/euslisp/kxr-interface.l @@ -45,7 +45,7 @@ (defclass kxr-interface ;; :super robot-interface :super robot-move-base-interface - :slots (joint-names servo-on-off-client controller-name control-pressure)) + :slots (joint-names servo-on-off-client controller-name control-pressure use-sim-time)) (defmethod kxr-interface @@ -72,28 +72,32 @@ :cmd-vel-topic "/diff_drive_controller/cmd_vel" args) - (setq servo-on-off-client (instance ros::simple-action-client :init - (if namespace (format nil "/~A/~A/servo_on_off_real_interface" namespace controller-name) - (format nil "/~A/servo_on_off_real_interface" controller-name)) - kxr_controller::ServoOnOffAction + (let (actions) + (setq use-sim-time (and (ros::has-param "/use_sim_time") (ros::get-param "/use_sim_time"))) + (when (not use-sim-time) + (setq servo-on-off-client (instance ros::simple-action-client :init + (if namespace (format nil "/~A/~A/servo_on_off_real_interface" namespace controller-name) + (format nil "/~A/servo_on_off_real_interface" controller-name)) + kxr_controller::ServoOnOffAction + :groupname groupname)) + (setq stretch-client (instance ros::simple-action-client :init + (if namespace (format nil "/~A/~A/stretch_interface" namespace controller-name) + (format nil "/~A/stretch_interface" controller-name)) + kxr_controller::StretchAction :groupname groupname)) - (setq stretch-client (instance ros::simple-action-client :init - (if namespace (format nil "/~A/~A/stretch_interface" namespace controller-name) - (format nil "/~A/stretch_interface" controller-name)) - kxr_controller::StretchAction - :groupname groupname)) - (when control-pressure - (setq pressure-control-client (instance ros::simple-action-client :init - (if namespace (format nil "/~A/~A/pressure_control_interface" namespace controller-name) - (format nil "/~A/pressure_control_interface" controller-name)) - kxr_controller::PressureControlAction - :groupname groupname))) - (dolist (action (if control-pressure (list servo-on-off-client stretch-client pressure-control-client) - (list servo-on-off-client stretch-client))) - (unless (and joint-action-enable (send action :wait-for-server 3)) - (setq joint-action-enable nil) - (ros::ros-warn "~A is not respond, kxr-interface is disabled" action) - (return))) + (setq actions (list servo-on-off-client stretch-client)) + (when control-pressure + (setq pressure-control-client (instance ros::simple-action-client :init + (if namespace (format nil "/~A/~A/pressure_control_interface" namespace controller-name) + (format nil "/~A/pressure_control_interface" controller-name)) + kxr_controller::PressureControlAction + :groupname groupname)) + (setq actions (list servo-on-off-client stretch-client pressure-control-client)))) + (dolist (action actions) + (unless (and joint-action-enable (send action :wait-for-server 3)) + (setq joint-action-enable nil) + (ros::ros-warn "~A is not respond, kxr-interface is disabled" action) + (return)))) t) (:default-controller () @@ -108,6 +112,8 @@ (cons :joint-names joint-names)))) (:servo-on (&key (names nil)) + (when use-sim-time + (return-from :servo-on t)) (unless joint-action-enable (if viewer (send self :draw-objects)) (return-from :servo-on t)) @@ -120,6 +126,8 @@ (send servo-on-off-client :send-goal goal))) (:servo-off (&key (names nil)) + (when use-sim-time + (return-from :servo-on t)) (unless joint-action-enable (if viewer (send self :draw-objects)) (return-from :servo-off t)) @@ -132,6 +140,8 @@ (send servo-on-off-client :send-goal goal))) (:send-stretch (&key (value 127) (names nil)) + (when use-sim-time + (return-from :servo-on t)) (when (null names) (setq names joint-names)) (let* (goal result) @@ -141,12 +151,16 @@ (send stretch-client :send-goal goal))) (:read-stretch () + (when use-sim-time + (return-from :servo-on t)) (one-shot-subscribe (if namespace (format nil "/~A/~A/stretch" namespace controller-name) (format nil "/~A/stretch" controller-name)) kxr_controller::Stretch)) (:send-pressure-control (&key board-idx start-pressure stop-pressure release) + (when use-sim-time + (return-from :servo-on t)) (let* (goal result) (setq goal (instance kxr_controller::PressureControlGoal :init)) (send goal :board_idx board-idx) @@ -156,6 +170,8 @@ (send pressure-control-client :send-goal goal))) (:read-pressure (&key board-idx) + (when use-sim-time + (return-from :servo-on t)) (one-shot-subscribe (if namespace (format nil "/~A/~A/pressure/~A" namespace controller-name board-idx) (format nil "/~A/pressure/~A" controller-name board-idx)) From f192a3806ba40789e881a2fc18846bec0e08d667 Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Wed, 20 Nov 2024 02:09:04 +0900 Subject: [PATCH 13/28] Fixed quiet behaviour for bash or zsh --- rcb4/ics.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rcb4/ics.py b/rcb4/ics.py index 5618dd63..f42909fd 100644 --- a/rcb4/ics.py +++ b/rcb4/ics.py @@ -22,6 +22,10 @@ def run(self): while self.running: try: key = readchar.readkey() + if key == 'q': + with self.lock: + self.key = key + break except KeyboardInterrupt: self.stop() break From 9dd5efcc1cefcff41387f8b06ffdf8fdc9e32da4 Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Wed, 20 Nov 2024 02:36:32 +0900 Subject: [PATCH 14/28] Fixed reinitialize function --- ros/kxr_controller/node_scripts/rcb4_ros_bridge.py | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py index ff3e101a..94f67098 100644 --- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py +++ b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py @@ -1028,11 +1028,7 @@ def reinitialize_interface(self): rospy.loginfo("Reinitialize interface.") self.unsubscribe() self.interface.close() - self.interface = self.setup_interface() - if self.read_current: - serial_call_with_retry(self.interface.switch_reading_servo_current, enable=True, max_retries=3) - if self.read_temperature: - serial_call_with_retry(self.interface.switch_reading_servo_temperature, enable=True, max_retries=3) + self.setup_interface_and_servo_parameters() self.subscribe() rospy.loginfo("Successfully reinitialized interface.") From 0217f0e43e07d8f72c6bb3f5b681c40d09b9d66d Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Wed, 20 Nov 2024 12:49:12 +0900 Subject: [PATCH 15/28] After powering on, the ACK value becomes unstable for some reason, so the process is repeated several times. --- rcb4/armh7interface.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/rcb4/armh7interface.py b/rcb4/armh7interface.py index f8dee192..2fc148f7 100644 --- a/rcb4/armh7interface.py +++ b/rcb4/armh7interface.py @@ -177,9 +177,12 @@ def open(self, port="/dev/ttyUSB1", baudrate=1000000, timeout=0.01): except serial.SerialException as e: print(f"Error opening serial port: {e}") raise serial.SerialException(e) - ack = self.check_ack() - if ack is not True: - return False + # After powering on, the ACK value becomes unstable for some reason, + # so the process is repeated several times. + for _ in range(10): + ack = self.check_ack() + if ack is True: + break self.check_firmware_version() self.copy_worm_params_from_flash() self.search_worm_ids() From 5f621bfdde8e7465f4b329bdcfb58e7c541c9ff1 Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Wed, 20 Nov 2024 14:16:21 +0900 Subject: [PATCH 16/28] 0.0.6 --- ros/kxr_controller/package.xml | 2 +- ros/kxr_models/package.xml | 2 +- ros/kxreus/package.xml | 2 +- setup.py | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ros/kxr_controller/package.xml b/ros/kxr_controller/package.xml index 3d3fc270..579d63c7 100644 --- a/ros/kxr_controller/package.xml +++ b/ros/kxr_controller/package.xml @@ -1,7 +1,7 @@ kxr_controller - 0.0.5 + 0.0.6 The kxr_controller package Iori Yanokura diff --git a/ros/kxr_models/package.xml b/ros/kxr_models/package.xml index a72fb48a..21a3f5c2 100644 --- a/ros/kxr_models/package.xml +++ b/ros/kxr_models/package.xml @@ -1,6 +1,6 @@ kxr_models - 0.0.5 + 0.0.6

URDF Description package for kxr_models

This package contains configuration data, 3D models and launch files for kxr robot

diff --git a/ros/kxreus/package.xml b/ros/kxreus/package.xml index 2bc27a17..7a1e221f 100644 --- a/ros/kxreus/package.xml +++ b/ros/kxreus/package.xml @@ -1,7 +1,7 @@ kxreus - 0.0.5 + 0.0.6 roseus interface for kxr robots. diff --git a/setup.py b/setup.py index 0b6ce5dc..47a4d26d 100644 --- a/setup.py +++ b/setup.py @@ -6,7 +6,7 @@ from setuptools import find_packages from setuptools import setup -version = "0.0.5" +version = "0.0.6" if sys.argv[-1] == "release": From a599e6e2432d074d70c24fc300e5914a3b706f1e Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Fri, 29 Nov 2024 17:57:40 +0900 Subject: [PATCH 17/28] Add set response and set stretch function --- rcb4/ics.py | 53 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 53 insertions(+) diff --git a/rcb4/ics.py b/rcb4/ics.py index f42909fd..40efe682 100644 --- a/rcb4/ics.py +++ b/rcb4/ics.py @@ -188,6 +188,28 @@ def set_servo_id(self, servo_id): ret = self.ics.read(5) return 0x1F & ret[4] + def set_response(self, value, servo_id=None): + """ + Set the response parameter to the specified value. + """ + if servo_id is None: + servo_id = self.get_servo_id() + + # Read the current parameters + ics_param64, _ = self.read_param(servo_id=servo_id) + + # Update the 'response' field + indices = [51, 52] # Indices for the 'response' parameter + ics_param64[indices[0] - 1] = (value >> 4) & 0x0F # High 4 bits + ics_param64[indices[1] - 1] = value & 0x0F # Low 4 bits + + # Write back the updated parameters + self.set_param(ics_param64, servo_id=servo_id) + + # Confirm the change + _, result = self.read_param(servo_id=servo_id) + print(f"Response set to {result['response']}") + def reset_servo_position(self): self.set_angle(7500) print(f"{Fore.YELLOW}Servo reset to zero position.{Fore.RESET}") @@ -257,6 +279,8 @@ def set_flag(self, flag_name, value, servo_id=None): # Calculate byte and bit for manipulation byte_idx = 14 if flag_name in ["slave", "rotation"] else 15 bit_position = 3 if flag_name in ["slave", "serial"] else 0 + if flag_name == 'b2': + bit_position = 2 mask = 1 << bit_position # Set or clear the bit based on the `value` argument @@ -274,6 +298,35 @@ def set_slave(self, slave=None, servo_id=None): def set_rotation(self, rotation=None, servo_id=None): return self.set_flag("rotation", rotation, servo_id=servo_id) + def set_stretch(self, stretch_values, servo_id=None): + if servo_id is None: + servo_id = self.get_servo_id() + + # Read the current parameters + ics_param64, _ = self.read_param(servo_id=servo_id) + + # Update the 'stretch-1', 'stretch-2', 'stretch-3' fields + for stretch_key, value in stretch_values.items(): + if stretch_key == "stretch-1": + indices = [59, 60] + elif stretch_key == "stretch-2": + indices = [61, 62] + elif stretch_key == "stretch-3": + indices = [63, 64] + else: + raise ValueError(f"Unsupported stretch parameter: {stretch_key}") + + # Split the value into two 4-bit chunks and store them in the corresponding indices + ics_param64[indices[0] - 1] = (value >> 4) & 0x0F # High 4 bits + ics_param64[indices[1] - 1] = value & 0x0F # Low 4 bits + + # Write back the updated parameters + self.set_param(ics_param64, servo_id=servo_id) + + # Confirm the change + _, result = self.read_param(servo_id=servo_id) + print(f"Stretch parameters set to: {stretch_values}") + def set_serial(self, serial=None, servo_id=None): return self.set_flag("serial", serial, servo_id=servo_id) From 39730853e4fceb1cef5f1afa725b6925c94dd901 Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Tue, 3 Dec 2024 00:22:32 +0900 Subject: [PATCH 18/28] Fixed baudrate settings for 115200 and 625000 --- rcb4/ics.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rcb4/ics.py b/rcb4/ics.py index 40efe682..86351bb2 100644 --- a/rcb4/ics.py +++ b/rcb4/ics.py @@ -139,7 +139,8 @@ def open_connection(self): timeout=self.timeout, parity=serial.PARITY_EVEN, ) - if baudrate != self.baudrate: + current_baudrate = self.baud() + if current_baudrate != self.baudrate: self.baud(self.baudrate) return True except IndexError: From 5636dc9cd95d09e4d01ebf92815f9435e602ca7d Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Tue, 3 Dec 2024 02:05:49 +0900 Subject: [PATCH 19/28] [ics-manager] Fixed max and min angle --- rcb4/ics.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rcb4/ics.py b/rcb4/ics.py index 86351bb2..a92450dd 100644 --- a/rcb4/ics.py +++ b/rcb4/ics.py @@ -232,13 +232,13 @@ def set_free_mode(self): def increase_angle(self): angle = self.read_angle() - angle = min(10000, angle + 500) + angle = min(11500, angle + 500) self.set_angle(angle) print(f"{Fore.BLUE}Angle increased to {angle}{Fore.RESET}") def decrease_angle(self): angle = self.read_angle() - angle = max(0, angle - 500) + angle = max(3500, angle - 500) self.set_angle(angle) print(f"{Fore.RED}Angle decreased to {angle}{Fore.RESET}") From 7c1340a357c904abc4e2e95e34764a35e167f0ac Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Tue, 3 Dec 2024 02:19:11 +0900 Subject: [PATCH 20/28] Use degree-to-pulse conversion for angle operations --- rcb4/ics.py | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/rcb4/ics.py b/rcb4/ics.py index a92450dd..586326cb 100644 --- a/rcb4/ics.py +++ b/rcb4/ics.py @@ -10,6 +10,9 @@ import yaml +degree_to_pulse = 29.633 + + class KeyListener(threading.Thread): def __init__(self): super().__init__() @@ -232,13 +235,13 @@ def set_free_mode(self): def increase_angle(self): angle = self.read_angle() - angle = min(11500, angle + 500) + angle = min(11500, angle + degree_to_pulse * 15) self.set_angle(angle) print(f"{Fore.BLUE}Angle increased to {angle}{Fore.RESET}") def decrease_angle(self): angle = self.read_angle() - angle = max(3500, angle - 500) + angle = max(3500, angle - degree_to_pulse * 15) self.set_angle(angle) print(f"{Fore.RED}Angle decreased to {angle}{Fore.RESET}") @@ -547,7 +550,9 @@ def get_status(self, option, param=None, selected=False): s += f' -> Next Servo ID: {str}' return s elif option == "Angle": - return f"{self.read_angle()}" + angle = self.read_angle() + angle = int((angle - 7500) / degree_to_pulse) + return f"{angle}" elif option == "Baud Rate": if param is not None: baudrate = param["baud"] @@ -595,6 +600,7 @@ def read_angle(self, sid=None): return angle def set_angle(self, v=7500, servo_id=None): + v = int(v) if servo_id is None: servo_id = self.get_servo_id() self.ics.write(bytes([0x80 | (servo_id & 0x1F), (v >> 7) & 0x7F, v & 0x7F])) From a1965d56c78a021488206959c4213914ec0570cd Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Tue, 3 Dec 2024 02:32:55 +0900 Subject: [PATCH 21/28] [ics-manager] Add setting default EEPROM parameters function to fix ics broken servo --- rcb4/ics.py | 26 ++++++++++++++++++++++++-- 1 file changed, 24 insertions(+), 2 deletions(-) diff --git a/rcb4/ics.py b/rcb4/ics.py index 586326cb..1a1b2901 100644 --- a/rcb4/ics.py +++ b/rcb4/ics.py @@ -9,7 +9,6 @@ import serial.tools.list_ports import yaml - degree_to_pulse = 29.633 @@ -150,6 +149,14 @@ def open_connection(self): continue return False + def set_default_eeprom_param(self): + self.set_param( + [5, 10, 11, 4, 7, 15, 0, 0, 0, 2, 1, 14, + 15, 10, 0, 6, 2, 12, 14, 12, 0, 13, 10, + 12, 0, 0, 0, 0, 0, 10, 1, 14, 1, 2, 9, 8, + 14, 13, 9, 13, 6, 14, 9, 11, 10, 12, 9, 5, + 0, 14, 0, 1, 0, 0, 13, 2, 0, 0, 3, 12, 7, 8, 15, 14],) + def read_baud(self): _, result = self.read_param() return result["baud"] @@ -450,7 +457,8 @@ def display_status(self): print( "Press 'r' to toggle rotation mode (enables continuous wheel-like rotation)" ) - print("Press 'f' to set free mode\n") + print("Press 'f' to set free mode") + print(f"Press 'd' to set default EEPROM parameters {Fore.RED}(WARNING: This action will overwrite the servo's EEPROM).{Style.RESET_ALL}\n") print("'q' to quit.") key = key_listener.get_key() @@ -466,6 +474,20 @@ def display_status(self): elif key == "q": print("Exiting...") break + elif key == "d": + print( + f"{Fore.RED}WARNING: This will overwrite the servo's EEPROM with default values.{Style.RESET_ALL}" + ) + print("Press 'y' to proceed or any other key to cancel.") + while key_listener.running: + confirm_key = key_listener.get_key() + if confirm_key == "y": + print(f"{Fore.RED}Setting default EEPROM parameters...{Style.RESET_ALL}") + self.set_default_eeprom_param() + break + elif confirm_key is not None: + print(f"{Fore.YELLOW}Action canceled.{Style.RESET_ALL}") + break elif key == readchar.key.UP: self.selected_index = (self.selected_index - 1) % len( selectable_options From d56b5da930d6cf8093c4544e1df9531c8f8c8ba8 Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Tue, 3 Dec 2024 02:57:38 +0900 Subject: [PATCH 22/28] 0.0.7 --- ros/kxr_controller/package.xml | 2 +- ros/kxr_models/package.xml | 2 +- ros/kxreus/package.xml | 2 +- setup.py | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ros/kxr_controller/package.xml b/ros/kxr_controller/package.xml index 579d63c7..04033dda 100644 --- a/ros/kxr_controller/package.xml +++ b/ros/kxr_controller/package.xml @@ -1,7 +1,7 @@ kxr_controller - 0.0.6 + 0.0.7 The kxr_controller package Iori Yanokura diff --git a/ros/kxr_models/package.xml b/ros/kxr_models/package.xml index 21a3f5c2..b2962bab 100644 --- a/ros/kxr_models/package.xml +++ b/ros/kxr_models/package.xml @@ -1,6 +1,6 @@ kxr_models - 0.0.6 + 0.0.7

URDF Description package for kxr_models

This package contains configuration data, 3D models and launch files for kxr robot

diff --git a/ros/kxreus/package.xml b/ros/kxreus/package.xml index 7a1e221f..51ec376b 100644 --- a/ros/kxreus/package.xml +++ b/ros/kxreus/package.xml @@ -1,7 +1,7 @@ kxreus - 0.0.6 + 0.0.7 roseus interface for kxr robots. diff --git a/setup.py b/setup.py index 47a4d26d..66151bff 100644 --- a/setup.py +++ b/setup.py @@ -6,7 +6,7 @@ from setuptools import find_packages from setuptools import setup -version = "0.0.6" +version = "0.0.7" if sys.argv[-1] == "release": From aa808601936f0d59fa83eddf64cd811bb963aba9 Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Thu, 5 Dec 2024 00:54:50 +0900 Subject: [PATCH 23/28] Add use_fullbody_controller option to seperate controller --- .../launch/kxr_controller.launch | 6 ++ .../launch/kxr_controller_for_wheel.launch | 5 ++ .../node_scripts/rcb4_ros_bridge.py | 78 ++++++++++++------- 3 files changed, 60 insertions(+), 29 deletions(-) diff --git a/ros/kxr_controller/launch/kxr_controller.launch b/ros/kxr_controller/launch/kxr_controller.launch index 2722dc9e..cdfb9b23 100644 --- a/ros/kxr_controller/launch/kxr_controller.launch +++ b/ros/kxr_controller/launch/kxr_controller.launch @@ -18,6 +18,10 @@ + @@ -52,6 +56,7 @@ temperature_limit: $(arg temperature_limit) read_current: $(arg read_current) read_temperature: $(arg read_temperature) + use_fullbody_controller: $(arg use_fullbody_controller) diff --git a/ros/kxr_controller/launch/kxr_controller_for_wheel.launch b/ros/kxr_controller/launch/kxr_controller_for_wheel.launch index 52b99831..d103b8f4 100644 --- a/ros/kxr_controller/launch/kxr_controller_for_wheel.launch +++ b/ros/kxr_controller/launch/kxr_controller_for_wheel.launch @@ -15,6 +15,10 @@ + @@ -32,6 +36,7 @@ + diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py index 94f67098..1b5ebb81 100644 --- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py +++ b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py @@ -181,6 +181,11 @@ def setup_paths_and_params(self): self.read_temperature = rospy.get_param("~read_temperature", False) and not self.use_rcb4 self.read_current = rospy.get_param("~read_current", False) and not self.use_rcb4 self.base_namespace = self.get_base_namespace() + self.use_fullbody_controller = rospy.get_param("~use_fullbody_controller", True) + if self.use_fullbody_controller is False: + self.default_controller = rospy.get_param(self.base_namespace + "/default_controller", []) + else: + self.default_controller = ['fullbody_controller'] def setup_urdf_and_model(self): robot_model = RobotModel() @@ -277,11 +282,19 @@ def setup_action_servers_and_clients(self): rospy.sleep(0.1) self.servo_on_off_server.start() - self.traj_action_client = actionlib.SimpleActionClient( - self.base_namespace + "/fullbody_controller/follow_joint_trajectory", - FollowJointTrajectoryAction, - ) - self.traj_action_client.wait_for_server() + self.traj_action_clients = [] + self.traj_action_joint_names = [] + for controller in self.default_controller: + controller_name = self.base_namespace + f"/{controller}/follow_joint_trajectory" + traj_action_client = actionlib.SimpleActionClient( + controller_name, + FollowJointTrajectoryAction, + ) + self.traj_action_joint_names.append( + rospy.get_param(self.base_namespace + f"/{controller}/joints", [])) + rospy.loginfo(f'Waiting {controller_name}') + traj_action_client.wait_for_server() + self.traj_action_clients.append(traj_action_client) # Adjust angle vector action server self.adjust_angle_vector_server = actionlib.SimpleActionServer( @@ -420,13 +433,18 @@ def log_error_and_close_interface(function_name): return True def run_ros_robot_controllers(self): + controllers = ["joint_state_controller"] + if self.use_fullbody_controller: + controllers.append('fullbody_controller') + else: + controllers.extend(self.default_controller) self.proc_controller_spawner = subprocess.Popen( [ f'/opt/ros/{os.environ["ROS_DISTRO"]}/bin/rosrun', "controller_manager", "spawner", ] - + ["joint_state_controller", "fullbody_controller"] + + controllers, ) self.proc_robot_state_publisher = run_robot_state_publisher(self.base_namespace) self.proc_kxr_controller = run_kxr_controller(namespace=self.base_namespace) @@ -645,29 +663,31 @@ def servo_on_off_callback(self, goal): text="Failed to call servo on off. " + "Control board is switch off or cable is disconnected?" ) - joint_names = [] - positions = [] - for joint_name in self.fullbody_jointnames: - angle = 0 - if joint_name in self.joint_name_to_id: - servo_id = self.joint_name_to_id[joint_name] - idx = self.interface.servo_id_to_index(servo_id) - if idx is not None: - angle = np.deg2rad(av[idx]) - joint_names.append(joint_name) - positions.append(angle) - # Create JointTrajectoryGoal to set current position on follow_joint_trajectory - trajectory_goal = FollowJointTrajectoryGoal() - trajectory_goal.trajectory.joint_names = joint_names - point = JointTrajectoryPoint() - point.positions = positions - point.time_from_start = rospy.Duration( - 0.5 - ) # Short duration for immediate application - trajectory_goal.trajectory.points = [point] - # Initialize action client and wait for server - self.traj_action_client.send_goal(trajectory_goal) - self.traj_action_client.wait_for_result() # Wait for trajectory to complete + for client, joint_name_list in zip(self.traj_action_clients, self.traj_action_joint_names): + joint_names = [] + positions = [] + for joint_name in joint_name_list: + angle = 0 + if joint_name in self.joint_name_to_id: + servo_id = self.joint_name_to_id[joint_name] + idx = self.interface.servo_id_to_index(servo_id) + if idx is not None: + angle = np.deg2rad(av[idx]) + joint_names.append(joint_name) + positions.append(angle) + # Create JointTrajectoryGoal to set current position on follow_joint_trajectory + trajectory_goal = FollowJointTrajectoryGoal() + trajectory_goal.trajectory.joint_names = joint_names + point = JointTrajectoryPoint() + point.positions = positions + point.time_from_start = rospy.Duration( + 0.5 + ) # Short duration for immediate application + trajectory_goal.trajectory.points = [point] + # Initialize action client and wait for server + client.send_goal(trajectory_goal) + for client in self.traj_action_clients: + client.wait_for_result() # Wait for trajectory to complete self._during_servo_off = False return self.servo_on_off_server.set_succeeded(ServoOnOffResult()) From 21b472d0b981b53d04defd171d7cad09c671a612 Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Wed, 4 Dec 2024 23:48:14 +0900 Subject: [PATCH 24/28] Add head, larm and rarm controller for pooh --- .../config/pooh_controllers.yaml | 32 ++++++++++ ros/kxr_controller/launch/pooh_bringup.launch | 6 ++ .../python/kxr_controller/pooh_interface.py | 60 +++++++++++++++++++ .../scripts/neck_command_listener.py | 20 +++---- ros/kxr_controller/scripts/neck_test.py | 20 +++---- 5 files changed, 118 insertions(+), 20 deletions(-) create mode 100644 ros/kxr_controller/config/pooh_controllers.yaml create mode 100644 ros/kxr_controller/python/kxr_controller/pooh_interface.py diff --git a/ros/kxr_controller/config/pooh_controllers.yaml b/ros/kxr_controller/config/pooh_controllers.yaml new file mode 100644 index 00000000..363113c5 --- /dev/null +++ b/ros/kxr_controller/config/pooh_controllers.yaml @@ -0,0 +1,32 @@ +head_controller: + type: position_controllers/JointTrajectoryController + joints: + - head_neck_y + - head_neck_p + - head_neck_r + + state_publish_rate: 100 # Override default + action_monitor_rate: 100 # Override default + stop_trajectory_duration: 0 # Override default + +rarm_controller: + type: position_controllers/JointTrajectoryController + joints: + - rarm_shoulder_p + - rarm_shoulder_r + - rarm_elbow + + state_publish_rate: 100 # Override default + action_monitor_rate: 100 # Override default + stop_trajectory_duration: 0 # Override default + +larm_controller: + type: position_controllers/JointTrajectoryController + joints: + - larm_shoulder_p + - larm_shoulder_r + - larm_elbow + + state_publish_rate: 100 # Override default + action_monitor_rate: 100 # Override default + stop_trajectory_duration: 0 # Override default diff --git a/ros/kxr_controller/launch/pooh_bringup.launch b/ros/kxr_controller/launch/pooh_bringup.launch index 6c3d05ac..e9b06b06 100644 --- a/ros/kxr_controller/launch/pooh_bringup.launch +++ b/ros/kxr_controller/launch/pooh_bringup.launch @@ -10,6 +10,12 @@ + + Date: Thu, 5 Dec 2024 12:57:44 +0900 Subject: [PATCH 25/28] Use head controller --- .../scripts/neck_command_listener.py | 27 ++++++++++----- .../neck_command_listener_independent.py | 33 +++++++++++++------ ros/kxr_controller/scripts/neck_test.py | 33 ++++++++++++------- 3 files changed, 64 insertions(+), 29 deletions(-) diff --git a/ros/kxr_controller/scripts/neck_command_listener.py b/ros/kxr_controller/scripts/neck_command_listener.py index 24777eec..e4976b18 100644 --- a/ros/kxr_controller/scripts/neck_command_listener.py +++ b/ros/kxr_controller/scripts/neck_command_listener.py @@ -23,31 +23,42 @@ # 動作の定義 def nod(send_time=1): + controller_type = 'head_controller' robot_model.head_neck_p.joint_angle(np.deg2rad(30)) - ri.angle_vector(robot_model.angle_vector(), send_time) + ri.angle_vector(robot_model.angle_vector(), send_time, + controller_type=controller_type) time.sleep(1) robot_model.head_neck_p.joint_angle(np.deg2rad(0)) - ri.angle_vector(robot_model.angle_vector(), send_time) + ri.angle_vector(robot_model.angle_vector(), send_time, + controller_type=controller_type) def disagree(send_time=1): + controller_type = 'head_controller' robot_model.head_neck_y.joint_angle(np.deg2rad(30)) - ri.angle_vector(robot_model.angle_vector(), send_time) + ri.angle_vector(robot_model.angle_vector(), send_time, + controller_type=controller_type) time.sleep(1) robot_model.head_neck_y.joint_angle(np.deg2rad(-30)) - ri.angle_vector(robot_model.angle_vector(), send_time) + ri.angle_vector(robot_model.angle_vector(), send_time, + controller_type=controller_type) time.sleep(1) robot_model.head_neck_y.joint_angle(np.deg2rad(30)) - ri.angle_vector(robot_model.angle_vector(), send_time) + ri.angle_vector(robot_model.angle_vector(), send_time, + controller_type=controller_type) time.sleep(1) robot_model.head_neck_y.joint_angle(np.deg2rad(0)) - ri.angle_vector(robot_model.angle_vector(), send_time) + ri.angle_vector(robot_model.angle_vector(), send_time, + controller_type=controller_type) def tilt(send_time=1): + controller_type = 'head_controller' robot_model.head_neck_r.joint_angle(np.deg2rad(30)) - ri.angle_vector(robot_model.angle_vector(), send_time) + ri.angle_vector(robot_model.angle_vector(), send_time, + controller_type=controller_type) time.sleep(2) robot_model.head_neck_r.joint_angle(np.deg2rad(0)) - ri.angle_vector(robot_model.angle_vector(), send_time) + ri.angle_vector(robot_model.angle_vector(), send_time, + controller_type=controller_type) # コールバック関数: /neck_motionのメッセージを受け取り対応する動作を実行 def neck_motion_callback(msg): diff --git a/ros/kxr_controller/scripts/neck_command_listener_independent.py b/ros/kxr_controller/scripts/neck_command_listener_independent.py index 25ae671b..642076bf 100644 --- a/ros/kxr_controller/scripts/neck_command_listener_independent.py +++ b/ros/kxr_controller/scripts/neck_command_listener_independent.py @@ -33,38 +33,51 @@ # nod動作 def nod(send_time=1): - ri.angle_vector(robot_model.init_pose()) + controller_type = 'head_controller' + ri.angle_vector(robot_model.init_pose(), + controller_type=controller_type) robot_model.head_neck_p.joint_angle(np.deg2rad(30)) - ri.angle_vector(robot_model.angle_vector(), send_time) + ri.angle_vector(robot_model.angle_vector(), send_time, + controller_type=controller_type) ri.wait_interpolation() robot_model.head_neck_p.joint_angle(np.deg2rad(0)) - ri.angle_vector(robot_model.angle_vector(), send_time) + ri.angle_vector(robot_model.angle_vector(), send_time, + controller_type=controller_type) ri.wait_interpolation() # disagree動作 def disagree(send_time=1): - ri.angle_vector(robot_model.init_pose()) + controller_type = 'head_controller' + ri.angle_vector(robot_model.init_pose(), + controller_type=controller_type) robot_model.head_neck_y.joint_angle(np.deg2rad(30)) - ri.angle_vector(robot_model.angle_vector(), send_time) + ri.angle_vector(robot_model.angle_vector(), send_time, + controller_type=controller_type) ri.wait_interpolation() robot_model.head_neck_y.joint_angle(np.deg2rad(-30)) - ri.angle_vector(robot_model.angle_vector(), send_time) + ri.angle_vector(robot_model.angle_vector(), send_time, + controller_type=controller_type) ri.wait_interpolation() #robot_model.head_neck_y.joint_angle(np.deg2rad(30)) #ri.angle_vector(robot_model.angle_vector(), send_time) #ri.wait_interpolation() robot_model.head_neck_y.joint_angle(np.deg2rad(0)) - ri.angle_vector(robot_model.angle_vector(), send_time) + ri.angle_vector(robot_model.angle_vector(), send_time, + controller_type=controller_type) ri.wait_interpolation() # tilt動作 def tilt(send_time=1): - ri.angle_vector(robot_model.init_pose()) + controller_type = 'head_controller' + ri.angle_vector(robot_model.init_pose(), + controller_type=controller_type) robot_model.head_neck_r.joint_angle(np.deg2rad(30)) - ri.angle_vector(robot_model.angle_vector(), send_time) + ri.angle_vector(robot_model.angle_vector(), send_time, + controller_type=controller_type) ri.wait_interpolation() robot_model.head_neck_r.joint_angle(np.deg2rad(0)) - ri.angle_vector(robot_model.angle_vector(), send_time) + ri.angle_vector(robot_model.angle_vector(), send_time, + controller_type=controller_type) ri.wait_interpolation() # コールバック関数 diff --git a/ros/kxr_controller/scripts/neck_test.py b/ros/kxr_controller/scripts/neck_test.py index 9e0826f7..976341da 100644 --- a/ros/kxr_controller/scripts/neck_test.py +++ b/ros/kxr_controller/scripts/neck_test.py @@ -24,31 +24,42 @@ #define nod, disagree, tilt and then define test def nod(send_time = 1): + controller_type = 'head_controller' robot_model.head_neck_p.joint_angle(np.deg2rad(30)) - ri.angle_vector(robot_model.angle_vector(), send_time) + ri.angle_vector(robot_model.angle_vector(), send_time, + controller_type=controller_type) time.sleep(1) robot_model.head_neck_p.joint_angle(np.deg2rad(0)) - ri.angle_vector(robot_model.angle_vector(), send_time) + ri.angle_vector(robot_model.angle_vector(), send_time, + controller_type=controller_type) def disagree(send_time = 1): + controller_type = 'head_controller' robot_model.head_neck_y.joint_angle(np.deg2rad(30)) - ri.angle_vector(robot_model.angle_vector(), send_time) - time.sleep(1) + ri.angle_vector(robot_model.angle_vector(), send_time, + controller_type=controller_type) + time.sleep(1) robot_model.head_neck_y.joint_angle(np.deg2rad(-30)) - ri.angle_vector(robot_model.angle_vector(), send_time) - time.sleep(1) + ri.angle_vector(robot_model.angle_vector(), send_time, + controller_type=controller_type) + time.sleep(1) robot_model.head_neck_y.joint_angle(np.deg2rad(30)) - ri.angle_vector(robot_model.angle_vector(), send_time) - time.sleep(1) + ri.angle_vector(robot_model.angle_vector(), send_time, + controller_type=controller_type) + time.sleep(1) robot_model.head_neck_y.joint_angle(np.deg2rad(0)) - ri.angle_vector(robot_model.angle_vector(), send_time) + ri.angle_vector(robot_model.angle_vector(), send_time, + controller_type=controller_type) def tilt(send_time = 1): + controller_type = 'head_controller' robot_model.head_neck_r.joint_angle(np.deg2rad(30)) - ri.angle_vector(robot_model.angle_vector(), send_time) + ri.angle_vector(robot_model.angle_vector(), send_time, + controller_type=controller_type) time.sleep(2) robot_model.head_neck_r.joint_angle(np.deg2rad(0)) - ri.angle_vector(robot_model.angle_vector(), send_time) + ri.angle_vector(robot_model.angle_vector(), send_time, + controller_type=controller_type) def test(): From c5840aa64c53cb03a8f3b669d69adfebd0922bcf Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Thu, 5 Dec 2024 13:24:34 +0900 Subject: [PATCH 26/28] Fixed head controller name --- ros/kxr_controller/python/kxr_controller/pooh_interface.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ros/kxr_controller/python/kxr_controller/pooh_interface.py b/ros/kxr_controller/python/kxr_controller/pooh_interface.py index fd51ce5e..f819ff87 100644 --- a/ros/kxr_controller/python/kxr_controller/pooh_interface.py +++ b/ros/kxr_controller/python/kxr_controller/pooh_interface.py @@ -41,8 +41,8 @@ def rarm_controller(self): def head_controller(self): return dict( controller_type='head_controller', - controller_action='head_traj_controller/follow_joint_trajectory', - controller_state='head_traj_controller/state', + controller_action='head_controller/follow_joint_trajectory', + controller_state='head_controller/state', action_type=control_msgs.msg.FollowJointTrajectoryAction, joint_names=['head_neck_y', 'head_neck_p', 'head_neck_r']) From a0f308ad8069f649de7efa4e3a6d8508449af6a7 Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Thu, 5 Dec 2024 16:24:52 +0900 Subject: [PATCH 27/28] Modified arduino eye brow joint_states --- ros/kxr_controller/scripts/i2c_joint_control.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros/kxr_controller/scripts/i2c_joint_control.py b/ros/kxr_controller/scripts/i2c_joint_control.py index ab601d26..cd03ca8d 100755 --- a/ros/kxr_controller/scripts/i2c_joint_control.py +++ b/ros/kxr_controller/scripts/i2c_joint_control.py @@ -31,7 +31,7 @@ def joint_state_callback(msg): def main(): rospy.init_node('joint_state_to_i2c', anonymous=True) - rospy.Subscriber("joint_states", JointState, joint_state_callback) + rospy.Subscriber("eye_brow/joint_states", JointState, joint_state_callback) rospy.loginfo("Started JointState to I2C bridge") rospy.spin() From 1af9c2d178470af7d8b353a9a53b2f72c1482557 Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Thu, 5 Dec 2024 16:35:35 +0900 Subject: [PATCH 28/28] Use pooh ros robot interface --- .../scripts/neck_command_listener_independent.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ros/kxr_controller/scripts/neck_command_listener_independent.py b/ros/kxr_controller/scripts/neck_command_listener_independent.py index 642076bf..c8be3561 100644 --- a/ros/kxr_controller/scripts/neck_command_listener_independent.py +++ b/ros/kxr_controller/scripts/neck_command_listener_independent.py @@ -5,7 +5,7 @@ import time import rospy from std_msgs.msg import String -from kxr_controller.kxr_interface import KXRROSRobotInterface +from kxr_controller.pooh_interface import PoohROSRobotInterface from kxr_models.download_urdf import download_urdf_mesh_files from skrobot.model import RobotModel import numpy as np @@ -20,7 +20,7 @@ robot_model.load_urdf_from_robot_description( namespace + '/robot_description_viz') rospy.loginfo("Init Real Robot Interface") -ri = KXRROSRobotInterface(robot_model, namespace=None, controller_timeout=60.0) +ri = PoohROSRobotInterface(robot_model, namespace=None, controller_timeout=60.0) rospy.loginfo("Init Real Robot Interface Done") # サーボをONにし、init_pose