| @@ -58,6 +58,7 @@ def wrap_joints_and_values( | |||||
| struct_array = wrap_joints_and_values(joints, value) | 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) | ||||
| @@ -171,6 +172,7 @@ class DynamixelBus: | |||||
| description: A dictionary containing the description of the motors connected to the 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 | The keys are the motor names and the values are tuples containing the motor id | ||||
| and the motor model. | and the motor model. | ||||
| """ | """ | ||||
| self.port = port | self.port = port | ||||
| self.descriptions = description | self.descriptions = description | ||||
| @@ -211,6 +213,7 @@ class DynamixelBus: | |||||
| Args: | Args: | ||||
| data_name: Name of the parameter to write. | data_name: Name of the parameter to write. | ||||
| data: Structured array containing the data 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"] | ||||
| @@ -288,6 +291,7 @@ class DynamixelBus: | |||||
| Returns: | Returns: | ||||
| Structured array containing the read data. | 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 | ||||
| @@ -336,6 +340,7 @@ class DynamixelBus: | |||||
| Args: | Args: | ||||
| torque_mode: Structured array containing the torque mode for each servo. | torque_mode: Structured array containing the torque mode for each servo. | ||||
| """ | """ | ||||
| self.write("Torque_Enable", torque_mode) | self.write("Torque_Enable", torque_mode) | ||||
| @@ -344,6 +349,7 @@ class DynamixelBus: | |||||
| Args: | Args: | ||||
| operating_mode: Structured array containing the operating mode for each servo. | operating_mode: Structured array containing the operating mode for each servo. | ||||
| """ | """ | ||||
| self.write("Operating_Mode", operating_mode) | self.write("Operating_Mode", operating_mode) | ||||
| @@ -355,6 +361,7 @@ class DynamixelBus: | |||||
| Returns: | Returns: | ||||
| Structured array containing the current positions. | Structured array containing the current positions. | ||||
| """ | """ | ||||
| return self.read("Present_Position", motor_names) | return self.read("Present_Position", motor_names) | ||||
| @@ -366,6 +373,7 @@ class DynamixelBus: | |||||
| Returns: | Returns: | ||||
| Structured array containing the current velocities. | Structured array containing the current velocities. | ||||
| """ | """ | ||||
| return self.read("Present_Velocity", motor_names) | return self.read("Present_Velocity", motor_names) | ||||
| @@ -377,6 +385,7 @@ class DynamixelBus: | |||||
| Returns: | Returns: | ||||
| Structured array containing the current currents. | Structured array containing the current currents. | ||||
| """ | """ | ||||
| return self.read("Present_Current", motor_names) | return self.read("Present_Current", motor_names) | ||||
| @@ -385,6 +394,7 @@ class DynamixelBus: | |||||
| Args: | Args: | ||||
| goal_position: Structured array containing the goal positions. | goal_position: Structured array containing the goal positions. | ||||
| """ | """ | ||||
| self.write("Goal_Position", goal_position) | self.write("Goal_Position", goal_position) | ||||
| @@ -393,6 +403,7 @@ class DynamixelBus: | |||||
| Args: | Args: | ||||
| goal_current: Structured array containing the goal currents. | goal_current: Structured array containing the goal currents. | ||||
| """ | """ | ||||
| self.write("Goal_Current", goal_current) | self.write("Goal_Current", goal_current) | ||||
| @@ -401,6 +412,7 @@ class DynamixelBus: | |||||
| Args: | Args: | ||||
| position_p_gain: Structured array containing the position P gains. | 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) | ||||
| @@ -409,6 +421,7 @@ class DynamixelBus: | |||||
| Args: | Args: | ||||
| position_i_gain: Structured array containing the position I gains. | 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) | ||||
| @@ -417,5 +430,6 @@ class DynamixelBus: | |||||
| Args: | Args: | ||||
| position_d_gain: Structured array containing the position D gains. | 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,4 +1,7 @@ | |||||
| """LCR Configuration Tool: This program is used to automatically configure the Low Cost Robot (LCR) for the user. | |||||
| """Module for configuring and setting up the Low Cost Robot (LCR) hardware. | |||||
| This module provides functionality for initializing and configuring the LCR robot's | |||||
| servo motors and other hardware components. | |||||
| The program will: | The program will: | ||||
| 1. Disable all torque motors of provided LCR. | 1. Disable all torque motors of provided LCR. | ||||
| @@ -46,10 +49,17 @@ GRIPPER = pa.array(["gripper"], type=pa.string()) | |||||
| def pause(): | def pause(): | ||||
| """Pause execution and wait for user input to continue.""" | |||||
| input("Press Enter to continue...") | input("Press Enter to continue...") | ||||
| def configure_servos(bus: DynamixelBus): | def configure_servos(bus: DynamixelBus): | ||||
| """Configure servo motors with appropriate settings. | |||||
| Args: | |||||
| bus: DynamixelBus instance for servo communication | |||||
| """ | |||||
| bus.write_torque_enable( | bus.write_torque_enable( | ||||
| wrap_joints_and_values(FULL_ARM, [TorqueMode.DISABLED.value] * 6), | wrap_joints_and_values(FULL_ARM, [TorqueMode.DISABLED.value] * 6), | ||||
| ) | ) | ||||
| @@ -68,6 +78,7 @@ def configure_servos(bus: DynamixelBus): | |||||
| def main(): | def main(): | ||||
| """Initialize and configure the LCR robot hardware components.""" | |||||
| parser = argparse.ArgumentParser( | parser = argparse.ArgumentParser( | ||||
| description="LCR Auto Configure: This program is used to automatically configure the Low Cost Robot (LCR) for " | description="LCR Auto Configure: This program is used to automatically configure the Low Cost Robot (LCR) for " | ||||
| "the user.", | "the user.", | ||||
| @@ -1,3 +1,9 @@ | |||||
| """Module for interpolating between LCR robot configurations. | |||||
| This module provides functionality for calculating and interpolating between | |||||
| different LCR robot configurations to ensure smooth transitions. | |||||
| """ | |||||
| import argparse | import argparse | ||||
| import json | import json | ||||
| import os | import os | ||||
| @@ -14,6 +20,7 @@ from pwm_position_control.transform import ( | |||||
| def main(): | def main(): | ||||
| """Initialize and run the LCR interpolation node.""" | |||||
| parser = argparse.ArgumentParser( | parser = argparse.ArgumentParser( | ||||
| description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " | description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " | ||||
| "LCR followers knowing a Leader position and Follower position.", | "LCR followers knowing a Leader position and Follower position.", | ||||
| @@ -1,3 +1,5 @@ | |||||
| """TODO: Add docstring.""" | |||||
| import argparse | import argparse | ||||
| import json | import json | ||||
| import os | import os | ||||
| @@ -13,6 +15,7 @@ from pwm_position_control.transform import ( | |||||
| def main(): | def main(): | ||||
| """TODO: Add docstring.""" | |||||
| parser = argparse.ArgumentParser( | parser = argparse.ArgumentParser( | ||||
| description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " | description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " | ||||
| "LCR followers knowing a Leader position and Follower position.", | "LCR followers knowing a Leader position and Follower position.", | ||||
| @@ -1,3 +1,9 @@ | |||||
| """Module for interpolating between LCR and simulated LCR configurations. | |||||
| This module provides functionality for calculating and interpolating between | |||||
| physical LCR robot configurations and their simulated counterparts. | |||||
| """ | |||||
| import argparse | import argparse | ||||
| import json | import json | ||||
| import os | import os | ||||
| @@ -15,6 +21,7 @@ from pwm_position_control.transform import ( | |||||
| def main(): | def main(): | ||||
| """Initialize and run the LCR to simulated LCR interpolation node.""" | |||||
| parser = argparse.ArgumentParser( | parser = argparse.ArgumentParser( | ||||
| description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " | description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " | ||||
| "LCR followers knowing a Leader position and Follower position.", | "LCR followers knowing a Leader position and Follower position.", | ||||
| @@ -1,3 +1,5 @@ | |||||
| """TODO: Add docstring.""" | |||||
| import argparse | import argparse | ||||
| import json | import json | ||||
| import os | import os | ||||
| @@ -8,6 +10,7 @@ from pwm_position_control.transform import logical_to_pwm_with_offset_arrow | |||||
| def main(): | def main(): | ||||
| """TODO: Add docstring.""" | |||||
| parser = argparse.ArgumentParser( | parser = argparse.ArgumentParser( | ||||
| description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " | description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " | ||||
| "LCR followers knowing a Leader position and Follower position.", | "LCR followers knowing a Leader position and Follower position.", | ||||
| @@ -1,3 +1,10 @@ | |||||
| """Module for managing Dynamixel servo communication and control. | |||||
| This module provides functionality for communicating with and controlling Dynamixel servos | |||||
| through a serial interface. It includes methods for reading and writing servo parameters, | |||||
| managing operating modes, and handling joint positions and velocities. | |||||
| """ | |||||
| from __future__ import annotations | from __future__ import annotations | ||||
| import enum | import enum | ||||
| @@ -10,6 +17,8 @@ from dynamixel_sdk import * # Uses Dynamixel SDK library | |||||
| class ReadAttribute(enum.Enum): | class ReadAttribute(enum.Enum): | ||||
| """Enumeration for Dynamixel servo read attributes.""" | |||||
| TEMPERATURE = 146 | TEMPERATURE = 146 | ||||
| VOLTAGE = 145 | VOLTAGE = 145 | ||||
| VELOCITY = 128 | VELOCITY = 128 | ||||
| @@ -22,6 +31,8 @@ class ReadAttribute(enum.Enum): | |||||
| class OperatingMode(enum.Enum): | class OperatingMode(enum.Enum): | ||||
| """Enumeration for Dynamixel servo operating modes.""" | |||||
| VELOCITY = 1 | VELOCITY = 1 | ||||
| POSITION = 3 | POSITION = 3 | ||||
| CURRENT_CONTROLLED_POSITION = 5 | CURRENT_CONTROLLED_POSITION = 5 | ||||
| @@ -30,6 +41,8 @@ class OperatingMode(enum.Enum): | |||||
| class Dynamixel: | class Dynamixel: | ||||
| """Class for managing communication with Dynamixel servos.""" | |||||
| ADDR_TORQUE_ENABLE = 64 | ADDR_TORQUE_ENABLE = 64 | ||||
| ADDR_GOAL_POSITION = 116 | ADDR_GOAL_POSITION = 116 | ||||
| ADDR_VELOCITY_LIMIT = 44 | ADDR_VELOCITY_LIMIT = 44 | ||||
| @@ -41,7 +54,10 @@ class Dynamixel: | |||||
| @dataclass | @dataclass | ||||
| class Config: | class Config: | ||||
| """Configuration class for Dynamixel servo settings.""" | |||||
| def instantiate(self): | def instantiate(self): | ||||
| """Create a new Dynamixel instance with this configuration.""" | |||||
| return Dynamixel(self) | return Dynamixel(self) | ||||
| baudrate: int = 57600 | baudrate: int = 57600 | ||||
| @@ -50,10 +66,17 @@ class Dynamixel: | |||||
| dynamixel_id: int = 1 | dynamixel_id: int = 1 | ||||
| def __init__(self, config: Config): | def __init__(self, config: Config): | ||||
| """Initialize the Dynamixel servo connection. | |||||
| Args: | |||||
| config: Configuration object containing connection settings. | |||||
| """ | |||||
| self.config = config | self.config = config | ||||
| self.connect() | self.connect() | ||||
| def connect(self): | def connect(self): | ||||
| """Establish connection with the Dynamixel servo.""" | |||||
| if self.config.device_name == "": | if self.config.device_name == "": | ||||
| for port_name in os.listdir("/dev"): | for port_name in os.listdir("/dev"): | ||||
| if "ttyUSB" in port_name or "ttyACM" in port_name: | if "ttyUSB" in port_name or "ttyACM" in port_name: | ||||
| @@ -77,9 +100,17 @@ class Dynamixel: | |||||
| return True | return True | ||||
| def disconnect(self): | def disconnect(self): | ||||
| """Close the connection with the Dynamixel servo.""" | |||||
| self.portHandler.closePort() | self.portHandler.closePort() | ||||
| def set_goal_position(self, motor_id, goal_position): | def set_goal_position(self, motor_id, goal_position): | ||||
| """Set the goal position for the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to control. | |||||
| goal_position: Target position value. | |||||
| """ | |||||
| # if self.operating_modes[motor_id] is not OperatingMode.POSITION: | # if self.operating_modes[motor_id] is not OperatingMode.POSITION: | ||||
| # self._disable_torque(motor_id) | # self._disable_torque(motor_id) | ||||
| # self.set_operating_mode(motor_id, OperatingMode.POSITION) | # self.set_operating_mode(motor_id, OperatingMode.POSITION) | ||||
| @@ -95,6 +126,14 @@ class Dynamixel: | |||||
| # print(f'set position of motor {motor_id} to {goal_position}') | # print(f'set position of motor {motor_id} to {goal_position}') | ||||
| def set_pwm_value(self, motor_id: int, pwm_value, tries=3): | def set_pwm_value(self, motor_id: int, pwm_value, tries=3): | ||||
| """Set the PWM value for the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to control. | |||||
| pwm_value: PWM value to set. | |||||
| tries: Number of attempts to set the value. | |||||
| """ | |||||
| if self.operating_modes[motor_id] is not OperatingMode.PWM: | if self.operating_modes[motor_id] is not OperatingMode.PWM: | ||||
| self._disable_torque(motor_id) | self._disable_torque(motor_id) | ||||
| self.set_operating_mode(motor_id, OperatingMode.PWM) | self.set_operating_mode(motor_id, OperatingMode.PWM) | ||||
| @@ -123,9 +162,27 @@ class Dynamixel: | |||||
| ) | ) | ||||
| def read_temperature(self, motor_id: int): | def read_temperature(self, motor_id: int): | ||||
| """Read the temperature of the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to read from. | |||||
| Returns: | |||||
| The current temperature value. | |||||
| """ | |||||
| return self._read_value(motor_id, ReadAttribute.TEMPERATURE, 1) | return self._read_value(motor_id, ReadAttribute.TEMPERATURE, 1) | ||||
| def read_velocity(self, motor_id: int): | def read_velocity(self, motor_id: int): | ||||
| """Read the current velocity of the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to read from. | |||||
| Returns: | |||||
| The current velocity value. | |||||
| """ | |||||
| pos = self._read_value(motor_id, ReadAttribute.VELOCITY, 4) | pos = self._read_value(motor_id, ReadAttribute.VELOCITY, 4) | ||||
| if pos > 2**31: | if pos > 2**31: | ||||
| pos -= 2**32 | pos -= 2**32 | ||||
| @@ -133,6 +190,15 @@ class Dynamixel: | |||||
| return pos | return pos | ||||
| def read_position(self, motor_id: int): | def read_position(self, motor_id: int): | ||||
| """Read the current position of the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to read from. | |||||
| Returns: | |||||
| The current position value. | |||||
| """ | |||||
| pos = self._read_value(motor_id, ReadAttribute.POSITION, 4) | pos = self._read_value(motor_id, ReadAttribute.POSITION, 4) | ||||
| if pos > 2**31: | if pos > 2**31: | ||||
| pos -= 2**32 | pos -= 2**32 | ||||
| @@ -140,30 +206,77 @@ class Dynamixel: | |||||
| return pos | return pos | ||||
| def read_position_degrees(self, motor_id: int) -> float: | def read_position_degrees(self, motor_id: int) -> float: | ||||
| """Read the current position of the specified servo in degrees. | |||||
| Args: | |||||
| motor_id: ID of the servo to read from. | |||||
| Returns: | |||||
| The current position in degrees. | |||||
| """ | |||||
| return (self.read_position(motor_id) / 4096) * 360 | return (self.read_position(motor_id) / 4096) * 360 | ||||
| def read_position_radians(self, motor_id: int) -> float: | def read_position_radians(self, motor_id: int) -> float: | ||||
| """Read the current position of the specified servo in radians. | |||||
| Args: | |||||
| motor_id: ID of the servo to read from. | |||||
| Returns: | |||||
| The current position in radians. | |||||
| """ | |||||
| return (self.read_position(motor_id) / 4096) * 2 * math.pi | return (self.read_position(motor_id) / 4096) * 2 * math.pi | ||||
| def read_current(self, motor_id: int): | def read_current(self, motor_id: int): | ||||
| """Read the current value of the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to read from. | |||||
| Returns: | |||||
| The current value. | |||||
| """ | |||||
| current = self._read_value(motor_id, ReadAttribute.CURRENT, 2) | current = self._read_value(motor_id, ReadAttribute.CURRENT, 2) | ||||
| if current > 2**15: | if current > 2**15: | ||||
| current -= 2**16 | current -= 2**16 | ||||
| return current | return current | ||||
| def read_present_pwm(self, motor_id: int): | def read_present_pwm(self, motor_id: int): | ||||
| """Read the current PWM value of the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to read from. | |||||
| Returns: | |||||
| The current PWM value. | |||||
| """ | |||||
| return self._read_value(motor_id, ReadAttribute.PWM, 2) | return self._read_value(motor_id, ReadAttribute.PWM, 2) | ||||
| def read_hardware_error_status(self, motor_id: int): | def read_hardware_error_status(self, motor_id: int): | ||||
| """Read the hardware error status of the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to read from. | |||||
| Returns: | |||||
| The hardware error status value. | |||||
| """ | |||||
| return self._read_value(motor_id, ReadAttribute.HARDWARE_ERROR_STATUS, 1) | return self._read_value(motor_id, ReadAttribute.HARDWARE_ERROR_STATUS, 1) | ||||
| def set_id(self, old_id, new_id, use_broadcast_id: bool = False): | def set_id(self, old_id, new_id, use_broadcast_id: bool = False): | ||||
| """Sets the id of the dynamixel servo | |||||
| @param old_id: current id of the servo | |||||
| @param new_id: new id | |||||
| @param use_broadcast_id: set ids of all connected dynamixels if True. | |||||
| If False, change only servo with self.config.id | |||||
| @return: | |||||
| """Set the ID of the Dynamixel servo. | |||||
| Args: | |||||
| old_id: Current ID of the servo. | |||||
| new_id: New ID to set. | |||||
| use_broadcast_id: If True, set IDs of all connected servos. | |||||
| If False, change only servo with self.config.id | |||||
| """ | """ | ||||
| if use_broadcast_id: | if use_broadcast_id: | ||||
| current_id = 254 | current_id = 254 | ||||
| @@ -176,6 +289,12 @@ class Dynamixel: | |||||
| self.config.id = id | self.config.id = id | ||||
| def _enable_torque(self, motor_id): | def _enable_torque(self, motor_id): | ||||
| """Enable torque for the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to control. | |||||
| """ | |||||
| dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx( | dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx( | ||||
| self.portHandler, motor_id, self.ADDR_TORQUE_ENABLE, 1, | self.portHandler, motor_id, self.ADDR_TORQUE_ENABLE, 1, | ||||
| ) | ) | ||||
| @@ -183,6 +302,12 @@ class Dynamixel: | |||||
| self.torque_enabled[motor_id] = True | self.torque_enabled[motor_id] = True | ||||
| def _disable_torque(self, motor_id): | def _disable_torque(self, motor_id): | ||||
| """Disable torque for the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to control. | |||||
| """ | |||||
| dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx( | dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx( | ||||
| self.portHandler, motor_id, self.ADDR_TORQUE_ENABLE, 0, | self.portHandler, motor_id, self.ADDR_TORQUE_ENABLE, 0, | ||||
| ) | ) | ||||
| @@ -190,6 +315,18 @@ class Dynamixel: | |||||
| self.torque_enabled[motor_id] = False | self.torque_enabled[motor_id] = False | ||||
| def _process_response(self, dxl_comm_result: int, dxl_error: int, motor_id: int): | def _process_response(self, dxl_comm_result: int, dxl_error: int, motor_id: int): | ||||
| """Process the response from a servo communication. | |||||
| Args: | |||||
| dxl_comm_result: Communication result. | |||||
| dxl_error: Error value. | |||||
| motor_id: ID of the servo that was communicated with. | |||||
| Raises: | |||||
| ConnectionError: If communication failed. | |||||
| RuntimeError: If servo reported an error. | |||||
| """ | |||||
| if dxl_comm_result != COMM_SUCCESS: | if dxl_comm_result != COMM_SUCCESS: | ||||
| raise ConnectionError( | raise ConnectionError( | ||||
| f"dxl_comm_result for motor {motor_id}: {self.packetHandler.getTxRxResult(dxl_comm_result)}", | f"dxl_comm_result for motor {motor_id}: {self.packetHandler.getTxRxResult(dxl_comm_result)}", | ||||
| @@ -201,6 +338,13 @@ class Dynamixel: | |||||
| ) | ) | ||||
| def set_operating_mode(self, motor_id: int, operating_mode: OperatingMode): | def set_operating_mode(self, motor_id: int, operating_mode: OperatingMode): | ||||
| """Set the operating mode for the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to control. | |||||
| operating_mode: Operating mode to set. | |||||
| """ | |||||
| dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( | dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( | ||||
| self.portHandler, motor_id, self.OPERATING_MODE_ADDR, operating_mode.value, | self.portHandler, motor_id, self.OPERATING_MODE_ADDR, operating_mode.value, | ||||
| ) | ) | ||||
| @@ -208,30 +352,67 @@ class Dynamixel: | |||||
| self.operating_modes[motor_id] = operating_mode | self.operating_modes[motor_id] = operating_mode | ||||
| def set_pwm_limit(self, motor_id: int, limit: int): | def set_pwm_limit(self, motor_id: int, limit: int): | ||||
| """Set the PWM limit for the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to control. | |||||
| limit: PWM limit value to set. | |||||
| """ | |||||
| dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( | dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( | ||||
| self.portHandler, motor_id, 36, limit, | self.portHandler, motor_id, 36, limit, | ||||
| ) | ) | ||||
| self._process_response(dxl_comm_result, dxl_error, motor_id) | self._process_response(dxl_comm_result, dxl_error, motor_id) | ||||
| def set_velocity_limit(self, motor_id: int, velocity_limit): | def set_velocity_limit(self, motor_id: int, velocity_limit): | ||||
| """Set the velocity limit for the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to control. | |||||
| velocity_limit: Velocity limit value to set. | |||||
| """ | |||||
| dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx( | dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx( | ||||
| self.portHandler, motor_id, self.ADDR_VELOCITY_LIMIT, velocity_limit, | self.portHandler, motor_id, self.ADDR_VELOCITY_LIMIT, velocity_limit, | ||||
| ) | ) | ||||
| self._process_response(dxl_comm_result, dxl_error, motor_id) | self._process_response(dxl_comm_result, dxl_error, motor_id) | ||||
| def set_P(self, motor_id: int, P: int): | def set_P(self, motor_id: int, P: int): | ||||
| """Set the position P gain for the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to control. | |||||
| P: Position P gain value to set. | |||||
| """ | |||||
| dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( | dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( | ||||
| self.portHandler, motor_id, self.POSITION_P, P, | self.portHandler, motor_id, self.POSITION_P, P, | ||||
| ) | ) | ||||
| self._process_response(dxl_comm_result, dxl_error, motor_id) | self._process_response(dxl_comm_result, dxl_error, motor_id) | ||||
| def set_I(self, motor_id: int, I: int): | def set_I(self, motor_id: int, I: int): | ||||
| """Set the position I gain for the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to control. | |||||
| I: Position I gain value to set. | |||||
| """ | |||||
| dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( | dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( | ||||
| self.portHandler, motor_id, self.POSITION_I, I, | self.portHandler, motor_id, self.POSITION_I, I, | ||||
| ) | ) | ||||
| self._process_response(dxl_comm_result, dxl_error, motor_id) | self._process_response(dxl_comm_result, dxl_error, motor_id) | ||||
| def read_home_offset(self, motor_id: int): | def read_home_offset(self, motor_id: int): | ||||
| """Read the home offset of the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to read from. | |||||
| Returns: | |||||
| The home offset value. | |||||
| """ | |||||
| self._disable_torque(motor_id) | self._disable_torque(motor_id) | ||||
| # dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx(self.portHandler, motor_id, | # dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx(self.portHandler, motor_id, | ||||
| # ReadAttribute.HOMING_OFFSET.value, home_position) | # ReadAttribute.HOMING_OFFSET.value, home_position) | ||||
| @@ -241,6 +422,13 @@ class Dynamixel: | |||||
| return home_offset | return home_offset | ||||
| def set_home_offset(self, motor_id: int, home_position: int): | def set_home_offset(self, motor_id: int, home_position: int): | ||||
| """Set the home offset for the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to control. | |||||
| home_position: Home position value to set. | |||||
| """ | |||||
| self._disable_torque(motor_id) | self._disable_torque(motor_id) | ||||
| dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx( | dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx( | ||||
| self.portHandler, motor_id, ReadAttribute.HOMING_OFFSET.value, home_position, | self.portHandler, motor_id, ReadAttribute.HOMING_OFFSET.value, home_position, | ||||
| @@ -249,6 +437,13 @@ class Dynamixel: | |||||
| self._enable_torque(motor_id) | self._enable_torque(motor_id) | ||||
| def set_baudrate(self, motor_id: int, baudrate): | def set_baudrate(self, motor_id: int, baudrate): | ||||
| """Set the baudrate for the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to control. | |||||
| baudrate: Baudrate value to set. | |||||
| """ | |||||
| # translate baudrate into dynamixel baudrate setting id | # translate baudrate into dynamixel baudrate setting id | ||||
| if baudrate == 57600: | if baudrate == 57600: | ||||
| baudrate_id = 1 | baudrate_id = 1 | ||||
| @@ -270,6 +465,18 @@ class Dynamixel: | |||||
| self._process_response(dxl_comm_result, dxl_error, motor_id) | self._process_response(dxl_comm_result, dxl_error, motor_id) | ||||
| def _read_value(self, motor_id, attribute: ReadAttribute, num_bytes: int, tries=10): | def _read_value(self, motor_id, attribute: ReadAttribute, num_bytes: int, tries=10): | ||||
| """Read a value from the specified servo address. | |||||
| Args: | |||||
| motor_id: ID of the servo to read from. | |||||
| attribute: Attribute to read. | |||||
| num_bytes: Number of bytes to read. | |||||
| tries: Number of attempts to read the value. | |||||
| Returns: | |||||
| The read value. | |||||
| """ | |||||
| try: | try: | ||||
| if num_bytes == 1: | if num_bytes == 1: | ||||
| value, dxl_comm_result, dxl_error = self.packetHandler.read1ByteTxRx( | value, dxl_comm_result, dxl_error = self.packetHandler.read1ByteTxRx( | ||||
| @@ -310,6 +517,12 @@ class Dynamixel: | |||||
| return value | return value | ||||
| def set_home_position(self, motor_id: int): | def set_home_position(self, motor_id: int): | ||||
| """Set the home position for the specified servo. | |||||
| Args: | |||||
| motor_id: ID of the servo to control. | |||||
| """ | |||||
| print(f"setting home position for motor {motor_id}") | print(f"setting home position for motor {motor_id}") | ||||
| self.set_home_offset(motor_id, 0) | self.set_home_offset(motor_id, 0) | ||||
| current_position = self.read_position(motor_id) | current_position = self.read_position(motor_id) | ||||
| @@ -1,3 +1,9 @@ | |||||
| """Module for controlling robot hardware and movements. | |||||
| This module provides functionality for controlling robot hardware components, | |||||
| including servo motors and various control modes. | |||||
| """ | |||||
| import time | import time | ||||
| from enum import Enum, auto | from enum import Enum, auto | ||||
| from typing import Union | from typing import Union | ||||
| @@ -15,6 +21,12 @@ from dynamixel_sdk import ( | |||||
| class MotorControlType(Enum): | class MotorControlType(Enum): | ||||
| """Enumeration of different motor control modes. | |||||
| Defines the various control modes available for the robot's motors, | |||||
| including PWM control, position control, and disabled states. | |||||
| """ | |||||
| PWM = auto() | PWM = auto() | ||||
| POSITION_CONTROL = auto() | POSITION_CONTROL = auto() | ||||
| DISABLED = auto() | DISABLED = auto() | ||||
| @@ -22,8 +34,22 @@ class MotorControlType(Enum): | |||||
| class Robot: | class Robot: | ||||
| """A class for controlling robot hardware components. | |||||
| This class provides methods for controlling robot servos, managing different | |||||
| control modes, and reading sensor data. | |||||
| """ | |||||
| # def __init__(self, device_name: str, baudrate=1_000_000, servo_ids=[1, 2, 3, 4, 5]): | # def __init__(self, device_name: str, baudrate=1_000_000, servo_ids=[1, 2, 3, 4, 5]): | ||||
| def __init__(self, dynamixel, baudrate=1_000_000, servo_ids=[1, 2, 3, 4, 5]): | def __init__(self, dynamixel, baudrate=1_000_000, servo_ids=[1, 2, 3, 4, 5]): | ||||
| """Initialize the robot controller. | |||||
| Args: | |||||
| dynamixel: Dynamixel controller instance | |||||
| baudrate: Communication baudrate for servo control | |||||
| servo_ids: List of servo IDs to control | |||||
| """ | |||||
| self.servo_ids = servo_ids | self.servo_ids = servo_ids | ||||
| self.dynamixel = dynamixel | self.dynamixel = dynamixel | ||||
| # self.dynamixel = Dynamixel.Config(baudrate=baudrate, device_name=device_name).instantiate() | # self.dynamixel = Dynamixel.Config(baudrate=baudrate, device_name=device_name).instantiate() | ||||
| @@ -66,9 +92,14 @@ class Robot: | |||||
| self.motor_control_state = MotorControlType.DISABLED | self.motor_control_state = MotorControlType.DISABLED | ||||
| def read_position(self, tries=2): | def read_position(self, tries=2): | ||||
| """Reads the joint positions of the robot. 2048 is the center position. 0 and 4096 are 180 degrees in each direction. | |||||
| :param tries: maximum number of tries to read the position | |||||
| :return: list of joint positions in range [0, 4096] | |||||
| """Read the current joint positions of the robot. | |||||
| Args: | |||||
| tries: Maximum number of attempts to read positions | |||||
| Returns: | |||||
| list: Joint positions in range [0, 4096] | |||||
| """ | """ | ||||
| result = self.position_reader.txRxPacket() | result = self.position_reader.txRxPacket() | ||||
| if result != 0: | if result != 0: | ||||
| @@ -84,8 +115,11 @@ class Robot: | |||||
| return positions | return positions | ||||
| def read_velocity(self): | def read_velocity(self): | ||||
| """Reads the joint velocities of the robot. | |||||
| :return: list of joint velocities, | |||||
| """Read the current joint velocities of the robot. | |||||
| Returns: | |||||
| list: Current joint velocities | |||||
| """ | """ | ||||
| self.velocity_reader.txRxPacket() | self.velocity_reader.txRxPacket() | ||||
| velocties = [] | velocties = [] | ||||
| @@ -97,7 +131,11 @@ 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] | |||||
| """Set goal positions for the robot joints. | |||||
| Args: | |||||
| action: List or numpy array of target joint positions in range [0, 4096] | |||||
| """ | """ | ||||
| if self.motor_control_state is not MotorControlType.POSITION_CONTROL: | if self.motor_control_state is not MotorControlType.POSITION_CONTROL: | ||||
| self._set_position_control() | self._set_position_control() | ||||
| @@ -113,8 +151,11 @@ 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. | |||||
| :param action: list or numpy array of pwm values in range [0, 885] | |||||
| """Set PWM values for the servos. | |||||
| Args: | |||||
| action: List or numpy array of PWM values in range [0, 885] | |||||
| """ | """ | ||||
| if self.motor_control_state is not MotorControlType.PWM: | if self.motor_control_state is not MotorControlType.PWM: | ||||
| self._set_pwm_control() | self._set_pwm_control() | ||||
| @@ -128,15 +169,19 @@ class Robot: | |||||
| self.pwm_writer.txPacket() | self.pwm_writer.txPacket() | ||||
| def set_trigger_torque(self): | def set_trigger_torque(self): | ||||
| """Sets a constant torque torque for the last servo in the chain. This is useful for the trigger of the leader arm | |||||
| """Set a constant torque for the last servo in the chain. | |||||
| This is useful for the trigger of the leader arm. | |||||
| """ | """ | ||||
| self.dynamixel._enable_torque(self.servo_ids[-1]) | self.dynamixel._enable_torque(self.servo_ids[-1]) | ||||
| self.dynamixel.set_pwm_value(self.servo_ids[-1], 200) | self.dynamixel.set_pwm_value(self.servo_ids[-1], 200) | ||||
| def limit_pwm(self, limit: Union[int, list, np.ndarray]): | def limit_pwm(self, limit: Union[int, list, np.ndarray]): | ||||
| """Limits the pwm values for the servos in for position control | |||||
| @param limit: 0 ~ 885 | |||||
| @return: | |||||
| """Limit the PWM values for the servos in position control. | |||||
| Args: | |||||
| limit: PWM limit value in range 0-885 | |||||
| """ | """ | ||||
| if isinstance(limit, int): | if isinstance(limit, int): | ||||
| limits = [ | limits = [ | ||||
| @@ -1,3 +1,9 @@ | |||||
| """Module for teleoperating a physical robot. | |||||
| This module provides functionality for controlling a physical robot through | |||||
| teleoperation, allowing real-time control of robot movements and actions. | |||||
| """ | |||||
| from dynamixel import Dynamixel | from dynamixel import Dynamixel | ||||
| from robot import Robot | from robot import Robot | ||||
| @@ -1,3 +1,9 @@ | |||||
| """Module for ROS2-based robot teleoperation. | |||||
| This module provides functionality for controlling robots through ROS2, | |||||
| enabling teleoperation and real-time control of robot movements. | |||||
| """ | |||||
| import dora | import dora | ||||
| import numpy as np | import numpy as np | ||||
| import pyarrow as pa | import pyarrow as pa | ||||
| @@ -1,3 +1,9 @@ | |||||
| """Module for integrating gym environments with Dora nodes. | |||||
| This module provides functionality for running gym environments as Dora nodes, | |||||
| including replay capabilities for recorded robot actions. | |||||
| """ | |||||
| import time | import time | ||||
| from pathlib import Path | from pathlib import Path | ||||
| @@ -12,7 +18,20 @@ observation = env.reset() | |||||
| class ReplayPolicy: | class ReplayPolicy: | ||||
| """A policy class for replaying recorded robot actions. | |||||
| This class handles loading and replaying recorded actions from a dataset, | |||||
| maintaining timing between actions to match the original recording. | |||||
| """ | |||||
| def __init__(self, example_path, epidode=0): | def __init__(self, example_path, epidode=0): | ||||
| """Initialize the replay policy. | |||||
| Args: | |||||
| example_path: Path to the directory containing recorded actions | |||||
| epidode: Index of the episode to replay | |||||
| """ | |||||
| df_action = pd.read_parquet(example_path / "action.parquet") | df_action = pd.read_parquet(example_path / "action.parquet") | ||||
| df_episode_index = pd.read_parquet(example_path / "episode_index.parquet") | df_episode_index = pd.read_parquet(example_path / "episode_index.parquet") | ||||
| self.df = pd.merge_asof( | self.df = pd.merge_asof( | ||||
| @@ -29,6 +48,16 @@ class ReplayPolicy: | |||||
| self.finished = False | self.finished = False | ||||
| def select_action(self, obs): | def select_action(self, obs): | ||||
| """Select the next action to replay. | |||||
| Args: | |||||
| obs: Current observation from the environment (unused) | |||||
| Returns: | |||||
| tuple: (action, finished) where action is the next action to take | |||||
| and finished indicates if all actions have been replayed | |||||
| """ | |||||
| if self.index < len(self.df): | if self.index < len(self.df): | ||||
| self.index += 1 | self.index += 1 | ||||
| else: | else: | ||||
| @@ -1,3 +1,9 @@ | |||||
| """Module for handling keyboard input events in robot control. | |||||
| This module provides functionality for capturing and processing keyboard input | |||||
| events to control robot movements and actions. | |||||
| """ | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from dora import Node | from dora import Node | ||||
| from pynput import keyboard | from pynput import keyboard | ||||
| @@ -1,3 +1,5 @@ | |||||
| """TODO: Add docstring.""" | |||||
| #!/usr/bin/env python3 | #!/usr/bin/env python3 | ||||
| import os | import os | ||||
| @@ -1,3 +1,9 @@ | |||||
| """Module for language model-based robot control. | |||||
| This module provides functionality for controlling robots using natural language | |||||
| commands processed by large language models. | |||||
| """ | |||||
| import gc # garbage collect library | import gc # garbage collect library | ||||
| import os | import os | ||||
| import re | import re | ||||
| @@ -39,15 +45,14 @@ tokenizer = AutoTokenizer.from_pretrained(MODEL_NAME_OR_PATH, use_fast=True) | |||||
| def extract_python_code_blocks(text): | def extract_python_code_blocks(text): | ||||
| """Extracts Python code blocks from the given text that are enclosed in triple backticks with a python language identifier. | |||||
| """Extract Python code blocks from the given text. | |||||
| Parameters | |||||
| ---------- | |||||
| - text: A string that may contain one or more Python code blocks. | |||||
| Args: | |||||
| text: A string that may contain one or more Python code blocks. | |||||
| Returns | |||||
| ------- | |||||
| - A list of strings, where each string is a block of Python code extracted from the text. | |||||
| Returns: | |||||
| list: A list of strings, where each string is a block of Python code | |||||
| extracted from the text. | |||||
| """ | """ | ||||
| pattern = r"```python\n(.*?)\n```" | pattern = r"```python\n(.*?)\n```" | ||||
| @@ -63,15 +68,13 @@ def extract_python_code_blocks(text): | |||||
| def remove_last_line(python_code): | def remove_last_line(python_code): | ||||
| """Removes the last line from a given string of Python code. | |||||
| """Remove the last line from a given string of Python code. | |||||
| Parameters | |||||
| ---------- | |||||
| - python_code: A string representing Python source code. | |||||
| Args: | |||||
| python_code: A string representing Python source code. | |||||
| Returns | |||||
| ------- | |||||
| - A string with the last line removed. | |||||
| Returns: | |||||
| str: A string with the last line removed. | |||||
| """ | """ | ||||
| lines = python_code.split("\n") # Split the string into lines | lines = python_code.split("\n") # Split the string into lines | ||||
| @@ -82,7 +85,16 @@ def remove_last_line(python_code): | |||||
| def calculate_similarity(source, target): | def calculate_similarity(source, target): | ||||
| """Calculate a similarity score between the source and target strings. | """Calculate a similarity score between the source and target strings. | ||||
| This uses the edit distance relative to the length of the strings. | This uses the edit distance relative to the length of the strings. | ||||
| Args: | |||||
| source: First string to compare | |||||
| target: Second string to compare | |||||
| Returns: | |||||
| float: Similarity score between 0 and 1 | |||||
| """ | """ | ||||
| edit_distance = pylcs.edit_distance(source, target) | edit_distance = pylcs.edit_distance(source, target) | ||||
| max_length = max(len(source), len(target)) | max_length = max(len(source), len(target)) | ||||
| @@ -92,8 +104,17 @@ def calculate_similarity(source, target): | |||||
| def find_best_match_location(source_code, target_block): | def find_best_match_location(source_code, target_block): | ||||
| """Find the best match for the target_block within the source_code by searching line by line, | |||||
| considering blocks of varying lengths. | |||||
| """Find the best match for the target_block within the source_code. | |||||
| This function searches line by line, considering blocks of varying lengths. | |||||
| Args: | |||||
| source_code: The source code to search within | |||||
| target_block: The code block to find a match for | |||||
| Returns: | |||||
| tuple: (start_index, end_index) of the best matching location | |||||
| """ | """ | ||||
| source_lines = source_code.split("\n") | source_lines = source_code.split("\n") | ||||
| target_lines = target_block.split("\n") | target_lines = target_block.split("\n") | ||||
| @@ -122,7 +143,15 @@ def find_best_match_location(source_code, target_block): | |||||
| def replace_code_in_source(source_code, replacement_block: str): | def replace_code_in_source(source_code, replacement_block: str): | ||||
| """Replace the best matching block in the source_code with the replacement_block, considering variable block lengths. | |||||
| """Replace the best matching block in the source_code with the replacement_block. | |||||
| Args: | |||||
| source_code: The original source code | |||||
| replacement_block: The new code block to insert | |||||
| Returns: | |||||
| str: The modified source code | |||||
| """ | """ | ||||
| replacement_block = extract_python_code_blocks(replacement_block)[0] | replacement_block = extract_python_code_blocks(replacement_block)[0] | ||||
| start_index, end_index = find_best_match_location(source_code, replacement_block) | start_index, end_index = find_best_match_location(source_code, replacement_block) | ||||
| @@ -136,7 +165,14 @@ def replace_code_in_source(source_code, replacement_block: str): | |||||
| class Operator: | class Operator: | ||||
| """A class for managing LLM-based code modifications. | |||||
| This class handles the process of using LLMs to modify code based on | |||||
| natural language instructions and managing the modification workflow. | |||||
| """ | |||||
| def __init__(self) -> None: | def __init__(self) -> None: | ||||
| """Initialize the operator with policy initialization flag.""" | |||||
| self.policy_init = False | self.policy_init = False | ||||
| def on_event( | def on_event( | ||||
| @@ -144,6 +180,16 @@ class Operator: | |||||
| dora_event, | dora_event, | ||||
| send_output, | send_output, | ||||
| ) -> DoraStatus: | ) -> DoraStatus: | ||||
| """Handle incoming events and process LLM-based code modifications. | |||||
| Args: | |||||
| dora_event: Dictionary containing event information | |||||
| send_output: Function to send output to the dataflow | |||||
| Returns: | |||||
| DoraStatus: Status indicating whether to continue processing | |||||
| """ | |||||
| global model, tokenizer | global model, tokenizer | ||||
| if dora_event["type"] == "INPUT" and dora_event["id"] == "text": | if dora_event["type"] == "INPUT" and dora_event["id"] == "text": | ||||
| input = dora_event["value"][0].as_py() | input = dora_event["value"][0].as_py() | ||||
| @@ -177,7 +223,15 @@ class Operator: | |||||
| return DoraStatus.CONTINUE | return DoraStatus.CONTINUE | ||||
| def ask_llm(self, prompt): | def ask_llm(self, prompt): | ||||
| """Send a prompt to the LLM and get its response. | |||||
| Args: | |||||
| prompt: The prompt to send to the LLM | |||||
| Returns: | |||||
| str: The LLM's response | |||||
| """ | |||||
| # Generate output | # Generate output | ||||
| # prompt = PROMPT_TEMPLATE.format(system_message=system_message, prompt=prompt)) | # prompt = PROMPT_TEMPLATE.format(system_message=system_message, prompt=prompt)) | ||||
| input = tokenizer(prompt, return_tensors="pt") | input = tokenizer(prompt, return_tensors="pt") | ||||
| @@ -1,3 +1,5 @@ | |||||
| """TODO: Add docstring.""" | |||||
| import os | import os | ||||
| import cv2 | import cv2 | ||||
| @@ -1,11 +1,34 @@ | |||||
| """Module for implementing robot control policies. | |||||
| This module provides functionality for implementing and managing robot control policies, | |||||
| including action selection and state management. | |||||
| """ | |||||
| from dora import DoraStatus | from dora import DoraStatus | ||||
| class Operator: | class Operator: | ||||
| """A class for implementing robot control policies. | |||||
| This class handles the selection and execution of robot actions based on | |||||
| input events and current state. | |||||
| """ | |||||
| def __init__(self): | def __init__(self): | ||||
| """Initialize the operator with available actions.""" | |||||
| self.actions = ["get_food", "get_hat"] | self.actions = ["get_food", "get_hat"] | ||||
| def on_event(self, event: dict, send_output) -> DoraStatus: | def on_event(self, event: dict, send_output) -> DoraStatus: | ||||
| """Handle incoming events and generate appropriate actions. | |||||
| Args: | |||||
| event: Dictionary containing event information | |||||
| send_output: Function to send output to the dataflow | |||||
| Returns: | |||||
| DoraStatus: Status indicating whether to continue processing | |||||
| """ | |||||
| if event["type"] == "INPUT": | if event["type"] == "INPUT": | ||||
| id = event["id"] | id = event["id"] | ||||
| # On initialization | # On initialization | ||||
| @@ -1,3 +1,9 @@ | |||||
| """Module for handling Intel RealSense camera data. | |||||
| This module provides functionality for capturing and processing data from | |||||
| Intel RealSense cameras, including depth and color images. | |||||
| """ | |||||
| import os | import os | ||||
| import cv2 | import cv2 | ||||
| @@ -1,3 +1,5 @@ | |||||
| """TODO: Add docstring.""" | |||||
| import time | import time | ||||
| import pandas as pd | import pandas as pd | ||||
| @@ -1,3 +1,9 @@ | |||||
| """Module for handling webcam input and processing. | |||||
| This module provides functionality for capturing and processing video input | |||||
| from webcam devices for robot vision applications. | |||||
| """ | |||||
| #!/usr/bin/env python3 | #!/usr/bin/env python3 | ||||
| import os | import os | ||||
| @@ -1,3 +1,9 @@ | |||||
| """Module for speech recognition using Whisper. | |||||
| This module provides functionality for capturing audio input and converting | |||||
| it to text using the Whisper speech recognition model. | |||||
| """ | |||||
| import numpy as np | import numpy as np | ||||
| import pyarrow as pa | import pyarrow as pa | ||||
| import sounddevice as sd | import sounddevice as sd | ||||
| @@ -14,7 +20,15 @@ node = Node() | |||||
| def get_text(duration) -> str: | def get_text(duration) -> str: | ||||
| """Capture audio and convert it to text using Whisper. | |||||
| Args: | |||||
| duration: Duration of audio to capture in seconds | |||||
| Returns: | |||||
| str: Transcribed text from the audio input | |||||
| """ | |||||
| ## Microphone | ## Microphone | ||||
| audio_data = sd.rec( | audio_data = sd.rec( | ||||
| int(SAMPLE_RATE * duration), | int(SAMPLE_RATE * duration), | ||||
| @@ -1,3 +1,5 @@ | |||||
| """TODO: Add docstring.""" | |||||
| import os | import os | ||||
| import cv2 | import cv2 | ||||
| @@ -1,3 +1,9 @@ | |||||
| """Module for interpolating between robot configurations. | |||||
| This module provides functionality for creating smooth transitions between | |||||
| robot configurations and movements. | |||||
| """ | |||||
| from enum import Enum | from enum import Enum | ||||
| import pyarrow as pa | import pyarrow as pa | ||||
| @@ -1,3 +1,9 @@ | |||||
| """Module for interpolating between keyframe positions. | |||||
| This module provides functionality for creating smooth transitions between | |||||
| keyframe positions in robot motion sequences. | |||||
| """ | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from dora import Node | from dora import Node | ||||
| @@ -1,3 +1,5 @@ | |||||
| """TODO: Add docstring.""" | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from dora import Node | from dora import Node | ||||
| @@ -1 +1,5 @@ | |||||
| """Module for handling vision-language model prompts. | |||||
| This module provides functionality for generating and processing prompts | |||||
| for vision-language models used in robot control. | |||||
| """ | |||||
| @@ -1,3 +1,9 @@ | |||||
| """Module for interpolating between voice commands. | |||||
| This module provides functionality for processing and interpolating between | |||||
| voice commands used for robot control. | |||||
| """ | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from dora import Node | from dora import Node | ||||
| @@ -1,3 +1,5 @@ | |||||
| """TODO: Add docstring.""" | |||||
| import time | import time | ||||
| import gym_dora # noqa: F401 | import gym_dora # noqa: F401 | ||||
| @@ -12,7 +14,20 @@ observation = env.reset() | |||||
| class ReplayPolicy: | class ReplayPolicy: | ||||
| """A policy class for replaying recorded robot actions. | |||||
| This class handles loading and replaying recorded actions from a dataset, | |||||
| maintaining timing between actions to match the original recording. | |||||
| """ | |||||
| def __init__(self, example_path, epidode=0): | def __init__(self, example_path, epidode=0): | ||||
| """Initialize the replay policy. | |||||
| Args: | |||||
| example_path: Path to the directory containing recorded actions | |||||
| epidode: Index of the episode to replay | |||||
| """ | |||||
| df_action = pd.read_parquet(example_path / "action.parquet") | df_action = pd.read_parquet(example_path / "action.parquet") | ||||
| df_episode_index = pd.read_parquet(example_path / "episode_index.parquet") | df_episode_index = pd.read_parquet(example_path / "episode_index.parquet") | ||||
| self.df = pd.merge_asof( | self.df = pd.merge_asof( | ||||
| @@ -29,6 +44,16 @@ class ReplayPolicy: | |||||
| self.finished = False | self.finished = False | ||||
| def select_action(self, obs): | def select_action(self, obs): | ||||
| """Select the next action to replay. | |||||
| Args: | |||||
| obs: Current observation from the environment (unused) | |||||
| Returns: | |||||
| tuple: (action, finished) where action is the next action to take | |||||
| and finished indicates if all actions have been replayed | |||||
| """ | |||||
| if self.index < len(self.df): | if self.index < len(self.df): | ||||
| self.index += 1 | self.index += 1 | ||||
| else: | else: | ||||
| @@ -42,7 +67,19 @@ class ReplayPolicy: | |||||
| class ReplayLeRobotPolicy: | class ReplayLeRobotPolicy: | ||||
| """A policy class for replaying actions from the LeRobot dataset. | |||||
| This class handles loading and replaying actions from the LeRobot dataset, | |||||
| maintaining the sequence of actions from the original recording. | |||||
| """ | |||||
| def __init__(self, episode=21): | def __init__(self, episode=21): | ||||
| """Initialize the LeRobot replay policy. | |||||
| Args: | |||||
| episode: Index of the episode to replay from the dataset | |||||
| """ | |||||
| self.index = 0 | self.index = 0 | ||||
| self.finished = False | self.finished = False | ||||
| # episode = 1 | # episode = 1 | ||||
| @@ -53,6 +90,16 @@ class ReplayLeRobotPolicy: | |||||
| self.actions = dataset.hf_dataset["action"][from_index:to_index] | self.actions = dataset.hf_dataset["action"][from_index:to_index] | ||||
| def select_action(self, obs): | def select_action(self, obs): | ||||
| """Select the next action to replay from the LeRobot dataset. | |||||
| Args: | |||||
| obs: Current observation from the environment (unused) | |||||
| Returns: | |||||
| tuple: (action, finished) where action is the next action to take | |||||
| and finished indicates if all actions have been replayed | |||||
| """ | |||||
| if self.index < len(self.actions): | if self.index < len(self.actions): | ||||
| self.index += 1 | self.index += 1 | ||||
| else: | else: | ||||
| @@ -1,3 +1,9 @@ | |||||
| """Module for handling keyboard input in the Reachy robot control system. | |||||
| This module provides functionality for capturing and processing keyboard input | |||||
| to control the Reachy robot's movements and actions. | |||||
| """ | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from dora import Node | from dora import Node | ||||
| from pynput import keyboard | from pynput import keyboard | ||||
| @@ -1,3 +1,5 @@ | |||||
| """TODO: Add docstring.""" | |||||
| #!/usr/bin/env python3 | #!/usr/bin/env python3 | ||||
| import os | import os | ||||
| @@ -1,3 +1,9 @@ | |||||
| """Module for visualizing robot data and states. | |||||
| This module provides functionality for plotting and displaying various aspects | |||||
| of the robot's state, including positions, movements, and sensor data. | |||||
| """ | |||||
| import os | import os | ||||
| import cv2 | import cv2 | ||||
| @@ -1,3 +1,5 @@ | |||||
| """TODO: Add docstring.""" | |||||
| import time | import time | ||||
| # import h5py | # import h5py | ||||
| @@ -1,3 +1,5 @@ | |||||
| """TODO: Add docstring.""" | |||||
| import time | import time | ||||
| import numpy as np | import numpy as np | ||||
| @@ -1,3 +1,5 @@ | |||||
| """TODO: Add docstring.""" | |||||
| import time | import time | ||||
| import pyarrow as pa | import pyarrow as pa | ||||
| @@ -1,3 +1,5 @@ | |||||
| """TODO: Add docstring.""" | |||||
| import time | import time | ||||
| import gym_dora # noqa: F401 | import gym_dora # noqa: F401 | ||||
| @@ -12,7 +14,10 @@ observation = env.reset() | |||||
| class ReplayPolicy: | class ReplayPolicy: | ||||
| """TODO: Add docstring.""" | |||||
| def __init__(self, example_path, epidode=0): | def __init__(self, example_path, epidode=0): | ||||
| """TODO: Add docstring.""" | |||||
| df_action = pd.read_parquet(example_path / "action.parquet") | df_action = pd.read_parquet(example_path / "action.parquet") | ||||
| df_episode_index = pd.read_parquet(example_path / "episode_index.parquet") | df_episode_index = pd.read_parquet(example_path / "episode_index.parquet") | ||||
| self.df = pd.merge_asof( | self.df = pd.merge_asof( | ||||
| @@ -29,6 +34,7 @@ class ReplayPolicy: | |||||
| self.finished = False | self.finished = False | ||||
| def select_action(self, obs): | def select_action(self, obs): | ||||
| """TODO: Add docstring.""" | |||||
| if self.index < len(self.df): | if self.index < len(self.df): | ||||
| self.index += 1 | self.index += 1 | ||||
| else: | else: | ||||
| @@ -42,7 +48,10 @@ class ReplayPolicy: | |||||
| class ReplayLeRobotPolicy: | class ReplayLeRobotPolicy: | ||||
| """TODO: Add docstring.""" | |||||
| def __init__(self, episode=21): | def __init__(self, episode=21): | ||||
| """TODO: Add docstring.""" | |||||
| self.index = 0 | self.index = 0 | ||||
| self.finished = False | self.finished = False | ||||
| # episode = 1 | # episode = 1 | ||||
| @@ -53,6 +62,7 @@ class ReplayLeRobotPolicy: | |||||
| self.actions = dataset.hf_dataset["action"][from_index:to_index] | self.actions = dataset.hf_dataset["action"][from_index:to_index] | ||||
| def select_action(self, obs): | def select_action(self, obs): | ||||
| """TODO: Add docstring.""" | |||||
| if self.index < len(self.actions): | if self.index < len(self.actions): | ||||
| self.index += 1 | self.index += 1 | ||||
| else: | else: | ||||
| @@ -1,3 +1,5 @@ | |||||
| """TODO: Add docstring.""" | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from dora import Node | from dora import Node | ||||
| @@ -1,3 +1,5 @@ | |||||
| """TODO: Add docstring.""" | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from dora import Node | from dora import Node | ||||
| from pynput import keyboard | from pynput import keyboard | ||||
| @@ -1,3 +1,9 @@ | |||||
| """Module for saving webcam feed data from LeRobot experiments. | |||||
| This module provides functionality for capturing and saving webcam feed data | |||||
| during LeRobot experiments with the Reachy robot. | |||||
| """ | |||||
| #!/usr/bin/env python3 | #!/usr/bin/env python3 | ||||
| import os | import os | ||||
| @@ -1,3 +1,9 @@ | |||||
| """Module for visualizing robot data and states. | |||||
| This module provides functionality for creating real-time visualizations of | |||||
| robot states, movements, and sensor data during operation. | |||||
| """ | |||||
| import os | import os | ||||
| import cv2 | import cv2 | ||||
| @@ -1,3 +1,9 @@ | |||||
| """Module for client-side communication with the Reachy robot. | |||||
| This module provides functionality for establishing and managing communication | |||||
| with the Reachy robot, including sending commands and receiving sensor data. | |||||
| """ | |||||
| import time | import time | ||||
| # import h5py | # import h5py | ||||
| @@ -1,3 +1,9 @@ | |||||
| """Module for handling vision-related communication with the Reachy robot. | |||||
| This module provides functionality for processing and managing visual data | |||||
| from the Reachy robot's cameras and vision sensors. | |||||
| """ | |||||
| import time | import time | ||||
| import numpy as np | import numpy as np | ||||
| @@ -1,3 +1,9 @@ | |||||
| """Module for replaying recorded robot actions and movements. | |||||
| This module provides functionality for loading and replaying previously recorded | |||||
| robot actions and movements, allowing for demonstration and analysis of robot behavior. | |||||
| """ | |||||
| import time | import time | ||||
| import pyarrow as pa | import pyarrow as pa | ||||
| @@ -1,3 +1,9 @@ | |||||
| """Module for interpolating between text-based robot commands. | |||||
| This module provides functionality for processing and interpolating between | |||||
| text-based commands to create smooth transitions in robot behavior. | |||||
| """ | |||||
| import pyarrow as pa | import pyarrow as pa | ||||
| from dora import Node | from dora import Node | ||||
| @@ -1,3 +1,9 @@ | |||||
| """Module for managing Feetech servo motor communication. | |||||
| This module provides functionality for communicating with and controlling | |||||
| Feetech servo motors through a serial bus interface. | |||||
| """ | |||||
| import enum | import enum | ||||
| from typing import Union | from typing import Union | ||||
| @@ -23,6 +29,16 @@ def wrap_joints_and_values( | |||||
| joints: Union[list[str], pa.Array], | joints: Union[list[str], pa.Array], | ||||
| values: Union[list[int], pa.Array], | values: Union[list[int], pa.Array], | ||||
| ) -> pa.StructArray: | ) -> pa.StructArray: | ||||
| """Create a structured array from joint names and their values. | |||||
| Args: | |||||
| joints: List of joint names or Arrow array of joint names | |||||
| values: List of values or Arrow array of values | |||||
| Returns: | |||||
| pa.StructArray: Structured array containing joint names and values | |||||
| """ | |||||
| return pa.StructArray.from_arrays( | return pa.StructArray.from_arrays( | ||||
| arrays=[joints, values], | arrays=[joints, values], | ||||
| names=["joints", "values"], | names=["joints", "values"], | ||||
| @@ -30,11 +46,15 @@ def wrap_joints_and_values( | |||||
| class TorqueMode(enum.Enum): | class TorqueMode(enum.Enum): | ||||
| """Enumeration of torque control modes for servo motors.""" | |||||
| ENABLED = pa.scalar(1, pa.uint32()) | ENABLED = pa.scalar(1, pa.uint32()) | ||||
| DISABLED = pa.scalar(0, pa.uint32()) | DISABLED = pa.scalar(0, pa.uint32()) | ||||
| class OperatingMode(enum.Enum): | class OperatingMode(enum.Enum): | ||||
| """Enumeration of operating modes for servo motors.""" | |||||
| ONE_TURN = pa.scalar(0, pa.uint32()) | ONE_TURN = pa.scalar(0, pa.uint32()) | ||||
| @@ -92,12 +112,21 @@ MODEL_CONTROL_TABLE = { | |||||
| class FeetechBus: | class FeetechBus: | ||||
| """A class for managing communication with Feetech servo motors. | |||||
| This class handles the low-level communication with Feetech servo motors | |||||
| through a serial bus interface, providing methods for reading and writing | |||||
| motor parameters. | |||||
| """ | |||||
| def __init__(self, port: str, description: dict[str, (np.uint8, str)]): | def __init__(self, port: str, description: dict[str, (np.uint8, str)]): | ||||
| """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. | |||||
| """Initialize the Feetech bus interface. | |||||
| 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 model. | |||||
| """ | """ | ||||
| self.port = port | self.port = port | ||||
| @@ -130,9 +159,17 @@ class FeetechBus: | |||||
| self.group_writers = {} | self.group_writers = {} | ||||
| def close(self): | def close(self): | ||||
| """Close the serial port connection.""" | |||||
| self.port_handler.closePort() | self.port_handler.closePort() | ||||
| def write(self, data_name: str, data: pa.StructArray): | def write(self, data_name: str, data: pa.StructArray): | ||||
| """Write data to the specified motor parameters. | |||||
| Args: | |||||
| data_name: Name of the parameter to write | |||||
| data: Structured array containing the data to write | |||||
| """ | |||||
| motor_ids = [ | motor_ids = [ | ||||
| self.motor_ctrl[motor_name.as_py()]["id"] | self.motor_ctrl[motor_name.as_py()]["id"] | ||||
| for motor_name in data.field("joints") | for motor_name in data.field("joints") | ||||
| @@ -199,6 +236,16 @@ class FeetechBus: | |||||
| ) | ) | ||||
| def read(self, data_name: str, motor_names: pa.Array) -> pa.StructArray: | def read(self, data_name: str, motor_names: pa.Array) -> pa.StructArray: | ||||
| """Read data from the specified motor parameters. | |||||
| Args: | |||||
| data_name: Name of the parameter to read | |||||
| motor_names: Array of motor names to read from | |||||
| Returns: | |||||
| pa.StructArray: Structured array containing the read data | |||||
| """ | |||||
| motor_ids = [ | motor_ids = [ | ||||
| self.motor_ctrl[motor_name.as_py()]["id"] for motor_name in motor_names | self.motor_ctrl[motor_name.as_py()]["id"] for motor_name in motor_names | ||||
| ] | ] | ||||
| @@ -249,25 +296,82 @@ class FeetechBus: | |||||
| return wrap_joints_and_values(motor_names, values) | return wrap_joints_and_values(motor_names, values) | ||||
| def write_torque_enable(self, torque_mode: pa.StructArray): | def write_torque_enable(self, torque_mode: pa.StructArray): | ||||
| """Enable or disable torque for the specified motors. | |||||
| Args: | |||||
| torque_mode: Structured array containing torque enable/disable values | |||||
| """ | |||||
| self.write("Torque_Enable", torque_mode) | self.write("Torque_Enable", torque_mode) | ||||
| def write_operating_mode(self, operating_mode: pa.StructArray): | def write_operating_mode(self, operating_mode: pa.StructArray): | ||||
| """Set the operating mode for the specified motors. | |||||
| Args: | |||||
| operating_mode: Structured array containing operating mode values | |||||
| """ | |||||
| self.write("Mode", operating_mode) | self.write("Mode", operating_mode) | ||||
| def read_position(self, motor_names: pa.Array) -> pa.StructArray: | def read_position(self, motor_names: pa.Array) -> pa.StructArray: | ||||
| """Read current position from the specified motors. | |||||
| Args: | |||||
| motor_names: Array of motor names to read positions from | |||||
| Returns: | |||||
| pa.StructArray: Structured array containing position values | |||||
| """ | |||||
| return self.read("Present_Position", motor_names) | return self.read("Present_Position", motor_names) | ||||
| def read_velocity(self, motor_names: pa.Array) -> pa.StructArray: | def read_velocity(self, motor_names: pa.Array) -> pa.StructArray: | ||||
| """Read current velocity from the specified motors. | |||||
| Args: | |||||
| motor_names: Array of motor names to read velocities from | |||||
| Returns: | |||||
| pa.StructArray: Structured array containing velocity values | |||||
| """ | |||||
| return self.read("Present_Velocity", motor_names) | return self.read("Present_Velocity", motor_names) | ||||
| def read_current(self, motor_names: pa.Array) -> pa.StructArray: | def read_current(self, motor_names: pa.Array) -> pa.StructArray: | ||||
| """Read current current from the specified motors. | |||||
| Args: | |||||
| motor_names: Array of motor names to read currents from | |||||
| Returns: | |||||
| pa.StructArray: Structured array containing current values | |||||
| """ | |||||
| return self.read("Present_Current", motor_names) | return self.read("Present_Current", motor_names) | ||||
| def write_goal_position(self, goal_position: pa.StructArray): | def write_goal_position(self, goal_position: pa.StructArray): | ||||
| """Set goal positions for the specified motors. | |||||
| Args: | |||||
| goal_position: Structured array containing goal position values | |||||
| """ | |||||
| self.write("Goal_Position", goal_position) | self.write("Goal_Position", goal_position) | ||||
| def write_max_angle_limit(self, max_angle_limit: pa.StructArray): | def write_max_angle_limit(self, max_angle_limit: pa.StructArray): | ||||
| """Set maximum angle limits for the specified motors. | |||||
| Args: | |||||
| max_angle_limit: Structured array containing maximum angle limit values | |||||
| """ | |||||
| self.write("Max_Angle_Limit", max_angle_limit) | self.write("Max_Angle_Limit", max_angle_limit) | ||||
| def write_min_angle_limit(self, min_angle_limit: pa.StructArray): | def write_min_angle_limit(self, min_angle_limit: pa.StructArray): | ||||
| """Set minimum angle limits for the specified motors. | |||||
| Args: | |||||
| min_angle_limit: Structured array containing minimum angle limit values | |||||
| """ | |||||
| self.write("Min_Angle_Limit", min_angle_limit) | self.write("Min_Angle_Limit", min_angle_limit) | ||||
| @@ -1,4 +1,7 @@ | |||||
| """SO100 Auto Configure: This program is used to automatically configure the SO-ARM100 (SO100) for the user. | |||||
| """Module for configuring and setting up the SO100 robot hardware. | |||||
| This module provides functionality for initializing and configuring the SO100 robot's | |||||
| servo motors and other hardware components. | |||||
| The program will: | The program will: | ||||
| 1. Disable all torque motors of provided SO100. | 1. Disable all torque motors of provided SO100. | ||||
| @@ -55,6 +58,7 @@ def configure_servos(bus: FeetechBus): | |||||
| Args: | Args: | ||||
| bus: The FeetechBus instance to configure. | 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), | ||||
| @@ -74,7 +78,7 @@ def configure_servos(bus: FeetechBus): | |||||
| def main(): | def main(): | ||||
| """Main function to run the servo configuration process.""" | |||||
| """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.", | ||||
| @@ -1,3 +1,5 @@ | |||||
| """TODO: Add docstring.""" | |||||
| import argparse | import argparse | ||||
| import json | import json | ||||
| import os | import os | ||||
| @@ -14,7 +16,7 @@ from pwm_position_control.transform import ( | |||||
| def main(): | def main(): | ||||
| """Main function to handle interpolation between LCR and SO100 robot configurations.""" | |||||
| """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 " | ||||
| @@ -1,3 +1,9 @@ | |||||
| """Module for recording and interpolating between LCR and SO100 configurations. | |||||
| This module provides functionality for recording robot configurations and | |||||
| interpolating between LCR and SO100 robots to create smooth motion sequences. | |||||
| """ | |||||
| import argparse | import argparse | ||||
| import json | import json | ||||
| import os | import os | ||||
| @@ -13,7 +19,7 @@ from pwm_position_control.transform import ( | |||||
| def main(): | def main(): | ||||
| """Main function to handle interpolation and recording between LCR and SO100 robot configurations.""" | |||||
| """Handle interpolation and recording between LCR and SO100 configurations.""" | |||||
| parser = argparse.ArgumentParser( | parser = argparse.ArgumentParser( | ||||
| description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " | description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " | ||||
| "LCR followers knowing a Leader position and Follower position.", | "LCR followers knowing a Leader position and Follower position.", | ||||
| @@ -1,3 +1,9 @@ | |||||
| """Module for interpolating between replay and SO100 robot configurations. | |||||
| This module provides functionality for interpolating between recorded robot actions | |||||
| and SO100 robot configurations to create smooth motion sequences. | |||||
| """ | |||||
| import argparse | import argparse | ||||
| import json | import json | ||||
| import os | import os | ||||
| @@ -8,7 +14,7 @@ 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 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 " | ||||
| @@ -1,3 +1,5 @@ | |||||
| """TODO: Add docstring.""" | |||||
| from dora import run | from dora import run | ||||
| # Make sure to build it first with the CLI. | # Make sure to build it first with the CLI. | ||||