Browse Source

Fixed necessary linting issues

tags/v0.3.11-rc1
7SOMAY 10 months ago
parent
commit
abf77bfb75
48 changed files with 771 additions and 45 deletions
  1. +14
    -0
      examples/alexk-lcr/bus.py
  2. +12
    -1
      examples/alexk-lcr/configure.py
  3. +7
    -0
      examples/alexk-lcr/nodes/interpolate_lcr_to_lcr.py
  4. +3
    -0
      examples/alexk-lcr/nodes/interpolate_lcr_to_record.py
  5. +7
    -0
      examples/alexk-lcr/nodes/interpolate_lcr_to_simu_lcr.py
  6. +3
    -0
      examples/alexk-lcr/nodes/interpolate_replay_to_lcr.py
  7. +219
    -6
      examples/aloha/benchmark/python/dynamixel.py
  8. +57
    -12
      examples/aloha/benchmark/python/robot.py
  9. +6
    -0
      examples/aloha/benchmark/python/teleoperate_real_robot.py
  10. +6
    -0
      examples/aloha/benchmark/ros2/teleop.py
  11. +29
    -0
      examples/aloha/nodes/gym_dora_node.py
  12. +6
    -0
      examples/aloha/nodes/keyboard_node.py
  13. +2
    -0
      examples/aloha/nodes/lerobot_webcam_saver.py
  14. +71
    -17
      examples/aloha/nodes/llm_op.py
  15. +2
    -0
      examples/aloha/nodes/plot_node.py
  16. +23
    -0
      examples/aloha/nodes/policy.py
  17. +6
    -0
      examples/aloha/nodes/realsense_node.py
  18. +2
    -0
      examples/aloha/nodes/replay.py
  19. +6
    -0
      examples/aloha/nodes/webcam.py
  20. +14
    -0
      examples/aloha/nodes/whisper_node.py
  21. +2
    -0
      examples/aloha/tests/test_realsense.py
  22. +6
    -0
      examples/lebai/nodes/interpolation.py
  23. +6
    -0
      examples/lebai/nodes/key_interpolation.py
  24. +2
    -0
      examples/lebai/nodes/prompt_interpolation.py
  25. +4
    -0
      examples/lebai/nodes/vlm_prompt.py
  26. +6
    -0
      examples/lebai/nodes/voice_interpolation.py
  27. +47
    -0
      examples/reachy/nodes/gym_dora_node.py
  28. +6
    -0
      examples/reachy/nodes/keyboard_node.py
  29. +2
    -0
      examples/reachy/nodes/lerobot_webcam_saver.py
  30. +6
    -0
      examples/reachy/nodes/plot_node.py
  31. +2
    -0
      examples/reachy/nodes/reachy_client.py
  32. +2
    -0
      examples/reachy/nodes/reachy_vision_client.py
  33. +2
    -0
      examples/reachy/nodes/replay_node.py
  34. +10
    -0
      examples/reachy1/nodes/gym_dora_node.py
  35. +2
    -0
      examples/reachy1/nodes/key_interpolation.py
  36. +2
    -0
      examples/reachy1/nodes/keyboard_node.py
  37. +6
    -0
      examples/reachy1/nodes/lerobot_webcam_saver.py
  38. +6
    -0
      examples/reachy1/nodes/plot_node.py
  39. +6
    -0
      examples/reachy1/nodes/reachy_client.py
  40. +6
    -0
      examples/reachy1/nodes/reachy_vision_client.py
  41. +6
    -0
      examples/reachy1/nodes/replay_node.py
  42. +6
    -0
      examples/reachy1/nodes/text_interpolation.py
  43. +108
    -4
      examples/so100/bus.py
  44. +6
    -2
      examples/so100/configure.py
  45. +3
    -1
      examples/so100/nodes/interpolate_lcr_to_so100.py
  46. +7
    -1
      examples/so100/nodes/interpolate_lcr_x_so100_to_record.py
  47. +7
    -1
      examples/so100/nodes/interpolate_replay_to_so100.py
  48. +2
    -0
      tests/llm/run_script.py

+ 14
- 0
examples/alexk-lcr/bus.py View File

@@ -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)

+ 12
- 1
examples/alexk-lcr/configure.py View File

@@ -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.",


+ 7
- 0
examples/alexk-lcr/nodes/interpolate_lcr_to_lcr.py View File

@@ -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.",


+ 3
- 0
examples/alexk-lcr/nodes/interpolate_lcr_to_record.py View File

@@ -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.",


+ 7
- 0
examples/alexk-lcr/nodes/interpolate_lcr_to_simu_lcr.py View File

@@ -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.",


+ 3
- 0
examples/alexk-lcr/nodes/interpolate_replay_to_lcr.py View File

@@ -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.",


+ 219
- 6
examples/aloha/benchmark/python/dynamixel.py View File

@@ -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)


+ 57
- 12
examples/aloha/benchmark/python/robot.py View File

