| @@ -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,37 @@ 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 +74,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 +161,17 @@ 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 +202,16 @@ 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 +264,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 +276,19 @@ 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 +315,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 +332,90 @@ 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,4 @@ | |||||
| """ | |||||
| LCR Configuration Tool: This program is used to automatically configure the Low Cost Robot (LCR) for the user. | |||||
| """LCR Configuration Tool: This program is used to automatically configure the Low Cost Robot (LCR) for the user. | |||||
| The program will: | The program will: | ||||
| 1. Disable all torque motors of provided LCR. | 1. Disable all torque motors of provided LCR. | ||||
| @@ -14,21 +13,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( | ||||
| [ | [ | ||||
| @@ -56,26 +51,26 @@ def pause(): | |||||
| def configure_servos(bus: DynamixelBus): | def configure_servos(bus: DynamixelBus): | ||||
| 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(): | ||||
| 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 +90,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 +133,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 +175,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 +184,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,22 @@ | |||||
| import os | |||||
| 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(): | ||||
| 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 +44,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 +64,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 +128,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,21 @@ | |||||
| import os | |||||
| 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(): | ||||
| 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 +43,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 +63,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 +96,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,23 @@ | |||||
| import os | |||||
| 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(): | ||||
| 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 +39,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 +112,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,9 +1,8 @@ | |||||
| import os | |||||
| 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 | ||||
| @@ -11,7 +10,7 @@ from pwm_position_control.transform import logical_to_pwm_with_offset_arrow | |||||
| def main(): | def main(): | ||||
| 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 +32,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 +64,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,10 +1,12 @@ | |||||
| 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): | ||||
| @@ -87,7 +89,7 @@ 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}') | ||||
| @@ -101,24 +103,23 @@ 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): | ||||
| @@ -157,8 +158,7 @@ class Dynamixel: | |||||
| 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 | |||||
| """Sets the id of the dynamixel servo | |||||
| @param old_id: current id of the servo | @param old_id: current id of the servo | ||||
| @param new_id: new id | @param new_id: new id | ||||
| @param use_broadcast_id: set ids of all connected dynamixels if True. | @param use_broadcast_id: set ids of all connected dynamixels if True. | ||||
| @@ -170,21 +170,21 @@ class Dynamixel: | |||||
| 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): | ||||
| 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): | ||||
| 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 | ||||
| @@ -192,42 +192,42 @@ class Dynamixel: | |||||
| 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): | ||||
| 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): | ||||
| 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): | ||||
| 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): | ||||
| 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): | ||||
| 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): | ||||
| 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) | ||||
| @@ -243,7 +243,7 @@ class Dynamixel: | |||||
| def set_home_offset(self, motor_id: int, home_position: int): | def set_home_offset(self, motor_id: int, home_position: int): | ||||
| 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) | ||||
| @@ -265,7 +265,7 @@ 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) | ||||
| @@ -273,43 +273,40 @@ class Dynamixel: | |||||
| 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): | ||||
| @@ -1,16 +1,17 @@ | |||||
| import numpy as np | |||||
| from dynamixel import Dynamixel, OperatingMode, ReadAttribute | |||||
| 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): | ||||
| @@ -65,8 +66,7 @@ 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. | |||||
| """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 | :param tries: maximum number of tries to read the position | ||||
| :return: list of joint positions in range [0, 4096] | :return: list of joint positions in range [0, 4096] | ||||
| """ | """ | ||||
| @@ -74,8 +74,7 @@ class Robot: | |||||
| 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,8 +84,7 @@ class Robot: | |||||
| return positions | return positions | ||||
| def read_velocity(self): | def read_velocity(self): | ||||
| """ | |||||
| Reads the joint velocities of the robot. | |||||
| """Reads the joint velocities of the robot. | |||||
| :return: list of joint velocities, | :return: list of joint velocities, | ||||
| """ | """ | ||||
| self.velocity_reader.txRxPacket() | self.velocity_reader.txRxPacket() | ||||
| @@ -99,11 +97,9 @@ class Robot: | |||||
| return velocties | return velocties | ||||
| def set_goal_pos(self, action): | def set_goal_pos(self, action): | ||||
| """:param 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 +113,10 @@ class Robot: | |||||
| self.pos_writer.txPacket() | self.pos_writer.txPacket() | ||||
| def set_pwm(self, action): | def set_pwm(self, action): | ||||
| """ | |||||
| Sets the pwm values for the servos. | |||||
| """Sets the pwm values for the servos. | |||||
| :param action: list or numpy array of pwm values in range [0, 885] | :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,15 +128,13 @@ 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 | |||||
| """Sets a constant torque 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 | |||||
| """Limits the pwm values for the servos in for position control | |||||
| @param limit: 0 ~ 885 | @param limit: 0 ~ 885 | ||||
| @return: | @return: | ||||
| """ | """ | ||||
| @@ -1,11 +1,11 @@ | |||||
| from robot import Robot | |||||
| 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,7 @@ | |||||
| 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 +12,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 +21,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 +29,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 +73,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 +83,7 @@ for event in dora_node: | |||||
| { | { | ||||
| "name": "arm", | "name": "arm", | ||||
| "cmd": values[:6], | "cmd": values[:6], | ||||
| } | |||||
| ] | |||||
| }, | |||||
| ], | |||||
| ), | ), | ||||
| ) | ) | ||||
| @@ -1,12 +1,12 @@ | |||||
| import gymnasium as gym | |||||
| import time | |||||
| from pathlib import Path | |||||
| import gym_dora # noqa: F401 | import gym_dora # noqa: F401 | ||||
| import gymnasium as gym | |||||
| import pandas as pd | import pandas as pd | ||||
| import time | |||||
| from pathlib import Path | |||||
| 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() | ||||
| @@ -49,8 +49,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,7 @@ | |||||
| from pynput import keyboard | |||||
| from pynput.keyboard import Key, Events | |||||
| 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 +16,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 +24,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,12 @@ | |||||
| #!/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 +38,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,14 @@ | |||||
| from dora import DoraStatus | |||||
| import pylcs | |||||
| import os | |||||
| import pyarrow as pa | |||||
| from transformers import AutoModelForCausalLM, AutoTokenizer | |||||
| import torch | |||||
| 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 +39,16 @@ 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. | |||||
| """Extracts Python code blocks from the given text that are enclosed in triple backticks with a python language identifier. | |||||
| Parameters: | |||||
| Parameters | |||||
| ---------- | |||||
| - text: A string that may contain one or more Python code blocks. | - 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. | - 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 +57,22 @@ 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. | |||||
| """Removes the last line from a given string of Python code. | |||||
| Parameters: | |||||
| Parameters | |||||
| ---------- | |||||
| - python_code: A string representing Python source code. | - python_code: A string representing Python source code. | ||||
| Returns: | |||||
| Returns | |||||
| ------- | |||||
| - A string with the last line removed. | - 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,8 +81,7 @@ 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. | ||||
| """ | """ | ||||
| edit_distance = pylcs.edit_distance(source, target) | edit_distance = pylcs.edit_distance(source, target) | ||||
| @@ -90,8 +92,7 @@ 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, | |||||
| """Find the best match for the target_block within the source_code by searching line by line, | |||||
| considering blocks of varying lengths. | considering blocks of varying lengths. | ||||
| """ | """ | ||||
| source_lines = source_code.split("\n") | source_lines = source_code.split("\n") | ||||
| @@ -121,8 +122,7 @@ 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, considering variable block lengths. | |||||
| """ | """ | ||||
| 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,8 +132,7 @@ 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: | ||||
| @@ -155,14 +154,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) | ||||
| @@ -213,7 +212,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 +225,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,8 @@ | |||||
| 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,4 +1,3 @@ | |||||
| import pyarrow as pa | |||||
| from dora import DoraStatus | from dora import DoraStatus | ||||
| @@ -1,9 +1,10 @@ | |||||
| import pyrealsense2 as rs | |||||
| import numpy as np | |||||
| from dora import Node | |||||
| import pyarrow as pa | |||||
| 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,8 @@ | |||||
| from dora import Node | |||||
| import pandas as pd | |||||
| import pyarrow as pa | |||||
| 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,10 @@ | |||||
| #!/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 +24,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,10 @@ | |||||
| import pyarrow as pa | |||||
| import whisper | |||||
| from pynput import keyboard | |||||
| from pynput.keyboard import Key, Events | |||||
| from dora import Node | |||||
| 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") | ||||
| @@ -48,7 +44,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,8 @@ | |||||
| import pyrealsense2 as rs | |||||
| 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 +13,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,13 @@ | |||||
| from dora import Node | |||||
| import pyarrow as pa | |||||
| 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,5 @@ | |||||
| from dora import Node | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from dora import Node | |||||
| node = Node() | node = Node() | ||||
| @@ -1,5 +1,5 @@ | |||||
| from dora import Node | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from dora import Node | |||||
| node = Node() | node = Node() | ||||
| @@ -15,7 +15,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,5 +1,5 @@ | |||||
| from dora import Node | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from dora import Node | |||||
| node = Node() | node = Node() | ||||
| @@ -1,14 +1,12 @@ | |||||
| 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() | ||||
| @@ -1,8 +1,7 @@ | |||||
| from pynput import keyboard | |||||
| from pynput.keyboard import Key, Events | |||||
| 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 +16,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 +24,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,12 @@ | |||||
| #!/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 +38,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,8 @@ | |||||
| 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,4 @@ | |||||
| import argparse | |||||
| import os | |||||
| import time | import time | ||||
| from pathlib import Path | |||||
| # import h5py | # import h5py | ||||
| import numpy as np | import numpy as np | ||||
| @@ -83,7 +80,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 +94,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 +107,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,4 @@ | |||||
| import argparse | |||||
| import os | |||||
| 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 +18,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 +37,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,8 @@ | |||||
| from lerobot.common.datasets.lerobot_dataset import LeRobotDataset | |||||
| 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,14 +1,12 @@ | |||||
| 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() | ||||
| @@ -1,5 +1,5 @@ | |||||
| from dora import Node | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from dora import Node | |||||
| node = Node() | node = Node() | ||||
| @@ -1,8 +1,7 @@ | |||||
| from pynput import keyboard | |||||
| from pynput.keyboard import Key, Events | |||||
| 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 +16,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 +24,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,12 @@ | |||||
| #!/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 +38,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,8 @@ | |||||
| 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,4 @@ | |||||
| import argparse | |||||
| import os | |||||
| import time | import time | ||||
| from pathlib import Path | |||||
| # import h5py | # import h5py | ||||
| import numpy as np | import numpy as np | ||||
| @@ -83,7 +80,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 +94,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 +107,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,4 @@ | |||||
| import argparse | |||||
| import os | |||||
| 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 +18,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 +37,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,8 @@ | |||||
| from lerobot.common.datasets.lerobot_dataset import LeRobotDataset | |||||
| 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,5 @@ | |||||
| from dora import Node | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| import numpy as np | |||||
| from dora import Node | |||||
| node = Node() | node = Node() | ||||
| @@ -19,19 +18,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 +53,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 +64,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,18 @@ | |||||
| import enum | import enum | ||||
| import pyarrow as pa | |||||
| 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 | ||||
| @@ -93,13 +94,12 @@ MODEL_CONTROL_TABLE = { | |||||
| class FeetechBus: | class FeetechBus: | ||||
| def __init__(self, port: str, description: dict[str, (np.uint8, str)]): | def __init__(self, port: str, description: dict[str, (np.uint8, str)]): | ||||
| """ | |||||
| 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. | |||||
| """ | |||||
| """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. | |||||
| """ | |||||
| self.port = port | self.port = port | ||||
| self.descriptions = description | self.descriptions = description | ||||
| self.motor_ctrl = {} | self.motor_ctrl = {} | ||||
| @@ -183,7 +183,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,7 +195,7 @@ 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: | ||||
| @@ -225,13 +225,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 | ||||
| ], | ], | ||||
| @@ -1,5 +1,4 @@ | |||||
| """ | |||||
| SO100 Auto Configure: This program is used to automatically configure the SO-ARM100 (SO100) for the user. | |||||
| """SO100 Auto Configure: This program is used to automatically configure the SO-ARM100 (SO100) for the user. | |||||
| The program will: | The program will: | ||||
| 1. Disable all torque motors of provided SO100. | 1. Disable all torque motors of provided SO100. | ||||
| @@ -14,20 +13,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 +46,42 @@ 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(): | ||||
| """Main function to 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 +133,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 +152,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 +161,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,24 @@ | |||||
| import os | |||||
| 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(): | ||||
| """Main function to 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 +47,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 +67,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 +131,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,22 @@ | |||||
| import os | |||||
| 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(): | ||||
| """Main function to handle interpolation and recording between LCR and SO100 robot 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 +44,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 +64,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 +97,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,18 @@ | |||||
| import os | |||||
| 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(): | ||||
| """Main function to 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 +34,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 +66,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"]) | ||||
| @@ -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}, | |||||
| ) | ) | ||||
| @@ -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={}, | |||||
| ) | ) | ||||
| @@ -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 | ||||
| @@ -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 | |||||
| @@ -2,4 +2,3 @@ | |||||
| def test_import_main(): | def test_import_main(): | ||||
| """TODO: Add docstring.""" | """TODO: Add docstring.""" | ||||
| pass | |||||
| @@ -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"] | ||||
| @@ -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) | ||||
| @@ -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(): | ||||
| @@ -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(): | ||||
| @@ -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(): | ||||
| @@ -7,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." | ||||
| @@ -2,6 +2,5 @@ | |||||
| from .main import main | from .main import main | ||||
| if __name__ == "__main__": | if __name__ == "__main__": | ||||
| main() | main() | ||||
| @@ -1,11 +1,12 @@ | |||||
| """TODO: Add docstring.""" | """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) | ||||
| @@ -34,23 +35,23 @@ 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") | ||||
| @@ -61,37 +62,37 @@ 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(): | ||||
| @@ -111,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__": | ||||
| @@ -1,19 +1,20 @@ | |||||
| """TODO: Add docstring.""" | """TODO: Add docstring.""" | ||||
| import enum | import enum | ||||
| import pyarrow as pa | |||||
| 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 | ||||
| @@ -70,7 +71,7 @@ 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() | ||||
| @@ -248,7 +249,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: | ||||
| @@ -260,7 +261,7 @@ 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: | ||||
| @@ -291,13 +292,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 | ||||
| ], | ], | ||||
| @@ -3,13 +3,12 @@ | |||||
| It can be used to read positions, velocities, currents, and set goal positions and currents. | It can be used to read positions, velocities, currents, and set goal positions and currents. | ||||
| """ | """ | ||||
| import os | |||||
| import time | |||||
| import argparse | import argparse | ||||
| import json | import json | ||||
| import os | |||||
| import time | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from dora import Node | from dora import Node | ||||
| from .bus import DynamixelBus, TorqueMode, wrap_joints_and_values | from .bus import DynamixelBus, TorqueMode, wrap_joints_and_values | ||||
| @@ -76,7 +75,7 @@ class Client: | |||||
| wrap_joints_and_values( | wrap_joints_and_values( | ||||
| self.config["joints"], | self.config["joints"], | ||||
| [TorqueMode.DISABLED.value] * len(self.config["joints"]), | [TorqueMode.DISABLED.value] * len(self.config["joints"]), | ||||
| ) | |||||
| ), | |||||
| ) | ) | ||||
| def pull_position(self, node, metadata): | def pull_position(self, node, metadata): | ||||
| @@ -132,7 +131,7 @@ def main(): | |||||
| """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="Dynamixel Client: This node is used to represent a chain of dynamixel motors. It can be used to " | description="Dynamixel Client: This node is used to represent a chain of dynamixel motors. It can be used to " | ||||
| "read positions, velocities, currents, and set goal positions and currents." | |||||
| "read positions, velocities, currents, and set goal positions and currents.", | |||||
| ) | ) | ||||
| parser.add_argument( | parser.add_argument( | ||||
| @@ -162,7 +161,7 @@ def main(): | |||||
| if not os.environ.get("PORT") and args.port is None: | if not os.environ.get("PORT") and args.port is None: | ||||
| raise ValueError( | raise ValueError( | ||||
| "The port is not set. Please set the port of the dynamixel motors in the environment variables or as an " | "The port is not set. Please set the port of the dynamixel motors in the environment variables or as an " | ||||
| "argument." | |||||
| "argument.", | |||||
| ) | ) | ||||
| port = os.environ.get("PORT") if args.port is None else args.port | port = os.environ.get("PORT") if args.port is None else args.port | ||||
| @@ -171,7 +170,7 @@ def main(): | |||||
| if not os.environ.get("CONFIG") and args.config is None: | if not os.environ.get("CONFIG") and args.config is None: | ||||
| raise ValueError( | raise ValueError( | ||||
| "The configuration is not set. Please set the configuration of the dynamixel motors in the environment " | "The configuration is not set. Please set the configuration of the dynamixel motors in the environment " | ||||
| "variables or as an argument." | |||||
| "variables or as an argument.", | |||||
| ) | ) | ||||
| with open(os.environ.get("CONFIG") if args.config is None else args.config) as file: | with open(os.environ.get("CONFIG") if args.config is None else args.config) as file: | ||||
| @@ -203,7 +202,7 @@ def main(): | |||||
| "goal_current": wrap_joints_and_values( | "goal_current": wrap_joints_and_values( | ||||
| pa.array(config.keys(), pa.string()), | pa.array(config.keys(), pa.string()), | ||||
| pa.array( | pa.array( | ||||
| [config[joint]["goal_current"] for joint in joints], type=pa.uint32() | |||||
| [config[joint]["goal_current"] for joint in joints], type=pa.uint32(), | |||||
| ), | ), | ||||
| ), | ), | ||||
| "P": wrap_joints_and_values( | "P": wrap_joints_and_values( | ||||
| @@ -4,7 +4,6 @@ | |||||
| def test_pass(): | def test_pass(): | ||||
| """TODO: Add docstring.""" | """TODO: Add docstring.""" | ||||
| pass | |||||
| # def test_import_main(): | # def test_import_main(): | ||||
| @@ -1 +1 @@ | |||||
| """TODO: Add docstring.""" | |||||
| """TODO: Add docstring.""" | |||||
| @@ -194,7 +194,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: | ||||
| @@ -206,7 +206,7 @@ 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: | ||||
| @@ -237,13 +237,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 | ||||
| ], | ], | ||||
| @@ -1,11 +1,10 @@ | |||||
| """Feetech Client: This node is used to represent a chain of feetech motors. It can be used to read positions, velocities, currents, and set goal positions and currents.""" | """Feetech Client: This node is used to represent a chain of feetech motors. It can be used to read positions, velocities, currents, and set goal positions and currents.""" | ||||
| import os | |||||
| import argparse | import argparse | ||||
| import json | import json | ||||
| import os | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from dora import Node | from dora import Node | ||||
| from .bus import FeetechBus, TorqueMode, wrap_joints_and_values | from .bus import FeetechBus, TorqueMode, wrap_joints_and_values | ||||
| @@ -60,7 +59,7 @@ class Client: | |||||
| wrap_joints_and_values( | wrap_joints_and_values( | ||||
| self.config["joints"], | self.config["joints"], | ||||
| [TorqueMode.DISABLED.value] * len(self.config["joints"]), | [TorqueMode.DISABLED.value] * len(self.config["joints"]), | ||||
| ) | |||||
| ), | |||||
| ) | ) | ||||
| def pull_position(self, node, metadata): | def pull_position(self, node, metadata): | ||||
| @@ -110,7 +109,7 @@ def main(): | |||||
| parser = argparse.ArgumentParser( | parser = argparse.ArgumentParser( | ||||
| description="Feetech Client: This node is used to represent a chain of feetech motors. " | description="Feetech Client: This node is used to represent a chain of feetech motors. " | ||||
| "It can be used to read " | "It can be used to read " | ||||
| "positions, velocities, currents, and set goal positions and currents." | |||||
| "positions, velocities, currents, and set goal positions and currents.", | |||||
| ) | ) | ||||
| parser.add_argument( | parser.add_argument( | ||||
| @@ -140,7 +139,7 @@ def main(): | |||||
| if not os.environ.get("PORT") and args.port is None: | if not os.environ.get("PORT") and args.port is None: | ||||
| raise ValueError( | raise ValueError( | ||||
| "The port is not set. Please set the port of the feetech motors in the environment variables or as an " | "The port is not set. Please set the port of the feetech motors in the environment variables or as an " | ||||
| "argument." | |||||
| "argument.", | |||||
| ) | ) | ||||
| port = os.environ.get("PORT") if args.port is None else args.port | port = os.environ.get("PORT") if args.port is None else args.port | ||||
| @@ -149,7 +148,7 @@ def main(): | |||||
| if not os.environ.get("CONFIG") and args.config is None: | if not os.environ.get("CONFIG") and args.config is None: | ||||
| raise ValueError( | raise ValueError( | ||||
| "The configuration is not set. Please set the configuration of the feetech motors in the environment " | "The configuration is not set. Please set the configuration of the feetech motors in the environment " | ||||
| "variables or as an argument." | |||||
| "variables or as an argument.", | |||||
| ) | ) | ||||
| with open(os.environ.get("CONFIG") if args.config is None else args.config) as file: | with open(os.environ.get("CONFIG") if args.config is None else args.config) as file: | ||||
| @@ -4,7 +4,6 @@ | |||||
| def test_pass(): | def test_pass(): | ||||
| """TODO: Add docstring.""" | """TODO: Add docstring.""" | ||||
| pass | |||||
| # def test_import_main(): | # def test_import_main(): | ||||
| @@ -1,11 +1,12 @@ | |||||
| """TODO: Add docstring.""" | """TODO: Add docstring.""" | ||||
| import lebai_sdk | |||||
| import numpy as np | |||||
| from dora import Node | |||||
| import json | import json | ||||
| import os | import os | ||||
| import time | import time | ||||
| import lebai_sdk | |||||
| import numpy as np | |||||
| from dora import Node | |||||
| def load_json_file(file_path): | def load_json_file(file_path): | ||||
| """Load JSON file and return the dictionary.""" | """Load JSON file and return the dictionary.""" | ||||
| @@ -28,7 +29,7 @@ SAVED_POSE_PATH = "pose_library.json" | |||||
| lebai_sdk.init() | lebai_sdk.init() | ||||
| ROBOT_IP = os.getenv( | ROBOT_IP = os.getenv( | ||||
| "LEBAI_IP", "10.42.0.253" | |||||
| "LEBAI_IP", "10.42.0.253", | |||||
| ) # 设定机器人ip地址,需要根据机器人实际ip地址修改 | ) # 设定机器人ip地址,需要根据机器人实际ip地址修改 | ||||
| @@ -180,7 +181,7 @@ def main(): | |||||
| "duration": time.time() - start_time, | "duration": time.time() - start_time, | ||||
| "joint_position": joint_position, | "joint_position": joint_position, | ||||
| "t": t * 2 if t == 0.1 else t, | "t": t * 2 if t == 0.1 else t, | ||||
| } | |||||
| }, | |||||
| ] | ] | ||||
| start_time = time.time() | start_time = time.time() | ||||
| @@ -1,7 +1,6 @@ | |||||
| """TODO: Add docstring.""" | """TODO: Add docstring.""" | ||||
| import lebai_sdk | import lebai_sdk | ||||
| lebai_sdk.init() | lebai_sdk.init() | ||||
| @@ -26,7 +25,7 @@ def main(): | |||||
| r = 0 # 交融半径 (m)。用于指定路径的平滑效果 | r = 0 # 交融半径 (m)。用于指定路径的平滑效果 | ||||
| lebai.movel( | lebai.movel( | ||||
| cartesian_pose, a, v, t, r | |||||
| cartesian_pose, a, v, t, r, | |||||
| ) # 直线运动 https://help.lebai.ltd/sdk/motion.html#%E7%9B%B4%E7%BA%BF%E8%BF%90%E5%8A%A8 | ) # 直线运动 https://help.lebai.ltd/sdk/motion.html#%E7%9B%B4%E7%BA%BF%E8%BF%90%E5%8A%A8 | ||||
| lebai.wait_move() # 等待运动完成 | lebai.wait_move() # 等待运动完成 | ||||
| # scene_number = "10000" #需要调用的场景编号 | # scene_number = "10000" #需要调用的场景编号 | ||||
| @@ -4,7 +4,6 @@ | |||||
| def test_pass(): | def test_pass(): | ||||
| """TODO: Add docstring.""" | """TODO: Add docstring.""" | ||||
| pass | |||||
| # def test_import_main(): | # def test_import_main(): | ||||
| @@ -1 +1 @@ | |||||
| """TODO: Add docstring.""" | |||||
| """TODO: Add docstring.""" | |||||
| @@ -1,19 +1,17 @@ | |||||
| """Following Dora node is a minimalistic interface that shows two images and text in a Pygame window.""" | """Following Dora node is a minimalistic interface that shows two images and text in a Pygame window.""" | ||||
| import os | |||||
| import argparse | import argparse | ||||
| import pygame | |||||
| import os | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| import pygame | |||||
| from dora import Node | from dora import Node | ||||
| def main(): | def main(): | ||||
| """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="LeRobot Record: This node is used to record episodes of a robot interacting with the environment." | |||||
| description="LeRobot Record: This node is used to record episodes of a robot interacting with the environment.", | |||||
| ) | ) | ||||
| parser.add_argument( | parser.add_argument( | ||||
| @@ -66,7 +64,7 @@ def main(): | |||||
| if event_type == "STOP": | if event_type == "STOP": | ||||
| break | break | ||||
| elif event_type == "INPUT": | |||||
| if event_type == "INPUT": | |||||
| event_id = event["id"] | event_id = event["id"] | ||||
| if event_id == "image_left": | if event_id == "image_left": | ||||
| @@ -80,7 +78,7 @@ def main(): | |||||
| } | } | ||||
| image_left = pygame.image.frombuffer( | image_left = pygame.image.frombuffer( | ||||
| image["data"], (image["width"], image["height"]), "BGR" | |||||
| image["data"], (image["width"], image["height"]), "BGR", | |||||
| ) | ) | ||||
| elif event_id == "image_right": | elif event_id == "image_right": | ||||
| @@ -93,7 +91,7 @@ def main(): | |||||
| } | } | ||||
| image_right = pygame.image.frombuffer( | image_right = pygame.image.frombuffer( | ||||
| image["data"], (image["width"], image["height"]), "BGR" | |||||
| image["data"], (image["width"], image["height"]), "BGR", | |||||
| ) | ) | ||||
| elif event_id == "tick": | elif event_id == "tick": | ||||
| @@ -104,7 +102,7 @@ def main(): | |||||
| if pygame_event.type == pygame.QUIT: | if pygame_event.type == pygame.QUIT: | ||||
| running = False | running = False | ||||
| break | break | ||||
| elif pygame_event.type == pygame.KEYDOWN: | |||||
| if pygame_event.type == pygame.KEYDOWN: | |||||
| key = pygame.key.name(pygame_event.key) | key = pygame.key.name(pygame_event.key) | ||||
| if key == "space": | if key == "space": | ||||
| @@ -4,7 +4,6 @@ | |||||
| def test_pass(): | def test_pass(): | ||||
| """TODO: Add docstring.""" | """TODO: Add docstring.""" | ||||
| pass | |||||
| # def test_import_main(): | # def test_import_main(): | ||||
| @@ -1 +1 @@ | |||||
| """TODO: Add docstring.""" | |||||
| """TODO: Add docstring.""" | |||||
| @@ -1,16 +1,14 @@ | |||||
| """Mujoco Client: This node is used to represent simulated robot, it can be used to read virtual positions, or can be controlled.""" | """Mujoco Client: This node is used to represent simulated robot, it can be used to read virtual positions, or can be controlled.""" | ||||
| import os | |||||
| import argparse | import argparse | ||||
| import time | |||||
| import json | import json | ||||
| import pyarrow as pa | |||||
| from dora import Node | |||||
| import os | |||||
| import time | |||||
| import mujoco | import mujoco | ||||
| import mujoco.viewer | import mujoco.viewer | ||||
| import pyarrow as pa | |||||
| from dora import Node | |||||
| class Client: | class Client: | ||||
| @@ -66,22 +64,19 @@ class Client: | |||||
| elif event_type == "ERROR": | elif event_type == "ERROR": | ||||
| raise ValueError( | raise ValueError( | ||||
| "An error occurred in the dataflow: " + event["error"] | |||||
| "An error occurred in the dataflow: " + event["error"], | |||||
| ) | ) | ||||
| self.node.send_output("end", pa.array([])) | self.node.send_output("end", pa.array([])) | ||||
| def pull_position(self, node, metadata): | def pull_position(self, node, metadata): | ||||
| """TODO: Add docstring.""" | """TODO: Add docstring.""" | ||||
| pass | |||||
| def pull_velocity(self, node, metadata): | def pull_velocity(self, node, metadata): | ||||
| """TODO: Add docstring.""" | """TODO: Add docstring.""" | ||||
| pass | |||||
| def pull_current(self, node, metadata): | def pull_current(self, node, metadata): | ||||
| """TODO: Add docstring.""" | """TODO: Add docstring.""" | ||||
| pass | |||||
| def write_goal_position(self, goal_position_with_joints): | def write_goal_position(self, goal_position_with_joints): | ||||
| """TODO: Add docstring.""" | """TODO: Add docstring.""" | ||||
| @@ -96,7 +91,7 @@ def main(): | |||||
| """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="MujoCo Client: This node is used to represent a MuJoCo simulation. It can be used instead of a " | description="MujoCo Client: This node is used to represent a MuJoCo simulation. It can be used instead of a " | ||||
| "follower arm to test the dataflow." | |||||
| "follower arm to test the dataflow.", | |||||
| ) | ) | ||||
| parser.add_argument( | parser.add_argument( | ||||
| @@ -114,14 +109,14 @@ def main(): | |||||
| ) | ) | ||||
| parser.add_argument( | parser.add_argument( | ||||
| "--config", type=str, help="The configuration of the joints.", default=None | |||||
| "--config", type=str, help="The configuration of the joints.", default=None, | |||||
| ) | ) | ||||
| args = parser.parse_args() | args = parser.parse_args() | ||||
| if not os.getenv("SCENE") and args.scene is None: | if not os.getenv("SCENE") and args.scene is None: | ||||
| raise ValueError( | raise ValueError( | ||||
| "Please set the SCENE environment variable or pass the --scene argument." | |||||
| "Please set the SCENE environment variable or pass the --scene argument.", | |||||
| ) | ) | ||||
| scene = os.getenv("SCENE", args.scene) | scene = os.getenv("SCENE", args.scene) | ||||
| @@ -130,7 +125,7 @@ def main(): | |||||
| if not os.environ.get("CONFIG") and args.config is None: | if not os.environ.get("CONFIG") and args.config is None: | ||||
| raise ValueError( | raise ValueError( | ||||
| "The configuration is not set. Please set the configuration of the simulated motors in the environment " | "The configuration is not set. Please set the configuration of the simulated motors in the environment " | ||||
| "variables or as an argument." | |||||
| "variables or as an argument.", | |||||
| ) | ) | ||||
| with open(os.environ.get("CONFIG") if args.config is None else args.config) as file: | with open(os.environ.get("CONFIG") if args.config is None else args.config) as file: | ||||
| @@ -4,7 +4,6 @@ | |||||
| def test_pass(): | def test_pass(): | ||||
| """TODO: Add docstring.""" | """TODO: Add docstring.""" | ||||
| pass | |||||
| # def test_import_main(): | # def test_import_main(): | ||||
| @@ -1 +1 @@ | |||||
| """TODO: Add docstring.""" | |||||
| """TODO: Add docstring.""" | |||||
| @@ -3,12 +3,11 @@ | |||||
| reading a dataset of actions and joints from a specific episode. | reading a dataset of actions and joints from a specific episode. | ||||
| """ | """ | ||||
| import os | |||||
| import argparse | import argparse | ||||
| import os | |||||
| import pyarrow as pa | |||||
| import pandas as pd | import pandas as pd | ||||
| import pyarrow as pa | |||||
| from dora import Node | from dora import Node | ||||
| @@ -78,7 +77,7 @@ class Client: | |||||
| def main(): | def main(): | ||||
| """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="Replay Client: This node is used to replay a sequence of goals for a followee robot." | |||||
| description="Replay Client: This node is used to replay a sequence of goals for a followee robot.", | |||||
| ) | ) | ||||
| parser.add_argument( | parser.add_argument( | ||||
| @@ -4,7 +4,6 @@ | |||||
| def test_pass(): | def test_pass(): | ||||
| """TODO: Add docstring.""" | """TODO: Add docstring.""" | ||||
| pass | |||||
| # def test_import_main(): | # def test_import_main(): | ||||
| @@ -4,7 +4,6 @@ | |||||
| def test_pass(): | def test_pass(): | ||||
| """TODO: Add docstring.""" | """TODO: Add docstring.""" | ||||
| pass | |||||
| # def test_import_main(): | # def test_import_main(): | ||||
| @@ -1 +1 @@ | |||||
| """TODO: Doc String.""" | |||||
| """TODO: Doc String.""" | |||||
| @@ -1,13 +1,11 @@ | |||||
| """TODO : Doc String.""" | """TODO : Doc String.""" | ||||
| import argparse | |||||
| import os | import os | ||||
| from pathlib import Path | from pathlib import Path | ||||
| import cv2 | import cv2 | ||||
| import argparse | |||||
| import numpy as np | import numpy as np | ||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from dora import Node | from dora import Node | ||||
| from ffmpeg import FFmpeg | from ffmpeg import FFmpeg | ||||
| @@ -15,7 +13,7 @@ from ffmpeg import FFmpeg | |||||
| def main(): | def main(): | ||||
| """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="Video Encoder: This node is used to record episodes of a robot interacting with the environment." | |||||
| description="Video Encoder: This node is used to record episodes of a robot interacting with the environment.", | |||||
| ) | ) | ||||
| parser.add_argument( | parser.add_argument( | ||||
| @@ -77,8 +75,8 @@ def main(): | |||||
| { | { | ||||
| "path": f"videos/{name}", | "path": f"videos/{name}", | ||||
| "timestamp": float(frame_count) / fps, | "timestamp": float(frame_count) / fps, | ||||
| } | |||||
| ] | |||||
| }, | |||||
| ], | |||||
| ), | ), | ||||
| event["metadata"], | event["metadata"], | ||||
| ) | ) | ||||
| @@ -92,7 +90,7 @@ def main(): | |||||
| } | } | ||||
| data = image["data"].reshape( | data = image["data"].reshape( | ||||
| (image["height"], image["width"], image["channels"]) | |||||
| (image["height"], image["width"], image["channels"]), | |||||
| ) | ) | ||||
| path = str(out_dir / f"frame_{frame_count:06d}.png") | path = str(out_dir / f"frame_{frame_count:06d}.png") | ||||
| @@ -118,7 +116,7 @@ def main(): | |||||
| .option("y") | .option("y") | ||||
| .input(str(out_dir / "frame_%06d.png"), f="image2", r=fps) | .input(str(out_dir / "frame_%06d.png"), f="image2", r=fps) | ||||
| .output( | .output( | ||||
| str(video_path), vcodec="libx264", g=2, pix_fmt="yuv444p" | |||||
| str(video_path), vcodec="libx264", g=2, pix_fmt="yuv444p", | |||||
| ) | ) | ||||
| ) | ) | ||||