#878 - Added PERF flag by using a custom python scripttags/v0.3.11-rc1
| @@ -280,7 +280,7 @@ jobs: | |||||
| run: | | run: | | ||||
| cargo install --path binaries/cli --locked | cargo install --path binaries/cli --locked | ||||
| - name: "Test CLI (Rust)" | - name: "Test CLI (Rust)" | ||||
| timeout-minutes: 30 | |||||
| timeout-minutes: 45 | |||||
| # fail-fast by using bash shell explictly | # fail-fast by using bash shell explictly | ||||
| shell: bash | shell: bash | ||||
| run: | | run: | | ||||
| @@ -312,7 +312,7 @@ jobs: | |||||
| enable-cache: true | enable-cache: true | ||||
| - name: "Test CLI (Python)" | - name: "Test CLI (Python)" | ||||
| timeout-minutes: 30 | |||||
| timeout-minutes: 45 | |||||
| # fail-fast by using bash shell explictly | # fail-fast by using bash shell explictly | ||||
| shell: bash | shell: bash | ||||
| run: | | run: | | ||||
| @@ -380,7 +380,7 @@ jobs: | |||||
| dora run tests/queue_size_latest_data_rust/dataflow.yaml --uv | dora run tests/queue_size_latest_data_rust/dataflow.yaml --uv | ||||
| - name: "Test CLI (C)" | - name: "Test CLI (C)" | ||||
| timeout-minutes: 30 | |||||
| timeout-minutes: 45 | |||||
| # fail-fast by using bash shell explictly | # fail-fast by using bash shell explictly | ||||
| shell: bash | shell: bash | ||||
| if: runner.os == 'Linux' | if: runner.os == 'Linux' | ||||
| @@ -399,7 +399,7 @@ jobs: | |||||
| dora destroy | dora destroy | ||||
| - name: "Test CLI (C++)" | - name: "Test CLI (C++)" | ||||
| timeout-minutes: 30 | |||||
| timeout-minutes: 45 | |||||
| # fail-fast by using bash shell explictly | # fail-fast by using bash shell explictly | ||||
| shell: bash | shell: bash | ||||
| if: runner.os == 'Linux' | if: runner.os == 'Linux' | ||||
| @@ -287,8 +287,7 @@ def arguments_stub( | |||||
| param_names = list(real_parameters.keys()) | param_names = list(real_parameters.keys()) | ||||
| if param_names and param_names[0] == "self": | if param_names and param_names[0] == "self": | ||||
| del param_names[0] | del param_names[0] | ||||
| for name, t in zip(param_names, builtin[0]): | |||||
| parsed_param_types[name] = t | |||||
| parsed_param_types = {name: t for name, t in zip(param_names, builtin[0])} | |||||
| # Types from comment | # Types from comment | ||||
| for match in re.findall( | for match in re.findall( | ||||
| @@ -2,6 +2,7 @@ | |||||
| from dora import Node | from dora import Node | ||||
| def main(): | def main(): | ||||
| """Listen for input events and print received messages.""" | """Listen for input events and print received messages.""" | ||||
| node = Node() | node = Node() | ||||
| @@ -3,6 +3,7 @@ | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from dora import Node | from dora import Node | ||||
| def main(): | def main(): | ||||
| """Process node input events and send speech output.""" | """Process node input events and send speech output.""" | ||||
| node = Node() | node = Node() | ||||
| @@ -1,17 +1,25 @@ | |||||
| import enum | |||||
| """Module for managing Dynamixel servo bus communication and control. | |||||
| import pyarrow as pa | |||||
| This module provides functionality for communicating with and controlling Dynamixel servos | |||||
| through a serial bus interface. It includes methods for reading and writing servo parameters, | |||||
| managing torque and operating modes, and handling joint positions and velocities. | |||||
| """ | |||||
| import enum | |||||
| from typing import Union | from typing import Union | ||||
| import pyarrow as pa | |||||
| from dynamixel_sdk import ( | from dynamixel_sdk import ( | ||||
| PacketHandler, | |||||
| PortHandler, | |||||
| COMM_SUCCESS, | COMM_SUCCESS, | ||||
| DXL_HIBYTE, | |||||
| DXL_HIWORD, | |||||
| DXL_LOBYTE, | |||||
| DXL_LOWORD, | |||||
| GroupSyncRead, | GroupSyncRead, | ||||
| GroupSyncWrite, | GroupSyncWrite, | ||||
| PacketHandler, | |||||
| PortHandler, | |||||
| ) | ) | ||||
| from dynamixel_sdk import DXL_HIBYTE, DXL_HIWORD, DXL_LOBYTE, DXL_LOWORD | |||||
| PROTOCOL_VERSION = 2.0 | PROTOCOL_VERSION = 2.0 | ||||
| BAUD_RATE = 1_000_000 | BAUD_RATE = 1_000_000 | ||||
| @@ -20,40 +28,38 @@ TIMEOUT_MS = 1000 | |||||
| def wrap_joints_and_values( | def wrap_joints_and_values( | ||||
| joints: Union[list[str], pa.Array], | joints: Union[list[str], pa.Array], | ||||
| values: Union[int, list[int], pa.Array], | |||||
| values: Union[list[int], pa.Array], | |||||
| ) -> pa.StructArray: | ) -> pa.StructArray: | ||||
| """ | |||||
| Wraps joints and their corresponding values into a structured array. | |||||
| """Wrap joints and their corresponding values into a structured array. | |||||
| :param joints: A list, numpy array, or pyarrow array of joint names. | |||||
| :type joints: Union[list[str], np.array, pa.Array] | |||||
| :param values: A single integer value, or a list, numpy array, or pyarrow array of integer values. | |||||
| If a single integer is provided, it will be broadcasted to all joints. | |||||
| :type values: Union[int, list[int], np.array, pa.Array] | |||||
| Args: | |||||
| joints: A list, numpy array, or pyarrow array of joint names. | |||||
| values: A single integer value, or a list, numpy array, or pyarrow array of integer values. | |||||
| If a single integer is provided, it will be broadcasted to all joints. | |||||
| :return: A structured array with two fields: | |||||
| - "joints": A string field containing the names of the joints. | |||||
| - "values": An Int32Array containing the values corresponding to the joints. | |||||
| :rtype: pa.StructArray | |||||
| Returns: | |||||
| A structured array with two fields: | |||||
| - "joints": A string field containing the names of the joints. | |||||
| - "values": An Int32Array containing the values corresponding to the joints. | |||||
| :raises ValueError: If lengths of joints and values do not match. | |||||
| Raises: | |||||
| ValueError: If lengths of joints and values do not match. | |||||
| Example: | Example: | ||||
| -------- | |||||
| joints = ["shoulder_pan", "shoulder_lift", "elbow_flex"] | |||||
| values = [100, 200, 300] | |||||
| struct_array = wrap_joints_and_values(joints, values) | |||||
| joints = ["shoulder_pan", "shoulder_lift", "elbow_flex"] | |||||
| values = [100, 200, 300] | |||||
| struct_array = wrap_joints_and_values(joints, values) | |||||
| This example wraps the given joints and their corresponding values into a structured array. | |||||
| This example wraps the given joints and their corresponding values into a structured array. | |||||
| Another example with a single integer value: | |||||
| joints = ["shoulder_pan", "shoulder_lift", "elbow_flex"] | |||||
| value = 150 | |||||
| struct_array = wrap_joints_and_values(joints, value) | |||||
| Another example with a single integer value: | |||||
| joints = ["shoulder_pan", "shoulder_lift", "elbow_flex"] | |||||
| value = 150 | |||||
| struct_array = wrap_joints_and_values(joints, value) | |||||
| This example broadcasts the single integer value to all joints and wraps them into a structured array. | |||||
| """ | |||||
| This example broadcasts the single integer value to all joints and wraps them into a structured array. | |||||
| """ | |||||
| if isinstance(values, int): | if isinstance(values, int): | ||||
| values = [values] * len(joints) | values = [values] * len(joints) | ||||
| @@ -69,16 +75,20 @@ def wrap_joints_and_values( | |||||
| mask = values.is_null() | mask = values.is_null() | ||||
| return pa.StructArray.from_arrays( | return pa.StructArray.from_arrays( | ||||
| arrays=[joints, values], names=["joints", "values"], mask=mask | |||||
| arrays=[joints, values], names=["joints", "values"], mask=mask, | |||||
| ).drop_null() | ).drop_null() | ||||
| class TorqueMode(enum.Enum): | class TorqueMode(enum.Enum): | ||||
| """Enumeration for servo torque control modes.""" | |||||
| ENABLED = pa.scalar(1, pa.uint32()) | ENABLED = pa.scalar(1, pa.uint32()) | ||||
| DISABLED = pa.scalar(0, pa.uint32()) | DISABLED = pa.scalar(0, pa.uint32()) | ||||
| class OperatingMode(enum.Enum): | class OperatingMode(enum.Enum): | ||||
| """Enumeration for servo operating modes.""" | |||||
| VELOCITY = pa.scalar(1, pa.uint32()) | VELOCITY = pa.scalar(1, pa.uint32()) | ||||
| POSITION = pa.scalar(3, pa.uint32()) | POSITION = pa.scalar(3, pa.uint32()) | ||||
| EXTENDED_POSITION = pa.scalar(4, pa.uint32()) | EXTENDED_POSITION = pa.scalar(4, pa.uint32()) | ||||
| @@ -152,8 +162,18 @@ MODEL_CONTROL_TABLE = { | |||||
| class DynamixelBus: | class DynamixelBus: | ||||
| """Class for managing communication with Dynamixel servos on a serial bus.""" | |||||
| def __init__(self, port: str, description: dict[str, (int, str)]): | def __init__(self, port: str, description: dict[str, (int, str)]): | ||||
| """Initialize the Dynamixel bus connection. | |||||
| Args: | |||||
| port: The serial port to connect to the Dynamixel bus. | |||||
| description: A dictionary containing the description of the motors connected to the bus. | |||||
| The keys are the motor names and the values are tuples containing the motor id | |||||
| and the motor model. | |||||
| """ | |||||
| self.port = port | self.port = port | ||||
| self.descriptions = description | self.descriptions = description | ||||
| self.motor_ctrl = {} | self.motor_ctrl = {} | ||||
| @@ -184,9 +204,17 @@ class DynamixelBus: | |||||
| self.group_writers = {} | self.group_writers = {} | ||||
| def close(self): | def close(self): | ||||
| """Close the serial port connection.""" | |||||
| self.port_handler.closePort() | self.port_handler.closePort() | ||||
| def write(self, data_name: str, data: pa.StructArray): | def write(self, data_name: str, data: pa.StructArray): | ||||
| """Write data to the specified servo parameters. | |||||
| Args: | |||||
| data_name: Name of the parameter to write. | |||||
| data: Structured array containing the data to write. | |||||
| """ | |||||
| motor_ids = [ | motor_ids = [ | ||||
| self.motor_ctrl[motor_name.as_py()]["id"] | self.motor_ctrl[motor_name.as_py()]["id"] | ||||
| for motor_name in data.field("joints") | for motor_name in data.field("joints") | ||||
| @@ -239,7 +267,7 @@ class DynamixelBus: | |||||
| else: | else: | ||||
| raise NotImplementedError( | raise NotImplementedError( | ||||
| f"Value of the number of bytes to be sent is expected to be in [1, 2, 4], but {packet_bytes_size} " | f"Value of the number of bytes to be sent is expected to be in [1, 2, 4], but {packet_bytes_size} " | ||||
| f"is provided instead." | |||||
| f"is provided instead.", | |||||
| ) | ) | ||||
| if init_group: | if init_group: | ||||
| @@ -251,10 +279,20 @@ class DynamixelBus: | |||||
| if comm != COMM_SUCCESS: | if comm != COMM_SUCCESS: | ||||
| raise ConnectionError( | raise ConnectionError( | ||||
| f"Write failed due to communication error on port {self.port} for group_key {group_key}: " | f"Write failed due to communication error on port {self.port} for group_key {group_key}: " | ||||
| f"{self.packet_handler.getTxRxResult(comm)}" | |||||
| f"{self.packet_handler.getTxRxResult(comm)}", | |||||
| ) | ) | ||||
| def read(self, data_name: str, motor_names: pa.Array) -> pa.StructArray: | def read(self, data_name: str, motor_names: pa.Array) -> pa.StructArray: | ||||
| """Read data from the specified servo parameters. | |||||
| Args: | |||||
| data_name: Name of the parameter to read. | |||||
| motor_names: Array of motor names to read from. | |||||
| Returns: | |||||
| Structured array containing the read data. | |||||
| """ | |||||
| motor_ids = [ | motor_ids = [ | ||||
| self.motor_ctrl[motor_name.as_py()]["id"] for motor_name in motor_names | self.motor_ctrl[motor_name.as_py()]["id"] for motor_name in motor_names | ||||
| ] | ] | ||||
| @@ -281,13 +319,13 @@ class DynamixelBus: | |||||
| if comm != COMM_SUCCESS: | if comm != COMM_SUCCESS: | ||||
| raise ConnectionError( | raise ConnectionError( | ||||
| f"Read failed due to communication error on port {self.port} for group_key {group_key}: " | f"Read failed due to communication error on port {self.port} for group_key {group_key}: " | ||||
| f"{self.packet_handler.getTxRxResult(comm)}" | |||||
| f"{self.packet_handler.getTxRxResult(comm)}", | |||||
| ) | ) | ||||
| values = pa.array( | values = pa.array( | ||||
| [ | [ | ||||
| self.group_readers[group_key].getData( | self.group_readers[group_key].getData( | ||||
| idx, packet_address, packet_bytes_size | |||||
| idx, packet_address, packet_bytes_size, | |||||
| ) | ) | ||||
| for idx in motor_ids | for idx in motor_ids | ||||
| ], | ], | ||||
| @@ -298,31 +336,100 @@ class DynamixelBus: | |||||
| return wrap_joints_and_values(motor_names, values) | return wrap_joints_and_values(motor_names, values) | ||||
| def write_torque_enable(self, torque_mode: pa.StructArray): | def write_torque_enable(self, torque_mode: pa.StructArray): | ||||
| """Enable or disable torque for the specified servos. | |||||
| Args: | |||||
| torque_mode: Structured array containing the torque mode for each servo. | |||||
| """ | |||||
| self.write("Torque_Enable", torque_mode) | self.write("Torque_Enable", torque_mode) | ||||
| def write_operating_mode(self, operating_mode: pa.StructArray): | def write_operating_mode(self, operating_mode: pa.StructArray): | ||||
| """Set the operating mode for the specified servos. | |||||
| Args: | |||||
| operating_mode: Structured array containing the operating mode for each servo. | |||||
| """ | |||||
| self.write("Operating_Mode", operating_mode) | self.write("Operating_Mode", operating_mode) | ||||
| def read_position(self, motor_names: pa.Array) -> pa.StructArray: | def read_position(self, motor_names: pa.Array) -> pa.StructArray: | ||||
| """Read the current position of the specified servos. | |||||
| Args: | |||||
| motor_names: Array of motor names to read positions from. | |||||
| Returns: | |||||
| Structured array containing the current positions. | |||||
| """ | |||||
| return self.read("Present_Position", motor_names) | return self.read("Present_Position", motor_names) | ||||
| def read_velocity(self, motor_names: pa.Array) -> pa.StructArray: | def read_velocity(self, motor_names: pa.Array) -> pa.StructArray: | ||||
| """Read the current velocity of the specified servos. | |||||
| Args: | |||||
| motor_names: Array of motor names to read velocities from. | |||||
| Returns: | |||||
| Structured array containing the current velocities. | |||||
| """ | |||||
| return self.read("Present_Velocity", motor_names) | return self.read("Present_Velocity", motor_names) | ||||
| def read_current(self, motor_names: pa.Array) -> pa.StructArray: | def read_current(self, motor_names: pa.Array) -> pa.StructArray: | ||||
| """Read the current current of the specified servos. | |||||
| Args: | |||||
| motor_names: Array of motor names to read currents from. | |||||
| Returns: | |||||
| Structured array containing the current currents. | |||||
| """ | |||||
| return self.read("Present_Current", motor_names) | return self.read("Present_Current", motor_names) | ||||
| def write_goal_position(self, goal_position: pa.StructArray): | def write_goal_position(self, goal_position: pa.StructArray): | ||||
| """Set the goal position for the specified servos. | |||||
| Args: | |||||
| goal_position: Structured array containing the goal positions. | |||||
| """ | |||||
| self.write("Goal_Position", goal_position) | self.write("Goal_Position", goal_position) | ||||
| def write_goal_current(self, goal_current: pa.StructArray): | def write_goal_current(self, goal_current: pa.StructArray): | ||||
| """Set the goal current for the specified servos. | |||||
| Args: | |||||
| goal_current: Structured array containing the goal currents. | |||||
| """ | |||||
| self.write("Goal_Current", goal_current) | self.write("Goal_Current", goal_current) | ||||
| def write_position_p_gain(self, position_p_gain: pa.StructArray): | def write_position_p_gain(self, position_p_gain: pa.StructArray): | ||||
| """Set the position P gain for the specified servos. | |||||
| Args: | |||||
| position_p_gain: Structured array containing the position P gains. | |||||
| """ | |||||
| self.write("Position_P_Gain", position_p_gain) | self.write("Position_P_Gain", position_p_gain) | ||||
| def write_position_i_gain(self, position_i_gain: pa.StructArray): | def write_position_i_gain(self, position_i_gain: pa.StructArray): | ||||
| """Set the position I gain for the specified servos. | |||||
| Args: | |||||
| position_i_gain: Structured array containing the position I gains. | |||||
| """ | |||||
| self.write("Position_I_Gain", position_i_gain) | self.write("Position_I_Gain", position_i_gain) | ||||
| def write_position_d_gain(self, position_d_gain: pa.StructArray): | def write_position_d_gain(self, position_d_gain: pa.StructArray): | ||||
| """Set the position D gain for the specified servos. | |||||
| Args: | |||||
| position_d_gain: Structured array containing the position D gains. | |||||
| """ | |||||
| self.write("Position_D_Gain", position_d_gain) | self.write("Position_D_Gain", position_d_gain) | ||||
| @@ -1,5 +1,7 @@ | |||||
| """ | |||||
| LCR Configuration Tool: This program is used to automatically configure the Low Cost Robot (LCR) for the user. | |||||
| """Module for configuring and setting up the Low Cost Robot (LCR) hardware. | |||||
| This module provides functionality for initializing and configuring the LCR robot's | |||||
| servo motors and other hardware components. | |||||
| The program will: | The program will: | ||||
| 1. Disable all torque motors of provided LCR. | 1. Disable all torque motors of provided LCR. | ||||
| @@ -14,21 +16,17 @@ It will also enable all appropriate operating modes for the LCR. | |||||
| """ | """ | ||||
| import argparse | import argparse | ||||
| import time | |||||
| import json | import json | ||||
| import time | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from bus import DynamixelBus, TorqueMode, OperatingMode | |||||
| from pwm_position_control.transform import pwm_to_logical_arrow, wrap_joints_and_values | |||||
| from bus import DynamixelBus, OperatingMode, TorqueMode | |||||
| from pwm_position_control.functions import construct_control_table | |||||
| from pwm_position_control.tables import ( | from pwm_position_control.tables import ( | ||||
| construct_logical_to_pwm_conversion_table_arrow, | construct_logical_to_pwm_conversion_table_arrow, | ||||
| construct_pwm_to_logical_conversion_table_arrow, | construct_pwm_to_logical_conversion_table_arrow, | ||||
| ) | ) | ||||
| from pwm_position_control.functions import construct_control_table | |||||
| from pwm_position_control.transform import pwm_to_logical_arrow, wrap_joints_and_values | |||||
| FULL_ARM = pa.array( | FULL_ARM = pa.array( | ||||
| [ | [ | ||||
| @@ -51,31 +49,39 @@ GRIPPER = pa.array(["gripper"], type=pa.string()) | |||||
| def pause(): | def pause(): | ||||
| """Pause execution and wait for user input to continue.""" | |||||
| input("Press Enter to continue...") | input("Press Enter to continue...") | ||||
| def configure_servos(bus: DynamixelBus): | def configure_servos(bus: DynamixelBus): | ||||
| """Configure servo motors with appropriate settings. | |||||
| Args: | |||||
| bus: DynamixelBus instance for servo communication | |||||
| """ | |||||
| bus.write_torque_enable( | bus.write_torque_enable( | ||||
| wrap_joints_and_values(FULL_ARM, [TorqueMode.DISABLED.value] * 6) | |||||
| wrap_joints_and_values(FULL_ARM, [TorqueMode.DISABLED.value] * 6), | |||||
| ) | ) | ||||
| bus.write_operating_mode( | bus.write_operating_mode( | ||||
| wrap_joints_and_values( | wrap_joints_and_values( | ||||
| ARM_WITHOUT_GRIPPER, [OperatingMode.EXTENDED_POSITION.value] * 5 | |||||
| ) | |||||
| ARM_WITHOUT_GRIPPER, [OperatingMode.EXTENDED_POSITION.value] * 5, | |||||
| ), | |||||
| ) | ) | ||||
| bus.write_operating_mode( | bus.write_operating_mode( | ||||
| wrap_joints_and_values( | wrap_joints_and_values( | ||||
| GRIPPER, [OperatingMode.CURRENT_CONTROLLED_POSITION.value] | |||||
| ) | |||||
| GRIPPER, [OperatingMode.CURRENT_CONTROLLED_POSITION.value], | |||||
| ), | |||||
| ) | ) | ||||
| def main(): | def main(): | ||||
| """Initialize and configure the LCR robot hardware components.""" | |||||
| parser = argparse.ArgumentParser( | parser = argparse.ArgumentParser( | ||||
| description="LCR Auto Configure: This program is used to automatically configure the Low Cost Robot (LCR) for " | description="LCR Auto Configure: This program is used to automatically configure the Low Cost Robot (LCR) for " | ||||
| "the user." | |||||
| "the user.", | |||||
| ) | ) | ||||
| parser.add_argument("--port", type=str, required=True, help="The port of the LCR.") | parser.add_argument("--port", type=str, required=True, help="The port of the LCR.") | ||||
| @@ -95,7 +101,7 @@ def main(): | |||||
| help="If the LCR is the follower of the user.", | help="If the LCR is the follower of the user.", | ||||
| ) | ) | ||||
| parser.add_argument( | parser.add_argument( | ||||
| "--leader", action="store_true", help="If the LCR is the leader of the user." | |||||
| "--leader", action="store_true", help="If the LCR is the leader of the user.", | |||||
| ) | ) | ||||
| args = parser.parse_args() | args = parser.parse_args() | ||||
| @@ -138,10 +144,10 @@ def main(): | |||||
| pwm_positions = (pwm_position_1, pwm_position_2) | pwm_positions = (pwm_position_1, pwm_position_2) | ||||
| pwm_to_logical_conversion_table = construct_pwm_to_logical_conversion_table_arrow( | pwm_to_logical_conversion_table = construct_pwm_to_logical_conversion_table_arrow( | ||||
| pwm_positions, targets | |||||
| pwm_positions, targets, | |||||
| ) | ) | ||||
| logical_to_pwm_conversion_table = construct_logical_to_pwm_conversion_table_arrow( | logical_to_pwm_conversion_table = construct_logical_to_pwm_conversion_table_arrow( | ||||
| pwm_positions, targets | |||||
| pwm_positions, targets, | |||||
| ) | ) | ||||
| control_table_json = {} | control_table_json = {} | ||||
| @@ -180,7 +186,7 @@ def main(): | |||||
| path = ( | path = ( | ||||
| input( | input( | ||||
| f"Please enter the path of the configuration file (default is ./examples/alexk-lcr/configs/{leader}.{left}.json): " | |||||
| f"Please enter the path of the configuration file (default is ./examples/alexk-lcr/configs/{leader}.{left}.json): ", | |||||
| ) | ) | ||||
| or f"./examples/alexk-lcr/configs/{leader}.{left}.json" | or f"./examples/alexk-lcr/configs/{leader}.{left}.json" | ||||
| ) | ) | ||||
| @@ -189,21 +195,21 @@ def main(): | |||||
| json.dump(control_table_json, file) | json.dump(control_table_json, file) | ||||
| control_table = construct_control_table( | control_table = construct_control_table( | ||||
| pwm_to_logical_conversion_table, logical_to_pwm_conversion_table | |||||
| pwm_to_logical_conversion_table, logical_to_pwm_conversion_table, | |||||
| ) | ) | ||||
| while True: | while True: | ||||
| try: | try: | ||||
| pwm_position = arm.read_position(FULL_ARM) | pwm_position = arm.read_position(FULL_ARM) | ||||
| logical_position = pwm_to_logical_arrow( | logical_position = pwm_to_logical_arrow( | ||||
| pwm_position, control_table, ranged=True | |||||
| pwm_position, control_table, ranged=True, | |||||
| ).field("values") | ).field("values") | ||||
| print(f"Logical Position: {logical_position}") | print(f"Logical Position: {logical_position}") | ||||
| except ConnectionError: | except ConnectionError: | ||||
| print( | print( | ||||
| "Connection error occurred. Please check the connection and try again." | |||||
| "Connection error occurred. Please check the connection and try again.", | |||||
| ) | ) | ||||
| time.sleep(0.5) | time.sleep(0.5) | ||||
| @@ -1,24 +1,29 @@ | |||||
| import os | |||||
| """Module for interpolating between LCR robot configurations. | |||||
| This module provides functionality for calculating and interpolating between | |||||
| different LCR robot configurations to ensure smooth transitions. | |||||
| """ | |||||
| import argparse | import argparse | ||||
| import json | import json | ||||
| import os | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| import pyarrow.compute as pc | import pyarrow.compute as pc | ||||
| from dora import Node | from dora import Node | ||||
| from pwm_position_control.load import load_control_table_from_json_conversion_tables | |||||
| from pwm_position_control.transform import ( | from pwm_position_control.transform import ( | ||||
| wrap_joints_and_values, | |||||
| pwm_to_logical_arrow, | |||||
| logical_to_pwm_with_offset_arrow, | logical_to_pwm_with_offset_arrow, | ||||
| pwm_to_logical_arrow, | |||||
| wrap_joints_and_values, | |||||
| ) | ) | ||||
| from pwm_position_control.load import load_control_table_from_json_conversion_tables | |||||
| def main(): | def main(): | ||||
| """Initialize and run the LCR interpolation node.""" | |||||
| parser = argparse.ArgumentParser( | parser = argparse.ArgumentParser( | ||||
| description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " | description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " | ||||
| "LCR followers knowing a Leader position and Follower position." | |||||
| "LCR followers knowing a Leader position and Follower position.", | |||||
| ) | ) | ||||
| parser.add_argument( | parser.add_argument( | ||||
| @@ -46,19 +51,19 @@ def main(): | |||||
| if not os.environ.get("LEADER_CONTROL") and args.leader_control is None: | if not os.environ.get("LEADER_CONTROL") and args.leader_control is None: | ||||
| raise ValueError( | raise ValueError( | ||||
| "The leader control is not set. Please set the configuration of the leader in the environment variables or " | "The leader control is not set. Please set the configuration of the leader in the environment variables or " | ||||
| "as an argument." | |||||
| "as an argument.", | |||||
| ) | ) | ||||
| if not os.environ.get("FOLLOWER_CONTROL") and args.follower_control is None: | if not os.environ.get("FOLLOWER_CONTROL") and args.follower_control is None: | ||||
| raise ValueError( | raise ValueError( | ||||
| "The follower control is not set. Please set the configuration of the follower in the environment " | "The follower control is not set. Please set the configuration of the follower in the environment " | ||||
| "variables or as an argument." | |||||
| "variables or as an argument.", | |||||
| ) | ) | ||||
| with open( | with open( | ||||
| os.environ.get("LEADER_CONTROL") | os.environ.get("LEADER_CONTROL") | ||||
| if args.leader_control is None | if args.leader_control is None | ||||
| else args.leader_control | |||||
| else args.leader_control, | |||||
| ) as file: | ) as file: | ||||
| leader_control = json.load(file) | leader_control = json.load(file) | ||||
| load_control_table_from_json_conversion_tables(leader_control, leader_control) | load_control_table_from_json_conversion_tables(leader_control, leader_control) | ||||
| @@ -66,11 +71,11 @@ def main(): | |||||
| with open( | with open( | ||||
| os.environ.get("FOLLOWER_CONTROL") | os.environ.get("FOLLOWER_CONTROL") | ||||
| if args.follower_control is None | if args.follower_control is None | ||||
| else args.follower_control | |||||
| else args.follower_control, | |||||
| ) as file: | ) as file: | ||||
| follower_control = json.load(file) | follower_control = json.load(file) | ||||
| load_control_table_from_json_conversion_tables( | load_control_table_from_json_conversion_tables( | ||||
| follower_control, follower_control | |||||
| follower_control, follower_control, | |||||
| ) | ) | ||||
| initial_mask = [ | initial_mask = [ | ||||
| @@ -130,7 +135,7 @@ def main(): | |||||
| ) | ) | ||||
| pwm_goal = logical_to_pwm_with_offset_arrow( | pwm_goal = logical_to_pwm_with_offset_arrow( | ||||
| follower_position, logical_goal, follower_control | |||||
| follower_position, logical_goal, follower_control, | |||||
| ) | ) | ||||
| node.send_output("follower_goal", pwm_goal, event["metadata"]) | node.send_output("follower_goal", pwm_goal, event["metadata"]) | ||||
| @@ -1,23 +1,24 @@ | |||||
| import os | |||||
| """TODO: Add docstring.""" | |||||
| import argparse | import argparse | ||||
| import json | import json | ||||
| import os | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| import pyarrow.compute as pc | import pyarrow.compute as pc | ||||
| from dora import Node | from dora import Node | ||||
| from pwm_position_control.load import load_control_table_from_json_conversion_tables | from pwm_position_control.load import load_control_table_from_json_conversion_tables | ||||
| from pwm_position_control.transform import ( | from pwm_position_control.transform import ( | ||||
| wrap_joints_and_values, | |||||
| pwm_to_logical_arrow, | pwm_to_logical_arrow, | ||||
| wrap_joints_and_values, | |||||
| ) | ) | ||||
| def main(): | def main(): | ||||
| """TODO: Add docstring.""" | |||||
| parser = argparse.ArgumentParser( | parser = argparse.ArgumentParser( | ||||
| description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " | description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " | ||||
| "LCR followers knowing a Leader position and Follower position." | |||||
| "LCR followers knowing a Leader position and Follower position.", | |||||
| ) | ) | ||||
| parser.add_argument( | parser.add_argument( | ||||
| @@ -45,19 +46,19 @@ def main(): | |||||
| if not os.environ.get("LEADER_CONTROL") and args.leader_control is None: | if not os.environ.get("LEADER_CONTROL") and args.leader_control is None: | ||||
| raise ValueError( | raise ValueError( | ||||
| "The leader control is not set. Please set the configuration of the leader in the environment variables or " | "The leader control is not set. Please set the configuration of the leader in the environment variables or " | ||||
| "as an argument." | |||||
| "as an argument.", | |||||
| ) | ) | ||||
| if not os.environ.get("FOLLOWER_CONTROL") and args.follower_control is None: | if not os.environ.get("FOLLOWER_CONTROL") and args.follower_control is None: | ||||
| raise ValueError( | raise ValueError( | ||||
| "The follower control is not set. Please set the configuration of the follower in the environment " | "The follower control is not set. Please set the configuration of the follower in the environment " | ||||
| "variables or as an argument." | |||||
| "variables or as an argument.", | |||||
| ) | ) | ||||
| with open( | with open( | ||||
| os.environ.get("LEADER_CONTROL") | os.environ.get("LEADER_CONTROL") | ||||
| if args.leader_control is None | if args.leader_control is None | ||||
| else args.leader_control | |||||
| else args.leader_control, | |||||
| ) as file: | ) as file: | ||||
| leader_control = json.load(file) | leader_control = json.load(file) | ||||
| load_control_table_from_json_conversion_tables(leader_control, leader_control) | load_control_table_from_json_conversion_tables(leader_control, leader_control) | ||||
| @@ -65,11 +66,11 @@ def main(): | |||||
| with open( | with open( | ||||
| os.environ.get("FOLLOWER_CONTROL") | os.environ.get("FOLLOWER_CONTROL") | ||||
| if args.follower_control is None | if args.follower_control is None | ||||
| else args.follower_control | |||||
| else args.follower_control, | |||||
| ) as file: | ) as file: | ||||
| follower_control = json.load(file) | follower_control = json.load(file) | ||||
| load_control_table_from_json_conversion_tables( | load_control_table_from_json_conversion_tables( | ||||
| follower_control, follower_control | |||||
| follower_control, follower_control, | |||||
| ) | ) | ||||
| node = Node(args.name) | node = Node(args.name) | ||||
| @@ -98,11 +99,11 @@ def main(): | |||||
| follower_position = event["value"] | follower_position = event["value"] | ||||
| follower_position = pwm_to_logical_arrow( | follower_position = pwm_to_logical_arrow( | ||||
| follower_position, follower_control | |||||
| follower_position, follower_control, | |||||
| ) | ) | ||||
| node.send_output( | node.send_output( | ||||
| "logical_position", follower_position, event["metadata"] | |||||
| "logical_position", follower_position, event["metadata"], | |||||
| ) | ) | ||||
| elif event_type == "ERROR": | elif event_type == "ERROR": | ||||
| @@ -1,25 +1,30 @@ | |||||
| import os | |||||
| """Module for interpolating between LCR and simulated LCR configurations. | |||||
| This module provides functionality for calculating and interpolating between | |||||
| physical LCR robot configurations and their simulated counterparts. | |||||
| """ | |||||
| import argparse | import argparse | ||||
| import json | import json | ||||
| import os | |||||
| import numpy as np | import numpy as np | ||||
| import pyarrow as pa | import pyarrow as pa | ||||
| import pyarrow.compute as pc | import pyarrow.compute as pc | ||||
| from dora import Node | from dora import Node | ||||
| from pwm_position_control.load import load_control_table_from_json_conversion_tables | from pwm_position_control.load import load_control_table_from_json_conversion_tables | ||||
| from pwm_position_control.transform import ( | from pwm_position_control.transform import ( | ||||
| wrap_joints_and_values, | |||||
| pwm_to_logical_arrow, | |||||
| logical_to_pwm_with_offset_arrow, | logical_to_pwm_with_offset_arrow, | ||||
| pwm_to_logical_arrow, | |||||
| wrap_joints_and_values, | |||||
| ) | ) | ||||
| def main(): | def main(): | ||||
| """Initialize and run the LCR to simulated LCR interpolation node.""" | |||||
| parser = argparse.ArgumentParser( | parser = argparse.ArgumentParser( | ||||
| description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " | description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " | ||||
| "LCR followers knowing a Leader position and Follower position." | |||||
| "LCR followers knowing a Leader position and Follower position.", | |||||
| ) | ) | ||||
| parser.add_argument( | parser.add_argument( | ||||
| @@ -41,13 +46,13 @@ def main(): | |||||
| if not os.environ.get("LEADER_CONTROL") and args.leader_control is None: | if not os.environ.get("LEADER_CONTROL") and args.leader_control is None: | ||||
| raise ValueError( | raise ValueError( | ||||
| "The leader control is not set. Please set the configuration of the leader in the environment variables or " | "The leader control is not set. Please set the configuration of the leader in the environment variables or " | ||||
| "as an argument." | |||||
| "as an argument.", | |||||
| ) | ) | ||||
| with open( | with open( | ||||
| os.environ.get("LEADER_CONTROL") | os.environ.get("LEADER_CONTROL") | ||||
| if args.leader_control is None | if args.leader_control is None | ||||
| else args.leader_control | |||||
| else args.leader_control, | |||||
| ) as file: | ) as file: | ||||
| leader_control = json.load(file) | leader_control = json.load(file) | ||||
| load_control_table_from_json_conversion_tables(leader_control, leader_control) | load_control_table_from_json_conversion_tables(leader_control, leader_control) | ||||
| @@ -114,7 +119,7 @@ def main(): | |||||
| [ | [ | ||||
| joint.as_py() + "_joint" | joint.as_py() + "_joint" | ||||
| for joint in leader_position.field("joints") | for joint in leader_position.field("joints") | ||||
| ] | |||||
| ], | |||||
| ), | ), | ||||
| pc.multiply( | pc.multiply( | ||||
| pc.add(leader_position.field("values"), interpolation_a), | pc.add(leader_position.field("values"), interpolation_a), | ||||
| @@ -1,17 +1,19 @@ | |||||
| import os | |||||
| """TODO: Add docstring.""" | |||||
| import argparse | import argparse | ||||
| import json | import json | ||||
| import os | |||||
| from dora import Node | from dora import Node | ||||
| from pwm_position_control.load import load_control_table_from_json_conversion_tables | from pwm_position_control.load import load_control_table_from_json_conversion_tables | ||||
| from pwm_position_control.transform import logical_to_pwm_with_offset_arrow | from pwm_position_control.transform import logical_to_pwm_with_offset_arrow | ||||
| def main(): | def main(): | ||||
| """TODO: Add docstring.""" | |||||
| parser = argparse.ArgumentParser( | parser = argparse.ArgumentParser( | ||||
| description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " | description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " | ||||
| "LCR followers knowing a Leader position and Follower position." | |||||
| "LCR followers knowing a Leader position and Follower position.", | |||||
| ) | ) | ||||
| parser.add_argument( | parser.add_argument( | ||||
| @@ -33,17 +35,17 @@ def main(): | |||||
| if not os.environ.get("FOLLOWER_CONTROL") and args.follower_control is None: | if not os.environ.get("FOLLOWER_CONTROL") and args.follower_control is None: | ||||
| raise ValueError( | raise ValueError( | ||||
| "The follower control is not set. Please set the configuration of the follower in the environment " | "The follower control is not set. Please set the configuration of the follower in the environment " | ||||
| "variables or as an argument." | |||||
| "variables or as an argument.", | |||||
| ) | ) | ||||
| with open( | with open( | ||||
| os.environ.get("FOLLOWER_CONTROL") | os.environ.get("FOLLOWER_CONTROL") | ||||
| if args.follower_control is None | if args.follower_control is None | ||||
| else args.follower_control | |||||
| else args.follower_control, | |||||
| ) as file: | ) as file: | ||||
| follower_control = json.load(file) | follower_control = json.load(file) | ||||
| load_control_table_from_json_conversion_tables( | load_control_table_from_json_conversion_tables( | ||||
| follower_control, follower_control | |||||
| follower_control, follower_control, | |||||
| ) | ) | ||||
| node = Node(args.name) | node = Node(args.name) | ||||
| @@ -65,7 +67,7 @@ def main(): | |||||
| continue | continue | ||||
| physical_goal = logical_to_pwm_with_offset_arrow( | physical_goal = logical_to_pwm_with_offset_arrow( | ||||
| follower_position, leader_position, follower_control | |||||
| follower_position, leader_position, follower_control, | |||||
| ) | ) | ||||
| node.send_output("follower_goal", physical_goal, event["metadata"]) | node.send_output("follower_goal", physical_goal, event["metadata"]) | ||||
| @@ -1,13 +1,24 @@ | |||||
| """Module for managing Dynamixel servo communication and control. | |||||
| This module provides functionality for communicating with and controlling Dynamixel servos | |||||
| through a serial interface. It includes methods for reading and writing servo parameters, | |||||
| managing operating modes, and handling joint positions and velocities. | |||||
| """ | |||||
| from __future__ import annotations | from __future__ import annotations | ||||
| import enum | |||||
| import math | import math | ||||
| import os | import os | ||||
| from dynamixel_sdk import * # Uses Dynamixel SDK library | |||||
| from dataclasses import dataclass | |||||
| import enum | |||||
| import time | import time | ||||
| from dataclasses import dataclass | |||||
| from dynamixel_sdk import * # Uses Dynamixel SDK library | |||||
| class ReadAttribute(enum.Enum): | class ReadAttribute(enum.Enum): | ||||
| """Enumeration for Dynamixel servo read attributes.""" | |||||
| TEMPERATURE = 146 | TEMPERATURE = 146 | ||||
| VOLTAGE = 145 | VOLTAGE = 145 | ||||
| VELOCITY = 128 | VELOCITY = 128 | ||||
| @@ -20,6 +31,8 @@ class ReadAttribute(enum.Enum): | |||||
| class OperatingMode(enum.Enum): | class OperatingMode(enum.Enum): | ||||
| """Enumeration for Dynamixel servo operating modes.""" | |||||
| VELOCITY = 1 | VELOCITY = 1 | ||||
| POSITION = 3 | POSITION = 3 | ||||
| CURRENT_CONTROLLED_POSITION = 5 | CURRENT_CONTROLLED_POSITION = 5 | ||||
| @@ -28,6 +41,8 @@ class OperatingMode(enum.Enum): | |||||
| class Dynamixel: | class Dynamixel: | ||||
| """Class for managing communication with Dynamixel servos.""" | |||||
| ADDR_TORQUE_ENABLE = 64 | ADDR_TORQUE_ENABLE = 64 | ||||
| ADDR_GOAL_POSITION = 116 | ADDR_GOAL_POSITION = 116 | ||||
| ADDR_VELOCITY_LIMIT = 44 | ADDR_VELOCITY_LIMIT = 44 | ||||
| @@ -39,7 +54,10 @@ class Dynamixel: | |||||
| @dataclass | @dataclass | ||||
| class Config: | class Config: | ||||
| """Configuration class for Dynamixel servo settings.""" | |||||
| def instantiate(self): | def instantiate(self): | ||||
| """Create a new Dynamixel instance with this configuration.""" | |||||
| return Dynamixel(self) | return Dynamixel(self) | ||||
| baudrate: int = 57600 | baudrate: int = 57600 | ||||
| @@ -48,10 +66,17 @@ class Dynamixel: | |||||
| dynamixel_id: int = 1 | dynamixel_id: int = 1 | ||||
| def __init__(self, config: Config): | def __init__(self, config: Config): | ||||
| """Initialize the Dynamixel servo connection. | |||||
| Args: | |||||
| config: Configuration object containing connection settings. | |||||
| """ | |||||
| self.config = config | self.config = config | ||||
| self.connect() | self.connect() | ||||
| def connect(self): | def connect(self): | ||||
| """Establish connection with the Dynamixel servo.""" | |||||
| if self.config.device_name == "": | if self.config.device_name == "": | ||||
| for port_name in os.listdir("/dev"): | for port_name in os.listdir("/dev"): | ||||
| if "ttyUSB" in port_name or "ttyACM" in port_name: | if "ttyUSB" in port_name or "ttyACM" in port_name: | ||||
| @@ -75,9 +100,17 @@ class Dynamixel: | |||||
| return True | return True | ||||
| def disconnect(self): | def disconnect(self): | ||||
| """Close the connection with the Dynamixel servo.""" | |||||
| self.portHandler.closePort() | self.portHandler.closePort() | ||||
| def set_goal_position(self, motor_id, goal_position): | def set_goal_position(self, motor_id, goal_position): | ||||
| """Set the goal position for the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to control. | |||||
| goal_position: Target position value. | |||||
| """ | |||||
| # if self.operating_modes[motor_id] is not OperatingMode.POSITION: | # if self.operating_modes[motor_id] is not OperatingMode.POSITION: | ||||
| # self._disable_torque(motor_id) | # self._disable_torque(motor_id) | ||||
| # self.set_operating_mode(motor_id, OperatingMode.POSITION) | # self.set_operating_mode(motor_id, OperatingMode.POSITION) | ||||
| @@ -87,12 +120,20 @@ class Dynamixel: | |||||
| # self._enable_torque(motor_id) | # self._enable_torque(motor_id) | ||||
| dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx( | dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx( | ||||
| self.portHandler, motor_id, self.ADDR_GOAL_POSITION, goal_position | |||||
| self.portHandler, motor_id, self.ADDR_GOAL_POSITION, goal_position, | |||||
| ) | ) | ||||
| # self._process_response(dxl_comm_result, dxl_error) | # self._process_response(dxl_comm_result, dxl_error) | ||||
| # print(f'set position of motor {motor_id} to {goal_position}') | # print(f'set position of motor {motor_id} to {goal_position}') | ||||
| def set_pwm_value(self, motor_id: int, pwm_value, tries=3): | def set_pwm_value(self, motor_id: int, pwm_value, tries=3): | ||||
| """Set the PWM value for the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to control. | |||||
| pwm_value: PWM value to set. | |||||
| tries: Number of attempts to set the value. | |||||
| """ | |||||
| if self.operating_modes[motor_id] is not OperatingMode.PWM: | if self.operating_modes[motor_id] is not OperatingMode.PWM: | ||||
| self._disable_torque(motor_id) | self._disable_torque(motor_id) | ||||
| self.set_operating_mode(motor_id, OperatingMode.PWM) | self.set_operating_mode(motor_id, OperatingMode.PWM) | ||||
| @@ -101,30 +142,47 @@ class Dynamixel: | |||||
| self._enable_torque(motor_id) | self._enable_torque(motor_id) | ||||
| # print(f'enabling torque') | # print(f'enabling torque') | ||||
| dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( | dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( | ||||
| self.portHandler, motor_id, self.ADDR_GOAL_PWM, pwm_value | |||||
| self.portHandler, motor_id, self.ADDR_GOAL_PWM, pwm_value, | |||||
| ) | ) | ||||
| # self._process_response(dxl_comm_result, dxl_error) | # self._process_response(dxl_comm_result, dxl_error) | ||||
| # print(f'set pwm of motor {motor_id} to {pwm_value}') | # print(f'set pwm of motor {motor_id} to {pwm_value}') | ||||
| if dxl_comm_result != COMM_SUCCESS: | if dxl_comm_result != COMM_SUCCESS: | ||||
| if tries <= 1: | if tries <= 1: | ||||
| raise ConnectionError( | raise ConnectionError( | ||||
| f"dxl_comm_result: {self.packetHandler.getTxRxResult(dxl_comm_result)}" | |||||
| ) | |||||
| else: | |||||
| print( | |||||
| f"dynamixel pwm setting failure trying again with {tries - 1} tries" | |||||
| f"dxl_comm_result: {self.packetHandler.getTxRxResult(dxl_comm_result)}", | |||||
| ) | ) | ||||
| self.set_pwm_value(motor_id, pwm_value, tries=tries - 1) | |||||
| print( | |||||
| f"dynamixel pwm setting failure trying again with {tries - 1} tries", | |||||
| ) | |||||
| self.set_pwm_value(motor_id, pwm_value, tries=tries - 1) | |||||
| elif dxl_error != 0: | elif dxl_error != 0: | ||||
| print(f"dxl error {dxl_error}") | print(f"dxl error {dxl_error}") | ||||
| raise ConnectionError( | raise ConnectionError( | ||||
| f"dynamixel error: {self.packetHandler.getTxRxResult(dxl_error)}" | |||||
| f"dynamixel error: {self.packetHandler.getTxRxResult(dxl_error)}", | |||||
| ) | ) | ||||
| def read_temperature(self, motor_id: int): | def read_temperature(self, motor_id: int): | ||||
| """Read the temperature of the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to read from. | |||||
| Returns: | |||||
| The current temperature value. | |||||
| """ | |||||
| return self._read_value(motor_id, ReadAttribute.TEMPERATURE, 1) | return self._read_value(motor_id, ReadAttribute.TEMPERATURE, 1) | ||||
| def read_velocity(self, motor_id: int): | def read_velocity(self, motor_id: int): | ||||
| """Read the current velocity of the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to read from. | |||||
| Returns: | |||||
| The current velocity value. | |||||
| """ | |||||
| pos = self._read_value(motor_id, ReadAttribute.VELOCITY, 4) | pos = self._read_value(motor_id, ReadAttribute.VELOCITY, 4) | ||||
| if pos > 2**31: | if pos > 2**31: | ||||
| pos -= 2**32 | pos -= 2**32 | ||||
| @@ -132,6 +190,15 @@ class Dynamixel: | |||||
| return pos | return pos | ||||
| def read_position(self, motor_id: int): | def read_position(self, motor_id: int): | ||||
| """Read the current position of the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to read from. | |||||
| Returns: | |||||
| The current position value. | |||||
| """ | |||||
| pos = self._read_value(motor_id, ReadAttribute.POSITION, 4) | pos = self._read_value(motor_id, ReadAttribute.POSITION, 4) | ||||
| if pos > 2**31: | if pos > 2**31: | ||||
| pos -= 2**32 | pos -= 2**32 | ||||
| @@ -139,99 +206,213 @@ class Dynamixel: | |||||
| return pos | return pos | ||||
| def read_position_degrees(self, motor_id: int) -> float: | def read_position_degrees(self, motor_id: int) -> float: | ||||
| """Read the current position of the specified servo in degrees. | |||||
| Args: | |||||
| motor_id: ID of the servo to read from. | |||||
| Returns: | |||||
| The current position in degrees. | |||||
| """ | |||||
| return (self.read_position(motor_id) / 4096) * 360 | return (self.read_position(motor_id) / 4096) * 360 | ||||
| def read_position_radians(self, motor_id: int) -> float: | def read_position_radians(self, motor_id: int) -> float: | ||||
| """Read the current position of the specified servo in radians. | |||||
| Args: | |||||
| motor_id: ID of the servo to read from. | |||||
| Returns: | |||||
| The current position in radians. | |||||
| """ | |||||
| return (self.read_position(motor_id) / 4096) * 2 * math.pi | return (self.read_position(motor_id) / 4096) * 2 * math.pi | ||||
| def read_current(self, motor_id: int): | def read_current(self, motor_id: int): | ||||
| """Read the current value of the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to read from. | |||||
| Returns: | |||||
| The current value. | |||||
| """ | |||||
| current = self._read_value(motor_id, ReadAttribute.CURRENT, 2) | current = self._read_value(motor_id, ReadAttribute.CURRENT, 2) | ||||
| if current > 2**15: | if current > 2**15: | ||||
| current -= 2**16 | current -= 2**16 | ||||
| return current | return current | ||||
| def read_present_pwm(self, motor_id: int): | def read_present_pwm(self, motor_id: int): | ||||
| """Read the current PWM value of the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to read from. | |||||
| Returns: | |||||
| The current PWM value. | |||||
| """ | |||||
| return self._read_value(motor_id, ReadAttribute.PWM, 2) | return self._read_value(motor_id, ReadAttribute.PWM, 2) | ||||
| def read_hardware_error_status(self, motor_id: int): | def read_hardware_error_status(self, motor_id: int): | ||||
| """Read the hardware error status of the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to read from. | |||||
| Returns: | |||||
| The hardware error status value. | |||||
| """ | |||||
| return self._read_value(motor_id, ReadAttribute.HARDWARE_ERROR_STATUS, 1) | return self._read_value(motor_id, ReadAttribute.HARDWARE_ERROR_STATUS, 1) | ||||
| def set_id(self, old_id, new_id, use_broadcast_id: bool = False): | def set_id(self, old_id, new_id, use_broadcast_id: bool = False): | ||||
| """ | |||||
| sets the id of the dynamixel servo | |||||
| @param old_id: current id of the servo | |||||
| @param new_id: new id | |||||
| @param use_broadcast_id: set ids of all connected dynamixels if True. | |||||
| If False, change only servo with self.config.id | |||||
| @return: | |||||
| """Set the ID of the Dynamixel servo. | |||||
| Args: | |||||
| old_id: Current ID of the servo. | |||||
| new_id: New ID to set. | |||||
| use_broadcast_id: If True, set IDs of all connected servos. | |||||
| If False, change only servo with self.config.id | |||||
| """ | """ | ||||
| if use_broadcast_id: | if use_broadcast_id: | ||||
| current_id = 254 | current_id = 254 | ||||
| else: | else: | ||||
| current_id = old_id | current_id = old_id | ||||
| dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx( | dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx( | ||||
| self.portHandler, current_id, self.ADDR_ID, new_id | |||||
| self.portHandler, current_id, self.ADDR_ID, new_id, | |||||
| ) | ) | ||||
| self._process_response(dxl_comm_result, dxl_error, old_id) | self._process_response(dxl_comm_result, dxl_error, old_id) | ||||
| self.config.id = id | self.config.id = id | ||||
| def _enable_torque(self, motor_id): | def _enable_torque(self, motor_id): | ||||
| """Enable torque for the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to control. | |||||
| """ | |||||
| dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx( | dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx( | ||||
| self.portHandler, motor_id, self.ADDR_TORQUE_ENABLE, 1 | |||||
| self.portHandler, motor_id, self.ADDR_TORQUE_ENABLE, 1, | |||||
| ) | ) | ||||
| self._process_response(dxl_comm_result, dxl_error, motor_id) | self._process_response(dxl_comm_result, dxl_error, motor_id) | ||||
| self.torque_enabled[motor_id] = True | self.torque_enabled[motor_id] = True | ||||
| def _disable_torque(self, motor_id): | def _disable_torque(self, motor_id): | ||||
| """Disable torque for the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to control. | |||||
| """ | |||||
| dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx( | dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx( | ||||
| self.portHandler, motor_id, self.ADDR_TORQUE_ENABLE, 0 | |||||
| self.portHandler, motor_id, self.ADDR_TORQUE_ENABLE, 0, | |||||
| ) | ) | ||||
| self._process_response(dxl_comm_result, dxl_error, motor_id) | self._process_response(dxl_comm_result, dxl_error, motor_id) | ||||
| self.torque_enabled[motor_id] = False | self.torque_enabled[motor_id] = False | ||||
| def _process_response(self, dxl_comm_result: int, dxl_error: int, motor_id: int): | def _process_response(self, dxl_comm_result: int, dxl_error: int, motor_id: int): | ||||
| """Process the response from a servo communication. | |||||
| Args: | |||||
| dxl_comm_result: Communication result. | |||||
| dxl_error: Error value. | |||||
| motor_id: ID of the servo that was communicated with. | |||||
| Raises: | |||||
| ConnectionError: If communication failed. | |||||
| RuntimeError: If servo reported an error. | |||||
| """ | |||||
| if dxl_comm_result != COMM_SUCCESS: | if dxl_comm_result != COMM_SUCCESS: | ||||
| raise ConnectionError( | raise ConnectionError( | ||||
| f"dxl_comm_result for motor {motor_id}: {self.packetHandler.getTxRxResult(dxl_comm_result)}" | |||||
| f"dxl_comm_result for motor {motor_id}: {self.packetHandler.getTxRxResult(dxl_comm_result)}", | |||||
| ) | ) | ||||
| elif dxl_error != 0: | |||||
| if dxl_error != 0: | |||||
| print(f"dxl error {dxl_error}") | print(f"dxl error {dxl_error}") | ||||
| raise ConnectionError( | raise ConnectionError( | ||||
| f"dynamixel error for motor {motor_id}: {self.packetHandler.getTxRxResult(dxl_error)}" | |||||
| f"dynamixel error for motor {motor_id}: {self.packetHandler.getTxRxResult(dxl_error)}", | |||||
| ) | ) | ||||
| def set_operating_mode(self, motor_id: int, operating_mode: OperatingMode): | def set_operating_mode(self, motor_id: int, operating_mode: OperatingMode): | ||||
| """Set the operating mode for the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to control. | |||||
| operating_mode: Operating mode to set. | |||||
| """ | |||||
| dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( | dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( | ||||
| self.portHandler, motor_id, self.OPERATING_MODE_ADDR, operating_mode.value | |||||
| self.portHandler, motor_id, self.OPERATING_MODE_ADDR, operating_mode.value, | |||||
| ) | ) | ||||
| self._process_response(dxl_comm_result, dxl_error, motor_id) | self._process_response(dxl_comm_result, dxl_error, motor_id) | ||||
| self.operating_modes[motor_id] = operating_mode | self.operating_modes[motor_id] = operating_mode | ||||
| def set_pwm_limit(self, motor_id: int, limit: int): | def set_pwm_limit(self, motor_id: int, limit: int): | ||||
| """Set the PWM limit for the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to control. | |||||
| limit: PWM limit value to set. | |||||
| """ | |||||
| dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( | dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( | ||||
| self.portHandler, motor_id, 36, limit | |||||
| self.portHandler, motor_id, 36, limit, | |||||
| ) | ) | ||||
| self._process_response(dxl_comm_result, dxl_error, motor_id) | self._process_response(dxl_comm_result, dxl_error, motor_id) | ||||
| def set_velocity_limit(self, motor_id: int, velocity_limit): | def set_velocity_limit(self, motor_id: int, velocity_limit): | ||||
| """Set the velocity limit for the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to control. | |||||
| velocity_limit: Velocity limit value to set. | |||||
| """ | |||||
| dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx( | dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx( | ||||
| self.portHandler, motor_id, self.ADDR_VELOCITY_LIMIT, velocity_limit | |||||
| self.portHandler, motor_id, self.ADDR_VELOCITY_LIMIT, velocity_limit, | |||||
| ) | ) | ||||
| self._process_response(dxl_comm_result, dxl_error, motor_id) | self._process_response(dxl_comm_result, dxl_error, motor_id) | ||||
| def set_P(self, motor_id: int, P: int): | def set_P(self, motor_id: int, P: int): | ||||
| """Set the position P gain for the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to control. | |||||
| P: Position P gain value to set. | |||||
| """ | |||||
| dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( | dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( | ||||
| self.portHandler, motor_id, self.POSITION_P, P | |||||
| self.portHandler, motor_id, self.POSITION_P, P, | |||||
| ) | ) | ||||
| self._process_response(dxl_comm_result, dxl_error, motor_id) | self._process_response(dxl_comm_result, dxl_error, motor_id) | ||||
| def set_I(self, motor_id: int, I: int): | def set_I(self, motor_id: int, I: int): | ||||
| """Set the position I gain for the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to control. | |||||
| I: Position I gain value to set. | |||||
| """ | |||||
| dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( | dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( | ||||
| self.portHandler, motor_id, self.POSITION_I, I | |||||
| self.portHandler, motor_id, self.POSITION_I, I, | |||||
| ) | ) | ||||
| self._process_response(dxl_comm_result, dxl_error, motor_id) | self._process_response(dxl_comm_result, dxl_error, motor_id) | ||||
| def read_home_offset(self, motor_id: int): | def read_home_offset(self, motor_id: int): | ||||
| """Read the home offset of the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to read from. | |||||
| Returns: | |||||
| The home offset value. | |||||
| """ | |||||
| self._disable_torque(motor_id) | self._disable_torque(motor_id) | ||||
| # dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx(self.portHandler, motor_id, | # dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx(self.portHandler, motor_id, | ||||
| # ReadAttribute.HOMING_OFFSET.value, home_position) | # ReadAttribute.HOMING_OFFSET.value, home_position) | ||||
| @@ -241,14 +422,28 @@ class Dynamixel: | |||||
| return home_offset | return home_offset | ||||
| def set_home_offset(self, motor_id: int, home_position: int): | def set_home_offset(self, motor_id: int, home_position: int): | ||||
| """Set the home offset for the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to control. | |||||
| home_position: Home position value to set. | |||||
| """ | |||||
| self._disable_torque(motor_id) | self._disable_torque(motor_id) | ||||
| dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx( | dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx( | ||||
| self.portHandler, motor_id, ReadAttribute.HOMING_OFFSET.value, home_position | |||||
| self.portHandler, motor_id, ReadAttribute.HOMING_OFFSET.value, home_position, | |||||
| ) | ) | ||||
| self._process_response(dxl_comm_result, dxl_error, motor_id) | self._process_response(dxl_comm_result, dxl_error, motor_id) | ||||
| self._enable_torque(motor_id) | self._enable_torque(motor_id) | ||||
| def set_baudrate(self, motor_id: int, baudrate): | def set_baudrate(self, motor_id: int, baudrate): | ||||
| """Set the baudrate for the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to control. | |||||
| baudrate: Baudrate value to set. | |||||
| """ | |||||
| # translate baudrate into dynamixel baudrate setting id | # translate baudrate into dynamixel baudrate setting id | ||||
| if baudrate == 57600: | if baudrate == 57600: | ||||
| baudrate_id = 1 | baudrate_id = 1 | ||||
| @@ -265,54 +460,69 @@ class Dynamixel: | |||||
| self._disable_torque(motor_id) | self._disable_torque(motor_id) | ||||
| dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx( | dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx( | ||||
| self.portHandler, motor_id, ReadAttribute.BAUDRATE.value, baudrate_id | |||||
| self.portHandler, motor_id, ReadAttribute.BAUDRATE.value, baudrate_id, | |||||
| ) | ) | ||||
| self._process_response(dxl_comm_result, dxl_error, motor_id) | self._process_response(dxl_comm_result, dxl_error, motor_id) | ||||
| def _read_value(self, motor_id, attribute: ReadAttribute, num_bytes: int, tries=10): | def _read_value(self, motor_id, attribute: ReadAttribute, num_bytes: int, tries=10): | ||||
| """Read a value from the specified servo address. | |||||
| Args: | |||||
| motor_id: ID of the servo to read from. | |||||
| attribute: Attribute to read. | |||||
| num_bytes: Number of bytes to read. | |||||
| tries: Number of attempts to read the value. | |||||
| Returns: | |||||
| The read value. | |||||
| """ | |||||
| try: | try: | ||||
| if num_bytes == 1: | if num_bytes == 1: | ||||
| value, dxl_comm_result, dxl_error = self.packetHandler.read1ByteTxRx( | value, dxl_comm_result, dxl_error = self.packetHandler.read1ByteTxRx( | ||||
| self.portHandler, motor_id, attribute.value | |||||
| self.portHandler, motor_id, attribute.value, | |||||
| ) | ) | ||||
| elif num_bytes == 2: | elif num_bytes == 2: | ||||
| value, dxl_comm_result, dxl_error = self.packetHandler.read2ByteTxRx( | value, dxl_comm_result, dxl_error = self.packetHandler.read2ByteTxRx( | ||||
| self.portHandler, motor_id, attribute.value | |||||
| self.portHandler, motor_id, attribute.value, | |||||
| ) | ) | ||||
| elif num_bytes == 4: | elif num_bytes == 4: | ||||
| value, dxl_comm_result, dxl_error = self.packetHandler.read4ByteTxRx( | value, dxl_comm_result, dxl_error = self.packetHandler.read4ByteTxRx( | ||||
| self.portHandler, motor_id, attribute.value | |||||
| self.portHandler, motor_id, attribute.value, | |||||
| ) | ) | ||||
| except Exception: | except Exception: | ||||
| if tries == 0: | if tries == 0: | ||||
| raise Exception | raise Exception | ||||
| else: | |||||
| return self._read_value(motor_id, attribute, num_bytes, tries=tries - 1) | |||||
| return self._read_value(motor_id, attribute, num_bytes, tries=tries - 1) | |||||
| if dxl_comm_result != COMM_SUCCESS: | if dxl_comm_result != COMM_SUCCESS: | ||||
| if tries <= 1: | if tries <= 1: | ||||
| # print("%s" % self.packetHandler.getTxRxResult(dxl_comm_result)) | # print("%s" % self.packetHandler.getTxRxResult(dxl_comm_result)) | ||||
| raise ConnectionError( | raise ConnectionError( | ||||
| f"dxl_comm_result {dxl_comm_result} for servo {motor_id} value {value}" | |||||
| ) | |||||
| else: | |||||
| print( | |||||
| f"dynamixel read failure for servo {motor_id} trying again with {tries - 1} tries" | |||||
| f"dxl_comm_result {dxl_comm_result} for servo {motor_id} value {value}", | |||||
| ) | ) | ||||
| time.sleep(0.02) | |||||
| return self._read_value(motor_id, attribute, num_bytes, tries=tries - 1) | |||||
| elif ( | |||||
| print( | |||||
| f"dynamixel read failure for servo {motor_id} trying again with {tries - 1} tries", | |||||
| ) | |||||
| time.sleep(0.02) | |||||
| return self._read_value(motor_id, attribute, num_bytes, tries=tries - 1) | |||||
| if ( | |||||
| dxl_error != 0 | dxl_error != 0 | ||||
| ): # # print("%s" % self.packetHandler.getRxPacketError(dxl_error)) | ): # # print("%s" % self.packetHandler.getRxPacketError(dxl_error)) | ||||
| # raise ConnectionError(f'dxl_error {dxl_error} binary ' + "{0:b}".format(37)) | # raise ConnectionError(f'dxl_error {dxl_error} binary ' + "{0:b}".format(37)) | ||||
| if tries == 0 and dxl_error != 128: | if tries == 0 and dxl_error != 128: | ||||
| raise Exception( | raise Exception( | ||||
| f"Failed to read value from motor {motor_id} error is {dxl_error}" | |||||
| f"Failed to read value from motor {motor_id} error is {dxl_error}", | |||||
| ) | ) | ||||
| else: | |||||
| return self._read_value(motor_id, attribute, num_bytes, tries=tries - 1) | |||||
| return self._read_value(motor_id, attribute, num_bytes, tries=tries - 1) | |||||
| return value | return value | ||||
| def set_home_position(self, motor_id: int): | def set_home_position(self, motor_id: int): | ||||
| """Set the home position for the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to control. | |||||
| """ | |||||
| print(f"setting home position for motor {motor_id}") | print(f"setting home position for motor {motor_id}") | ||||
| self.set_home_offset(motor_id, 0) | self.set_home_offset(motor_id, 0) | ||||
| current_position = self.read_position(motor_id) | current_position = self.read_position(motor_id) | ||||
| @@ -1,19 +1,32 @@ | |||||
| import numpy as np | |||||
| from dynamixel import Dynamixel, OperatingMode, ReadAttribute | |||||
| """Module for controlling robot hardware and movements. | |||||
| This module provides functionality for controlling robot hardware components, | |||||
| including servo motors and various control modes. | |||||
| """ | |||||
| import time | import time | ||||
| from enum import Enum, auto | |||||
| from typing import Union | |||||
| import numpy as np | |||||
| from dynamixel import OperatingMode, ReadAttribute | |||||
| from dynamixel_sdk import ( | from dynamixel_sdk import ( | ||||
| GroupSyncRead, | |||||
| GroupSyncWrite, | |||||
| DXL_LOBYTE, | |||||
| DXL_HIBYTE, | DXL_HIBYTE, | ||||
| DXL_LOWORD, | |||||
| DXL_HIWORD, | DXL_HIWORD, | ||||
| DXL_LOBYTE, | |||||
| DXL_LOWORD, | |||||
| GroupSyncRead, | |||||
| GroupSyncWrite, | |||||
| ) | ) | ||||
| from enum import Enum, auto | |||||
| from typing import Union | |||||
| class MotorControlType(Enum): | class MotorControlType(Enum): | ||||
| """Enumeration of different motor control modes. | |||||
| Defines the various control modes available for the robot's motors, | |||||
| including PWM control, position control, and disabled states. | |||||
| """ | |||||
| PWM = auto() | PWM = auto() | ||||
| POSITION_CONTROL = auto() | POSITION_CONTROL = auto() | ||||
| DISABLED = auto() | DISABLED = auto() | ||||
| @@ -21,8 +34,22 @@ class MotorControlType(Enum): | |||||
| class Robot: | class Robot: | ||||
| """A class for controlling robot hardware components. | |||||
| This class provides methods for controlling robot servos, managing different | |||||
| control modes, and reading sensor data. | |||||
| """ | |||||
| # def __init__(self, device_name: str, baudrate=1_000_000, servo_ids=[1, 2, 3, 4, 5]): | # def __init__(self, device_name: str, baudrate=1_000_000, servo_ids=[1, 2, 3, 4, 5]): | ||||
| def __init__(self, dynamixel, baudrate=1_000_000, servo_ids=[1, 2, 3, 4, 5]): | def __init__(self, dynamixel, baudrate=1_000_000, servo_ids=[1, 2, 3, 4, 5]): | ||||
| """Initialize the robot controller. | |||||
| Args: | |||||
| dynamixel: Dynamixel controller instance | |||||
| baudrate: Communication baudrate for servo control | |||||
| servo_ids: List of servo IDs to control | |||||
| """ | |||||
| self.servo_ids = servo_ids | self.servo_ids = servo_ids | ||||
| self.dynamixel = dynamixel | self.dynamixel = dynamixel | ||||
| # self.dynamixel = Dynamixel.Config(baudrate=baudrate, device_name=device_name).instantiate() | # self.dynamixel = Dynamixel.Config(baudrate=baudrate, device_name=device_name).instantiate() | ||||
| @@ -65,17 +92,20 @@ class Robot: | |||||
| self.motor_control_state = MotorControlType.DISABLED | self.motor_control_state = MotorControlType.DISABLED | ||||
| def read_position(self, tries=2): | def read_position(self, tries=2): | ||||
| """ | |||||
| Reads the joint positions of the robot. 2048 is the center position. 0 and 4096 are 180 degrees in each direction. | |||||
| :param tries: maximum number of tries to read the position | |||||
| :return: list of joint positions in range [0, 4096] | |||||
| """Read the current joint positions of the robot. | |||||
| Args: | |||||
| tries: Maximum number of attempts to read positions | |||||
| Returns: | |||||
| list: Joint positions in range [0, 4096] | |||||
| """ | """ | ||||
| result = self.position_reader.txRxPacket() | result = self.position_reader.txRxPacket() | ||||
| if result != 0: | if result != 0: | ||||
| if tries > 0: | if tries > 0: | ||||
| return self.read_position(tries=tries - 1) | return self.read_position(tries=tries - 1) | ||||
| else: | |||||
| print(f"failed to read position!!!!!!!!!!!!!!!!!!!!!!!!!!!!!") | |||||
| print("failed to read position!!!!!!!!!!!!!!!!!!!!!!!!!!!!!") | |||||
| positions = [] | positions = [] | ||||
| for id in self.servo_ids: | for id in self.servo_ids: | ||||
| position = self.position_reader.getData(id, ReadAttribute.POSITION.value, 4) | position = self.position_reader.getData(id, ReadAttribute.POSITION.value, 4) | ||||
| @@ -85,9 +115,11 @@ class Robot: | |||||
| return positions | return positions | ||||
| def read_velocity(self): | def read_velocity(self): | ||||
| """ | |||||
| Reads the joint velocities of the robot. | |||||
| :return: list of joint velocities, | |||||
| """Read the current joint velocities of the robot. | |||||
| Returns: | |||||
| list: Current joint velocities | |||||
| """ | """ | ||||
| self.velocity_reader.txRxPacket() | self.velocity_reader.txRxPacket() | ||||
| velocties = [] | velocties = [] | ||||
| @@ -99,11 +131,13 @@ class Robot: | |||||
| return velocties | return velocties | ||||
| def set_goal_pos(self, action): | def set_goal_pos(self, action): | ||||
| """ | |||||
| """Set goal positions for the robot joints. | |||||
| Args: | |||||
| action: List or numpy array of target joint positions in range [0, 4096] | |||||
| :param action: list or numpy array of target joint positions in range [0, 4096] | |||||
| """ | """ | ||||
| if not self.motor_control_state is MotorControlType.POSITION_CONTROL: | |||||
| if self.motor_control_state is not MotorControlType.POSITION_CONTROL: | |||||
| self._set_position_control() | self._set_position_control() | ||||
| for i, motor_id in enumerate(self.servo_ids): | for i, motor_id in enumerate(self.servo_ids): | ||||
| data_write = [ | data_write = [ | ||||
| @@ -117,11 +151,13 @@ class Robot: | |||||
| self.pos_writer.txPacket() | self.pos_writer.txPacket() | ||||
| def set_pwm(self, action): | def set_pwm(self, action): | ||||
| """Set PWM values for the servos. | |||||
| Args: | |||||
| action: List or numpy array of PWM values in range [0, 885] | |||||
| """ | """ | ||||
| Sets the pwm values for the servos. | |||||
| :param action: list or numpy array of pwm values in range [0, 885] | |||||
| """ | |||||
| if not self.motor_control_state is MotorControlType.PWM: | |||||
| if self.motor_control_state is not MotorControlType.PWM: | |||||
| self._set_pwm_control() | self._set_pwm_control() | ||||
| for i, motor_id in enumerate(self.servo_ids): | for i, motor_id in enumerate(self.servo_ids): | ||||
| data_write = [ | data_write = [ | ||||
| @@ -133,17 +169,19 @@ class Robot: | |||||
| self.pwm_writer.txPacket() | self.pwm_writer.txPacket() | ||||
| def set_trigger_torque(self): | def set_trigger_torque(self): | ||||
| """ | |||||
| Sets a constant torque torque for the last servo in the chain. This is useful for the trigger of the leader arm | |||||
| """Set a constant torque for the last servo in the chain. | |||||
| This is useful for the trigger of the leader arm. | |||||
| """ | """ | ||||
| self.dynamixel._enable_torque(self.servo_ids[-1]) | self.dynamixel._enable_torque(self.servo_ids[-1]) | ||||
| self.dynamixel.set_pwm_value(self.servo_ids[-1], 200) | self.dynamixel.set_pwm_value(self.servo_ids[-1], 200) | ||||
| def limit_pwm(self, limit: Union[int, list, np.ndarray]): | def limit_pwm(self, limit: Union[int, list, np.ndarray]): | ||||
| """ | |||||
| Limits the pwm values for the servos in for position control | |||||
| @param limit: 0 ~ 885 | |||||
| @return: | |||||
| """Limit the PWM values for the servos in position control. | |||||
| Args: | |||||
| limit: PWM limit value in range 0-885 | |||||
| """ | """ | ||||
| if isinstance(limit, int): | if isinstance(limit, int): | ||||
| limits = [ | limits = [ | ||||
| @@ -1,11 +1,17 @@ | |||||
| from robot import Robot | |||||
| """Module for teleoperating a physical robot. | |||||
| This module provides functionality for controlling a physical robot through | |||||
| teleoperation, allowing real-time control of robot movements and actions. | |||||
| """ | |||||
| from dynamixel import Dynamixel | from dynamixel import Dynamixel | ||||
| from robot import Robot | |||||
| leader_dynamixel = Dynamixel.Config( | leader_dynamixel = Dynamixel.Config( | ||||
| baudrate=1_000_000, device_name="/dev/ttyDXL_master_right" | |||||
| baudrate=1_000_000, device_name="/dev/ttyDXL_master_right", | |||||
| ).instantiate() | ).instantiate() | ||||
| follower_dynamixel = Dynamixel.Config( | follower_dynamixel = Dynamixel.Config( | ||||
| baudrate=1_000_000, device_name="/dev/ttyDXL_puppet_right" | |||||
| baudrate=1_000_000, device_name="/dev/ttyDXL_puppet_right", | |||||
| ).instantiate() | ).instantiate() | ||||
| follower = Robot(follower_dynamixel, servo_ids=[1, 2, 3, 4, 5, 6, 7, 8]) | follower = Robot(follower_dynamixel, servo_ids=[1, 2, 3, 4, 5, 6, 7, 8]) | ||||
| leader = Robot(leader_dynamixel, servo_ids=[1, 2, 3, 4, 5, 6, 7, 8]) | leader = Robot(leader_dynamixel, servo_ids=[1, 2, 3, 4, 5, 6, 7, 8]) | ||||
| @@ -1,8 +1,13 @@ | |||||
| """Module for ROS2-based robot teleoperation. | |||||
| This module provides functionality for controlling robots through ROS2, | |||||
| enabling teleoperation and real-time control of robot movements. | |||||
| """ | |||||
| import dora | import dora | ||||
| from dora import Node | |||||
| import pyarrow as pa | |||||
| import numpy as np | import numpy as np | ||||
| import pyarrow as pa | |||||
| from dora import Node | |||||
| ros2_context = dora.experimental.ros2_bridge.Ros2Context() | ros2_context = dora.experimental.ros2_bridge.Ros2Context() | ||||
| ros2_node = ros2_context.new_node( | ros2_node = ros2_context.new_node( | ||||
| @@ -13,7 +18,7 @@ ros2_node = ros2_context.new_node( | |||||
| # Define a ROS2 QOS | # Define a ROS2 QOS | ||||
| topic_qos = dora.experimental.ros2_bridge.Ros2QosPolicies( | topic_qos = dora.experimental.ros2_bridge.Ros2QosPolicies( | ||||
| reliable=True, max_blocking_time=0.1 | |||||
| reliable=True, max_blocking_time=0.1, | |||||
| ) | ) | ||||
| # Create a publisher to cmd_vel topic | # Create a publisher to cmd_vel topic | ||||
| @@ -22,7 +27,7 @@ puppet_arm_command = ros2_node.create_publisher( | |||||
| "/robot_model_puppet/commands/joint_group", | "/robot_model_puppet/commands/joint_group", | ||||
| "interbotix_xs_msgs/JointGroupCommand", | "interbotix_xs_msgs/JointGroupCommand", | ||||
| topic_qos, | topic_qos, | ||||
| ) | |||||
| ), | |||||
| ) | ) | ||||
| gripper_command = ros2_node.create_publisher( | gripper_command = ros2_node.create_publisher( | ||||
| @@ -30,13 +35,13 @@ gripper_command = ros2_node.create_publisher( | |||||
| "/robot_model_puppet/commands/joint_single", | "/robot_model_puppet/commands/joint_single", | ||||
| "interbotix_xs_msgs/JointSingleCommand", | "interbotix_xs_msgs/JointSingleCommand", | ||||
| topic_qos, | topic_qos, | ||||
| ) | |||||
| ), | |||||
| ) | ) | ||||
| # Create a listener to pose topic | # Create a listener to pose topic | ||||
| master_state = ros2_node.create_subscription( | master_state = ros2_node.create_subscription( | ||||
| ros2_node.create_topic( | ros2_node.create_topic( | ||||
| "/robot_model_master/joint_states", "sensor_msgs/JointState", topic_qos | |||||
| ) | |||||
| "/robot_model_master/joint_states", "sensor_msgs/JointState", topic_qos, | |||||
| ), | |||||
| ) | ) | ||||
| # Create a dora node | # Create a dora node | ||||
| @@ -74,8 +79,8 @@ for event in dora_node: | |||||
| { | { | ||||
| "name": "gripper", | "name": "gripper", | ||||
| "cmd": np.float32(values[6]), | "cmd": np.float32(values[6]), | ||||
| } | |||||
| ] | |||||
| }, | |||||
| ], | |||||
| ), | ), | ||||
| ) | ) | ||||
| puppet_arm_command.publish( | puppet_arm_command.publish( | ||||
| @@ -84,7 +89,7 @@ for event in dora_node: | |||||
| { | { | ||||
| "name": "arm", | "name": "arm", | ||||
| "cmd": values[:6], | "cmd": values[:6], | ||||
| } | |||||
| ] | |||||
| }, | |||||
| ], | |||||
| ), | ), | ||||
| ) | ) | ||||
| @@ -1,18 +1,37 @@ | |||||
| import gymnasium as gym | |||||
| """Module for integrating gym environments with Dora nodes. | |||||
| This module provides functionality for running gym environments as Dora nodes, | |||||
| including replay capabilities for recorded robot actions. | |||||
| """ | |||||
| import gym_dora # noqa: F401 | |||||
| import pandas as pd | |||||
| import time | import time | ||||
| from pathlib import Path | from pathlib import Path | ||||
| import gym_dora # noqa: F401 | |||||
| import gymnasium as gym | |||||
| import pandas as pd | |||||
| env = gym.make( | env = gym.make( | ||||
| "gym_dora/DoraAloha-v0", disable_env_checker=True, max_episode_steps=10000 | |||||
| "gym_dora/DoraAloha-v0", disable_env_checker=True, max_episode_steps=10000, | |||||
| ) | ) | ||||
| observation = env.reset() | observation = env.reset() | ||||
| class ReplayPolicy: | class ReplayPolicy: | ||||
| """A policy class for replaying recorded robot actions. | |||||
| This class handles loading and replaying recorded actions from a dataset, | |||||
| maintaining timing between actions to match the original recording. | |||||
| """ | |||||
| def __init__(self, example_path, epidode=0): | def __init__(self, example_path, epidode=0): | ||||
| """Initialize the replay policy. | |||||
| Args: | |||||
| example_path: Path to the directory containing recorded actions | |||||
| epidode: Index of the episode to replay | |||||
| """ | |||||
| df_action = pd.read_parquet(example_path / "action.parquet") | df_action = pd.read_parquet(example_path / "action.parquet") | ||||
| df_episode_index = pd.read_parquet(example_path / "episode_index.parquet") | df_episode_index = pd.read_parquet(example_path / "episode_index.parquet") | ||||
| self.df = pd.merge_asof( | self.df = pd.merge_asof( | ||||
| @@ -29,6 +48,16 @@ class ReplayPolicy: | |||||
| self.finished = False | self.finished = False | ||||
| def select_action(self, obs): | def select_action(self, obs): | ||||
| """Select the next action to replay. | |||||
| Args: | |||||
| obs: Current observation from the environment (unused) | |||||
| Returns: | |||||
| tuple: (action, finished) where action is the next action to take | |||||
| and finished indicates if all actions have been replayed | |||||
| """ | |||||
| if self.index < len(self.df): | if self.index < len(self.df): | ||||
| self.index += 1 | self.index += 1 | ||||
| else: | else: | ||||
| @@ -49,8 +78,8 @@ class ReplayPolicy: | |||||
| policy = ReplayPolicy( | policy = ReplayPolicy( | ||||
| Path( | Path( | ||||
| "/home/rcadene/dora-aloha/aloha/graphs/out/018fa4ad-5942-7235-93d3-3efebe9b8a12" | |||||
| ) | |||||
| "/home/rcadene/dora-aloha/aloha/graphs/out/018fa4ad-5942-7235-93d3-3efebe9b8a12", | |||||
| ), | |||||
| ) | ) | ||||
| @@ -1,8 +1,13 @@ | |||||
| from pynput import keyboard | |||||
| from pynput.keyboard import Key, Events | |||||
| """Module for handling keyboard input events in robot control. | |||||
| This module provides functionality for capturing and processing keyboard input | |||||
| events to control robot movements and actions. | |||||
| """ | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from dora import Node | from dora import Node | ||||
| from pynput import keyboard | |||||
| from pynput.keyboard import Events, Key | |||||
| node = Node() | node = Node() | ||||
| buffer_text = "" | buffer_text = "" | ||||
| @@ -17,7 +22,7 @@ with keyboard.Events() as events: | |||||
| cursor += 1 | cursor += 1 | ||||
| node.send_output("space", pa.array([cursor])) | node.send_output("space", pa.array([cursor])) | ||||
| space = True | space = True | ||||
| elif event is not None and isinstance(event, Events.Release): | elif event is not None and isinstance(event, Events.Release): | ||||
| if event.key == Key.space: | if event.key == Key.space: | ||||
| @@ -25,7 +30,7 @@ with keyboard.Events() as events: | |||||
| space = False | space = False | ||||
| elif event.key == Key.backspace: | elif event.key == Key.backspace: | ||||
| node.send_output("failed", pa.array([cursor])) | node.send_output("failed", pa.array([cursor])) | ||||
| if node.next(0.001) is None: | if node.next(0.001) is None: | ||||
| break | break | ||||
| @@ -1,14 +1,14 @@ | |||||
| """TODO: Add docstring.""" | |||||
| #!/usr/bin/env python3 | #!/usr/bin/env python3 | ||||
| # -*- coding: utf-8 -*- | |||||
| import os | import os | ||||
| import time | |||||
| import numpy as np | |||||
| import subprocess | |||||
| from pathlib import Path | |||||
| import cv2 | import cv2 | ||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from pathlib import Path | |||||
| from dora import Node | from dora import Node | ||||
| import subprocess | |||||
| node = Node() | node = Node() | ||||
| @@ -40,12 +40,12 @@ for event in node: | |||||
| f"ffmpeg -r {FPS} " | f"ffmpeg -r {FPS} " | ||||
| "-f image2 " | "-f image2 " | ||||
| "-loglevel error " | "-loglevel error " | ||||
| f"-i {str(out_dir / 'frame_%06d.png')} " | |||||
| f"-i {out_dir / 'frame_%06d.png'!s} " | |||||
| "-vcodec libx264 " | "-vcodec libx264 " | ||||
| "-g 2 " | "-g 2 " | ||||
| "-pix_fmt yuv444p " | "-pix_fmt yuv444p " | ||||
| f"{str(video_path)} &&" | |||||
| f"rm -r {str(out_dir)}" | |||||
| f"{video_path!s} &&" | |||||
| f"rm -r {out_dir!s}" | |||||
| ) | ) | ||||
| print(ffmpeg_cmd, flush=True) | print(ffmpeg_cmd, flush=True) | ||||
| subprocess.Popen([ffmpeg_cmd], start_new_session=True, shell=True) | subprocess.Popen([ffmpeg_cmd], start_new_session=True, shell=True) | ||||
| @@ -1,14 +1,20 @@ | |||||
| from dora import DoraStatus | |||||
| import pylcs | |||||
| import os | |||||
| import pyarrow as pa | |||||
| from transformers import AutoModelForCausalLM, AutoTokenizer | |||||
| import torch | |||||
| """Module for language model-based robot control. | |||||
| This module provides functionality for controlling robots using natural language | |||||
| commands processed by large language models. | |||||
| """ | |||||
| import gc # garbage collect library | import gc # garbage collect library | ||||
| import os | |||||
| import re | import re | ||||
| import time | import time | ||||
| import pyarrow as pa | |||||
| import pylcs | |||||
| import torch | |||||
| from dora import DoraStatus | |||||
| from transformers import AutoModelForCausalLM, AutoTokenizer | |||||
| CHATGPT = False | CHATGPT = False | ||||
| MODEL_NAME_OR_PATH = "TheBloke/deepseek-coder-6.7B-instruct-GPTQ" | MODEL_NAME_OR_PATH = "TheBloke/deepseek-coder-6.7B-instruct-GPTQ" | ||||
| @@ -39,14 +45,15 @@ tokenizer = AutoTokenizer.from_pretrained(MODEL_NAME_OR_PATH, use_fast=True) | |||||
| def extract_python_code_blocks(text): | def extract_python_code_blocks(text): | ||||
| """ | |||||
| Extracts Python code blocks from the given text that are enclosed in triple backticks with a python language identifier. | |||||
| """Extract Python code blocks from the given text. | |||||
| Parameters: | |||||
| - text: A string that may contain one or more Python code blocks. | |||||
| Args: | |||||
| text: A string that may contain one or more Python code blocks. | |||||
| Returns: | Returns: | ||||
| - A list of strings, where each string is a block of Python code extracted from the text. | |||||
| list: A list of strings, where each string is a block of Python code | |||||
| extracted from the text. | |||||
| """ | """ | ||||
| pattern = r"```python\n(.*?)\n```" | pattern = r"```python\n(.*?)\n```" | ||||
| matches = re.findall(pattern, text, re.DOTALL) | matches = re.findall(pattern, text, re.DOTALL) | ||||
| @@ -55,21 +62,20 @@ def extract_python_code_blocks(text): | |||||
| matches = re.findall(pattern, text, re.DOTALL) | matches = re.findall(pattern, text, re.DOTALL) | ||||
| if len(matches) == 0: | if len(matches) == 0: | ||||
| return [text] | return [text] | ||||
| else: | |||||
| matches = [remove_last_line(matches[0])] | |||||
| matches = [remove_last_line(matches[0])] | |||||
| return matches | return matches | ||||
| def remove_last_line(python_code): | def remove_last_line(python_code): | ||||
| """ | |||||
| Removes the last line from a given string of Python code. | |||||
| """Remove the last line from a given string of Python code. | |||||
| Parameters: | |||||
| - python_code: A string representing Python source code. | |||||
| Args: | |||||
| python_code: A string representing Python source code. | |||||
| Returns: | Returns: | ||||
| - A string with the last line removed. | |||||
| str: A string with the last line removed. | |||||
| """ | """ | ||||
| lines = python_code.split("\n") # Split the string into lines | lines = python_code.split("\n") # Split the string into lines | ||||
| if lines: # Check if there are any lines to remove | if lines: # Check if there are any lines to remove | ||||
| @@ -78,9 +84,17 @@ def remove_last_line(python_code): | |||||
| def calculate_similarity(source, target): | def calculate_similarity(source, target): | ||||
| """ | |||||
| Calculate a similarity score between the source and target strings. | |||||
| """Calculate a similarity score between the source and target strings. | |||||
| This uses the edit distance relative to the length of the strings. | This uses the edit distance relative to the length of the strings. | ||||
| Args: | |||||
| source: First string to compare | |||||
| target: Second string to compare | |||||
| Returns: | |||||
| float: Similarity score between 0 and 1 | |||||
| """ | """ | ||||
| edit_distance = pylcs.edit_distance(source, target) | edit_distance = pylcs.edit_distance(source, target) | ||||
| max_length = max(len(source), len(target)) | max_length = max(len(source), len(target)) | ||||
| @@ -90,9 +104,17 @@ def calculate_similarity(source, target): | |||||
| def find_best_match_location(source_code, target_block): | def find_best_match_location(source_code, target_block): | ||||
| """ | |||||
| Find the best match for the target_block within the source_code by searching line by line, | |||||
| considering blocks of varying lengths. | |||||
| """Find the best match for the target_block within the source_code. | |||||
| This function searches line by line, considering blocks of varying lengths. | |||||
| Args: | |||||
| source_code: The source code to search within | |||||
| target_block: The code block to find a match for | |||||
| Returns: | |||||
| tuple: (start_index, end_index) of the best matching location | |||||
| """ | """ | ||||
| source_lines = source_code.split("\n") | source_lines = source_code.split("\n") | ||||
| target_lines = target_block.split("\n") | target_lines = target_block.split("\n") | ||||
| @@ -121,8 +143,15 @@ def find_best_match_location(source_code, target_block): | |||||
| def replace_code_in_source(source_code, replacement_block: str): | def replace_code_in_source(source_code, replacement_block: str): | ||||
| """ | |||||
| Replace the best matching block in the source_code with the replacement_block, considering variable block lengths. | |||||
| """Replace the best matching block in the source_code with the replacement_block. | |||||
| Args: | |||||
| source_code: The original source code | |||||
| replacement_block: The new code block to insert | |||||
| Returns: | |||||
| str: The modified source code | |||||
| """ | """ | ||||
| replacement_block = extract_python_code_blocks(replacement_block)[0] | replacement_block = extract_python_code_blocks(replacement_block)[0] | ||||
| start_index, end_index = find_best_match_location(source_code, replacement_block) | start_index, end_index = find_best_match_location(source_code, replacement_block) | ||||
| @@ -132,12 +161,18 @@ def replace_code_in_source(source_code, replacement_block: str): | |||||
| source_code[:start_index] + replacement_block + source_code[end_index:] | source_code[:start_index] + replacement_block + source_code[end_index:] | ||||
| ) | ) | ||||
| return new_source | return new_source | ||||
| else: | |||||
| return source_code | |||||
| return source_code | |||||
| class Operator: | class Operator: | ||||
| """A class for managing LLM-based code modifications. | |||||
| This class handles the process of using LLMs to modify code based on | |||||
| natural language instructions and managing the modification workflow. | |||||
| """ | |||||
| def __init__(self) -> None: | def __init__(self) -> None: | ||||
| """Initialize the operator with policy initialization flag.""" | |||||
| self.policy_init = False | self.policy_init = False | ||||
| def on_event( | def on_event( | ||||
| @@ -145,6 +180,16 @@ class Operator: | |||||
| dora_event, | dora_event, | ||||
| send_output, | send_output, | ||||
| ) -> DoraStatus: | ) -> DoraStatus: | ||||
| """Handle incoming events and process LLM-based code modifications. | |||||
| Args: | |||||
| dora_event: Dictionary containing event information | |||||
| send_output: Function to send output to the dataflow | |||||
| Returns: | |||||
| DoraStatus: Status indicating whether to continue processing | |||||
| """ | |||||
| global model, tokenizer | global model, tokenizer | ||||
| if dora_event["type"] == "INPUT" and dora_event["id"] == "text": | if dora_event["type"] == "INPUT" and dora_event["id"] == "text": | ||||
| input = dora_event["value"][0].as_py() | input = dora_event["value"][0].as_py() | ||||
| @@ -155,14 +200,14 @@ class Operator: | |||||
| current_directory = os.path.dirname(current_file_path) | current_directory = os.path.dirname(current_file_path) | ||||
| path = current_directory + "/policy.py" | path = current_directory + "/policy.py" | ||||
| with open(path, "r", encoding="utf8") as f: | |||||
| with open(path, encoding="utf8") as f: | |||||
| code = f.read() | code = f.read() | ||||
| user_message = input | user_message = input | ||||
| start_llm = time.time() | start_llm = time.time() | ||||
| output = self.ask_llm( | output = self.ask_llm( | ||||
| CODE_MODIFIER_TEMPLATE.format(code=code, user_message=user_message) | |||||
| CODE_MODIFIER_TEMPLATE.format(code=code, user_message=user_message), | |||||
| ) | ) | ||||
| source_code = replace_code_in_source(code, output) | source_code = replace_code_in_source(code, output) | ||||
| @@ -178,7 +223,15 @@ class Operator: | |||||
| return DoraStatus.CONTINUE | return DoraStatus.CONTINUE | ||||
| def ask_llm(self, prompt): | def ask_llm(self, prompt): | ||||
| """Send a prompt to the LLM and get its response. | |||||
| Args: | |||||
| prompt: The prompt to send to the LLM | |||||
| Returns: | |||||
| str: The LLM's response | |||||
| """ | |||||
| # Generate output | # Generate output | ||||
| # prompt = PROMPT_TEMPLATE.format(system_message=system_message, prompt=prompt)) | # prompt = PROMPT_TEMPLATE.format(system_message=system_message, prompt=prompt)) | ||||
| input = tokenizer(prompt, return_tensors="pt") | input = tokenizer(prompt, return_tensors="pt") | ||||
| @@ -213,7 +266,7 @@ if __name__ == "__main__": | |||||
| current_directory = os.path.dirname(current_file_path) | current_directory = os.path.dirname(current_file_path) | ||||
| path = current_directory + "/policy.py" | path = current_directory + "/policy.py" | ||||
| with open(path, "r", encoding="utf8") as f: | |||||
| with open(path, encoding="utf8") as f: | |||||
| raw = f.read() | raw = f.read() | ||||
| op.on_event( | op.on_event( | ||||
| @@ -226,7 +279,7 @@ if __name__ == "__main__": | |||||
| "path": path, | "path": path, | ||||
| "user_message": "When I say suit up, get the hat and the get the food.", | "user_message": "When I say suit up, get the hat and the get the food.", | ||||
| }, | }, | ||||
| ] | |||||
| ], | |||||
| ), | ), | ||||
| "metadata": [], | "metadata": [], | ||||
| }, | }, | ||||
| @@ -1,8 +1,10 @@ | |||||
| """TODO: Add docstring.""" | |||||
| import os | import os | ||||
| import cv2 | import cv2 | ||||
| from dora import Node | from dora import Node | ||||
| IMAGE_WIDTH = int(os.getenv("IMAGE_WIDTH", "1280")) | IMAGE_WIDTH = int(os.getenv("IMAGE_WIDTH", "1280")) | ||||
| IMAGE_HEIGHT = int(os.getenv("IMAGE_HEIGHT", "720")) | IMAGE_HEIGHT = int(os.getenv("IMAGE_HEIGHT", "720")) | ||||
| FONT = cv2.FONT_HERSHEY_SIMPLEX | FONT = cv2.FONT_HERSHEY_SIMPLEX | ||||
| @@ -1,12 +1,34 @@ | |||||
| import pyarrow as pa | |||||
| """Module for implementing robot control policies. | |||||
| This module provides functionality for implementing and managing robot control policies, | |||||
| including action selection and state management. | |||||
| """ | |||||
| from dora import DoraStatus | from dora import DoraStatus | ||||
| class Operator: | class Operator: | ||||
| """A class for implementing robot control policies. | |||||
| This class handles the selection and execution of robot actions based on | |||||
| input events and current state. | |||||
| """ | |||||
| def __init__(self): | def __init__(self): | ||||
| """Initialize the operator with available actions.""" | |||||
| self.actions = ["get_food", "get_hat"] | self.actions = ["get_food", "get_hat"] | ||||
| def on_event(self, event: dict, send_output) -> DoraStatus: | def on_event(self, event: dict, send_output) -> DoraStatus: | ||||
| """Handle incoming events and generate appropriate actions. | |||||
| Args: | |||||
| event: Dictionary containing event information | |||||
| send_output: Function to send output to the dataflow | |||||
| Returns: | |||||
| DoraStatus: Status indicating whether to continue processing | |||||
| """ | |||||
| if event["type"] == "INPUT": | if event["type"] == "INPUT": | ||||
| id = event["id"] | id = event["id"] | ||||
| # On initialization | # On initialization | ||||
| @@ -1,9 +1,16 @@ | |||||
| import pyrealsense2 as rs | |||||
| import numpy as np | |||||
| from dora import Node | |||||
| import pyarrow as pa | |||||
| """Module for handling Intel RealSense camera data. | |||||
| This module provides functionality for capturing and processing data from | |||||
| Intel RealSense cameras, including depth and color images. | |||||
| """ | |||||
| import os | import os | ||||
| import cv2 | import cv2 | ||||
| import numpy as np | |||||
| import pyarrow as pa | |||||
| import pyrealsense2 as rs | |||||
| from dora import Node | |||||
| IMAGE_WIDTH = int(os.getenv("IMAGE_WIDTH", "640")) | IMAGE_WIDTH = int(os.getenv("IMAGE_WIDTH", "640")) | ||||
| IMAGE_HEIGHT = int(os.getenv("IMAGE_HEIGHT", "480")) | IMAGE_HEIGHT = int(os.getenv("IMAGE_HEIGHT", "480")) | ||||
| @@ -1,8 +1,10 @@ | |||||
| from dora import Node | |||||
| import pandas as pd | |||||
| import pyarrow as pa | |||||
| """TODO: Add docstring.""" | |||||
| import time | import time | ||||
| import pandas as pd | |||||
| import pyarrow as pa | |||||
| from dora import Node | |||||
| TOPIC = "puppet_goal_position" | TOPIC = "puppet_goal_position" | ||||
| @@ -1,12 +1,16 @@ | |||||
| """Module for handling webcam input and processing. | |||||
| This module provides functionality for capturing and processing video input | |||||
| from webcam devices for robot vision applications. | |||||
| """ | |||||
| #!/usr/bin/env python3 | #!/usr/bin/env python3 | ||||
| # -*- coding: utf-8 -*- | |||||
| import os | import os | ||||
| import time | |||||
| import numpy as np | |||||
| import cv2 | import cv2 | ||||
| import numpy as np | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from dora import Node | from dora import Node | ||||
| node = Node() | node = Node() | ||||
| @@ -26,8 +30,8 @@ for event in node: | |||||
| frame = np.zeros((CAMERA_HEIGHT, CAMERA_WIDTH, 3), dtype=np.uint8) | frame = np.zeros((CAMERA_HEIGHT, CAMERA_WIDTH, 3), dtype=np.uint8) | ||||
| cv2.putText( | cv2.putText( | ||||
| frame, | frame, | ||||
| "No Webcam was found at index %d" % (CAMERA_ID), | |||||
| (int(30), int(30)), | |||||
| f"No Webcam was found at index {CAMERA_ID}", | |||||
| (30, 30), | |||||
| font, | font, | ||||
| 0.75, | 0.75, | ||||
| (255, 255, 255), | (255, 255, 255), | ||||
| @@ -1,14 +1,16 @@ | |||||
| import pyarrow as pa | |||||
| import whisper | |||||
| from pynput import keyboard | |||||
| from pynput.keyboard import Key, Events | |||||
| from dora import Node | |||||
| """Module for speech recognition using Whisper. | |||||
| This module provides functionality for capturing audio input and converting | |||||
| it to text using the Whisper speech recognition model. | |||||
| """ | |||||
| import torch | |||||
| import numpy as np | import numpy as np | ||||
| import pyarrow as pa | import pyarrow as pa | ||||
| import sounddevice as sd | import sounddevice as sd | ||||
| import gc # garbage collect library | |||||
| import whisper | |||||
| from dora import Node | |||||
| from pynput import keyboard | |||||
| from pynput.keyboard import Events, Key | |||||
| model = whisper.load_model("base") | model = whisper.load_model("base") | ||||
| @@ -18,7 +20,15 @@ node = Node() | |||||
| def get_text(duration) -> str: | def get_text(duration) -> str: | ||||
| """Capture audio and convert it to text using Whisper. | |||||
| Args: | |||||
| duration: Duration of audio to capture in seconds | |||||
| Returns: | |||||
| str: Transcribed text from the audio input | |||||
| """ | |||||
| ## Microphone | ## Microphone | ||||
| audio_data = sd.rec( | audio_data = sd.rec( | ||||
| int(SAMPLE_RATE * duration), | int(SAMPLE_RATE * duration), | ||||
| @@ -48,7 +58,7 @@ with keyboard.Events() as events: | |||||
| if event.key == Key.alt_r: | if event.key == Key.alt_r: | ||||
| result = get_text(5) | result = get_text(5) | ||||
| node.send_output( | node.send_output( | ||||
| "text_llm", pa.array([result["text"]]), dora_event["metadata"] | |||||
| "text_llm", pa.array([result["text"]]), dora_event["metadata"], | |||||
| ) | ) | ||||
| elif event.key == Key.ctrl_r: | elif event.key == Key.ctrl_r: | ||||
| result = get_text(3) | result = get_text(3) | ||||
| @@ -1,7 +1,10 @@ | |||||
| import pyrealsense2 as rs | |||||
| """TODO: Add docstring.""" | |||||
| import os | |||||
| import cv2 | import cv2 | ||||
| import numpy as np | import numpy as np | ||||
| import os | |||||
| import pyrealsense2 as rs | |||||
| CAMERA_ID = os.getenv("CAMERA_ID") | CAMERA_ID = os.getenv("CAMERA_ID") | ||||
| pipe = rs.pipeline() | pipe = rs.pipeline() | ||||
| @@ -12,7 +15,7 @@ profile = pipe.start(config) | |||||
| try: | try: | ||||
| for i in range(0, 1000): | |||||
| for i in range(1000): | |||||
| frames = pipe.wait_for_frames() | frames = pipe.wait_for_frames() | ||||
| color_frame = frames.get_color_frame() | color_frame = frames.get_color_frame() | ||||
| color_images = np.asanyarray(color_frame.get_data()) | color_images = np.asanyarray(color_frame.get_data()) | ||||
| @@ -1,14 +1,19 @@ | |||||
| from dora import Node | |||||
| import pyarrow as pa | |||||
| """Module for interpolating between robot configurations. | |||||
| This module provides functionality for creating smooth transitions between | |||||
| robot configurations and movements. | |||||
| """ | |||||
| from enum import Enum | from enum import Enum | ||||
| import pyarrow as pa | |||||
| from dora import Node | |||||
| node = Node() | node = Node() | ||||
| class Action(Enum): | class Action(Enum): | ||||
| """ | |||||
| Action abstraction | |||||
| """ | |||||
| """Action abstraction for robot movement commands.""" | |||||
| YAW_RIGHT = ("yaw right", "movej", [0, 0, 0, 0, -0.1, 0, 0.1]) | YAW_RIGHT = ("yaw right", "movej", [0, 0, 0, 0, -0.1, 0, 0.1]) | ||||
| YAW_LEFT = ("yaw left", "movej", [0, 0, 0, 0, 0.1, 0, 0.1]) | YAW_LEFT = ("yaw left", "movej", [0, 0, 0, 0, 0.1, 0, 0.1]) | ||||
| @@ -1,5 +1,11 @@ | |||||
| from dora import Node | |||||
| """Module for interpolating between keyframe positions. | |||||
| This module provides functionality for creating smooth transitions between | |||||
| keyframe positions in robot motion sequences. | |||||
| """ | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from dora import Node | |||||
| node = Node() | node = Node() | ||||
| @@ -1,5 +1,7 @@ | |||||
| from dora import Node | |||||
| """TODO: Add docstring.""" | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from dora import Node | |||||
| node = Node() | node = Node() | ||||
| @@ -15,7 +17,7 @@ for event in node: | |||||
| pa.array( | pa.array( | ||||
| [ | [ | ||||
| "Respond with left, right, forward, back, up, down or go home in order for the robotic arm to " | "Respond with left, right, forward, back, up, down or go home in order for the robotic arm to " | ||||
| + text | |||||
| ] | |||||
| + text, | |||||
| ], | |||||
| ), | ), | ||||
| ) | ) | ||||
| @@ -1 +1,5 @@ | |||||
| """Module for handling vision-language model prompts. | |||||
| This module provides functionality for generating and processing prompts | |||||
| for vision-language models used in robot control. | |||||
| """ | |||||
| @@ -1,5 +1,11 @@ | |||||
| from dora import Node | |||||
| """Module for interpolating between voice commands. | |||||
| This module provides functionality for processing and interpolating between | |||||
| voice commands used for robot control. | |||||
| """ | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from dora import Node | |||||
| node = Node() | node = Node() | ||||
| @@ -1,20 +1,33 @@ | |||||
| """TODO: Add docstring.""" | |||||
| import time | import time | ||||
| from pathlib import Path | |||||
| import gym_dora # noqa: F401 | |||||
| import gymnasium as gym | import gymnasium as gym | ||||
| import pandas as pd | import pandas as pd | ||||
| from lerobot.common.datasets.lerobot_dataset import LeRobotDataset | from lerobot.common.datasets.lerobot_dataset import LeRobotDataset | ||||
| import gym_dora # noqa: F401 | |||||
| env = gym.make( | env = gym.make( | ||||
| "gym_dora/DoraReachy2-v0", disable_env_checker=True, max_episode_steps=10000 | |||||
| "gym_dora/DoraReachy2-v0", disable_env_checker=True, max_episode_steps=10000, | |||||
| ) | ) | ||||
| observation = env.reset() | observation = env.reset() | ||||
| class ReplayPolicy: | class ReplayPolicy: | ||||
| """A policy class for replaying recorded robot actions. | |||||
| This class handles loading and replaying recorded actions from a dataset, | |||||
| maintaining timing between actions to match the original recording. | |||||
| """ | |||||
| def __init__(self, example_path, epidode=0): | def __init__(self, example_path, epidode=0): | ||||
| """Initialize the replay policy. | |||||
| Args: | |||||
| example_path: Path to the directory containing recorded actions | |||||
| epidode: Index of the episode to replay | |||||
| """ | |||||
| df_action = pd.read_parquet(example_path / "action.parquet") | df_action = pd.read_parquet(example_path / "action.parquet") | ||||
| df_episode_index = pd.read_parquet(example_path / "episode_index.parquet") | df_episode_index = pd.read_parquet(example_path / "episode_index.parquet") | ||||
| self.df = pd.merge_asof( | self.df = pd.merge_asof( | ||||
| @@ -31,6 +44,16 @@ class ReplayPolicy: | |||||
| self.finished = False | self.finished = False | ||||
| def select_action(self, obs): | def select_action(self, obs): | ||||
| """Select the next action to replay. | |||||
| Args: | |||||
| obs: Current observation from the environment (unused) | |||||
| Returns: | |||||
| tuple: (action, finished) where action is the next action to take | |||||
| and finished indicates if all actions have been replayed | |||||
| """ | |||||
| if self.index < len(self.df): | if self.index < len(self.df): | ||||
| self.index += 1 | self.index += 1 | ||||
| else: | else: | ||||
| @@ -44,7 +67,19 @@ class ReplayPolicy: | |||||
| class ReplayLeRobotPolicy: | class ReplayLeRobotPolicy: | ||||
| """A policy class for replaying actions from the LeRobot dataset. | |||||
| This class handles loading and replaying actions from the LeRobot dataset, | |||||
| maintaining the sequence of actions from the original recording. | |||||
| """ | |||||
| def __init__(self, episode=21): | def __init__(self, episode=21): | ||||
| """Initialize the LeRobot replay policy. | |||||
| Args: | |||||
| episode: Index of the episode to replay from the dataset | |||||
| """ | |||||
| self.index = 0 | self.index = 0 | ||||
| self.finished = False | self.finished = False | ||||
| # episode = 1 | # episode = 1 | ||||
| @@ -55,6 +90,16 @@ class ReplayLeRobotPolicy: | |||||
| self.actions = dataset.hf_dataset["action"][from_index:to_index] | self.actions = dataset.hf_dataset["action"][from_index:to_index] | ||||
| def select_action(self, obs): | def select_action(self, obs): | ||||
| """Select the next action to replay from the LeRobot dataset. | |||||
| Args: | |||||
| obs: Current observation from the environment (unused) | |||||
| Returns: | |||||
| tuple: (action, finished) where action is the next action to take | |||||
| and finished indicates if all actions have been replayed | |||||
| """ | |||||
| if self.index < len(self.actions): | if self.index < len(self.actions): | ||||
| self.index += 1 | self.index += 1 | ||||
| else: | else: | ||||
| @@ -1,8 +1,13 @@ | |||||
| from pynput import keyboard | |||||
| from pynput.keyboard import Key, Events | |||||
| """Module for handling keyboard input in the Reachy robot control system. | |||||
| This module provides functionality for capturing and processing keyboard input | |||||
| to control the Reachy robot's movements and actions. | |||||
| """ | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from dora import Node | from dora import Node | ||||
| from pynput import keyboard | |||||
| from pynput.keyboard import Events, Key | |||||
| node = Node() | node = Node() | ||||
| buffer_text = "" | buffer_text = "" | ||||
| @@ -17,7 +22,7 @@ with keyboard.Events() as events: | |||||
| cursor += 1 | cursor += 1 | ||||
| node.send_output("space", pa.array([cursor])) | node.send_output("space", pa.array([cursor])) | ||||
| space = True | space = True | ||||
| elif event is not None and isinstance(event, Events.Release): | elif event is not None and isinstance(event, Events.Release): | ||||
| if event.key == Key.space: | if event.key == Key.space: | ||||
| @@ -25,7 +30,7 @@ with keyboard.Events() as events: | |||||
| space = False | space = False | ||||
| elif event.key == Key.backspace: | elif event.key == Key.backspace: | ||||
| node.send_output("failed", pa.array([cursor])) | node.send_output("failed", pa.array([cursor])) | ||||
| if node.next(0.001) is None: | if node.next(0.001) is None: | ||||
| break | break | ||||
| @@ -1,14 +1,14 @@ | |||||
| """TODO: Add docstring.""" | |||||
| #!/usr/bin/env python3 | #!/usr/bin/env python3 | ||||
| # -*- coding: utf-8 -*- | |||||
| import os | import os | ||||
| import time | |||||
| import numpy as np | |||||
| import subprocess | |||||
| from pathlib import Path | |||||
| import cv2 | import cv2 | ||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from pathlib import Path | |||||
| from dora import Node | from dora import Node | ||||
| import subprocess | |||||
| node = Node() | node = Node() | ||||
| @@ -40,12 +40,12 @@ for event in node: | |||||
| f"ffmpeg -r {FPS} " | f"ffmpeg -r {FPS} " | ||||
| "-f image2 " | "-f image2 " | ||||
| "-loglevel error " | "-loglevel error " | ||||
| f"-i {str(out_dir / 'frame_%06d.png')} " | |||||
| f"-i {out_dir / 'frame_%06d.png'!s} " | |||||
| "-vcodec libx264 " | "-vcodec libx264 " | ||||
| "-g 2 " | "-g 2 " | ||||
| "-pix_fmt yuv444p " | "-pix_fmt yuv444p " | ||||
| f"{str(video_path)} &&" | |||||
| f"rm -r {str(out_dir)}" | |||||
| f"{video_path!s} &&" | |||||
| f"rm -r {out_dir!s}" | |||||
| ) | ) | ||||
| print(ffmpeg_cmd, flush=True) | print(ffmpeg_cmd, flush=True) | ||||
| subprocess.Popen([ffmpeg_cmd], start_new_session=True, shell=True) | subprocess.Popen([ffmpeg_cmd], start_new_session=True, shell=True) | ||||
| @@ -1,8 +1,14 @@ | |||||
| """Module for visualizing robot data and states. | |||||
| This module provides functionality for plotting and displaying various aspects | |||||
| of the robot's state, including positions, movements, and sensor data. | |||||
| """ | |||||
| import os | import os | ||||
| import cv2 | import cv2 | ||||
| from dora import Node | from dora import Node | ||||
| IMAGE_WIDTH = int(os.getenv("IMAGE_WIDTH", "1280")) | IMAGE_WIDTH = int(os.getenv("IMAGE_WIDTH", "1280")) | ||||
| IMAGE_HEIGHT = int(os.getenv("IMAGE_HEIGHT", "720")) | IMAGE_HEIGHT = int(os.getenv("IMAGE_HEIGHT", "720")) | ||||
| FONT = cv2.FONT_HERSHEY_SIMPLEX | FONT = cv2.FONT_HERSHEY_SIMPLEX | ||||
| @@ -1,7 +1,6 @@ | |||||
| import argparse | |||||
| import os | |||||
| """TODO: Add docstring.""" | |||||
| import time | import time | ||||
| from pathlib import Path | |||||
| # import h5py | # import h5py | ||||
| import numpy as np | import numpy as np | ||||
| @@ -83,7 +82,7 @@ for event in node: | |||||
| # min(100, max(0, action[7] / 2.26 * 100)) | # min(100, max(0, action[7] / 2.26 * 100)) | ||||
| # ) # replay true action value | # ) # replay true action value | ||||
| reachy.l_arm.gripper.set_opening( | reachy.l_arm.gripper.set_opening( | ||||
| 0 if action[7] < 2.0 else 100 | |||||
| 0 if action[7] < 2.0 else 100, | |||||
| ) # trick to force the gripper to close fully | ) # trick to force the gripper to close fully | ||||
| reachy.r_arm.shoulder.pitch.goal_position = np.rad2deg(action[8]) | reachy.r_arm.shoulder.pitch.goal_position = np.rad2deg(action[8]) | ||||
| @@ -97,7 +96,7 @@ for event in node: | |||||
| # min(100, max(0, action[15] / 2.26 * 100)) | # min(100, max(0, action[15] / 2.26 * 100)) | ||||
| # ) # replay true action value | # ) # replay true action value | ||||
| reachy.r_arm.gripper.set_opening( | reachy.r_arm.gripper.set_opening( | ||||
| 0 if action[15] < 2.0 else 100 | |||||
| 0 if action[15] < 2.0 else 100, | |||||
| ) # trick to force the gripper to close fully | ) # trick to force the gripper to close fully | ||||
| reachy.mobile_base.set_speed(action[16], action[17], np.rad2deg(action[18])) | reachy.mobile_base.set_speed(action[16], action[17], np.rad2deg(action[18])) | ||||
| reachy.head.neck.roll.goal_position = np.rad2deg(action[19]) | reachy.head.neck.roll.goal_position = np.rad2deg(action[19]) | ||||
| @@ -110,38 +109,38 @@ for event in node: | |||||
| mobile_base_pos = {"vx": 0, "vy": 0, "vtheta": 0} | mobile_base_pos = {"vx": 0, "vy": 0, "vtheta": 0} | ||||
| qpos = { | qpos = { | ||||
| "l_arm_shoulder_pitch": np.deg2rad( | "l_arm_shoulder_pitch": np.deg2rad( | ||||
| reachy.l_arm.shoulder.pitch.present_position | |||||
| reachy.l_arm.shoulder.pitch.present_position, | |||||
| ), | ), | ||||
| "l_arm_shoulder_roll": np.deg2rad( | "l_arm_shoulder_roll": np.deg2rad( | ||||
| reachy.l_arm.shoulder.roll.present_position | |||||
| reachy.l_arm.shoulder.roll.present_position, | |||||
| ), | ), | ||||
| "l_arm_elbow_yaw": np.deg2rad(reachy.l_arm.elbow.yaw.present_position), | "l_arm_elbow_yaw": np.deg2rad(reachy.l_arm.elbow.yaw.present_position), | ||||
| "l_arm_elbow_pitch": np.deg2rad( | "l_arm_elbow_pitch": np.deg2rad( | ||||
| reachy.l_arm.elbow.pitch.present_position | |||||
| reachy.l_arm.elbow.pitch.present_position, | |||||
| ), | ), | ||||
| "l_arm_wrist_roll": np.deg2rad( | "l_arm_wrist_roll": np.deg2rad( | ||||
| reachy.l_arm.wrist.roll.present_position | |||||
| reachy.l_arm.wrist.roll.present_position, | |||||
| ), | ), | ||||
| "l_arm_wrist_pitch": np.deg2rad( | "l_arm_wrist_pitch": np.deg2rad( | ||||
| reachy.l_arm.wrist.pitch.present_position | |||||
| reachy.l_arm.wrist.pitch.present_position, | |||||
| ), | ), | ||||
| "l_arm_wrist_yaw": np.deg2rad(reachy.l_arm.wrist.yaw.present_position), | "l_arm_wrist_yaw": np.deg2rad(reachy.l_arm.wrist.yaw.present_position), | ||||
| "l_gripper": reachy.l_arm.gripper._present_position, | "l_gripper": reachy.l_arm.gripper._present_position, | ||||
| "r_arm_shoulder_pitch": np.deg2rad( | "r_arm_shoulder_pitch": np.deg2rad( | ||||
| reachy.r_arm.shoulder.pitch.present_position | |||||
| reachy.r_arm.shoulder.pitch.present_position, | |||||
| ), | ), | ||||
| "r_arm_shoulder_roll": np.deg2rad( | "r_arm_shoulder_roll": np.deg2rad( | ||||
| reachy.r_arm.shoulder.roll.present_position | |||||
| reachy.r_arm.shoulder.roll.present_position, | |||||
| ), | ), | ||||
| "r_arm_elbow_yaw": np.deg2rad(reachy.r_arm.elbow.yaw.present_position), | "r_arm_elbow_yaw": np.deg2rad(reachy.r_arm.elbow.yaw.present_position), | ||||
| "r_arm_elbow_pitch": np.deg2rad( | "r_arm_elbow_pitch": np.deg2rad( | ||||
| reachy.r_arm.elbow.pitch.present_position | |||||
| reachy.r_arm.elbow.pitch.present_position, | |||||
| ), | ), | ||||
| "r_arm_wrist_roll": np.deg2rad( | "r_arm_wrist_roll": np.deg2rad( | ||||
| reachy.r_arm.wrist.roll.present_position | |||||
| reachy.r_arm.wrist.roll.present_position, | |||||
| ), | ), | ||||
| "r_arm_wrist_pitch": np.deg2rad( | "r_arm_wrist_pitch": np.deg2rad( | ||||
| reachy.r_arm.wrist.pitch.present_position | |||||
| reachy.r_arm.wrist.pitch.present_position, | |||||
| ), | ), | ||||
| "r_arm_wrist_yaw": np.deg2rad(reachy.r_arm.wrist.yaw.present_position), | "r_arm_wrist_yaw": np.deg2rad(reachy.r_arm.wrist.yaw.present_position), | ||||
| "r_gripper": reachy.r_arm.gripper._present_position, | "r_gripper": reachy.r_arm.gripper._present_position, | ||||
| @@ -1,7 +1,6 @@ | |||||
| import argparse | |||||
| import os | |||||
| """TODO: Add docstring.""" | |||||
| import time | import time | ||||
| from pathlib import Path | |||||
| import numpy as np | import numpy as np | ||||
| import pyarrow as pa | import pyarrow as pa | ||||
| @@ -21,7 +20,6 @@ cam = SDKWrapper(get_config_file_path("CONFIG_SR"), fps=freq) | |||||
| # ret, image = cap.read() | # ret, image = cap.read() | ||||
| import cv2 | import cv2 | ||||
| import numpy as np | |||||
| episode = 1 | episode = 1 | ||||
| dataset = LeRobotDataset("cadene/reachy2_teleop_remi") | dataset = LeRobotDataset("cadene/reachy2_teleop_remi") | ||||
| @@ -41,8 +39,6 @@ images = images.astype(np.uint8) | |||||
| images = cv2.cvtColor(images, cv2.COLOR_RGB2BGR) | images = cv2.cvtColor(images, cv2.COLOR_RGB2BGR) | ||||
| # start = time.time() | # start = time.time() | ||||
| import PIL | |||||
| import torch | |||||
| # frame_hwc = (images.permute((1, 2, 0)) * 255).type(torch.uint8).cpu().numpy() | # frame_hwc = (images.permute((1, 2, 0)) * 255).type(torch.uint8).cpu().numpy() | ||||
| @@ -1,7 +1,10 @@ | |||||
| from lerobot.common.datasets.lerobot_dataset import LeRobotDataset | |||||
| """TODO: Add docstring.""" | |||||
| import time | import time | ||||
| from dora import Node | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from dora import Node | |||||
| from lerobot.common.datasets.lerobot_dataset import LeRobotDataset | |||||
| episode = 1 | episode = 1 | ||||
| dataset = LeRobotDataset("cadene/reachy2_teleop_remi") | dataset = LeRobotDataset("cadene/reachy2_teleop_remi") | ||||
| @@ -1,20 +1,23 @@ | |||||
| """TODO: Add docstring.""" | |||||
| import time | import time | ||||
| from pathlib import Path | |||||
| import gym_dora # noqa: F401 | |||||
| import gymnasium as gym | import gymnasium as gym | ||||
| import pandas as pd | import pandas as pd | ||||
| from lerobot.common.datasets.lerobot_dataset import LeRobotDataset | from lerobot.common.datasets.lerobot_dataset import LeRobotDataset | ||||
| import gym_dora # noqa: F401 | |||||
| env = gym.make( | env = gym.make( | ||||
| "gym_dora/DoraReachy2-v0", disable_env_checker=True, max_episode_steps=10000 | |||||
| "gym_dora/DoraReachy2-v0", disable_env_checker=True, max_episode_steps=10000, | |||||
| ) | ) | ||||
| observation = env.reset() | observation = env.reset() | ||||
| class ReplayPolicy: | class ReplayPolicy: | ||||
| """TODO: Add docstring.""" | |||||
| def __init__(self, example_path, epidode=0): | def __init__(self, example_path, epidode=0): | ||||
| """TODO: Add docstring.""" | |||||
| df_action = pd.read_parquet(example_path / "action.parquet") | df_action = pd.read_parquet(example_path / "action.parquet") | ||||
| df_episode_index = pd.read_parquet(example_path / "episode_index.parquet") | df_episode_index = pd.read_parquet(example_path / "episode_index.parquet") | ||||
| self.df = pd.merge_asof( | self.df = pd.merge_asof( | ||||
| @@ -31,6 +34,7 @@ class ReplayPolicy: | |||||
| self.finished = False | self.finished = False | ||||
| def select_action(self, obs): | def select_action(self, obs): | ||||
| """TODO: Add docstring.""" | |||||
| if self.index < len(self.df): | if self.index < len(self.df): | ||||
| self.index += 1 | self.index += 1 | ||||
| else: | else: | ||||
| @@ -44,7 +48,10 @@ class ReplayPolicy: | |||||
| class ReplayLeRobotPolicy: | class ReplayLeRobotPolicy: | ||||
| """TODO: Add docstring.""" | |||||
| def __init__(self, episode=21): | def __init__(self, episode=21): | ||||
| """TODO: Add docstring.""" | |||||
| self.index = 0 | self.index = 0 | ||||
| self.finished = False | self.finished = False | ||||
| # episode = 1 | # episode = 1 | ||||
| @@ -55,6 +62,7 @@ class ReplayLeRobotPolicy: | |||||
| self.actions = dataset.hf_dataset["action"][from_index:to_index] | self.actions = dataset.hf_dataset["action"][from_index:to_index] | ||||
| def select_action(self, obs): | def select_action(self, obs): | ||||
| """TODO: Add docstring.""" | |||||
| if self.index < len(self.actions): | if self.index < len(self.actions): | ||||
| self.index += 1 | self.index += 1 | ||||
| else: | else: | ||||
| @@ -1,5 +1,7 @@ | |||||
| from dora import Node | |||||
| """TODO: Add docstring.""" | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from dora import Node | |||||
| node = Node() | node = Node() | ||||
| @@ -1,8 +1,9 @@ | |||||
| from pynput import keyboard | |||||
| from pynput.keyboard import Key, Events | |||||
| """TODO: Add docstring.""" | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from dora import Node | from dora import Node | ||||
| from pynput import keyboard | |||||
| from pynput.keyboard import Events, Key | |||||
| node = Node() | node = Node() | ||||
| buffer_text = "" | buffer_text = "" | ||||
| @@ -17,7 +18,7 @@ with keyboard.Events() as events: | |||||
| cursor += 1 | cursor += 1 | ||||
| node.send_output("space", pa.array([cursor])) | node.send_output("space", pa.array([cursor])) | ||||
| space = True | space = True | ||||
| elif event is not None and isinstance(event, Events.Release): | elif event is not None and isinstance(event, Events.Release): | ||||
| if event.key == Key.space: | if event.key == Key.space: | ||||
| @@ -25,7 +26,7 @@ with keyboard.Events() as events: | |||||
| space = False | space = False | ||||
| elif event.key == Key.backspace: | elif event.key == Key.backspace: | ||||
| node.send_output("failed", pa.array([cursor])) | node.send_output("failed", pa.array([cursor])) | ||||
| if node.next(0.001) is None: | if node.next(0.001) is None: | ||||
| break | break | ||||
| @@ -1,14 +1,18 @@ | |||||
| """Module for saving webcam feed data from LeRobot experiments. | |||||
| This module provides functionality for capturing and saving webcam feed data | |||||
| during LeRobot experiments with the Reachy robot. | |||||
| """ | |||||
| #!/usr/bin/env python3 | #!/usr/bin/env python3 | ||||
| # -*- coding: utf-8 -*- | |||||
| import os | import os | ||||
| import time | |||||
| import numpy as np | |||||
| import subprocess | |||||
| from pathlib import Path | |||||
| import cv2 | import cv2 | ||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from pathlib import Path | |||||
| from dora import Node | from dora import Node | ||||
| import subprocess | |||||
| node = Node() | node = Node() | ||||
| @@ -40,12 +44,12 @@ for event in node: | |||||
| f"ffmpeg -r {FPS} " | f"ffmpeg -r {FPS} " | ||||
| "-f image2 " | "-f image2 " | ||||
| "-loglevel error " | "-loglevel error " | ||||
| f"-i {str(out_dir / 'frame_%06d.png')} " | |||||
| f"-i {out_dir / 'frame_%06d.png'!s} " | |||||
| "-vcodec libx264 " | "-vcodec libx264 " | ||||
| "-g 2 " | "-g 2 " | ||||
| "-pix_fmt yuv444p " | "-pix_fmt yuv444p " | ||||
| f"{str(video_path)} &&" | |||||
| f"rm -r {str(out_dir)}" | |||||
| f"{video_path!s} &&" | |||||
| f"rm -r {out_dir!s}" | |||||
| ) | ) | ||||
| print(ffmpeg_cmd, flush=True) | print(ffmpeg_cmd, flush=True) | ||||
| subprocess.Popen([ffmpeg_cmd], start_new_session=True, shell=True) | subprocess.Popen([ffmpeg_cmd], start_new_session=True, shell=True) | ||||
| @@ -1,8 +1,14 @@ | |||||
| """Module for visualizing robot data and states. | |||||
| This module provides functionality for creating real-time visualizations of | |||||
| robot states, movements, and sensor data during operation. | |||||
| """ | |||||
| import os | import os | ||||
| import cv2 | import cv2 | ||||
| from dora import Node | from dora import Node | ||||
| IMAGE_WIDTH = int(os.getenv("IMAGE_WIDTH", "1280")) | IMAGE_WIDTH = int(os.getenv("IMAGE_WIDTH", "1280")) | ||||
| IMAGE_HEIGHT = int(os.getenv("IMAGE_HEIGHT", "720")) | IMAGE_HEIGHT = int(os.getenv("IMAGE_HEIGHT", "720")) | ||||
| FONT = cv2.FONT_HERSHEY_SIMPLEX | FONT = cv2.FONT_HERSHEY_SIMPLEX | ||||
| @@ -1,7 +1,10 @@ | |||||
| import argparse | |||||
| import os | |||||
| """Module for client-side communication with the Reachy robot. | |||||
| This module provides functionality for establishing and managing communication | |||||
| with the Reachy robot, including sending commands and receiving sensor data. | |||||
| """ | |||||
| import time | import time | ||||
| from pathlib import Path | |||||
| # import h5py | # import h5py | ||||
| import numpy as np | import numpy as np | ||||
| @@ -83,7 +86,7 @@ for event in node: | |||||
| # min(100, max(0, action[7] / 2.26 * 100)) | # min(100, max(0, action[7] / 2.26 * 100)) | ||||
| # ) # replay true action value | # ) # replay true action value | ||||
| reachy.l_arm.gripper.set_opening( | reachy.l_arm.gripper.set_opening( | ||||
| 0 if action[7] < 2.0 else 100 | |||||
| 0 if action[7] < 2.0 else 100, | |||||
| ) # trick to force the gripper to close fully | ) # trick to force the gripper to close fully | ||||
| reachy.r_arm.shoulder.pitch.goal_position = np.rad2deg(action[8]) | reachy.r_arm.shoulder.pitch.goal_position = np.rad2deg(action[8]) | ||||
| @@ -97,7 +100,7 @@ for event in node: | |||||
| # min(100, max(0, action[15] / 2.26 * 100)) | # min(100, max(0, action[15] / 2.26 * 100)) | ||||
| # ) # replay true action value | # ) # replay true action value | ||||
| reachy.r_arm.gripper.set_opening( | reachy.r_arm.gripper.set_opening( | ||||
| 0 if action[15] < 2.0 else 100 | |||||
| 0 if action[15] < 2.0 else 100, | |||||
| ) # trick to force the gripper to close fully | ) # trick to force the gripper to close fully | ||||
| reachy.mobile_base.set_speed(action[16], action[17], np.rad2deg(action[18])) | reachy.mobile_base.set_speed(action[16], action[17], np.rad2deg(action[18])) | ||||
| reachy.head.neck.roll.goal_position = np.rad2deg(action[19]) | reachy.head.neck.roll.goal_position = np.rad2deg(action[19]) | ||||
| @@ -110,38 +113,38 @@ for event in node: | |||||
| mobile_base_pos = {"vx": 0, "vy": 0, "vtheta": 0} | mobile_base_pos = {"vx": 0, "vy": 0, "vtheta": 0} | ||||
| qpos = { | qpos = { | ||||
| "l_arm_shoulder_pitch": np.deg2rad( | "l_arm_shoulder_pitch": np.deg2rad( | ||||
| reachy.l_arm.shoulder.pitch.present_position | |||||
| reachy.l_arm.shoulder.pitch.present_position, | |||||
| ), | ), | ||||
| "l_arm_shoulder_roll": np.deg2rad( | "l_arm_shoulder_roll": np.deg2rad( | ||||
| reachy.l_arm.shoulder.roll.present_position | |||||
| reachy.l_arm.shoulder.roll.present_position, | |||||
| ), | ), | ||||
| "l_arm_elbow_yaw": np.deg2rad(reachy.l_arm.elbow.yaw.present_position), | "l_arm_elbow_yaw": np.deg2rad(reachy.l_arm.elbow.yaw.present_position), | ||||
| "l_arm_elbow_pitch": np.deg2rad( | "l_arm_elbow_pitch": np.deg2rad( | ||||
| reachy.l_arm.elbow.pitch.present_position | |||||
| reachy.l_arm.elbow.pitch.present_position, | |||||
| ), | ), | ||||
| "l_arm_wrist_roll": np.deg2rad( | "l_arm_wrist_roll": np.deg2rad( | ||||
| reachy.l_arm.wrist.roll.present_position | |||||
| reachy.l_arm.wrist.roll.present_position, | |||||
| ), | ), | ||||
| "l_arm_wrist_pitch": np.deg2rad( | "l_arm_wrist_pitch": np.deg2rad( | ||||
| reachy.l_arm.wrist.pitch.present_position | |||||
| reachy.l_arm.wrist.pitch.present_position, | |||||
| ), | ), | ||||
| "l_arm_wrist_yaw": np.deg2rad(reachy.l_arm.wrist.yaw.present_position), | "l_arm_wrist_yaw": np.deg2rad(reachy.l_arm.wrist.yaw.present_position), | ||||
| "l_gripper": reachy.l_arm.gripper._present_position, | "l_gripper": reachy.l_arm.gripper._present_position, | ||||
| "r_arm_shoulder_pitch": np.deg2rad( | "r_arm_shoulder_pitch": np.deg2rad( | ||||
| reachy.r_arm.shoulder.pitch.present_position | |||||
| reachy.r_arm.shoulder.pitch.present_position, | |||||
| ), | ), | ||||
| "r_arm_shoulder_roll": np.deg2rad( | "r_arm_shoulder_roll": np.deg2rad( | ||||
| reachy.r_arm.shoulder.roll.present_position | |||||
| reachy.r_arm.shoulder.roll.present_position, | |||||
| ), | ), | ||||
| "r_arm_elbow_yaw": np.deg2rad(reachy.r_arm.elbow.yaw.present_position), | "r_arm_elbow_yaw": np.deg2rad(reachy.r_arm.elbow.yaw.present_position), | ||||
| "r_arm_elbow_pitch": np.deg2rad( | "r_arm_elbow_pitch": np.deg2rad( | ||||
| reachy.r_arm.elbow.pitch.present_position | |||||
| reachy.r_arm.elbow.pitch.present_position, | |||||
| ), | ), | ||||
| "r_arm_wrist_roll": np.deg2rad( | "r_arm_wrist_roll": np.deg2rad( | ||||
| reachy.r_arm.wrist.roll.present_position | |||||
| reachy.r_arm.wrist.roll.present_position, | |||||
| ), | ), | ||||
| "r_arm_wrist_pitch": np.deg2rad( | "r_arm_wrist_pitch": np.deg2rad( | ||||
| reachy.r_arm.wrist.pitch.present_position | |||||
| reachy.r_arm.wrist.pitch.present_position, | |||||
| ), | ), | ||||
| "r_arm_wrist_yaw": np.deg2rad(reachy.r_arm.wrist.yaw.present_position), | "r_arm_wrist_yaw": np.deg2rad(reachy.r_arm.wrist.yaw.present_position), | ||||
| "r_gripper": reachy.r_arm.gripper._present_position, | "r_gripper": reachy.r_arm.gripper._present_position, | ||||
| @@ -1,7 +1,10 @@ | |||||
| import argparse | |||||
| import os | |||||
| """Module for handling vision-related communication with the Reachy robot. | |||||
| This module provides functionality for processing and managing visual data | |||||
| from the Reachy robot's cameras and vision sensors. | |||||
| """ | |||||
| import time | import time | ||||
| from pathlib import Path | |||||
| import numpy as np | import numpy as np | ||||
| import pyarrow as pa | import pyarrow as pa | ||||
| @@ -21,7 +24,6 @@ cam = SDKWrapper(get_config_file_path("CONFIG_SR"), fps=freq) | |||||
| # ret, image = cap.read() | # ret, image = cap.read() | ||||
| import cv2 | import cv2 | ||||
| import numpy as np | |||||
| episode = 1 | episode = 1 | ||||
| dataset = LeRobotDataset("cadene/reachy2_teleop_remi") | dataset = LeRobotDataset("cadene/reachy2_teleop_remi") | ||||
| @@ -41,8 +43,6 @@ images = images.astype(np.uint8) | |||||
| images = cv2.cvtColor(images, cv2.COLOR_RGB2BGR) | images = cv2.cvtColor(images, cv2.COLOR_RGB2BGR) | ||||
| # start = time.time() | # start = time.time() | ||||
| import PIL | |||||
| import torch | |||||
| # frame_hwc = (images.permute((1, 2, 0)) * 255).type(torch.uint8).cpu().numpy() | # frame_hwc = (images.permute((1, 2, 0)) * 255).type(torch.uint8).cpu().numpy() | ||||
| @@ -1,7 +1,14 @@ | |||||
| from lerobot.common.datasets.lerobot_dataset import LeRobotDataset | |||||
| """Module for replaying recorded robot actions and movements. | |||||
| This module provides functionality for loading and replaying previously recorded | |||||
| robot actions and movements, allowing for demonstration and analysis of robot behavior. | |||||
| """ | |||||
| import time | import time | ||||
| from dora import Node | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from dora import Node | |||||
| from lerobot.common.datasets.lerobot_dataset import LeRobotDataset | |||||
| episode = 1 | episode = 1 | ||||
| dataset = LeRobotDataset("cadene/reachy2_teleop_remi") | dataset = LeRobotDataset("cadene/reachy2_teleop_remi") | ||||
| @@ -1,6 +1,11 @@ | |||||
| from dora import Node | |||||
| """Module for interpolating between text-based robot commands. | |||||
| This module provides functionality for processing and interpolating between | |||||
| text-based commands to create smooth transitions in robot behavior. | |||||
| """ | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| import numpy as np | |||||
| from dora import Node | |||||
| node = Node() | node = Node() | ||||
| @@ -19,19 +24,19 @@ for event in node: | |||||
| node.send_output("head_action", pa.array([0, head_step, 0])) | node.send_output("head_action", pa.array([0, head_step, 0])) | ||||
| elif text == "look up": | elif text == "look up": | ||||
| node.send_output( | node.send_output( | ||||
| "head_action", pa.array([head_step / 2, 0, -head_step / 2]) | |||||
| "head_action", pa.array([head_step / 2, 0, -head_step / 2]), | |||||
| ) | ) | ||||
| elif text == "look down": | elif text == "look down": | ||||
| node.send_output( | node.send_output( | ||||
| "head_action", pa.array([-head_step / 2, 0, head_step / 2]) | |||||
| "head_action", pa.array([-head_step / 2, 0, head_step / 2]), | |||||
| ) | ) | ||||
| elif text == "look up": | elif text == "look up": | ||||
| node.send_output( | node.send_output( | ||||
| "head_action", pa.array([head_step / 2, 0, -head_step / 2]) | |||||
| "head_action", pa.array([head_step / 2, 0, -head_step / 2]), | |||||
| ) | ) | ||||
| elif text == "look down": | elif text == "look down": | ||||
| node.send_output( | node.send_output( | ||||
| "head_action", pa.array([-head_step / 2, 0, head_step / 2]) | |||||
| "head_action", pa.array([-head_step / 2, 0, head_step / 2]), | |||||
| ) | ) | ||||
| elif text == "smile": | elif text == "smile": | ||||
| node.send_output("antenna_action", pa.array(["smile"])) | node.send_output("antenna_action", pa.array(["smile"])) | ||||
| @@ -54,8 +59,8 @@ for event in node: | |||||
| "question", | "question", | ||||
| pa.array( | pa.array( | ||||
| [ | [ | ||||
| "Respond with right, left, forward, backward, open, or close to grab the trash" | |||||
| ] | |||||
| "Respond with right, left, forward, backward, open, or close to grab the trash", | |||||
| ], | |||||
| ), | ), | ||||
| ) | ) | ||||
| @@ -65,8 +70,8 @@ for event in node: | |||||
| "question", | "question", | ||||
| pa.array( | pa.array( | ||||
| [ | [ | ||||
| "Respond with right, left, forward, backward, open, or close to put the trash in your hand in the right bin" | |||||
| ] | |||||
| "Respond with right, left, forward, backward, open, or close to put the trash in your hand in the right bin", | |||||
| ], | |||||
| ), | ), | ||||
| ) | ) | ||||
| node.send_output("gripper_action", pa.array([100])) | node.send_output("gripper_action", pa.array([100])) | ||||
| @@ -1,17 +1,24 @@ | |||||
| import enum | |||||
| """Module for managing Feetech servo motor communication. | |||||
| import pyarrow as pa | |||||
| This module provides functionality for communicating with and controlling | |||||
| Feetech servo motors through a serial bus interface. | |||||
| """ | |||||
| import enum | |||||
| from typing import Union | from typing import Union | ||||
| import pyarrow as pa | |||||
| from scservo_sdk import ( | from scservo_sdk import ( | ||||
| PacketHandler, | |||||
| PortHandler, | |||||
| COMM_SUCCESS, | COMM_SUCCESS, | ||||
| SCS_HIBYTE, | |||||
| SCS_HIWORD, | |||||
| SCS_LOBYTE, | |||||
| SCS_LOWORD, | |||||
| GroupSyncRead, | GroupSyncRead, | ||||
| GroupSyncWrite, | GroupSyncWrite, | ||||
| PacketHandler, | |||||
| PortHandler, | |||||
| ) | ) | ||||
| from scservo_sdk import SCS_HIBYTE, SCS_HIWORD, SCS_LOBYTE, SCS_LOWORD | |||||
| PROTOCOL_VERSION = 0 | PROTOCOL_VERSION = 0 | ||||
| BAUD_RATE = 1_000_000 | BAUD_RATE = 1_000_000 | ||||
| @@ -22,6 +29,16 @@ def wrap_joints_and_values( | |||||
| joints: Union[list[str], pa.Array], | joints: Union[list[str], pa.Array], | ||||
| values: Union[list[int], pa.Array], | values: Union[list[int], pa.Array], | ||||
| ) -> pa.StructArray: | ) -> pa.StructArray: | ||||
| """Create a structured array from joint names and their values. | |||||
| Args: | |||||
| joints: List of joint names or Arrow array of joint names | |||||
| values: List of values or Arrow array of values | |||||
| Returns: | |||||
| pa.StructArray: Structured array containing joint names and values | |||||
| """ | |||||
| return pa.StructArray.from_arrays( | return pa.StructArray.from_arrays( | ||||
| arrays=[joints, values], | arrays=[joints, values], | ||||
| names=["joints", "values"], | names=["joints", "values"], | ||||
| @@ -29,11 +46,15 @@ def wrap_joints_and_values( | |||||
| class TorqueMode(enum.Enum): | class TorqueMode(enum.Enum): | ||||
| """Enumeration of torque control modes for servo motors.""" | |||||
| ENABLED = pa.scalar(1, pa.uint32()) | ENABLED = pa.scalar(1, pa.uint32()) | ||||
| DISABLED = pa.scalar(0, pa.uint32()) | DISABLED = pa.scalar(0, pa.uint32()) | ||||
| class OperatingMode(enum.Enum): | class OperatingMode(enum.Enum): | ||||
| """Enumeration of operating modes for servo motors.""" | |||||
| ONE_TURN = pa.scalar(0, pa.uint32()) | ONE_TURN = pa.scalar(0, pa.uint32()) | ||||
| @@ -91,15 +112,23 @@ MODEL_CONTROL_TABLE = { | |||||
| class FeetechBus: | class FeetechBus: | ||||
| """A class for managing communication with Feetech servo motors. | |||||
| This class handles the low-level communication with Feetech servo motors | |||||
| through a serial bus interface, providing methods for reading and writing | |||||
| motor parameters. | |||||
| """ | |||||
| def __init__(self, port: str, description: dict[str, (np.uint8, str)]): | def __init__(self, port: str, description: dict[str, (np.uint8, str)]): | ||||
| """ | |||||
| """Initialize the Feetech bus interface. | |||||
| Args: | Args: | ||||
| port: the serial port to connect to the Feetech bus | |||||
| description: a dictionary containing the description of the motors connected to the bus. The keys are the | |||||
| motor names and the values are tuples containing the motor id and the motor model. | |||||
| """ | |||||
| port: The serial port to connect to the Feetech bus | |||||
| description: A dictionary containing the description of the motors | |||||
| connected to the bus. The keys are the motor names and | |||||
| the values are tuples containing the motor id and model. | |||||
| """ | |||||
| self.port = port | self.port = port | ||||
| self.descriptions = description | self.descriptions = description | ||||
| self.motor_ctrl = {} | self.motor_ctrl = {} | ||||
| @@ -130,9 +159,17 @@ class FeetechBus: | |||||
| self.group_writers = {} | self.group_writers = {} | ||||
| def close(self): | def close(self): | ||||
| """Close the serial port connection.""" | |||||
| self.port_handler.closePort() | self.port_handler.closePort() | ||||
| def write(self, data_name: str, data: pa.StructArray): | def write(self, data_name: str, data: pa.StructArray): | ||||
| """Write data to the specified motor parameters. | |||||
| Args: | |||||
| data_name: Name of the parameter to write | |||||
| data: Structured array containing the data to write | |||||
| """ | |||||
| motor_ids = [ | motor_ids = [ | ||||
| self.motor_ctrl[motor_name.as_py()]["id"] | self.motor_ctrl[motor_name.as_py()]["id"] | ||||
| for motor_name in data.field("joints") | for motor_name in data.field("joints") | ||||
| @@ -183,7 +220,7 @@ class FeetechBus: | |||||
| else: | else: | ||||
| raise NotImplementedError( | raise NotImplementedError( | ||||
| f"Value of the number of bytes to be sent is expected to be in [1, 2, 4], but {packet_bytes_size} " | f"Value of the number of bytes to be sent is expected to be in [1, 2, 4], but {packet_bytes_size} " | ||||
| f"is provided instead." | |||||
| f"is provided instead.", | |||||
| ) | ) | ||||
| if init_group: | if init_group: | ||||
| @@ -195,10 +232,20 @@ class FeetechBus: | |||||
| if comm != COMM_SUCCESS: | if comm != COMM_SUCCESS: | ||||
| raise ConnectionError( | raise ConnectionError( | ||||
| f"Write failed due to communication error on port {self.port} for group_key {group_key}: " | f"Write failed due to communication error on port {self.port} for group_key {group_key}: " | ||||
| f"{self.packet_handler.getTxRxResult(comm)}" | |||||
| f"{self.packet_handler.getTxRxResult(comm)}", | |||||
| ) | ) | ||||
| def read(self, data_name: str, motor_names: pa.Array) -> pa.StructArray: | def read(self, data_name: str, motor_names: pa.Array) -> pa.StructArray: | ||||
| """Read data from the specified motor parameters. | |||||
| Args: | |||||
| data_name: Name of the parameter to read | |||||
| motor_names: Array of motor names to read from | |||||
| Returns: | |||||
| pa.StructArray: Structured array containing the read data | |||||
| """ | |||||
| motor_ids = [ | motor_ids = [ | ||||
| self.motor_ctrl[motor_name.as_py()]["id"] for motor_name in motor_names | self.motor_ctrl[motor_name.as_py()]["id"] for motor_name in motor_names | ||||
| ] | ] | ||||
| @@ -225,13 +272,13 @@ class FeetechBus: | |||||
| if comm != COMM_SUCCESS: | if comm != COMM_SUCCESS: | ||||
| raise ConnectionError( | raise ConnectionError( | ||||
| f"Read failed due to communication error on port {self.port} for group_key {group_key}: " | f"Read failed due to communication error on port {self.port} for group_key {group_key}: " | ||||
| f"{self.packet_handler.getTxRxResult(comm)}" | |||||
| f"{self.packet_handler.getTxRxResult(comm)}", | |||||
| ) | ) | ||||
| values = pa.array( | values = pa.array( | ||||
| [ | [ | ||||
| self.group_readers[group_key].getData( | self.group_readers[group_key].getData( | ||||
| idx, packet_address, packet_bytes_size | |||||
| idx, packet_address, packet_bytes_size, | |||||
| ) | ) | ||||
| for idx in motor_ids | for idx in motor_ids | ||||
| ], | ], | ||||
| @@ -249,25 +296,82 @@ class FeetechBus: | |||||
| return wrap_joints_and_values(motor_names, values) | return wrap_joints_and_values(motor_names, values) | ||||
| def write_torque_enable(self, torque_mode: pa.StructArray): | def write_torque_enable(self, torque_mode: pa.StructArray): | ||||
| """Enable or disable torque for the specified motors. | |||||
| Args: | |||||
| torque_mode: Structured array containing torque enable/disable values | |||||
| """ | |||||
| self.write("Torque_Enable", torque_mode) | self.write("Torque_Enable", torque_mode) | ||||
| def write_operating_mode(self, operating_mode: pa.StructArray): | def write_operating_mode(self, operating_mode: pa.StructArray): | ||||
| """Set the operating mode for the specified motors. | |||||
| Args: | |||||
| operating_mode: Structured array containing operating mode values | |||||
| """ | |||||
| self.write("Mode", operating_mode) | self.write("Mode", operating_mode) | ||||
| def read_position(self, motor_names: pa.Array) -> pa.StructArray: | def read_position(self, motor_names: pa.Array) -> pa.StructArray: | ||||
| """Read current position from the specified motors. | |||||
| Args: | |||||
| motor_names: Array of motor names to read positions from | |||||
| Returns: | |||||
| pa.StructArray: Structured array containing position values | |||||
| """ | |||||
| return self.read("Present_Position", motor_names) | return self.read("Present_Position", motor_names) | ||||
| def read_velocity(self, motor_names: pa.Array) -> pa.StructArray: | def read_velocity(self, motor_names: pa.Array) -> pa.StructArray: | ||||
| """Read current velocity from the specified motors. | |||||
| Args: | |||||
| motor_names: Array of motor names to read velocities from | |||||
| Returns: | |||||
| pa.StructArray: Structured array containing velocity values | |||||
| """ | |||||
| return self.read("Present_Velocity", motor_names) | return self.read("Present_Velocity", motor_names) | ||||
| def read_current(self, motor_names: pa.Array) -> pa.StructArray: | def read_current(self, motor_names: pa.Array) -> pa.StructArray: | ||||
| """Read current current from the specified motors. | |||||
| Args: | |||||
| motor_names: Array of motor names to read currents from | |||||
| Returns: | |||||
| pa.StructArray: Structured array containing current values | |||||
| """ | |||||
| return self.read("Present_Current", motor_names) | return self.read("Present_Current", motor_names) | ||||
| def write_goal_position(self, goal_position: pa.StructArray): | def write_goal_position(self, goal_position: pa.StructArray): | ||||
| """Set goal positions for the specified motors. | |||||
| Args: | |||||
| goal_position: Structured array containing goal position values | |||||
| """ | |||||
| self.write("Goal_Position", goal_position) | self.write("Goal_Position", goal_position) | ||||
| def write_max_angle_limit(self, max_angle_limit: pa.StructArray): | def write_max_angle_limit(self, max_angle_limit: pa.StructArray): | ||||
| """Set maximum angle limits for the specified motors. | |||||
| Args: | |||||
| max_angle_limit: Structured array containing maximum angle limit values | |||||
| """ | |||||
| self.write("Max_Angle_Limit", max_angle_limit) | self.write("Max_Angle_Limit", max_angle_limit) | ||||
| def write_min_angle_limit(self, min_angle_limit: pa.StructArray): | def write_min_angle_limit(self, min_angle_limit: pa.StructArray): | ||||
| """Set minimum angle limits for the specified motors. | |||||
| Args: | |||||
| min_angle_limit: Structured array containing minimum angle limit values | |||||
| """ | |||||
| self.write("Min_Angle_Limit", min_angle_limit) | self.write("Min_Angle_Limit", min_angle_limit) | ||||
| @@ -1,5 +1,7 @@ | |||||
| """ | |||||
| SO100 Auto Configure: This program is used to automatically configure the SO-ARM100 (SO100) for the user. | |||||
| """Module for configuring and setting up the SO100 robot hardware. | |||||
| This module provides functionality for initializing and configuring the SO100 robot's | |||||
| servo motors and other hardware components. | |||||
| The program will: | The program will: | ||||
| 1. Disable all torque motors of provided SO100. | 1. Disable all torque motors of provided SO100. | ||||
| @@ -14,20 +16,17 @@ It will also enable all appropriate operating modes for the SO100. | |||||
| """ | """ | ||||
| import argparse | import argparse | ||||
| import time | |||||
| import json | import json | ||||
| import time | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from bus import FeetechBus, TorqueMode, OperatingMode | |||||
| from pwm_position_control.transform import pwm_to_logical_arrow, wrap_joints_and_values | |||||
| from bus import FeetechBus, OperatingMode, TorqueMode | |||||
| from pwm_position_control.functions import construct_control_table | |||||
| from pwm_position_control.tables import ( | from pwm_position_control.tables import ( | ||||
| construct_logical_to_pwm_conversion_table_arrow, | construct_logical_to_pwm_conversion_table_arrow, | ||||
| construct_pwm_to_logical_conversion_table_arrow, | construct_pwm_to_logical_conversion_table_arrow, | ||||
| ) | ) | ||||
| from pwm_position_control.functions import construct_control_table | |||||
| from pwm_position_control.transform import pwm_to_logical_arrow, wrap_joints_and_values | |||||
| FULL_ARM = pa.array( | FULL_ARM = pa.array( | ||||
| [ | [ | ||||
| @@ -50,35 +49,43 @@ GRIPPER = pa.array(["gripper"], type=pa.string()) | |||||
| def pause(): | def pause(): | ||||
| """Pause execution and wait for user input.""" | |||||
| input("Press Enter to continue...") | input("Press Enter to continue...") | ||||
| def configure_servos(bus: FeetechBus): | def configure_servos(bus: FeetechBus): | ||||
| """Configure the servos with default settings. | |||||
| Args: | |||||
| bus: The FeetechBus instance to configure. | |||||
| """ | |||||
| bus.write_torque_enable( | bus.write_torque_enable( | ||||
| wrap_joints_and_values(FULL_ARM, [TorqueMode.DISABLED.value] * 6) | |||||
| wrap_joints_and_values(FULL_ARM, [TorqueMode.DISABLED.value] * 6), | |||||
| ) | ) | ||||
| bus.write_operating_mode( | bus.write_operating_mode( | ||||
| wrap_joints_and_values(FULL_ARM, [OperatingMode.ONE_TURN.value] * 6) | |||||
| wrap_joints_and_values(FULL_ARM, [OperatingMode.ONE_TURN.value] * 6), | |||||
| ) | ) | ||||
| bus.write_max_angle_limit( | bus.write_max_angle_limit( | ||||
| wrap_joints_and_values(FULL_ARM, [pa.scalar(0, pa.uint32())] * 6) | |||||
| wrap_joints_and_values(FULL_ARM, [pa.scalar(0, pa.uint32())] * 6), | |||||
| ) | ) | ||||
| bus.write_min_angle_limit( | bus.write_min_angle_limit( | ||||
| wrap_joints_and_values(FULL_ARM, [pa.scalar(0, pa.uint32())] * 6) | |||||
| wrap_joints_and_values(FULL_ARM, [pa.scalar(0, pa.uint32())] * 6), | |||||
| ) | ) | ||||
| def main(): | def main(): | ||||
| """Run the servo configuration process.""" | |||||
| parser = argparse.ArgumentParser( | parser = argparse.ArgumentParser( | ||||
| description="SO100 Auto Configure: This program is used to automatically configure the Low Cost Robot (SO100) " | description="SO100 Auto Configure: This program is used to automatically configure the Low Cost Robot (SO100) " | ||||
| "for the user." | |||||
| "for the user.", | |||||
| ) | ) | ||||
| parser.add_argument( | parser.add_argument( | ||||
| "--port", type=str, required=True, help="The port of the SO100." | |||||
| "--port", type=str, required=True, help="The port of the SO100.", | |||||
| ) | ) | ||||
| parser.add_argument( | parser.add_argument( | ||||
| "--right", | "--right", | ||||
| @@ -130,10 +137,10 @@ def main(): | |||||
| pwm_positions = (pwm_position_1, pwm_position_2) | pwm_positions = (pwm_position_1, pwm_position_2) | ||||
| pwm_to_logical_conversion_table = construct_pwm_to_logical_conversion_table_arrow( | pwm_to_logical_conversion_table = construct_pwm_to_logical_conversion_table_arrow( | ||||
| pwm_positions, targets | |||||
| pwm_positions, targets, | |||||
| ) | ) | ||||
| logical_to_pwm_conversion_table = construct_logical_to_pwm_conversion_table_arrow( | logical_to_pwm_conversion_table = construct_logical_to_pwm_conversion_table_arrow( | ||||
| pwm_positions, targets | |||||
| pwm_positions, targets, | |||||
| ) | ) | ||||
| control_table_json = {} | control_table_json = {} | ||||
| @@ -149,7 +156,7 @@ def main(): | |||||
| left = "left" if args.left else "right" | left = "left" if args.left else "right" | ||||
| path = ( | path = ( | ||||
| input( | input( | ||||
| f"Please enter the path of the configuration file (default is ./examples/so100/configs/follower.{left}.json): " | |||||
| f"Please enter the path of the configuration file (default is ./examples/so100/configs/follower.{left}.json): ", | |||||
| ) | ) | ||||
| or f"./examples/so100/configs/follower.{left}.json" | or f"./examples/so100/configs/follower.{left}.json" | ||||
| ) | ) | ||||
| @@ -158,21 +165,21 @@ def main(): | |||||
| json.dump(control_table_json, file) | json.dump(control_table_json, file) | ||||
| control_table = construct_control_table( | control_table = construct_control_table( | ||||
| pwm_to_logical_conversion_table, logical_to_pwm_conversion_table | |||||
| pwm_to_logical_conversion_table, logical_to_pwm_conversion_table, | |||||
| ) | ) | ||||
| while True: | while True: | ||||
| try: | try: | ||||
| pwm_position = arm.read_position(FULL_ARM) | pwm_position = arm.read_position(FULL_ARM) | ||||
| logical_position = pwm_to_logical_arrow( | logical_position = pwm_to_logical_arrow( | ||||
| pwm_position, control_table, ranged=True | |||||
| pwm_position, control_table, ranged=True, | |||||
| ).field("values") | ).field("values") | ||||
| print(f"Logical Position: {logical_position}") | print(f"Logical Position: {logical_position}") | ||||
| except ConnectionError: | except ConnectionError: | ||||
| print( | print( | ||||
| "Connection error occurred. Please check the connection and try again." | |||||
| "Connection error occurred. Please check the connection and try again.", | |||||
| ) | ) | ||||
| time.sleep(0.5) | time.sleep(0.5) | ||||
| @@ -1,25 +1,26 @@ | |||||
| import os | |||||
| """TODO: Add docstring.""" | |||||
| import argparse | import argparse | ||||
| import json | import json | ||||
| import os | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| import pyarrow.compute as pc | import pyarrow.compute as pc | ||||
| from dora import Node | from dora import Node | ||||
| from pwm_position_control.load import load_control_table_from_json_conversion_tables | |||||
| from pwm_position_control.transform import ( | from pwm_position_control.transform import ( | ||||
| wrap_joints_and_values, | |||||
| pwm_to_logical_arrow, | |||||
| logical_to_pwm_with_offset_arrow, | logical_to_pwm_with_offset_arrow, | ||||
| pwm_to_logical_arrow, | |||||
| wrap_joints_and_values, | |||||
| ) | ) | ||||
| from pwm_position_control.load import load_control_table_from_json_conversion_tables | |||||
| def main(): | def main(): | ||||
| """Handle interpolation between LCR and SO100 robot configurations.""" | |||||
| # Handle dynamic nodes, ask for the name of the node in the dataflow | # Handle dynamic nodes, ask for the name of the node in the dataflow | ||||
| parser = argparse.ArgumentParser( | parser = argparse.ArgumentParser( | ||||
| description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " | description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " | ||||
| "LCR followers knowing a Leader position and Follower position." | |||||
| "LCR followers knowing a Leader position and Follower position.", | |||||
| ) | ) | ||||
| parser.add_argument( | parser.add_argument( | ||||
| @@ -48,19 +49,19 @@ def main(): | |||||
| if not os.environ.get("LEADER_CONTROL") and args.leader_control is None: | if not os.environ.get("LEADER_CONTROL") and args.leader_control is None: | ||||
| raise ValueError( | raise ValueError( | ||||
| "The leader control is not set. Please set the configuration of the leader in the environment variables or " | "The leader control is not set. Please set the configuration of the leader in the environment variables or " | ||||
| "as an argument." | |||||
| "as an argument.", | |||||
| ) | ) | ||||
| if not os.environ.get("FOLLOWER_CONTROL") and args.follower_control is None: | if not os.environ.get("FOLLOWER_CONTROL") and args.follower_control is None: | ||||
| raise ValueError( | raise ValueError( | ||||
| "The follower control is not set. Please set the configuration of the follower in the environment " | "The follower control is not set. Please set the configuration of the follower in the environment " | ||||
| "variables or as an argument." | |||||
| "variables or as an argument.", | |||||
| ) | ) | ||||
| with open( | with open( | ||||
| os.environ.get("LEADER_CONTROL") | os.environ.get("LEADER_CONTROL") | ||||
| if args.leader_control is None | if args.leader_control is None | ||||
| else args.leader_control | |||||
| else args.leader_control, | |||||
| ) as file: | ) as file: | ||||
| leader_control = json.load(file) | leader_control = json.load(file) | ||||
| load_control_table_from_json_conversion_tables(leader_control, leader_control) | load_control_table_from_json_conversion_tables(leader_control, leader_control) | ||||
| @@ -68,11 +69,11 @@ def main(): | |||||
| with open( | with open( | ||||
| os.environ.get("FOLLOWER_CONTROL") | os.environ.get("FOLLOWER_CONTROL") | ||||
| if args.follower_control is None | if args.follower_control is None | ||||
| else args.follower_control | |||||
| else args.follower_control, | |||||
| ) as file: | ) as file: | ||||
| follower_control = json.load(file) | follower_control = json.load(file) | ||||
| load_control_table_from_json_conversion_tables( | load_control_table_from_json_conversion_tables( | ||||
| follower_control, follower_control | |||||
| follower_control, follower_control, | |||||
| ) | ) | ||||
| initial_mask = [ | initial_mask = [ | ||||
| @@ -132,7 +133,7 @@ def main(): | |||||
| ) | ) | ||||
| pwm_goal = logical_to_pwm_with_offset_arrow( | pwm_goal = logical_to_pwm_with_offset_arrow( | ||||
| follower_position, logical_goal, follower_control | |||||
| follower_position, logical_goal, follower_control, | |||||
| ) | ) | ||||
| node.send_output("follower_goal", pwm_goal, event["metadata"]) | node.send_output("follower_goal", pwm_goal, event["metadata"]) | ||||
| @@ -1,23 +1,28 @@ | |||||
| import os | |||||
| """Module for recording and interpolating between LCR and SO100 configurations. | |||||
| This module provides functionality for recording robot configurations and | |||||
| interpolating between LCR and SO100 robots to create smooth motion sequences. | |||||
| """ | |||||
| import argparse | import argparse | ||||
| import json | import json | ||||
| import os | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| import pyarrow.compute as pc | import pyarrow.compute as pc | ||||
| from dora import Node | from dora import Node | ||||
| from pwm_position_control.load import load_control_table_from_json_conversion_tables | from pwm_position_control.load import load_control_table_from_json_conversion_tables | ||||
| from pwm_position_control.transform import ( | from pwm_position_control.transform import ( | ||||
| wrap_joints_and_values, | |||||
| pwm_to_logical_arrow, | pwm_to_logical_arrow, | ||||
| wrap_joints_and_values, | |||||
| ) | ) | ||||
| def main(): | def main(): | ||||
| """Handle interpolation and recording between LCR and SO100 configurations.""" | |||||
| parser = argparse.ArgumentParser( | parser = argparse.ArgumentParser( | ||||
| description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " | description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " | ||||
| "LCR followers knowing a Leader position and Follower position." | |||||
| "LCR followers knowing a Leader position and Follower position.", | |||||
| ) | ) | ||||
| parser.add_argument( | parser.add_argument( | ||||
| @@ -45,19 +50,19 @@ def main(): | |||||
| if not os.environ.get("LEADER_CONTROL") and args.leader_control is None: | if not os.environ.get("LEADER_CONTROL") and args.leader_control is None: | ||||
| raise ValueError( | raise ValueError( | ||||
| "The leader control is not set. Please set the configuration of the leader in the environment variables or " | "The leader control is not set. Please set the configuration of the leader in the environment variables or " | ||||
| "as an argument." | |||||
| "as an argument.", | |||||
| ) | ) | ||||
| if not os.environ.get("FOLLOWER_CONTROL") and args.follower_control is None: | if not os.environ.get("FOLLOWER_CONTROL") and args.follower_control is None: | ||||
| raise ValueError( | raise ValueError( | ||||
| "The follower control is not set. Please set the configuration of the follower in the environment " | "The follower control is not set. Please set the configuration of the follower in the environment " | ||||
| "variables or as an argument." | |||||
| "variables or as an argument.", | |||||
| ) | ) | ||||
| with open( | with open( | ||||
| os.environ.get("LEADER_CONTROL") | os.environ.get("LEADER_CONTROL") | ||||
| if args.leader_control is None | if args.leader_control is None | ||||
| else args.leader_control | |||||
| else args.leader_control, | |||||
| ) as file: | ) as file: | ||||
| leader_control = json.load(file) | leader_control = json.load(file) | ||||
| load_control_table_from_json_conversion_tables(leader_control, leader_control) | load_control_table_from_json_conversion_tables(leader_control, leader_control) | ||||
| @@ -65,11 +70,11 @@ def main(): | |||||
| with open( | with open( | ||||
| os.environ.get("FOLLOWER_CONTROL") | os.environ.get("FOLLOWER_CONTROL") | ||||
| if args.follower_control is None | if args.follower_control is None | ||||
| else args.follower_control | |||||
| else args.follower_control, | |||||
| ) as file: | ) as file: | ||||
| follower_control = json.load(file) | follower_control = json.load(file) | ||||
| load_control_table_from_json_conversion_tables( | load_control_table_from_json_conversion_tables( | ||||
| follower_control, follower_control | |||||
| follower_control, follower_control, | |||||
| ) | ) | ||||
| node = Node(args.name) | node = Node(args.name) | ||||
| @@ -98,11 +103,11 @@ def main(): | |||||
| follower_position = event["value"] | follower_position = event["value"] | ||||
| follower_position = pwm_to_logical_arrow( | follower_position = pwm_to_logical_arrow( | ||||
| follower_position, follower_control | |||||
| follower_position, follower_control, | |||||
| ) | ) | ||||
| node.send_output( | node.send_output( | ||||
| "logical_position", follower_position, event["metadata"] | |||||
| "logical_position", follower_position, event["metadata"], | |||||
| ) | ) | ||||
| elif event_type == "ERROR": | elif event_type == "ERROR": | ||||
| @@ -1,20 +1,24 @@ | |||||
| import os | |||||
| """Module for interpolating between replay and SO100 robot configurations. | |||||
| This module provides functionality for interpolating between recorded robot actions | |||||
| and SO100 robot configurations to create smooth motion sequences. | |||||
| """ | |||||
| import argparse | import argparse | ||||
| import json | import json | ||||
| import pyarrow as pa | |||||
| import os | |||||
| from dora import Node | from dora import Node | ||||
| from pwm_position_control.load import load_control_table_from_json_conversion_tables | from pwm_position_control.load import load_control_table_from_json_conversion_tables | ||||
| from pwm_position_control.transform import logical_to_pwm_with_offset_arrow | from pwm_position_control.transform import logical_to_pwm_with_offset_arrow | ||||
| def main(): | def main(): | ||||
| """Handle replay interpolation for SO100 robot configurations.""" | |||||
| # Handle dynamic nodes, ask for the name of the node in the dataflow | # Handle dynamic nodes, ask for the name of the node in the dataflow | ||||
| parser = argparse.ArgumentParser( | parser = argparse.ArgumentParser( | ||||
| description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " | description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " | ||||
| "LCR followers knowing a Leader position and Follower position." | |||||
| "LCR followers knowing a Leader position and Follower position.", | |||||
| ) | ) | ||||
| parser.add_argument( | parser.add_argument( | ||||
| @@ -36,17 +40,17 @@ def main(): | |||||
| if not os.environ.get("FOLLOWER_CONTROL") and args.follower_control is None: | if not os.environ.get("FOLLOWER_CONTROL") and args.follower_control is None: | ||||
| raise ValueError( | raise ValueError( | ||||
| "The follower control is not set. Please set the configuration of the follower in the environment " | "The follower control is not set. Please set the configuration of the follower in the environment " | ||||
| "variables or as an argument." | |||||
| "variables or as an argument.", | |||||
| ) | ) | ||||
| with open( | with open( | ||||
| os.environ.get("FOLLOWER_CONTROL") | os.environ.get("FOLLOWER_CONTROL") | ||||
| if args.follower_control is None | if args.follower_control is None | ||||
| else args.follower_control | |||||
| else args.follower_control, | |||||
| ) as file: | ) as file: | ||||
| follower_control = json.load(file) | follower_control = json.load(file) | ||||
| load_control_table_from_json_conversion_tables( | load_control_table_from_json_conversion_tables( | ||||
| follower_control, follower_control | |||||
| follower_control, follower_control, | |||||
| ) | ) | ||||
| node = Node(args.name) | node = Node(args.name) | ||||
| @@ -68,7 +72,7 @@ def main(): | |||||
| continue | continue | ||||
| physical_goal = logical_to_pwm_with_offset_arrow( | physical_goal = logical_to_pwm_with_offset_arrow( | ||||
| follower_position, leader_position, follower_control | |||||
| follower_position, leader_position, follower_control, | |||||
| ) | ) | ||||
| node.send_output("follower_goal", physical_goal, event["metadata"]) | node.send_output("follower_goal", physical_goal, event["metadata"]) | ||||
| @@ -22,5 +22,6 @@ dora-argotranslate = "dora_argotranslate.main:main" | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP", | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -239,5 +239,5 @@ def main(): | |||||
| if text.strip() == "" or text.strip() == ".": | if text.strip() == "" or text.strip() == ".": | ||||
| continue | continue | ||||
| node.send_output( | node.send_output( | ||||
| "text", pa.array([text]), {"language": TARGET_LANGUAGE} | |||||
| "text", pa.array([text]), {"language": TARGET_LANGUAGE}, | |||||
| ) | ) | ||||
| @@ -29,6 +29,7 @@ dora-distil-whisper = "dora_distil_whisper.main:main" | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | |||||
| "UP", | |||||
| "D", # pydocstyle | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -21,5 +21,6 @@ dora-echo = "dora_echo.main:main" | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP" | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -34,5 +34,6 @@ dora-internvl = "dora_internvl.main:main" | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP", | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -18,5 +18,6 @@ dora-ios-lidar = "dora_ios_lidar.main:main" | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP", | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -27,5 +27,6 @@ dora-keyboard = "dora_keyboard.main:main" | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP", | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -19,5 +19,6 @@ features = ["python", "pyo3/extension-module"] | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP", | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -18,5 +18,6 @@ dora-kokoro-tts = "dora_kokoro_tts.main:main" | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP", | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -2,6 +2,5 @@ | |||||
| from .main import main | from .main import main | ||||
| if __name__ == "__main__": | if __name__ == "__main__": | ||||
| main() | main() | ||||
| @@ -1,10 +1,11 @@ | |||||
| """TODO: Add docstring.""" | """TODO: Add docstring.""" | ||||
| import logging | |||||
| import os | import os | ||||
| from pathlib import Path | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from dora import Node | from dora import Node | ||||
| from pathlib import Path | |||||
| import logging | |||||
| # Configure logging | # Configure logging | ||||
| logging.basicConfig(level=logging.INFO) | logging.basicConfig(level=logging.INFO) | ||||
| @@ -24,7 +25,7 @@ CONTEXT_SIZE = int(os.getenv("CONTEXT_SIZE", "4096")) | |||||
| def get_model(): | def get_model(): | ||||
| """Load a GGUF model using llama-cpp-python with optional GPU acceleration.""" | """Load a GGUF model using llama-cpp-python with optional GPU acceleration.""" | ||||
| from llama_cpp import Llama | from llama_cpp import Llama | ||||
| try: | try: | ||||
| # Check if path exists locally | # Check if path exists locally | ||||
| model_path = Path(MODEL_NAME_OR_PATH) | model_path = Path(MODEL_NAME_OR_PATH) | ||||
| @@ -35,7 +36,7 @@ def get_model(): | |||||
| n_gpu_layers=N_GPU_LAYERS, | n_gpu_layers=N_GPU_LAYERS, | ||||
| n_ctx=CONTEXT_SIZE, | n_ctx=CONTEXT_SIZE, | ||||
| n_threads=N_THREADS, | n_threads=N_THREADS, | ||||
| verbose=False | |||||
| verbose=False, | |||||
| ) | ) | ||||
| else: | else: | ||||
| # Load from HuggingFace | # Load from HuggingFace | ||||
| @@ -46,14 +47,14 @@ def get_model(): | |||||
| n_gpu_layers=N_GPU_LAYERS, | n_gpu_layers=N_GPU_LAYERS, | ||||
| n_ctx=CONTEXT_SIZE, | n_ctx=CONTEXT_SIZE, | ||||
| n_threads=N_THREADS, | n_threads=N_THREADS, | ||||
| verbose=False | |||||
| verbose=False, | |||||
| ) | ) | ||||
| logging.info("Model loaded successfully") | logging.info("Model loaded successfully") | ||||
| return llm | return llm | ||||
| except Exception as e: | except Exception as e: | ||||
| logging.error(f"Error loading model: {e}") | |||||
| logging.exception(f"Error loading model: {e}") | |||||
| raise | raise | ||||
| @@ -79,9 +80,9 @@ def main(): | |||||
| max_tokens=MAX_TOKENS, | max_tokens=MAX_TOKENS, | ||||
| stop=["Q:", "\n"], | stop=["Q:", "\n"], | ||||
| )["choices"][0]["text"] | )["choices"][0]["text"] | ||||
| node.send_output( | node.send_output( | ||||
| output_id="text", data=pa.array([response]), metadata={} | |||||
| output_id="text", data=pa.array([response]), metadata={}, | |||||
| ) | ) | ||||
| @@ -43,5 +43,6 @@ dora-llama-cpp-python = "dora_llama_cpp_python.main:main" | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP" | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -16,4 +16,4 @@ except FileNotFoundError: | |||||
| submodule_path = Path(__file__).resolve().parent / "Magma" | submodule_path = Path(__file__).resolve().parent / "Magma" | ||||
| sys.path.insert(0, str(submodule_path)) | |||||
| sys.path.insert(0, str(submodule_path)) | |||||
| @@ -27,7 +27,7 @@ def load_magma_models(): | |||||
| DEFAULT_PATH = str(magma_dir.parent) | DEFAULT_PATH = str(magma_dir.parent) | ||||
| if not os.path.exists(DEFAULT_PATH): | if not os.path.exists(DEFAULT_PATH): | ||||
| logger.warning( | logger.warning( | ||||
| "Warning: Magma submodule not found, falling back to HuggingFace version" | |||||
| "Warning: Magma submodule not found, falling back to HuggingFace version", | |||||
| ) | ) | ||||
| DEFAULT_PATH = "microsoft/Magma-8B" | DEFAULT_PATH = "microsoft/Magma-8B" | ||||
| @@ -42,7 +42,7 @@ def load_magma_models(): | |||||
| device_map="auto", | device_map="auto", | ||||
| ) | ) | ||||
| processor = AutoProcessor.from_pretrained( | processor = AutoProcessor.from_pretrained( | ||||
| MODEL_NAME_OR_PATH, trust_remote_code=True | |||||
| MODEL_NAME_OR_PATH, trust_remote_code=True, | |||||
| ) | ) | ||||
| except Exception as e: | except Exception as e: | ||||
| logger.error(f"Failed to load model: {e}") | logger.error(f"Failed to load model: {e}") | ||||
| @@ -72,7 +72,7 @@ def generate( | |||||
| ] | ] | ||||
| prompt = processor.tokenizer.apply_chat_template( | prompt = processor.tokenizer.apply_chat_template( | ||||
| convs, tokenize=False, add_generation_prompt=True | |||||
| convs, tokenize=False, add_generation_prompt=True, | |||||
| ) | ) | ||||
| try: | try: | ||||
| @@ -102,7 +102,7 @@ def generate( | |||||
| # Parse the trajectories using the same approach as in `https://github.com/microsoft/Magma/blob/main/agents/robot_traj/app.py` | # Parse the trajectories using the same approach as in `https://github.com/microsoft/Magma/blob/main/agents/robot_traj/app.py` | ||||
| traces_dict = ast.literal_eval( | traces_dict = ast.literal_eval( | ||||
| "{" + traces_str.strip().replace("\n\n", ",") + "}" | |||||
| "{" + traces_str.strip().replace("\n\n", ",") + "}", | |||||
| ) | ) | ||||
| for mark_id, trace in traces_dict.items(): | for mark_id, trace in traces_dict.items(): | ||||
| trajectories[mark_id] = ast.literal_eval(trace) | trajectories[mark_id] = ast.literal_eval(trace) | ||||
| @@ -153,7 +153,7 @@ def main(): | |||||
| frame = cv2.imdecode(storage, cv2.IMREAD_COLOR) | frame = cv2.imdecode(storage, cv2.IMREAD_COLOR) | ||||
| if frame is None: | if frame is None: | ||||
| raise ValueError( | raise ValueError( | ||||
| f"Failed to decode image with encoding {encoding}" | |||||
| f"Failed to decode image with encoding {encoding}", | |||||
| ) | ) | ||||
| frame = frame[:, :, ::-1] # Convert BGR to RGB | frame = frame[:, :, ::-1] # Convert BGR to RGB | ||||
| else: | else: | ||||
| @@ -181,7 +181,7 @@ def main(): | |||||
| image = frames[image_id] | image = frames[image_id] | ||||
| response, trajectories = generate(image, task_description) | response, trajectories = generate(image, task_description) | ||||
| node.send_output( | node.send_output( | ||||
| "text", pa.array([response]), {"image_id": image_id} | |||||
| "text", pa.array([response]), {"image_id": image_id}, | |||||
| ) | ) | ||||
| # Send trajectory data if available | # Send trajectory data if available | ||||
| @@ -43,5 +43,6 @@ extend.exclude = "dora_magma/Magma" | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP", | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -3,4 +3,4 @@ | |||||
| def test_import_main(): | def test_import_main(): | ||||
| """TODO: Add docstring.""" | """TODO: Add docstring.""" | ||||
| pass # Model is too big for the CI/CD | |||||
| # Model is too big for the CI/CD | |||||
| @@ -27,5 +27,6 @@ dora-microphone = "dora_microphone.main:main" | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP", | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -18,5 +18,6 @@ features = ["python", "pyo3/extension-module"] | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP", | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -30,6 +30,6 @@ dora-openai-server = "dora_openai_server.main:main" | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP" | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -2,4 +2,3 @@ | |||||
| def test_import_main(): | def test_import_main(): | ||||
| """TODO: Add docstring.""" | """TODO: Add docstring.""" | ||||
| pass | |||||
| @@ -31,5 +31,6 @@ dora-opus = "dora_opus.main:main" | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP", | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -3,6 +3,7 @@ | |||||
| import os | import os | ||||
| import pytest | import pytest | ||||
| from dora_outtetts.main import load_interface, main | from dora_outtetts.main import load_interface, main | ||||
| CI = os.getenv("CI", "false") in ["True", "true"] | CI = os.getenv("CI", "false") in ["True", "true"] | ||||
| @@ -31,5 +31,6 @@ dora-outtetts = "dora_outtetts.main:main" | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP", | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -33,5 +33,6 @@ dora-parler = "dora_parler.main:main" | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP", | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -27,7 +27,7 @@ else: | |||||
| MODEL_PATH = "microsoft/Phi-4-multimodal-instruct" | MODEL_PATH = "microsoft/Phi-4-multimodal-instruct" | ||||
| processor = AutoProcessor.from_pretrained( | processor = AutoProcessor.from_pretrained( | ||||
| MODEL_PATH, trust_remote_code=True, use_fast=True | |||||
| MODEL_PATH, trust_remote_code=True, use_fast=True, | |||||
| ) | ) | ||||
| # Define model config | # Define model config | ||||
| @@ -42,12 +42,12 @@ MODEL_CONFIG = { | |||||
| # Infer device map without full initialization | # Infer device map without full initialization | ||||
| device_map = infer_auto_device_map( | device_map = infer_auto_device_map( | ||||
| AutoModelForCausalLM.from_pretrained(MODEL_PATH, **MODEL_CONFIG) | |||||
| AutoModelForCausalLM.from_pretrained(MODEL_PATH, **MODEL_CONFIG), | |||||
| ) | ) | ||||
| # Load the model directly with the inferred device map | # Load the model directly with the inferred device map | ||||
| model = AutoModelForCausalLM.from_pretrained( | model = AutoModelForCausalLM.from_pretrained( | ||||
| MODEL_PATH, **MODEL_CONFIG, device_map=device_map | |||||
| MODEL_PATH, **MODEL_CONFIG, device_map=device_map, | |||||
| ) | ) | ||||
| generation_config = GenerationConfig.from_pretrained(MODEL_PATH) | generation_config = GenerationConfig.from_pretrained(MODEL_PATH) | ||||
| @@ -34,5 +34,6 @@ dora-phi4 = "dora_phi4.main:main" | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP" | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -18,5 +18,6 @@ dora-piper = "dora_piper.main:main" | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP", | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -23,5 +23,6 @@ dora-pyaudio = "dora_pyaudio.main:main" | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP", | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -21,5 +21,6 @@ dora-pyorbbecksdk = "dora_pyorbbecksdk.main:main" | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP", | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -24,5 +24,6 @@ dora-pyrealsense = "dora_pyrealsense.main:main" | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP", | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -45,5 +45,6 @@ dora-qwen = "dora_qwen.main:main" | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP", | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -51,5 +51,6 @@ build-backend = "setuptools.build_meta" | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP", | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -38,5 +38,6 @@ dora-qwenvl = "dora_qwenvl.main:main" | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP", | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -48,5 +48,6 @@ build-backend = "poetry.core.masonry.api" | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP", | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -16,7 +16,7 @@ def r_arm_inverse_kinematics(reachy, pose, action) -> list: | |||||
| [0, 1, 0, pose[1] + action[1]], | [0, 1, 0, pose[1] + action[1]], | ||||
| [1, 0, 0, pose[2] + action[2]], | [1, 0, 0, pose[2] + action[2]], | ||||
| [0, 0, 0, 1], | [0, 0, 0, 1], | ||||
| ] | |||||
| ], | |||||
| ) | ) | ||||
| return reachy.r_arm.inverse_kinematics(A) | return reachy.r_arm.inverse_kinematics(A) | ||||
| @@ -70,7 +70,7 @@ def main(): | |||||
| default_pose = r_arm_inverse_kinematics(reachy, r_arm_pose, [0, 0, 0]) | default_pose = r_arm_inverse_kinematics(reachy, r_arm_pose, [0, 0, 0]) | ||||
| goto( | goto( | ||||
| {joint: pos for joint, pos in zip(reachy.r_arm.joints.values(), default_pose)}, | |||||
| {joint: pos for joint, pos in zip(reachy.r_arm.joints.values(), default_pose, strict=False)}, | |||||
| duration=3, | duration=3, | ||||
| ) | ) | ||||
| @@ -84,7 +84,7 @@ def main(): | |||||
| goto( | goto( | ||||
| { | { | ||||
| joint: pos | joint: pos | ||||
| for joint, pos in zip(reachy.r_arm.joints.values(), joint_pose) | |||||
| for joint, pos in zip(reachy.r_arm.joints.values(), joint_pose, strict=False) | |||||
| }, | }, | ||||
| duration=0.200, | duration=0.200, | ||||
| ) | ) | ||||
| @@ -119,7 +119,7 @@ def main(): | |||||
| { | { | ||||
| joint: pos | joint: pos | ||||
| for joint, pos in zip( | for joint, pos in zip( | ||||
| reachy.r_arm.joints.values(), default_pose | |||||
| reachy.r_arm.joints.values(), default_pose, strict=False, | |||||
| ) | ) | ||||
| }, | }, | ||||
| duration=3, | duration=3, | ||||
| @@ -1,9 +1,10 @@ | |||||
| """TODO: Add docstring.""" | """TODO: Add docstring.""" | ||||
| from reachy_sdk import ReachySDK | |||||
| import os | import os | ||||
| from dora import Node | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from dora import Node | |||||
| from reachy_sdk import ReachySDK | |||||
| def main(): | def main(): | ||||
| @@ -30,5 +30,6 @@ build-backend = "poetry.core.masonry.api" | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP" | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -5,7 +5,6 @@ | |||||
| def test_pass(): | def test_pass(): | ||||
| """TODO: Add docstring.""" | """TODO: Add docstring.""" | ||||
| pass | |||||
| # def test_import_main(): | # def test_import_main(): | ||||
| @@ -27,5 +27,6 @@ dora-reachy2-head = "dora_reachy2.head:main" | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP", | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -5,7 +5,6 @@ | |||||
| def test_pass(): | def test_pass(): | ||||
| """TODO: Add docstring.""" | """TODO: Add docstring.""" | ||||
| pass | |||||
| # def test_import_camera_main(): | # def test_import_camera_main(): | ||||
| @@ -22,5 +22,6 @@ features = ["python", "pyo3/extension-module"] | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP", | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -23,5 +23,6 @@ dora-sam2 = "dora_sam2.main:main" | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP", | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -1,3 +1,5 @@ | |||||
| """TODO: Add docstring.""" | |||||
| import os | import os | ||||
| # Define the path to the README file relative to the package directory | # Define the path to the README file relative to the package directory | ||||
| @@ -5,7 +7,7 @@ readme_path = os.path.join(os.path.dirname(os.path.dirname(__file__)), "README.m | |||||
| # Read the content of the README file | # Read the content of the README file | ||||
| try: | try: | ||||
| with open(readme_path, "r", encoding="utf-8") as f: | |||||
| with open(readme_path, encoding="utf-8") as f: | |||||
| __doc__ = f.read() | __doc__ = f.read() | ||||
| except FileNotFoundError: | except FileNotFoundError: | ||||
| __doc__ = "README file not found." | __doc__ = "README file not found." | ||||
| @@ -1,5 +1,6 @@ | |||||
| from .main import main | |||||
| """TODO: Add docstring.""" | |||||
| from .main import main | |||||
| if __name__ == "__main__": | if __name__ == "__main__": | ||||
| main() | main() | ||||
| @@ -1,9 +1,12 @@ | |||||
| """TODO: Add docstring.""" | |||||
| import logging | |||||
| import os | import os | ||||
| import pyarrow as pa | import pyarrow as pa | ||||
| import torch | |||||
| from dora import Node | from dora import Node | ||||
| from transformers import AutoModelForCausalLM, AutoTokenizer | from transformers import AutoModelForCausalLM, AutoTokenizer | ||||
| import logging | |||||
| import torch | |||||
| # Configure logging | # Configure logging | ||||
| logging.basicConfig(level=logging.INFO) | logging.basicConfig(level=logging.INFO) | ||||
| @@ -31,25 +34,24 @@ ACTIVATION_WORDS = os.getenv("ACTIVATION_WORDS", "what how who where you").split | |||||
| def load_model(): | def load_model(): | ||||
| """Load the transformer model and tokenizer.""" | """Load the transformer model and tokenizer.""" | ||||
| logging.info(f"Loading model {MODEL_NAME} on {DEVICE}") | logging.info(f"Loading model {MODEL_NAME} on {DEVICE}") | ||||
| # Memory efficient loading | # Memory efficient loading | ||||
| model_kwargs = { | model_kwargs = { | ||||
| "torch_dtype": TORCH_DTYPE, | "torch_dtype": TORCH_DTYPE, | ||||
| "device_map": DEVICE, | "device_map": DEVICE, | ||||
| } | } | ||||
| if ENABLE_MEMORY_EFFICIENT and DEVICE == "cuda": | if ENABLE_MEMORY_EFFICIENT and DEVICE == "cuda": | ||||
| model_kwargs.update({ | model_kwargs.update({ | ||||
| "low_cpu_mem_usage": True, | "low_cpu_mem_usage": True, | ||||
| "offload_folder": "offload", | "offload_folder": "offload", | ||||
| "load_in_8bit": True | |||||
| "load_in_8bit": True, | |||||
| }) | }) | ||||
| model = AutoModelForCausalLM.from_pretrained( | model = AutoModelForCausalLM.from_pretrained( | ||||
| MODEL_NAME, | MODEL_NAME, | ||||
| **model_kwargs | |||||
| **model_kwargs, | |||||
| ) | ) | ||||
| tokenizer = AutoTokenizer.from_pretrained(MODEL_NAME) | tokenizer = AutoTokenizer.from_pretrained(MODEL_NAME) | ||||
| logging.info("Model loaded successfully") | logging.info("Model loaded successfully") | ||||
| @@ -60,40 +62,41 @@ def load_model(): | |||||
| def generate_response(model, tokenizer, text: str, history) -> tuple[str, list]: | def generate_response(model, tokenizer, text: str, history) -> tuple[str, list]: | ||||
| """Generate text using the transformer model.""" | """Generate text using the transformer model.""" | ||||
| history += [{"role": "user", "content": text}] | history += [{"role": "user", "content": text}] | ||||
| prompt = tokenizer.apply_chat_template( | prompt = tokenizer.apply_chat_template( | ||||
| history, tokenize=False, add_generation_prompt=True | |||||
| history, tokenize=False, add_generation_prompt=True, | |||||
| ) | ) | ||||
| model_inputs = tokenizer([prompt], return_tensors="pt").to(DEVICE) | model_inputs = tokenizer([prompt], return_tensors="pt").to(DEVICE) | ||||
| with torch.inference_mode(): | with torch.inference_mode(): | ||||
| generated_ids = model.generate( | generated_ids = model.generate( | ||||
| **model_inputs, | |||||
| **model_inputs, | |||||
| max_new_tokens=MAX_TOKENS, | max_new_tokens=MAX_TOKENS, | ||||
| pad_token_id=tokenizer.pad_token_id, | pad_token_id=tokenizer.pad_token_id, | ||||
| do_sample=True, | do_sample=True, | ||||
| temperature=0.7, | temperature=0.7, | ||||
| top_p=0.9, | |||||
| top_p=0.9, | |||||
| repetition_penalty=1.2, | repetition_penalty=1.2, | ||||
| length_penalty=0.5, | length_penalty=0.5, | ||||
| ) | ) | ||||
| generated_ids = [ | generated_ids = [ | ||||
| output_ids[len(input_ids):] | output_ids[len(input_ids):] | ||||
| for input_ids, output_ids in zip(model_inputs.input_ids, generated_ids) | for input_ids, output_ids in zip(model_inputs.input_ids, generated_ids) | ||||
| ] | ] | ||||
| response = tokenizer.batch_decode(generated_ids, skip_special_tokens=True)[0] | response = tokenizer.batch_decode(generated_ids, skip_special_tokens=True)[0] | ||||
| history += [{"role": "assistant", "content": response}] | history += [{"role": "assistant", "content": response}] | ||||
| # Clear CUDA cache after successful generation if enabled | # Clear CUDA cache after successful generation if enabled | ||||
| if ENABLE_MEMORY_EFFICIENT and DEVICE == "cuda": | if ENABLE_MEMORY_EFFICIENT and DEVICE == "cuda": | ||||
| torch.cuda.empty_cache() | torch.cuda.empty_cache() | ||||
| return response, history | return response, history | ||||
| def main(): | def main(): | ||||
| """TODO: Add docstring.""" | |||||
| # Initialize model and conversation history | # Initialize model and conversation history | ||||
| model, tokenizer = load_model() | model, tokenizer = load_model() | ||||
| # Initialize history with system prompt | # Initialize history with system prompt | ||||
| @@ -109,11 +112,11 @@ def main(): | |||||
| logging.info(f"Processing input: {text}") | logging.info(f"Processing input: {text}") | ||||
| response, history = generate_response(model, tokenizer, text, history) | response, history = generate_response(model, tokenizer, text, history) | ||||
| logging.info(f"Generated response: {response}") | logging.info(f"Generated response: {response}") | ||||
| node.send_output( | node.send_output( | ||||
| output_id="text", | |||||
| data=pa.array([response]), | |||||
| metadata={} | |||||
| output_id="text", | |||||
| data=pa.array([response]), | |||||
| metadata={}, | |||||
| ) | ) | ||||
| if __name__ == "__main__": | if __name__ == "__main__": | ||||
| @@ -24,3 +24,11 @@ dev = ["pytest >=8.1.1", "ruff >=0.9.1"] | |||||
| [project.scripts] | [project.scripts] | ||||
| dora-transformer = "dora_transformer.main:main" | dora-transformer = "dora_transformer.main:main" | ||||
| [tool.ruff.lint] | |||||
| extend-select = [ | |||||
| "D", # pydocstyle | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | |||||
| @@ -1,7 +1,10 @@ | |||||
| """TODO: Add docstring.""" | |||||
| import pytest | import pytest | ||||
| def test_import_main(): | def test_import_main(): | ||||
| """TODO: Add docstring.""" | |||||
| from dora_transformers.main import main | from dora_transformers.main import main | ||||
| # Check that everything is working, and catch dora Runtime Exception as we're not running in a dora dataflow. | # Check that everything is working, and catch dora Runtime Exception as we're not running in a dora dataflow. | ||||
| @@ -18,5 +18,6 @@ dora-ugv = "dora_ugv.main:main" | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP", | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||
| @@ -18,5 +18,6 @@ dora-vad = "dora_vad.main:main" | |||||
| [tool.ruff.lint] | [tool.ruff.lint] | ||||
| extend-select = [ | extend-select = [ | ||||
| "D", # pydocstyle | "D", # pydocstyle | ||||
| "UP", | |||||
| "UP", # Ruff's UP rule | |||||
| "PERF" # Ruff's PERF rule | |||||
| ] | ] | ||||