@@ -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 = [


+ 6
- 0
examples/aloha/benchmark/python/teleoperate_real_robot.py View File

@@ -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




+ 6
- 0
examples/aloha/benchmark/ros2/teleop.py View File

@@ -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


+ 29
- 0
examples/aloha/nodes/gym_dora_node.py View File

@@ -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:


+ 6
- 0
examples/aloha/nodes/keyboard_node.py View File

@@ -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


+ 2
- 0
examples/aloha/nodes/lerobot_webcam_saver.py View File

@@ -1,3 +1,5 @@
"""TODO: Add docstring."""

#!/usr/bin/env python3 #!/usr/bin/env python3


import os import os


+ 71
- 17
examples/aloha/nodes/llm_op.py View File

@@ -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")


+ 2
- 0
examples/aloha/nodes/plot_node.py View File

@@ -1,3 +1,5 @@
"""TODO: Add docstring."""

import os import os


import cv2 import cv2


+ 23
- 0
examples/aloha/nodes/policy.py View File

@@ -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


+ 6
- 0
examples/aloha/nodes/realsense_node.py View File

@@ -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


+ 2
- 0
examples/aloha/nodes/replay.py View File

@@ -1,3 +1,5 @@
"""TODO: Add docstring."""

import time import time


import pandas as pd import pandas as pd


+ 6
- 0
examples/aloha/nodes/webcam.py View File

@@ -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


+ 14
- 0
examples/aloha/nodes/whisper_node.py View File

@@ -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),


+ 2
- 0
examples/aloha/tests/test_realsense.py View File

@@ -1,3 +1,5 @@
"""TODO: Add docstring."""

import os import os


import cv2 import cv2


+ 6
- 0
examples/lebai/nodes/interpolation.py View File

@@ -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


+ 6
- 0
examples/lebai/nodes/key_interpolation.py View File

@@ -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




+ 2
- 0
examples/lebai/nodes/prompt_interpolation.py View File

@@ -1,3 +1,5 @@
"""TODO: Add docstring."""

import pyarrow as pa import pyarrow as pa
from dora import Node from dora import Node




+ 4
- 0
examples/lebai/nodes/vlm_prompt.py View File

@@ -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.
"""

+ 6
- 0
examples/lebai/nodes/voice_interpolation.py View File

@@ -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




+ 47
- 0
examples/reachy/nodes/gym_dora_node.py View File

@@ -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:


+ 6
- 0
examples/reachy/nodes/keyboard_node.py View File

@@ -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


+ 2
- 0
examples/reachy/nodes/lerobot_webcam_saver.py View File

@@ -1,3 +1,5 @@
"""TODO: Add docstring."""

#!/usr/bin/env python3 #!/usr/bin/env python3


import os import os


+ 6
- 0
examples/reachy/nodes/plot_node.py View File

@@ -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


+ 2
- 0
examples/reachy/nodes/reachy_client.py View File

@@ -1,3 +1,5 @@
"""TODO: Add docstring."""

import time import time


# import h5py # import h5py


+ 2
- 0
examples/reachy/nodes/reachy_vision_client.py View File

@@ -1,3 +1,5 @@
"""TODO: Add docstring."""

import time import time


import numpy as np import numpy as np


+ 2
- 0
examples/reachy/nodes/replay_node.py View File

@@ -1,3 +1,5 @@
"""TODO: Add docstring."""

import time import time


import pyarrow as pa import pyarrow as pa


+ 10
- 0
examples/reachy1/nodes/gym_dora_node.py View File

@@ -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:


+ 2
- 0
examples/reachy1/nodes/key_interpolation.py View File

@@ -1,3 +1,5 @@
"""TODO: Add docstring."""

import pyarrow as pa import pyarrow as pa
from dora import Node from dora import Node




+ 2
- 0
examples/reachy1/nodes/keyboard_node.py View File

@@ -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


+ 6
- 0
examples/reachy1/nodes/lerobot_webcam_saver.py View File

@@ -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


+ 6
- 0
examples/reachy1/nodes/plot_node.py View File

@@ -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


+ 6
- 0
examples/reachy1/nodes/reachy_client.py View File

@@ -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


+ 6
- 0
examples/reachy1/nodes/reachy_vision_client.py View File

@@ -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


+ 6
- 0
examples/reachy1/nodes/replay_node.py View File

@@ -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


+ 6
- 0
examples/reachy1/nodes/text_interpolation.py View File

@@ -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




+ 108
- 4
examples/so100/bus.py View File

@@ -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)

+ 6
- 2
examples/so100/configure.py View File

@@ -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.",


+ 3
- 1
examples/so100/nodes/interpolate_lcr_to_so100.py View File

@@ -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 "


+ 7
- 1
examples/so100/nodes/interpolate_lcr_x_so100_to_record.py View File

@@ -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.",


+ 7
- 1
examples/so100/nodes/interpolate_replay_to_so100.py View File

@@ -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 "


+ 2
- 0
tests/llm/run_script.py View File

@@ -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.


Loading…
Cancel
Save