Browse Source

Added "PERF" flag inside node-hub (#880)

#878 - Added PERF flag by using a custom python script
tags/v0.3.11-rc1
Haixuan Xavier Tao GitHub 10 months ago
parent
commit
86fd37e68a
No known key found for this signature in database GPG Key ID: B5690EEEBB952194
100 changed files with 1337 additions and 534 deletions
  1. +4
    -4
      .github/workflows/ci.yml
  2. +1
    -2
      apis/python/node/generate_stubs.py
  3. +1
    -0
      binaries/cli/src/template/python/listener/listener-template.py
  4. +1
    -0
      binaries/cli/src/template/python/talker/talker-template.py
  5. +141
    -34
      examples/alexk-lcr/bus.py
  6. +29
    -23
      examples/alexk-lcr/configure.py
  7. +18
    -13
      examples/alexk-lcr/nodes/interpolate_lcr_to_lcr.py
  8. +13
    -12
      examples/alexk-lcr/nodes/interpolate_lcr_to_record.py
  9. +14
    -9
      examples/alexk-lcr/nodes/interpolate_lcr_to_simu_lcr.py
  10. +9
    -7
      examples/alexk-lcr/nodes/interpolate_replay_to_lcr.py
  11. +258
    -48
      examples/aloha/benchmark/python/dynamixel.py
  12. +68
    -30
      examples/aloha/benchmark/python/robot.py
  13. +9
    -3
      examples/aloha/benchmark/python/teleoperate_real_robot.py
  14. +17
    -12
      examples/aloha/benchmark/ros2/teleop.py
  15. +35
    -6
      examples/aloha/nodes/gym_dora_node.py
  16. +10
    -5
      examples/aloha/nodes/keyboard_node.py
  17. +8
    -8
      examples/aloha/nodes/lerobot_webcam_saver.py
  18. +84
    -31
      examples/aloha/nodes/llm_op.py
  19. +3
    -1
      examples/aloha/nodes/plot_node.py
  20. +23
    -1
      examples/aloha/nodes/policy.py
  21. +11
    -4
      examples/aloha/nodes/realsense_node.py
  22. +5
    -3
      examples/aloha/nodes/replay.py
  23. +10
    -6
      examples/aloha/nodes/webcam.py
  24. +18
    -8
      examples/aloha/nodes/whisper_node.py
  25. +6
    -3
      examples/aloha/tests/test_realsense.py
  26. +10
    -5
      examples/lebai/nodes/interpolation.py
  27. +7
    -1
      examples/lebai/nodes/key_interpolation.py
  28. +5
    -3
      examples/lebai/nodes/prompt_interpolation.py
  29. +4
    -0
      examples/lebai/nodes/vlm_prompt.py
  30. +7
    -1
      examples/lebai/nodes/voice_interpolation.py
  31. +49
    -4
      examples/reachy/nodes/gym_dora_node.py
  32. +10
    -5
      examples/reachy/nodes/keyboard_node.py
  33. +8
    -8
      examples/reachy/nodes/lerobot_webcam_saver.py
  34. +7
    -1
      examples/reachy/nodes/plot_node.py
  35. +14
    -15
      examples/reachy/nodes/reachy_client.py
  36. +2
    -6
      examples/reachy/nodes/reachy_vision_client.py
  37. +5
    -2
      examples/reachy/nodes/replay_node.py
  38. +12
    -4
      examples/reachy1/nodes/gym_dora_node.py
  39. +3
    -1
      examples/reachy1/nodes/key_interpolation.py
  40. +6
    -5
      examples/reachy1/nodes/keyboard_node.py
  41. +12
    -8
      examples/reachy1/nodes/lerobot_webcam_saver.py
  42. +7
    -1
      examples/reachy1/nodes/plot_node.py
  43. +18
    -15
      examples/reachy1/nodes/reachy_client.py
  44. +6
    -6
      examples/reachy1/nodes/reachy_vision_client.py
  45. +9
    -2
      examples/reachy1/nodes/replay_node.py
  46. +15
    -10
      examples/reachy1/nodes/text_interpolation.py
  47. +118
    -14
      examples/so100/bus.py
  48. +28
    -21
      examples/so100/configure.py
  49. +14
    -13
      examples/so100/nodes/interpolate_lcr_to_so100.py
  50. +17
    -12
      examples/so100/nodes/interpolate_lcr_x_so100_to_record.py
  51. +13
    -9
      examples/so100/nodes/interpolate_replay_to_so100.py
  52. +2
    -1
      node-hub/dora-argotranslate/pyproject.toml
  53. +1
    -1
      node-hub/dora-distil-whisper/dora_distil_whisper/main.py
  54. +3
    -2
      node-hub/dora-distil-whisper/pyproject.toml
  55. +2
    -1
      node-hub/dora-echo/pyproject.toml
  56. +2
    -1
      node-hub/dora-internvl/pyproject.toml
  57. +2
    -1
      node-hub/dora-ios-lidar/pyproject.toml
  58. +2
    -1
      node-hub/dora-keyboard/pyproject.toml
  59. +2
    -1
      node-hub/dora-kit-car/pyproject.toml
  60. +2
    -1
      node-hub/dora-kokoro-tts/pyproject.toml
  61. +0
    -1
      node-hub/dora-llama-cpp-python/dora_llama_cpp_python/__main__.py
  62. +11
    -10
      node-hub/dora-llama-cpp-python/dora_llama_cpp_python/main.py
  63. +2
    -1
      node-hub/dora-llama-cpp-python/pyproject.toml
  64. +1
    -1
      node-hub/dora-magma/dora_magma/__init__.py
  65. +6
    -6
      node-hub/dora-magma/dora_magma/main.py
  66. +2
    -1
      node-hub/dora-magma/pyproject.toml
  67. +1
    -1
      node-hub/dora-magma/tests/test_magma_node.py
  68. +2
    -1
      node-hub/dora-microphone/pyproject.toml
  69. +2
    -1
      node-hub/dora-object-to-pose/pyproject.toml
  70. +2
    -2
      node-hub/dora-openai-server/pyproject.toml
  71. +0
    -1
      node-hub/dora-openai-server/tests/test_dora_openai_server.py
  72. +2
    -1
      node-hub/dora-opus/pyproject.toml
  73. +1
    -0
      node-hub/dora-outtetts/dora_outtetts/tests/test_main.py
  74. +2
    -1
      node-hub/dora-outtetts/pyproject.toml
  75. +2
    -1
      node-hub/dora-parler/pyproject.toml
  76. +3
    -3
      node-hub/dora-phi4/dora_phi4/main.py
  77. +2
    -1
      node-hub/dora-phi4/pyproject.toml
  78. +2
    -1
      node-hub/dora-piper/pyproject.toml
  79. +2
    -1
      node-hub/dora-pyaudio/pyproject.toml
  80. +2
    -1
      node-hub/dora-pyorbbecksdk/pyproject.toml
  81. +2
    -1
      node-hub/dora-pyrealsense/pyproject.toml
  82. +2
    -1
      node-hub/dora-qwen/pyproject.toml
  83. +2
    -1
      node-hub/dora-qwen2-5-vl/pyproject.toml
  84. +2
    -1
      node-hub/dora-qwenvl/pyproject.toml
  85. +2
    -1
      node-hub/dora-rdt-1b/pyproject.toml
  86. +4
    -4
      node-hub/dora-reachy1/dora_reachy1/main.py
  87. +3
    -2
      node-hub/dora-reachy1/dora_reachy1_vision/main.py
  88. +2
    -1
      node-hub/dora-reachy1/pyproject.toml
  89. +0
    -1
      node-hub/dora-reachy1/tests/test_dora_reachy1.py
  90. +2
    -1
      node-hub/dora-reachy2/pyproject.toml
  91. +0
    -1
      node-hub/dora-reachy2/tests/test_dora_reachy2.py
  92. +2
    -1
      node-hub/dora-rerun/pyproject.toml
  93. +2
    -1
      node-hub/dora-sam2/pyproject.toml
  94. +3
    -1
      node-hub/dora-transformers/dora_transformers/__init__.py
  95. +2
    -1
      node-hub/dora-transformers/dora_transformers/__main__.py
  96. +25
    -22
      node-hub/dora-transformers/dora_transformers/main.py
  97. +8
    -0
      node-hub/dora-transformers/pyproject.toml
  98. +3
    -0
      node-hub/dora-transformers/tests/test_dora_transformers.py
  99. +2
    -1
      node-hub/dora-ugv/pyproject.toml
  100. +2
    -1
      node-hub/dora-vad/pyproject.toml

+ 4
- 4
.github/workflows/ci.yml View File

@@ -280,7 +280,7 @@ jobs:
run: | run: |
cargo install --path binaries/cli --locked cargo install --path binaries/cli --locked
- name: "Test CLI (Rust)" - name: "Test CLI (Rust)"
timeout-minutes: 30
timeout-minutes: 45
# fail-fast by using bash shell explictly # fail-fast by using bash shell explictly
shell: bash shell: bash
run: | run: |
@@ -312,7 +312,7 @@ jobs:
enable-cache: true enable-cache: true


- name: "Test CLI (Python)" - name: "Test CLI (Python)"
timeout-minutes: 30
timeout-minutes: 45
# fail-fast by using bash shell explictly # fail-fast by using bash shell explictly
shell: bash shell: bash
run: | run: |
@@ -380,7 +380,7 @@ jobs:
dora run tests/queue_size_latest_data_rust/dataflow.yaml --uv dora run tests/queue_size_latest_data_rust/dataflow.yaml --uv


- name: "Test CLI (C)" - name: "Test CLI (C)"
timeout-minutes: 30
timeout-minutes: 45
# fail-fast by using bash shell explictly # fail-fast by using bash shell explictly
shell: bash shell: bash
if: runner.os == 'Linux' if: runner.os == 'Linux'
@@ -399,7 +399,7 @@ jobs:
dora destroy dora destroy


- name: "Test CLI (C++)" - name: "Test CLI (C++)"
timeout-minutes: 30
timeout-minutes: 45
# fail-fast by using bash shell explictly # fail-fast by using bash shell explictly
shell: bash shell: bash
if: runner.os == 'Linux' if: runner.os == 'Linux'


+ 1
- 2
apis/python/node/generate_stubs.py View File

@@ -287,8 +287,7 @@ def arguments_stub(
param_names = list(real_parameters.keys()) param_names = list(real_parameters.keys())
if param_names and param_names[0] == "self": if param_names and param_names[0] == "self":
del param_names[0] del param_names[0]
for name, t in zip(param_names, builtin[0]):
parsed_param_types[name] = t
parsed_param_types = {name: t for name, t in zip(param_names, builtin[0])}


# Types from comment # Types from comment
for match in re.findall( for match in re.findall(


+ 1
- 0
binaries/cli/src/template/python/listener/listener-template.py View File

@@ -2,6 +2,7 @@


from dora import Node from dora import Node



def main(): def main():
"""Listen for input events and print received messages.""" """Listen for input events and print received messages."""
node = Node() node = Node()


+ 1
- 0
binaries/cli/src/template/python/talker/talker-template.py View File

@@ -3,6 +3,7 @@
import pyarrow as pa import pyarrow as pa
from dora import Node from dora import Node



def main(): def main():
"""Process node input events and send speech output.""" """Process node input events and send speech output."""
node = Node() node = Node()


+ 141
- 34
examples/alexk-lcr/bus.py View File

@@ -1,17 +1,25 @@
import enum
"""Module for managing Dynamixel servo bus communication and control.


import pyarrow as pa
This module provides functionality for communicating with and controlling Dynamixel servos
through a serial bus interface. It includes methods for reading and writing servo parameters,
managing torque and operating modes, and handling joint positions and velocities.
"""


import enum
from typing import Union from typing import Union


import pyarrow as pa
from dynamixel_sdk import ( from dynamixel_sdk import (
PacketHandler,
PortHandler,
COMM_SUCCESS, COMM_SUCCESS,
DXL_HIBYTE,
DXL_HIWORD,
DXL_LOBYTE,
DXL_LOWORD,
GroupSyncRead, GroupSyncRead,
GroupSyncWrite, GroupSyncWrite,
PacketHandler,
PortHandler,
) )
from dynamixel_sdk import DXL_HIBYTE, DXL_HIWORD, DXL_LOBYTE, DXL_LOWORD


PROTOCOL_VERSION = 2.0 PROTOCOL_VERSION = 2.0
BAUD_RATE = 1_000_000 BAUD_RATE = 1_000_000
@@ -20,40 +28,38 @@ TIMEOUT_MS = 1000


def wrap_joints_and_values( def wrap_joints_and_values(
joints: Union[list[str], pa.Array], joints: Union[list[str], pa.Array],
values: Union[int, list[int], pa.Array],
values: Union[list[int], pa.Array],
) -> pa.StructArray: ) -> pa.StructArray:
"""
Wraps joints and their corresponding values into a structured array.
"""Wrap joints and their corresponding values into a structured array.


:param joints: A list, numpy array, or pyarrow array of joint names.
:type joints: Union[list[str], np.array, pa.Array]
:param values: A single integer value, or a list, numpy array, or pyarrow array of integer values.
If a single integer is provided, it will be broadcasted to all joints.
:type values: Union[int, list[int], np.array, pa.Array]
Args:
joints: A list, numpy array, or pyarrow array of joint names.
values: A single integer value, or a list, numpy array, or pyarrow array of integer values.
If a single integer is provided, it will be broadcasted to all joints.


:return: A structured array with two fields:
- "joints": A string field containing the names of the joints.
- "values": An Int32Array containing the values corresponding to the joints.
:rtype: pa.StructArray
Returns:
A structured array with two fields:
- "joints": A string field containing the names of the joints.
- "values": An Int32Array containing the values corresponding to the joints.


:raises ValueError: If lengths of joints and values do not match.
Raises:
ValueError: If lengths of joints and values do not match.


Example: Example:
--------
joints = ["shoulder_pan", "shoulder_lift", "elbow_flex"]
values = [100, 200, 300]
struct_array = wrap_joints_and_values(joints, values)
joints = ["shoulder_pan", "shoulder_lift", "elbow_flex"]
values = [100, 200, 300]
struct_array = wrap_joints_and_values(joints, values)


This example wraps the given joints and their corresponding values into a structured array.
This example wraps the given joints and their corresponding values into a structured array.


Another example with a single integer value:
joints = ["shoulder_pan", "shoulder_lift", "elbow_flex"]
value = 150
struct_array = wrap_joints_and_values(joints, value)
Another example with a single integer value:
joints = ["shoulder_pan", "shoulder_lift", "elbow_flex"]
value = 150
struct_array = wrap_joints_and_values(joints, value)


This example broadcasts the single integer value to all joints and wraps them into a structured array.
"""
This example broadcasts the single integer value to all joints and wraps them into a structured array.


"""
if isinstance(values, int): if isinstance(values, int):
values = [values] * len(joints) values = [values] * len(joints)


@@ -69,16 +75,20 @@ def wrap_joints_and_values(
mask = values.is_null() mask = values.is_null()


return pa.StructArray.from_arrays( return pa.StructArray.from_arrays(
arrays=[joints, values], names=["joints", "values"], mask=mask
arrays=[joints, values], names=["joints", "values"], mask=mask,
).drop_null() ).drop_null()




class TorqueMode(enum.Enum): class TorqueMode(enum.Enum):
"""Enumeration for servo torque control modes."""

ENABLED = pa.scalar(1, pa.uint32()) ENABLED = pa.scalar(1, pa.uint32())
DISABLED = pa.scalar(0, pa.uint32()) DISABLED = pa.scalar(0, pa.uint32())




class OperatingMode(enum.Enum): class OperatingMode(enum.Enum):
"""Enumeration for servo operating modes."""

VELOCITY = pa.scalar(1, pa.uint32()) VELOCITY = pa.scalar(1, pa.uint32())
POSITION = pa.scalar(3, pa.uint32()) POSITION = pa.scalar(3, pa.uint32())
EXTENDED_POSITION = pa.scalar(4, pa.uint32()) EXTENDED_POSITION = pa.scalar(4, pa.uint32())
@@ -152,8 +162,18 @@ MODEL_CONTROL_TABLE = {




class DynamixelBus: class DynamixelBus:
"""Class for managing communication with Dynamixel servos on a serial bus."""


def __init__(self, port: str, description: dict[str, (int, str)]): def __init__(self, port: str, description: dict[str, (int, str)]):
"""Initialize the Dynamixel bus connection.

Args:
port: The serial port to connect to the Dynamixel bus.
description: A dictionary containing the description of the motors connected to the bus.
The keys are the motor names and the values are tuples containing the motor id
and the motor model.

"""
self.port = port self.port = port
self.descriptions = description self.descriptions = description
self.motor_ctrl = {} self.motor_ctrl = {}
@@ -184,9 +204,17 @@ class DynamixelBus:
self.group_writers = {} self.group_writers = {}


def close(self): def close(self):
"""Close the serial port connection."""
self.port_handler.closePort() self.port_handler.closePort()


def write(self, data_name: str, data: pa.StructArray): def write(self, data_name: str, data: pa.StructArray):
"""Write data to the specified servo parameters.

Args:
data_name: Name of the parameter to write.
data: Structured array containing the data to write.

"""
motor_ids = [ motor_ids = [
self.motor_ctrl[motor_name.as_py()]["id"] self.motor_ctrl[motor_name.as_py()]["id"]
for motor_name in data.field("joints") for motor_name in data.field("joints")
@@ -239,7 +267,7 @@ class DynamixelBus:
else: else:
raise NotImplementedError( raise NotImplementedError(
f"Value of the number of bytes to be sent is expected to be in [1, 2, 4], but {packet_bytes_size} " f"Value of the number of bytes to be sent is expected to be in [1, 2, 4], but {packet_bytes_size} "
f"is provided instead."
f"is provided instead.",
) )


if init_group: if init_group:
@@ -251,10 +279,20 @@ class DynamixelBus:
if comm != COMM_SUCCESS: if comm != COMM_SUCCESS:
raise ConnectionError( raise ConnectionError(
f"Write failed due to communication error on port {self.port} for group_key {group_key}: " f"Write failed due to communication error on port {self.port} for group_key {group_key}: "
f"{self.packet_handler.getTxRxResult(comm)}"
f"{self.packet_handler.getTxRxResult(comm)}",
) )


def read(self, data_name: str, motor_names: pa.Array) -> pa.StructArray: def read(self, data_name: str, motor_names: pa.Array) -> pa.StructArray:
"""Read data from the specified servo parameters.

Args:
data_name: Name of the parameter to read.
motor_names: Array of motor names to read from.

Returns:
Structured array containing the read data.

"""
motor_ids = [ motor_ids = [
self.motor_ctrl[motor_name.as_py()]["id"] for motor_name in motor_names self.motor_ctrl[motor_name.as_py()]["id"] for motor_name in motor_names
] ]
@@ -281,13 +319,13 @@ class DynamixelBus:
if comm != COMM_SUCCESS: if comm != COMM_SUCCESS:
raise ConnectionError( raise ConnectionError(
f"Read failed due to communication error on port {self.port} for group_key {group_key}: " f"Read failed due to communication error on port {self.port} for group_key {group_key}: "
f"{self.packet_handler.getTxRxResult(comm)}"
f"{self.packet_handler.getTxRxResult(comm)}",
) )


values = pa.array( values = pa.array(
[ [
self.group_readers[group_key].getData( self.group_readers[group_key].getData(
idx, packet_address, packet_bytes_size
idx, packet_address, packet_bytes_size,
) )
for idx in motor_ids for idx in motor_ids
], ],
@@ -298,31 +336,100 @@ class DynamixelBus:
return wrap_joints_and_values(motor_names, values) return wrap_joints_and_values(motor_names, values)


def write_torque_enable(self, torque_mode: pa.StructArray): def write_torque_enable(self, torque_mode: pa.StructArray):
"""Enable or disable torque for the specified servos.

Args:
torque_mode: Structured array containing the torque mode for each servo.

"""
self.write("Torque_Enable", torque_mode) self.write("Torque_Enable", torque_mode)


def write_operating_mode(self, operating_mode: pa.StructArray): def write_operating_mode(self, operating_mode: pa.StructArray):
"""Set the operating mode for the specified servos.

Args:
operating_mode: Structured array containing the operating mode for each servo.

"""
self.write("Operating_Mode", operating_mode) self.write("Operating_Mode", operating_mode)


def read_position(self, motor_names: pa.Array) -> pa.StructArray: def read_position(self, motor_names: pa.Array) -> pa.StructArray:
"""Read the current position of the specified servos.

Args:
motor_names: Array of motor names to read positions from.

Returns:
Structured array containing the current positions.

"""
return self.read("Present_Position", motor_names) return self.read("Present_Position", motor_names)


def read_velocity(self, motor_names: pa.Array) -> pa.StructArray: def read_velocity(self, motor_names: pa.Array) -> pa.StructArray:
"""Read the current velocity of the specified servos.

Args:
motor_names: Array of motor names to read velocities from.

Returns:
Structured array containing the current velocities.

"""
return self.read("Present_Velocity", motor_names) return self.read("Present_Velocity", motor_names)


def read_current(self, motor_names: pa.Array) -> pa.StructArray: def read_current(self, motor_names: pa.Array) -> pa.StructArray:
"""Read the current current of the specified servos.

Args:
motor_names: Array of motor names to read currents from.

Returns:
Structured array containing the current currents.

"""
return self.read("Present_Current", motor_names) return self.read("Present_Current", motor_names)


def write_goal_position(self, goal_position: pa.StructArray): def write_goal_position(self, goal_position: pa.StructArray):
"""Set the goal position for the specified servos.

Args:
goal_position: Structured array containing the goal positions.

"""
self.write("Goal_Position", goal_position) self.write("Goal_Position", goal_position)


def write_goal_current(self, goal_current: pa.StructArray): def write_goal_current(self, goal_current: pa.StructArray):
"""Set the goal current for the specified servos.

Args:
goal_current: Structured array containing the goal currents.

"""
self.write("Goal_Current", goal_current) self.write("Goal_Current", goal_current)


def write_position_p_gain(self, position_p_gain: pa.StructArray): def write_position_p_gain(self, position_p_gain: pa.StructArray):
"""Set the position P gain for the specified servos.

Args:
position_p_gain: Structured array containing the position P gains.

"""
self.write("Position_P_Gain", position_p_gain) self.write("Position_P_Gain", position_p_gain)


def write_position_i_gain(self, position_i_gain: pa.StructArray): def write_position_i_gain(self, position_i_gain: pa.StructArray):
"""Set the position I gain for the specified servos.

Args:
position_i_gain: Structured array containing the position I gains.

"""
self.write("Position_I_Gain", position_i_gain) self.write("Position_I_Gain", position_i_gain)


def write_position_d_gain(self, position_d_gain: pa.StructArray): def write_position_d_gain(self, position_d_gain: pa.StructArray):
"""Set the position D gain for the specified servos.

Args:
position_d_gain: Structured array containing the position D gains.

"""
self.write("Position_D_Gain", position_d_gain) self.write("Position_D_Gain", position_d_gain)

+ 29
- 23
examples/alexk-lcr/configure.py View File

@@ -1,5 +1,7 @@
"""
LCR Configuration Tool: This program is used to automatically configure the Low Cost Robot (LCR) for the user.
"""Module for configuring and setting up the Low Cost Robot (LCR) hardware.

This module provides functionality for initializing and configuring the LCR robot's
servo motors and other hardware components.


The program will: The program will:
1. Disable all torque motors of provided LCR. 1. Disable all torque motors of provided LCR.
@@ -14,21 +16,17 @@ It will also enable all appropriate operating modes for the LCR.
""" """


import argparse import argparse
import time
import json import json
import time


import pyarrow as pa import pyarrow as pa

from bus import DynamixelBus, TorqueMode, OperatingMode

from pwm_position_control.transform import pwm_to_logical_arrow, wrap_joints_and_values

from bus import DynamixelBus, OperatingMode, TorqueMode
from pwm_position_control.functions import construct_control_table
from pwm_position_control.tables import ( from pwm_position_control.tables import (
construct_logical_to_pwm_conversion_table_arrow, construct_logical_to_pwm_conversion_table_arrow,
construct_pwm_to_logical_conversion_table_arrow, construct_pwm_to_logical_conversion_table_arrow,
) )

from pwm_position_control.functions import construct_control_table
from pwm_position_control.transform import pwm_to_logical_arrow, wrap_joints_and_values


FULL_ARM = pa.array( FULL_ARM = pa.array(
[ [
@@ -51,31 +49,39 @@ GRIPPER = pa.array(["gripper"], type=pa.string())




def pause(): def pause():
"""Pause execution and wait for user input to continue."""
input("Press Enter to continue...") input("Press Enter to continue...")




def configure_servos(bus: DynamixelBus): def configure_servos(bus: DynamixelBus):
"""Configure servo motors with appropriate settings.

Args:
bus: DynamixelBus instance for servo communication

"""
bus.write_torque_enable( bus.write_torque_enable(
wrap_joints_and_values(FULL_ARM, [TorqueMode.DISABLED.value] * 6)
wrap_joints_and_values(FULL_ARM, [TorqueMode.DISABLED.value] * 6),
) )


bus.write_operating_mode( bus.write_operating_mode(
wrap_joints_and_values( wrap_joints_and_values(
ARM_WITHOUT_GRIPPER, [OperatingMode.EXTENDED_POSITION.value] * 5
)
ARM_WITHOUT_GRIPPER, [OperatingMode.EXTENDED_POSITION.value] * 5,
),
) )


bus.write_operating_mode( bus.write_operating_mode(
wrap_joints_and_values( wrap_joints_and_values(
GRIPPER, [OperatingMode.CURRENT_CONTROLLED_POSITION.value]
)
GRIPPER, [OperatingMode.CURRENT_CONTROLLED_POSITION.value],
),
) )




def main(): def main():
"""Initialize and configure the LCR robot hardware components."""
parser = argparse.ArgumentParser( parser = argparse.ArgumentParser(
description="LCR Auto Configure: This program is used to automatically configure the Low Cost Robot (LCR) for " description="LCR Auto Configure: This program is used to automatically configure the Low Cost Robot (LCR) for "
"the user."
"the user.",
) )


parser.add_argument("--port", type=str, required=True, help="The port of the LCR.") parser.add_argument("--port", type=str, required=True, help="The port of the LCR.")
@@ -95,7 +101,7 @@ def main():
help="If the LCR is the follower of the user.", help="If the LCR is the follower of the user.",
) )
parser.add_argument( parser.add_argument(
"--leader", action="store_true", help="If the LCR is the leader of the user."
"--leader", action="store_true", help="If the LCR is the leader of the user.",
) )


args = parser.parse_args() args = parser.parse_args()
@@ -138,10 +144,10 @@ def main():
pwm_positions = (pwm_position_1, pwm_position_2) pwm_positions = (pwm_position_1, pwm_position_2)


pwm_to_logical_conversion_table = construct_pwm_to_logical_conversion_table_arrow( pwm_to_logical_conversion_table = construct_pwm_to_logical_conversion_table_arrow(
pwm_positions, targets
pwm_positions, targets,
) )
logical_to_pwm_conversion_table = construct_logical_to_pwm_conversion_table_arrow( logical_to_pwm_conversion_table = construct_logical_to_pwm_conversion_table_arrow(
pwm_positions, targets
pwm_positions, targets,
) )


control_table_json = {} control_table_json = {}
@@ -180,7 +186,7 @@ def main():


path = ( path = (
input( input(
f"Please enter the path of the configuration file (default is ./examples/alexk-lcr/configs/{leader}.{left}.json): "
f"Please enter the path of the configuration file (default is ./examples/alexk-lcr/configs/{leader}.{left}.json): ",
) )
or f"./examples/alexk-lcr/configs/{leader}.{left}.json" or f"./examples/alexk-lcr/configs/{leader}.{left}.json"
) )
@@ -189,21 +195,21 @@ def main():
json.dump(control_table_json, file) json.dump(control_table_json, file)


control_table = construct_control_table( control_table = construct_control_table(
pwm_to_logical_conversion_table, logical_to_pwm_conversion_table
pwm_to_logical_conversion_table, logical_to_pwm_conversion_table,
) )


while True: while True:
try: try:
pwm_position = arm.read_position(FULL_ARM) pwm_position = arm.read_position(FULL_ARM)
logical_position = pwm_to_logical_arrow( logical_position = pwm_to_logical_arrow(
pwm_position, control_table, ranged=True
pwm_position, control_table, ranged=True,
).field("values") ).field("values")


print(f"Logical Position: {logical_position}") print(f"Logical Position: {logical_position}")


except ConnectionError: except ConnectionError:
print( print(
"Connection error occurred. Please check the connection and try again."
"Connection error occurred. Please check the connection and try again.",
) )


time.sleep(0.5) time.sleep(0.5)


+ 18
- 13
examples/alexk-lcr/nodes/interpolate_lcr_to_lcr.py View File

@@ -1,24 +1,29 @@
import os
"""Module for interpolating between LCR robot configurations.

This module provides functionality for calculating and interpolating between
different LCR robot configurations to ensure smooth transitions.
"""

import argparse import argparse
import json import json
import os


import pyarrow as pa import pyarrow as pa
import pyarrow.compute as pc import pyarrow.compute as pc

from dora import Node from dora import Node
from pwm_position_control.load import load_control_table_from_json_conversion_tables
from pwm_position_control.transform import ( from pwm_position_control.transform import (
wrap_joints_and_values,
pwm_to_logical_arrow,
logical_to_pwm_with_offset_arrow, logical_to_pwm_with_offset_arrow,
pwm_to_logical_arrow,
wrap_joints_and_values,
) )
from pwm_position_control.load import load_control_table_from_json_conversion_tables




def main(): def main():
"""Initialize and run the LCR interpolation node."""
parser = argparse.ArgumentParser( parser = argparse.ArgumentParser(
description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the "
"LCR followers knowing a Leader position and Follower position."
"LCR followers knowing a Leader position and Follower position.",
) )


parser.add_argument( parser.add_argument(
@@ -46,19 +51,19 @@ def main():
if not os.environ.get("LEADER_CONTROL") and args.leader_control is None: if not os.environ.get("LEADER_CONTROL") and args.leader_control is None:
raise ValueError( raise ValueError(
"The leader control is not set. Please set the configuration of the leader in the environment variables or " "The leader control is not set. Please set the configuration of the leader in the environment variables or "
"as an argument."
"as an argument.",
) )


if not os.environ.get("FOLLOWER_CONTROL") and args.follower_control is None: if not os.environ.get("FOLLOWER_CONTROL") and args.follower_control is None:
raise ValueError( raise ValueError(
"The follower control is not set. Please set the configuration of the follower in the environment " "The follower control is not set. Please set the configuration of the follower in the environment "
"variables or as an argument."
"variables or as an argument.",
) )


with open( with open(
os.environ.get("LEADER_CONTROL") os.environ.get("LEADER_CONTROL")
if args.leader_control is None if args.leader_control is None
else args.leader_control
else args.leader_control,
) as file: ) as file:
leader_control = json.load(file) leader_control = json.load(file)
load_control_table_from_json_conversion_tables(leader_control, leader_control) load_control_table_from_json_conversion_tables(leader_control, leader_control)
@@ -66,11 +71,11 @@ def main():
with open( with open(
os.environ.get("FOLLOWER_CONTROL") os.environ.get("FOLLOWER_CONTROL")
if args.follower_control is None if args.follower_control is None
else args.follower_control
else args.follower_control,
) as file: ) as file:
follower_control = json.load(file) follower_control = json.load(file)
load_control_table_from_json_conversion_tables( load_control_table_from_json_conversion_tables(
follower_control, follower_control
follower_control, follower_control,
) )


initial_mask = [ initial_mask = [
@@ -130,7 +135,7 @@ def main():
) )


pwm_goal = logical_to_pwm_with_offset_arrow( pwm_goal = logical_to_pwm_with_offset_arrow(
follower_position, logical_goal, follower_control
follower_position, logical_goal, follower_control,
) )


node.send_output("follower_goal", pwm_goal, event["metadata"]) node.send_output("follower_goal", pwm_goal, event["metadata"])


+ 13
- 12
examples/alexk-lcr/nodes/interpolate_lcr_to_record.py View File

@@ -1,23 +1,24 @@
import os
"""TODO: Add docstring."""

import argparse import argparse
import json import json
import os


import pyarrow as pa import pyarrow as pa
import pyarrow.compute as pc import pyarrow.compute as pc

from dora import Node from dora import Node

from pwm_position_control.load import load_control_table_from_json_conversion_tables from pwm_position_control.load import load_control_table_from_json_conversion_tables
from pwm_position_control.transform import ( from pwm_position_control.transform import (
wrap_joints_and_values,
pwm_to_logical_arrow, pwm_to_logical_arrow,
wrap_joints_and_values,
) )




def main(): def main():
"""TODO: Add docstring."""
parser = argparse.ArgumentParser( parser = argparse.ArgumentParser(
description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the "
"LCR followers knowing a Leader position and Follower position."
"LCR followers knowing a Leader position and Follower position.",
) )


parser.add_argument( parser.add_argument(
@@ -45,19 +46,19 @@ def main():
if not os.environ.get("LEADER_CONTROL") and args.leader_control is None: if not os.environ.get("LEADER_CONTROL") and args.leader_control is None:
raise ValueError( raise ValueError(
"The leader control is not set. Please set the configuration of the leader in the environment variables or " "The leader control is not set. Please set the configuration of the leader in the environment variables or "
"as an argument."
"as an argument.",
) )


if not os.environ.get("FOLLOWER_CONTROL") and args.follower_control is None: if not os.environ.get("FOLLOWER_CONTROL") and args.follower_control is None:
raise ValueError( raise ValueError(
"The follower control is not set. Please set the configuration of the follower in the environment " "The follower control is not set. Please set the configuration of the follower in the environment "
"variables or as an argument."
"variables or as an argument.",
) )


with open( with open(
os.environ.get("LEADER_CONTROL") os.environ.get("LEADER_CONTROL")
if args.leader_control is None if args.leader_control is None
else args.leader_control
else args.leader_control,
) as file: ) as file:
leader_control = json.load(file) leader_control = json.load(file)
load_control_table_from_json_conversion_tables(leader_control, leader_control) load_control_table_from_json_conversion_tables(leader_control, leader_control)
@@ -65,11 +66,11 @@ def main():
with open( with open(
os.environ.get("FOLLOWER_CONTROL") os.environ.get("FOLLOWER_CONTROL")
if args.follower_control is None if args.follower_control is None
else args.follower_control
else args.follower_control,
) as file: ) as file:
follower_control = json.load(file) follower_control = json.load(file)
load_control_table_from_json_conversion_tables( load_control_table_from_json_conversion_tables(
follower_control, follower_control
follower_control, follower_control,
) )


node = Node(args.name) node = Node(args.name)
@@ -98,11 +99,11 @@ def main():
follower_position = event["value"] follower_position = event["value"]


follower_position = pwm_to_logical_arrow( follower_position = pwm_to_logical_arrow(
follower_position, follower_control
follower_position, follower_control,
) )


node.send_output( node.send_output(
"logical_position", follower_position, event["metadata"]
"logical_position", follower_position, event["metadata"],
) )


elif event_type == "ERROR": elif event_type == "ERROR":


+ 14
- 9
examples/alexk-lcr/nodes/interpolate_lcr_to_simu_lcr.py View File

@@ -1,25 +1,30 @@
import os
"""Module for interpolating between LCR and simulated LCR configurations.

This module provides functionality for calculating and interpolating between
physical LCR robot configurations and their simulated counterparts.
"""

import argparse import argparse
import json import json
import os


import numpy as np import numpy as np
import pyarrow as pa import pyarrow as pa
import pyarrow.compute as pc import pyarrow.compute as pc

from dora import Node from dora import Node

from pwm_position_control.load import load_control_table_from_json_conversion_tables from pwm_position_control.load import load_control_table_from_json_conversion_tables
from pwm_position_control.transform import ( from pwm_position_control.transform import (
wrap_joints_and_values,
pwm_to_logical_arrow,
logical_to_pwm_with_offset_arrow, logical_to_pwm_with_offset_arrow,
pwm_to_logical_arrow,
wrap_joints_and_values,
) )




def main(): def main():
"""Initialize and run the LCR to simulated LCR interpolation node."""
parser = argparse.ArgumentParser( parser = argparse.ArgumentParser(
description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the "
"LCR followers knowing a Leader position and Follower position."
"LCR followers knowing a Leader position and Follower position.",
) )


parser.add_argument( parser.add_argument(
@@ -41,13 +46,13 @@ def main():
if not os.environ.get("LEADER_CONTROL") and args.leader_control is None: if not os.environ.get("LEADER_CONTROL") and args.leader_control is None:
raise ValueError( raise ValueError(
"The leader control is not set. Please set the configuration of the leader in the environment variables or " "The leader control is not set. Please set the configuration of the leader in the environment variables or "
"as an argument."
"as an argument.",
) )


with open( with open(
os.environ.get("LEADER_CONTROL") os.environ.get("LEADER_CONTROL")
if args.leader_control is None if args.leader_control is None
else args.leader_control
else args.leader_control,
) as file: ) as file:
leader_control = json.load(file) leader_control = json.load(file)
load_control_table_from_json_conversion_tables(leader_control, leader_control) load_control_table_from_json_conversion_tables(leader_control, leader_control)
@@ -114,7 +119,7 @@ def main():
[ [
joint.as_py() + "_joint" joint.as_py() + "_joint"
for joint in leader_position.field("joints") for joint in leader_position.field("joints")
]
],
), ),
pc.multiply( pc.multiply(
pc.add(leader_position.field("values"), interpolation_a), pc.add(leader_position.field("values"), interpolation_a),


+ 9
- 7
examples/alexk-lcr/nodes/interpolate_replay_to_lcr.py View File

@@ -1,17 +1,19 @@
import os
"""TODO: Add docstring."""

import argparse import argparse
import json import json
import os


from dora import Node from dora import Node

from pwm_position_control.load import load_control_table_from_json_conversion_tables from pwm_position_control.load import load_control_table_from_json_conversion_tables
from pwm_position_control.transform import logical_to_pwm_with_offset_arrow from pwm_position_control.transform import logical_to_pwm_with_offset_arrow




def main(): def main():
"""TODO: Add docstring."""
parser = argparse.ArgumentParser( parser = argparse.ArgumentParser(
description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the "
"LCR followers knowing a Leader position and Follower position."
"LCR followers knowing a Leader position and Follower position.",
) )


parser.add_argument( parser.add_argument(
@@ -33,17 +35,17 @@ def main():
if not os.environ.get("FOLLOWER_CONTROL") and args.follower_control is None: if not os.environ.get("FOLLOWER_CONTROL") and args.follower_control is None:
raise ValueError( raise ValueError(
"The follower control is not set. Please set the configuration of the follower in the environment " "The follower control is not set. Please set the configuration of the follower in the environment "
"variables or as an argument."
"variables or as an argument.",
) )


with open( with open(
os.environ.get("FOLLOWER_CONTROL") os.environ.get("FOLLOWER_CONTROL")
if args.follower_control is None if args.follower_control is None
else args.follower_control
else args.follower_control,
) as file: ) as file:
follower_control = json.load(file) follower_control = json.load(file)
load_control_table_from_json_conversion_tables( load_control_table_from_json_conversion_tables(
follower_control, follower_control
follower_control, follower_control,
) )


node = Node(args.name) node = Node(args.name)
@@ -65,7 +67,7 @@ def main():
continue continue


physical_goal = logical_to_pwm_with_offset_arrow( physical_goal = logical_to_pwm_with_offset_arrow(
follower_position, leader_position, follower_control
follower_position, leader_position, follower_control,
) )


node.send_output("follower_goal", physical_goal, event["metadata"]) node.send_output("follower_goal", physical_goal, event["metadata"])


+ 258
- 48
examples/aloha/benchmark/python/dynamixel.py View File

@@ -1,13 +1,24 @@
"""Module for managing Dynamixel servo communication and control.

This module provides functionality for communicating with and controlling Dynamixel servos
through a serial interface. It includes methods for reading and writing servo parameters,
managing operating modes, and handling joint positions and velocities.
"""

from __future__ import annotations from __future__ import annotations

import enum
import math import math
import os import os
from dynamixel_sdk import * # Uses Dynamixel SDK library
from dataclasses import dataclass
import enum
import time import time
from dataclasses import dataclass

from dynamixel_sdk import * # Uses Dynamixel SDK library




class ReadAttribute(enum.Enum): class ReadAttribute(enum.Enum):
"""Enumeration for Dynamixel servo read attributes."""

TEMPERATURE = 146 TEMPERATURE = 146
VOLTAGE = 145 VOLTAGE = 145
VELOCITY = 128 VELOCITY = 128
@@ -20,6 +31,8 @@ class ReadAttribute(enum.Enum):




class OperatingMode(enum.Enum): class OperatingMode(enum.Enum):
"""Enumeration for Dynamixel servo operating modes."""

VELOCITY = 1 VELOCITY = 1
POSITION = 3 POSITION = 3
CURRENT_CONTROLLED_POSITION = 5 CURRENT_CONTROLLED_POSITION = 5
@@ -28,6 +41,8 @@ class OperatingMode(enum.Enum):




class Dynamixel: class Dynamixel:
"""Class for managing communication with Dynamixel servos."""

ADDR_TORQUE_ENABLE = 64 ADDR_TORQUE_ENABLE = 64
ADDR_GOAL_POSITION = 116 ADDR_GOAL_POSITION = 116
ADDR_VELOCITY_LIMIT = 44 ADDR_VELOCITY_LIMIT = 44
@@ -39,7 +54,10 @@ class Dynamixel:


@dataclass @dataclass
class Config: class Config:
"""Configuration class for Dynamixel servo settings."""

def instantiate(self): def instantiate(self):
"""Create a new Dynamixel instance with this configuration."""
return Dynamixel(self) return Dynamixel(self)


baudrate: int = 57600 baudrate: int = 57600
@@ -48,10 +66,17 @@ class Dynamixel:
dynamixel_id: int = 1 dynamixel_id: int = 1


def __init__(self, config: Config): def __init__(self, config: Config):
"""Initialize the Dynamixel servo connection.

Args:
config: Configuration object containing connection settings.

"""
self.config = config self.config = config
self.connect() self.connect()


def connect(self): def connect(self):
"""Establish connection with the Dynamixel servo."""
if self.config.device_name == "": if self.config.device_name == "":
for port_name in os.listdir("/dev"): for port_name in os.listdir("/dev"):
if "ttyUSB" in port_name or "ttyACM" in port_name: if "ttyUSB" in port_name or "ttyACM" in port_name:
@@ -75,9 +100,17 @@ class Dynamixel:
return True return True


def disconnect(self): def disconnect(self):
"""Close the connection with the Dynamixel servo."""
self.portHandler.closePort() self.portHandler.closePort()


def set_goal_position(self, motor_id, goal_position): def set_goal_position(self, motor_id, goal_position):
"""Set the goal position for the specified servo.

Args:
motor_id: ID of the servo to control.
goal_position: Target position value.

"""
# if self.operating_modes[motor_id] is not OperatingMode.POSITION: # if self.operating_modes[motor_id] is not OperatingMode.POSITION:
# self._disable_torque(motor_id) # self._disable_torque(motor_id)
# self.set_operating_mode(motor_id, OperatingMode.POSITION) # self.set_operating_mode(motor_id, OperatingMode.POSITION)
@@ -87,12 +120,20 @@ class Dynamixel:


# self._enable_torque(motor_id) # self._enable_torque(motor_id)
dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx( dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx(
self.portHandler, motor_id, self.ADDR_GOAL_POSITION, goal_position
self.portHandler, motor_id, self.ADDR_GOAL_POSITION, goal_position,
) )
# self._process_response(dxl_comm_result, dxl_error) # self._process_response(dxl_comm_result, dxl_error)
# print(f'set position of motor {motor_id} to {goal_position}') # print(f'set position of motor {motor_id} to {goal_position}')


def set_pwm_value(self, motor_id: int, pwm_value, tries=3): def set_pwm_value(self, motor_id: int, pwm_value, tries=3):
"""Set the PWM value for the specified servo.

Args:
motor_id: ID of the servo to control.
pwm_value: PWM value to set.
tries: Number of attempts to set the value.

"""
if self.operating_modes[motor_id] is not OperatingMode.PWM: if self.operating_modes[motor_id] is not OperatingMode.PWM:
self._disable_torque(motor_id) self._disable_torque(motor_id)
self.set_operating_mode(motor_id, OperatingMode.PWM) self.set_operating_mode(motor_id, OperatingMode.PWM)
@@ -101,30 +142,47 @@ class Dynamixel:
self._enable_torque(motor_id) self._enable_torque(motor_id)
# print(f'enabling torque') # print(f'enabling torque')
dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx(
self.portHandler, motor_id, self.ADDR_GOAL_PWM, pwm_value
self.portHandler, motor_id, self.ADDR_GOAL_PWM, pwm_value,
) )
# self._process_response(dxl_comm_result, dxl_error) # self._process_response(dxl_comm_result, dxl_error)
# print(f'set pwm of motor {motor_id} to {pwm_value}') # print(f'set pwm of motor {motor_id} to {pwm_value}')
if dxl_comm_result != COMM_SUCCESS: if dxl_comm_result != COMM_SUCCESS:
if tries <= 1: if tries <= 1:
raise ConnectionError( raise ConnectionError(
f"dxl_comm_result: {self.packetHandler.getTxRxResult(dxl_comm_result)}"
)
else:
print(
f"dynamixel pwm setting failure trying again with {tries - 1} tries"
f"dxl_comm_result: {self.packetHandler.getTxRxResult(dxl_comm_result)}",
) )
self.set_pwm_value(motor_id, pwm_value, tries=tries - 1)
print(
f"dynamixel pwm setting failure trying again with {tries - 1} tries",
)
self.set_pwm_value(motor_id, pwm_value, tries=tries - 1)
elif dxl_error != 0: elif dxl_error != 0:
print(f"dxl error {dxl_error}") print(f"dxl error {dxl_error}")
raise ConnectionError( raise ConnectionError(
f"dynamixel error: {self.packetHandler.getTxRxResult(dxl_error)}"
f"dynamixel error: {self.packetHandler.getTxRxResult(dxl_error)}",
) )


def read_temperature(self, motor_id: int): def read_temperature(self, motor_id: int):
"""Read the temperature of the specified servo.

Args:
motor_id: ID of the servo to read from.

Returns:
The current temperature value.

"""
return self._read_value(motor_id, ReadAttribute.TEMPERATURE, 1) return self._read_value(motor_id, ReadAttribute.TEMPERATURE, 1)


def read_velocity(self, motor_id: int): def read_velocity(self, motor_id: int):
"""Read the current velocity of the specified servo.

Args:
motor_id: ID of the servo to read from.

Returns:
The current velocity value.

"""
pos = self._read_value(motor_id, ReadAttribute.VELOCITY, 4) pos = self._read_value(motor_id, ReadAttribute.VELOCITY, 4)
if pos > 2**31: if pos > 2**31:
pos -= 2**32 pos -= 2**32
@@ -132,6 +190,15 @@ class Dynamixel:
return pos return pos


def read_position(self, motor_id: int): def read_position(self, motor_id: int):
"""Read the current position of the specified servo.

Args:
motor_id: ID of the servo to read from.

Returns:
The current position value.

"""
pos = self._read_value(motor_id, ReadAttribute.POSITION, 4) pos = self._read_value(motor_id, ReadAttribute.POSITION, 4)
if pos > 2**31: if pos > 2**31:
pos -= 2**32 pos -= 2**32
@@ -139,99 +206,213 @@ class Dynamixel:
return pos return pos


def read_position_degrees(self, motor_id: int) -> float: def read_position_degrees(self, motor_id: int) -> float:
"""Read the current position of the specified servo in degrees.

Args:
motor_id: ID of the servo to read from.

Returns:
The current position in degrees.

"""
return (self.read_position(motor_id) / 4096) * 360 return (self.read_position(motor_id) / 4096) * 360


def read_position_radians(self, motor_id: int) -> float: def read_position_radians(self, motor_id: int) -> float:
"""Read the current position of the specified servo in radians.

Args:
motor_id: ID of the servo to read from.

Returns:
The current position in radians.

"""
return (self.read_position(motor_id) / 4096) * 2 * math.pi return (self.read_position(motor_id) / 4096) * 2 * math.pi


def read_current(self, motor_id: int): def read_current(self, motor_id: int):
"""Read the current value of the specified servo.

Args:
motor_id: ID of the servo to read from.

Returns:
The current value.

"""
current = self._read_value(motor_id, ReadAttribute.CURRENT, 2) current = self._read_value(motor_id, ReadAttribute.CURRENT, 2)
if current > 2**15: if current > 2**15:
current -= 2**16 current -= 2**16
return current return current


def read_present_pwm(self, motor_id: int): def read_present_pwm(self, motor_id: int):
"""Read the current PWM value of the specified servo.

Args:
motor_id: ID of the servo to read from.

Returns:
The current PWM value.

"""
return self._read_value(motor_id, ReadAttribute.PWM, 2) return self._read_value(motor_id, ReadAttribute.PWM, 2)


def read_hardware_error_status(self, motor_id: int): def read_hardware_error_status(self, motor_id: int):
"""Read the hardware error status of the specified servo.

Args:
motor_id: ID of the servo to read from.

Returns:
The hardware error status value.

"""
return self._read_value(motor_id, ReadAttribute.HARDWARE_ERROR_STATUS, 1) return self._read_value(motor_id, ReadAttribute.HARDWARE_ERROR_STATUS, 1)


def set_id(self, old_id, new_id, use_broadcast_id: bool = False): def set_id(self, old_id, new_id, use_broadcast_id: bool = False):
"""
sets the id of the dynamixel servo
@param old_id: current id of the servo
@param new_id: new id
@param use_broadcast_id: set ids of all connected dynamixels if True.
If False, change only servo with self.config.id
@return:
"""Set the ID of the Dynamixel servo.

Args:
old_id: Current ID of the servo.
new_id: New ID to set.
use_broadcast_id: If True, set IDs of all connected servos.
If False, change only servo with self.config.id

""" """
if use_broadcast_id: if use_broadcast_id:
current_id = 254 current_id = 254
else: else:
current_id = old_id current_id = old_id
dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx( dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx(
self.portHandler, current_id, self.ADDR_ID, new_id
self.portHandler, current_id, self.ADDR_ID, new_id,
) )
self._process_response(dxl_comm_result, dxl_error, old_id) self._process_response(dxl_comm_result, dxl_error, old_id)
self.config.id = id self.config.id = id


def _enable_torque(self, motor_id): def _enable_torque(self, motor_id):
"""Enable torque for the specified servo.

Args:
motor_id: ID of the servo to control.

"""
dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx( dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx(
self.portHandler, motor_id, self.ADDR_TORQUE_ENABLE, 1
self.portHandler, motor_id, self.ADDR_TORQUE_ENABLE, 1,
) )
self._process_response(dxl_comm_result, dxl_error, motor_id) self._process_response(dxl_comm_result, dxl_error, motor_id)
self.torque_enabled[motor_id] = True self.torque_enabled[motor_id] = True


def _disable_torque(self, motor_id): def _disable_torque(self, motor_id):
"""Disable torque for the specified servo.

Args:
motor_id: ID of the servo to control.

"""
dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx( dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx(
self.portHandler, motor_id, self.ADDR_TORQUE_ENABLE, 0
self.portHandler, motor_id, self.ADDR_TORQUE_ENABLE, 0,
) )
self._process_response(dxl_comm_result, dxl_error, motor_id) self._process_response(dxl_comm_result, dxl_error, motor_id)
self.torque_enabled[motor_id] = False self.torque_enabled[motor_id] = False


def _process_response(self, dxl_comm_result: int, dxl_error: int, motor_id: int): def _process_response(self, dxl_comm_result: int, dxl_error: int, motor_id: int):
"""Process the response from a servo communication.

Args:
dxl_comm_result: Communication result.
dxl_error: Error value.
motor_id: ID of the servo that was communicated with.

Raises:
ConnectionError: If communication failed.
RuntimeError: If servo reported an error.

"""
if dxl_comm_result != COMM_SUCCESS: if dxl_comm_result != COMM_SUCCESS:
raise ConnectionError( raise ConnectionError(
f"dxl_comm_result for motor {motor_id}: {self.packetHandler.getTxRxResult(dxl_comm_result)}"
f"dxl_comm_result for motor {motor_id}: {self.packetHandler.getTxRxResult(dxl_comm_result)}",
) )
elif dxl_error != 0:
if dxl_error != 0:
print(f"dxl error {dxl_error}") print(f"dxl error {dxl_error}")
raise ConnectionError( raise ConnectionError(
f"dynamixel error for motor {motor_id}: {self.packetHandler.getTxRxResult(dxl_error)}"
f"dynamixel error for motor {motor_id}: {self.packetHandler.getTxRxResult(dxl_error)}",
) )


def set_operating_mode(self, motor_id: int, operating_mode: OperatingMode): def set_operating_mode(self, motor_id: int, operating_mode: OperatingMode):
"""Set the operating mode for the specified servo.

Args:
motor_id: ID of the servo to control.
operating_mode: Operating mode to set.

"""
dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx(
self.portHandler, motor_id, self.OPERATING_MODE_ADDR, operating_mode.value
self.portHandler, motor_id, self.OPERATING_MODE_ADDR, operating_mode.value,
) )
self._process_response(dxl_comm_result, dxl_error, motor_id) self._process_response(dxl_comm_result, dxl_error, motor_id)
self.operating_modes[motor_id] = operating_mode self.operating_modes[motor_id] = operating_mode


def set_pwm_limit(self, motor_id: int, limit: int): def set_pwm_limit(self, motor_id: int, limit: int):
"""Set the PWM limit for the specified servo.

Args:
motor_id: ID of the servo to control.
limit: PWM limit value to set.

"""
dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx(
self.portHandler, motor_id, 36, limit
self.portHandler, motor_id, 36, limit,
) )
self._process_response(dxl_comm_result, dxl_error, motor_id) self._process_response(dxl_comm_result, dxl_error, motor_id)


def set_velocity_limit(self, motor_id: int, velocity_limit): def set_velocity_limit(self, motor_id: int, velocity_limit):
"""Set the velocity limit for the specified servo.

Args:
motor_id: ID of the servo to control.
velocity_limit: Velocity limit value to set.

"""
dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx( dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx(
self.portHandler, motor_id, self.ADDR_VELOCITY_LIMIT, velocity_limit
self.portHandler, motor_id, self.ADDR_VELOCITY_LIMIT, velocity_limit,
) )
self._process_response(dxl_comm_result, dxl_error, motor_id) self._process_response(dxl_comm_result, dxl_error, motor_id)


def set_P(self, motor_id: int, P: int): def set_P(self, motor_id: int, P: int):
"""Set the position P gain for the specified servo.

Args:
motor_id: ID of the servo to control.
P: Position P gain value to set.

"""
dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx(
self.portHandler, motor_id, self.POSITION_P, P
self.portHandler, motor_id, self.POSITION_P, P,
) )
self._process_response(dxl_comm_result, dxl_error, motor_id) self._process_response(dxl_comm_result, dxl_error, motor_id)


def set_I(self, motor_id: int, I: int): def set_I(self, motor_id: int, I: int):
"""Set the position I gain for the specified servo.

Args:
motor_id: ID of the servo to control.
I: Position I gain value to set.

"""
dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx(
self.portHandler, motor_id, self.POSITION_I, I
self.portHandler, motor_id, self.POSITION_I, I,
) )
self._process_response(dxl_comm_result, dxl_error, motor_id) self._process_response(dxl_comm_result, dxl_error, motor_id)


def read_home_offset(self, motor_id: int): def read_home_offset(self, motor_id: int):
"""Read the home offset of the specified servo.

Args:
motor_id: ID of the servo to read from.

Returns:
The home offset value.

"""
self._disable_torque(motor_id) self._disable_torque(motor_id)
# dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx(self.portHandler, motor_id, # dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx(self.portHandler, motor_id,
# ReadAttribute.HOMING_OFFSET.value, home_position) # ReadAttribute.HOMING_OFFSET.value, home_position)
@@ -241,14 +422,28 @@ class Dynamixel:
return home_offset return home_offset


def set_home_offset(self, motor_id: int, home_position: int): def set_home_offset(self, motor_id: int, home_position: int):
"""Set the home offset for the specified servo.

Args:
motor_id: ID of the servo to control.
home_position: Home position value to set.

"""
self._disable_torque(motor_id) self._disable_torque(motor_id)
dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx( dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx(
self.portHandler, motor_id, ReadAttribute.HOMING_OFFSET.value, home_position
self.portHandler, motor_id, ReadAttribute.HOMING_OFFSET.value, home_position,
) )
self._process_response(dxl_comm_result, dxl_error, motor_id) self._process_response(dxl_comm_result, dxl_error, motor_id)
self._enable_torque(motor_id) self._enable_torque(motor_id)


def set_baudrate(self, motor_id: int, baudrate): def set_baudrate(self, motor_id: int, baudrate):
"""Set the baudrate for the specified servo.

Args:
motor_id: ID of the servo to control.
baudrate: Baudrate value to set.

"""
# translate baudrate into dynamixel baudrate setting id # translate baudrate into dynamixel baudrate setting id
if baudrate == 57600: if baudrate == 57600:
baudrate_id = 1 baudrate_id = 1
@@ -265,54 +460,69 @@ class Dynamixel:


self._disable_torque(motor_id) self._disable_torque(motor_id)
dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx( dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx(
self.portHandler, motor_id, ReadAttribute.BAUDRATE.value, baudrate_id
self.portHandler, motor_id, ReadAttribute.BAUDRATE.value, baudrate_id,
) )
self._process_response(dxl_comm_result, dxl_error, motor_id) self._process_response(dxl_comm_result, dxl_error, motor_id)


def _read_value(self, motor_id, attribute: ReadAttribute, num_bytes: int, tries=10): def _read_value(self, motor_id, attribute: ReadAttribute, num_bytes: int, tries=10):
"""Read a value from the specified servo address.

Args:
motor_id: ID of the servo to read from.
attribute: Attribute to read.
num_bytes: Number of bytes to read.
tries: Number of attempts to read the value.

Returns:
The read value.

"""
try: try:
if num_bytes == 1: if num_bytes == 1:
value, dxl_comm_result, dxl_error = self.packetHandler.read1ByteTxRx( value, dxl_comm_result, dxl_error = self.packetHandler.read1ByteTxRx(
self.portHandler, motor_id, attribute.value
self.portHandler, motor_id, attribute.value,
) )
elif num_bytes == 2: elif num_bytes == 2:
value, dxl_comm_result, dxl_error = self.packetHandler.read2ByteTxRx( value, dxl_comm_result, dxl_error = self.packetHandler.read2ByteTxRx(
self.portHandler, motor_id, attribute.value
self.portHandler, motor_id, attribute.value,
) )
elif num_bytes == 4: elif num_bytes == 4:
value, dxl_comm_result, dxl_error = self.packetHandler.read4ByteTxRx( value, dxl_comm_result, dxl_error = self.packetHandler.read4ByteTxRx(
self.portHandler, motor_id, attribute.value
self.portHandler, motor_id, attribute.value,
) )
except Exception: except Exception:
if tries == 0: if tries == 0:
raise Exception raise Exception
else:
return self._read_value(motor_id, attribute, num_bytes, tries=tries - 1)
return self._read_value(motor_id, attribute, num_bytes, tries=tries - 1)
if dxl_comm_result != COMM_SUCCESS: if dxl_comm_result != COMM_SUCCESS:
if tries <= 1: if tries <= 1:
# print("%s" % self.packetHandler.getTxRxResult(dxl_comm_result)) # print("%s" % self.packetHandler.getTxRxResult(dxl_comm_result))
raise ConnectionError( raise ConnectionError(
f"dxl_comm_result {dxl_comm_result} for servo {motor_id} value {value}"
)
else:
print(
f"dynamixel read failure for servo {motor_id} trying again with {tries - 1} tries"
f"dxl_comm_result {dxl_comm_result} for servo {motor_id} value {value}",
) )
time.sleep(0.02)
return self._read_value(motor_id, attribute, num_bytes, tries=tries - 1)
elif (
print(
f"dynamixel read failure for servo {motor_id} trying again with {tries - 1} tries",
)
time.sleep(0.02)
return self._read_value(motor_id, attribute, num_bytes, tries=tries - 1)
if (
dxl_error != 0 dxl_error != 0
): # # print("%s" % self.packetHandler.getRxPacketError(dxl_error)) ): # # print("%s" % self.packetHandler.getRxPacketError(dxl_error))
# raise ConnectionError(f'dxl_error {dxl_error} binary ' + "{0:b}".format(37)) # raise ConnectionError(f'dxl_error {dxl_error} binary ' + "{0:b}".format(37))
if tries == 0 and dxl_error != 128: if tries == 0 and dxl_error != 128:
raise Exception( raise Exception(
f"Failed to read value from motor {motor_id} error is {dxl_error}"
f"Failed to read value from motor {motor_id} error is {dxl_error}",
) )
else:
return self._read_value(motor_id, attribute, num_bytes, tries=tries - 1)
return self._read_value(motor_id, attribute, num_bytes, tries=tries - 1)
return value return value


def set_home_position(self, motor_id: int): def set_home_position(self, motor_id: int):
"""Set the home position for the specified servo.

Args:
motor_id: ID of the servo to control.

"""
print(f"setting home position for motor {motor_id}") print(f"setting home position for motor {motor_id}")
self.set_home_offset(motor_id, 0) self.set_home_offset(motor_id, 0)
current_position = self.read_position(motor_id) current_position = self.read_position(motor_id)


+ 68
- 30
examples/aloha/benchmark/python/robot.py View File

@@ -1,19 +1,32 @@
import numpy as np
from dynamixel import Dynamixel, OperatingMode, ReadAttribute
"""Module for controlling robot hardware and movements.

This module provides functionality for controlling robot hardware components,
including servo motors and various control modes.
"""

import time import time
from enum import Enum, auto
from typing import Union

import numpy as np
from dynamixel import OperatingMode, ReadAttribute
from dynamixel_sdk import ( from dynamixel_sdk import (
GroupSyncRead,
GroupSyncWrite,
DXL_LOBYTE,
DXL_HIBYTE, DXL_HIBYTE,
DXL_LOWORD,
DXL_HIWORD, DXL_HIWORD,
DXL_LOBYTE,
DXL_LOWORD,
GroupSyncRead,
GroupSyncWrite,
) )
from enum import Enum, auto
from typing import Union




class MotorControlType(Enum): class MotorControlType(Enum):
"""Enumeration of different motor control modes.
Defines the various control modes available for the robot's motors,
including PWM control, position control, and disabled states.
"""

PWM = auto() PWM = auto()
POSITION_CONTROL = auto() POSITION_CONTROL = auto()
DISABLED = auto() DISABLED = auto()
@@ -21,8 +34,22 @@ class MotorControlType(Enum):




class Robot: class Robot:
"""A class for controlling robot hardware components.
This class provides methods for controlling robot servos, managing different
control modes, and reading sensor data.
"""

# def __init__(self, device_name: str, baudrate=1_000_000, servo_ids=[1, 2, 3, 4, 5]): # def __init__(self, device_name: str, baudrate=1_000_000, servo_ids=[1, 2, 3, 4, 5]):
def __init__(self, dynamixel, baudrate=1_000_000, servo_ids=[1, 2, 3, 4, 5]): def __init__(self, dynamixel, baudrate=1_000_000, servo_ids=[1, 2, 3, 4, 5]):
"""Initialize the robot controller.

Args:
dynamixel: Dynamixel controller instance
baudrate: Communication baudrate for servo control
servo_ids: List of servo IDs to control

"""
self.servo_ids = servo_ids self.servo_ids = servo_ids
self.dynamixel = dynamixel self.dynamixel = dynamixel
# self.dynamixel = Dynamixel.Config(baudrate=baudrate, device_name=device_name).instantiate() # self.dynamixel = Dynamixel.Config(baudrate=baudrate, device_name=device_name).instantiate()
@@ -65,17 +92,20 @@ class Robot:
self.motor_control_state = MotorControlType.DISABLED self.motor_control_state = MotorControlType.DISABLED


def read_position(self, tries=2): def read_position(self, tries=2):
"""
Reads the joint positions of the robot. 2048 is the center position. 0 and 4096 are 180 degrees in each direction.
:param tries: maximum number of tries to read the position
:return: list of joint positions in range [0, 4096]
"""Read the current joint positions of the robot.

Args:
tries: Maximum number of attempts to read positions

Returns:
list: Joint positions in range [0, 4096]

""" """
result = self.position_reader.txRxPacket() result = self.position_reader.txRxPacket()
if result != 0: if result != 0:
if tries > 0: if tries > 0:
return self.read_position(tries=tries - 1) return self.read_position(tries=tries - 1)
else:
print(f"failed to read position!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
print("failed to read position!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
positions = [] positions = []
for id in self.servo_ids: for id in self.servo_ids:
position = self.position_reader.getData(id, ReadAttribute.POSITION.value, 4) position = self.position_reader.getData(id, ReadAttribute.POSITION.value, 4)
@@ -85,9 +115,11 @@ class Robot:
return positions return positions


def read_velocity(self): def read_velocity(self):
"""
Reads the joint velocities of the robot.
:return: list of joint velocities,
"""Read the current joint velocities of the robot.

Returns:
list: Current joint velocities

""" """
self.velocity_reader.txRxPacket() self.velocity_reader.txRxPacket()
velocties = [] velocties = []
@@ -99,11 +131,13 @@ class Robot:
return velocties return velocties


def set_goal_pos(self, action): def set_goal_pos(self, action):
"""
"""Set goal positions for the robot joints.

Args:
action: List or numpy array of target joint positions in range [0, 4096]


:param action: list or numpy array of target joint positions in range [0, 4096]
""" """
if not self.motor_control_state is MotorControlType.POSITION_CONTROL:
if self.motor_control_state is not MotorControlType.POSITION_CONTROL:
self._set_position_control() self._set_position_control()
for i, motor_id in enumerate(self.servo_ids): for i, motor_id in enumerate(self.servo_ids):
data_write = [ data_write = [
@@ -117,11 +151,13 @@ class Robot:
self.pos_writer.txPacket() self.pos_writer.txPacket()


def set_pwm(self, action): def set_pwm(self, action):
"""Set PWM values for the servos.

Args:
action: List or numpy array of PWM values in range [0, 885]

""" """
Sets the pwm values for the servos.
:param action: list or numpy array of pwm values in range [0, 885]
"""
if not self.motor_control_state is MotorControlType.PWM:
if self.motor_control_state is not MotorControlType.PWM:
self._set_pwm_control() self._set_pwm_control()
for i, motor_id in enumerate(self.servo_ids): for i, motor_id in enumerate(self.servo_ids):
data_write = [ data_write = [
@@ -133,17 +169,19 @@ class Robot:
self.pwm_writer.txPacket() self.pwm_writer.txPacket()


def set_trigger_torque(self): def set_trigger_torque(self):
"""
Sets a constant torque torque for the last servo in the chain. This is useful for the trigger of the leader arm
"""Set a constant torque for the last servo in the chain.
This is useful for the trigger of the leader arm.
""" """
self.dynamixel._enable_torque(self.servo_ids[-1]) self.dynamixel._enable_torque(self.servo_ids[-1])
self.dynamixel.set_pwm_value(self.servo_ids[-1], 200) self.dynamixel.set_pwm_value(self.servo_ids[-1], 200)


def limit_pwm(self, limit: Union[int, list, np.ndarray]): def limit_pwm(self, limit: Union[int, list, np.ndarray]):
"""
Limits the pwm values for the servos in for position control
@param limit: 0 ~ 885
@return:
"""Limit the PWM values for the servos in position control.

Args:
limit: PWM limit value in range 0-885

""" """
if isinstance(limit, int): if isinstance(limit, int):
limits = [ limits = [


+ 9
- 3
examples/aloha/benchmark/python/teleoperate_real_robot.py View File

@@ -1,11 +1,17 @@
from robot import Robot
"""Module for teleoperating a physical robot.

This module provides functionality for controlling a physical robot through
teleoperation, allowing real-time control of robot movements and actions.
"""

from dynamixel import Dynamixel from dynamixel import Dynamixel
from robot import Robot


leader_dynamixel = Dynamixel.Config( leader_dynamixel = Dynamixel.Config(
baudrate=1_000_000, device_name="/dev/ttyDXL_master_right"
baudrate=1_000_000, device_name="/dev/ttyDXL_master_right",
).instantiate() ).instantiate()
follower_dynamixel = Dynamixel.Config( follower_dynamixel = Dynamixel.Config(
baudrate=1_000_000, device_name="/dev/ttyDXL_puppet_right"
baudrate=1_000_000, device_name="/dev/ttyDXL_puppet_right",
).instantiate() ).instantiate()
follower = Robot(follower_dynamixel, servo_ids=[1, 2, 3, 4, 5, 6, 7, 8]) follower = Robot(follower_dynamixel, servo_ids=[1, 2, 3, 4, 5, 6, 7, 8])
leader = Robot(leader_dynamixel, servo_ids=[1, 2, 3, 4, 5, 6, 7, 8]) leader = Robot(leader_dynamixel, servo_ids=[1, 2, 3, 4, 5, 6, 7, 8])


+ 17
- 12
examples/aloha/benchmark/ros2/teleop.py View File

@@ -1,8 +1,13 @@
"""Module for ROS2-based robot teleoperation.

This module provides functionality for controlling robots through ROS2,
enabling teleoperation and real-time control of robot movements.
"""

import dora import dora
from dora import Node
import pyarrow as pa
import numpy as np import numpy as np

import pyarrow as pa
from dora import Node


ros2_context = dora.experimental.ros2_bridge.Ros2Context() ros2_context = dora.experimental.ros2_bridge.Ros2Context()
ros2_node = ros2_context.new_node( ros2_node = ros2_context.new_node(
@@ -13,7 +18,7 @@ ros2_node = ros2_context.new_node(


# Define a ROS2 QOS # Define a ROS2 QOS
topic_qos = dora.experimental.ros2_bridge.Ros2QosPolicies( topic_qos = dora.experimental.ros2_bridge.Ros2QosPolicies(
reliable=True, max_blocking_time=0.1
reliable=True, max_blocking_time=0.1,
) )


# Create a publisher to cmd_vel topic # Create a publisher to cmd_vel topic
@@ -22,7 +27,7 @@ puppet_arm_command = ros2_node.create_publisher(
"/robot_model_puppet/commands/joint_group", "/robot_model_puppet/commands/joint_group",
"interbotix_xs_msgs/JointGroupCommand", "interbotix_xs_msgs/JointGroupCommand",
topic_qos, topic_qos,
)
),
) )


gripper_command = ros2_node.create_publisher( gripper_command = ros2_node.create_publisher(
@@ -30,13 +35,13 @@ gripper_command = ros2_node.create_publisher(
"/robot_model_puppet/commands/joint_single", "/robot_model_puppet/commands/joint_single",
"interbotix_xs_msgs/JointSingleCommand", "interbotix_xs_msgs/JointSingleCommand",
topic_qos, topic_qos,
)
),
) )
# Create a listener to pose topic # Create a listener to pose topic
master_state = ros2_node.create_subscription( master_state = ros2_node.create_subscription(
ros2_node.create_topic( ros2_node.create_topic(
"/robot_model_master/joint_states", "sensor_msgs/JointState", topic_qos
)
"/robot_model_master/joint_states", "sensor_msgs/JointState", topic_qos,
),
) )


# Create a dora node # Create a dora node
@@ -74,8 +79,8 @@ for event in dora_node:
{ {
"name": "gripper", "name": "gripper",
"cmd": np.float32(values[6]), "cmd": np.float32(values[6]),
}
]
},
],
), ),
) )
puppet_arm_command.publish( puppet_arm_command.publish(
@@ -84,7 +89,7 @@ for event in dora_node:
{ {
"name": "arm", "name": "arm",
"cmd": values[:6], "cmd": values[:6],
}
]
},
],
), ),
) )

+ 35
- 6
examples/aloha/nodes/gym_dora_node.py View File

@@ -1,18 +1,37 @@
import gymnasium as gym
"""Module for integrating gym environments with Dora nodes.

This module provides functionality for running gym environments as Dora nodes,
including replay capabilities for recorded robot actions.
"""


import gym_dora # noqa: F401
import pandas as pd
import time import time
from pathlib import Path from pathlib import Path


import gym_dora # noqa: F401
import gymnasium as gym
import pandas as pd

env = gym.make( env = gym.make(
"gym_dora/DoraAloha-v0", disable_env_checker=True, max_episode_steps=10000
"gym_dora/DoraAloha-v0", disable_env_checker=True, max_episode_steps=10000,
) )
observation = env.reset() observation = env.reset()




class ReplayPolicy: class ReplayPolicy:
"""A policy class for replaying recorded robot actions.
This class handles loading and replaying recorded actions from a dataset,
maintaining timing between actions to match the original recording.
"""

def __init__(self, example_path, epidode=0): def __init__(self, example_path, epidode=0):
"""Initialize the replay policy.

Args:
example_path: Path to the directory containing recorded actions
epidode: Index of the episode to replay

"""
df_action = pd.read_parquet(example_path / "action.parquet") df_action = pd.read_parquet(example_path / "action.parquet")
df_episode_index = pd.read_parquet(example_path / "episode_index.parquet") df_episode_index = pd.read_parquet(example_path / "episode_index.parquet")
self.df = pd.merge_asof( self.df = pd.merge_asof(
@@ -29,6 +48,16 @@ class ReplayPolicy:
self.finished = False self.finished = False


def select_action(self, obs): def select_action(self, obs):
"""Select the next action to replay.

Args:
obs: Current observation from the environment (unused)

Returns:
tuple: (action, finished) where action is the next action to take
and finished indicates if all actions have been replayed

"""
if self.index < len(self.df): if self.index < len(self.df):
self.index += 1 self.index += 1
else: else:
@@ -49,8 +78,8 @@ class ReplayPolicy:


policy = ReplayPolicy( policy = ReplayPolicy(
Path( Path(
"/home/rcadene/dora-aloha/aloha/graphs/out/018fa4ad-5942-7235-93d3-3efebe9b8a12"
)
"/home/rcadene/dora-aloha/aloha/graphs/out/018fa4ad-5942-7235-93d3-3efebe9b8a12",
),
) )






+ 10
- 5
examples/aloha/nodes/keyboard_node.py View File

@@ -1,8 +1,13 @@
from pynput import keyboard
from pynput.keyboard import Key, Events
"""Module for handling keyboard input events in robot control.

This module provides functionality for capturing and processing keyboard input
events to control robot movements and actions.
"""

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

from pynput import keyboard
from pynput.keyboard import Events, Key


node = Node() node = Node()
buffer_text = "" buffer_text = ""
@@ -17,7 +22,7 @@ with keyboard.Events() as events:
cursor += 1 cursor += 1
node.send_output("space", pa.array([cursor])) node.send_output("space", pa.array([cursor]))
space = True space = True


elif event is not None and isinstance(event, Events.Release): elif event is not None and isinstance(event, Events.Release):
if event.key == Key.space: if event.key == Key.space:
@@ -25,7 +30,7 @@ with keyboard.Events() as events:
space = False space = False
elif event.key == Key.backspace: elif event.key == Key.backspace:
node.send_output("failed", pa.array([cursor])) node.send_output("failed", pa.array([cursor]))


if node.next(0.001) is None: if node.next(0.001) is None:
break break

+ 8
- 8
examples/aloha/nodes/lerobot_webcam_saver.py View File

@@ -1,14 +1,14 @@
"""TODO: Add docstring."""

#!/usr/bin/env python3 #!/usr/bin/env python3
# -*- coding: utf-8 -*-


import os import os
import time
import numpy as np
import subprocess
from pathlib import Path

import cv2 import cv2
import pyarrow as pa import pyarrow as pa
from pathlib import Path
from dora import Node from dora import Node
import subprocess


node = Node() node = Node()


@@ -40,12 +40,12 @@ for event in node:
f"ffmpeg -r {FPS} " f"ffmpeg -r {FPS} "
"-f image2 " "-f image2 "
"-loglevel error " "-loglevel error "
f"-i {str(out_dir / 'frame_%06d.png')} "
f"-i {out_dir / 'frame_%06d.png'!s} "
"-vcodec libx264 " "-vcodec libx264 "
"-g 2 " "-g 2 "
"-pix_fmt yuv444p " "-pix_fmt yuv444p "
f"{str(video_path)} &&"
f"rm -r {str(out_dir)}"
f"{video_path!s} &&"
f"rm -r {out_dir!s}"
) )
print(ffmpeg_cmd, flush=True) print(ffmpeg_cmd, flush=True)
subprocess.Popen([ffmpeg_cmd], start_new_session=True, shell=True) subprocess.Popen([ffmpeg_cmd], start_new_session=True, shell=True)


+ 84
- 31
examples/aloha/nodes/llm_op.py View File

@@ -1,14 +1,20 @@
from dora import DoraStatus
import pylcs
import os
import pyarrow as pa
from transformers import AutoModelForCausalLM, AutoTokenizer
import torch
"""Module for language model-based robot control.

This module provides functionality for controlling robots using natural language
commands processed by large language models.
"""


import gc # garbage collect library import gc # garbage collect library
import os
import re import re
import time import time


import pyarrow as pa
import pylcs
import torch
from dora import DoraStatus
from transformers import AutoModelForCausalLM, AutoTokenizer

CHATGPT = False CHATGPT = False
MODEL_NAME_OR_PATH = "TheBloke/deepseek-coder-6.7B-instruct-GPTQ" MODEL_NAME_OR_PATH = "TheBloke/deepseek-coder-6.7B-instruct-GPTQ"


@@ -39,14 +45,15 @@ tokenizer = AutoTokenizer.from_pretrained(MODEL_NAME_OR_PATH, use_fast=True)




def extract_python_code_blocks(text): def extract_python_code_blocks(text):
"""
Extracts Python code blocks from the given text that are enclosed in triple backticks with a python language identifier.
"""Extract Python code blocks from the given text.


Parameters:
- text: A string that may contain one or more Python code blocks.
Args:
text: A string that may contain one or more Python code blocks.


Returns: Returns:
- A list of strings, where each string is a block of Python code extracted from the text.
list: A list of strings, where each string is a block of Python code
extracted from the text.

""" """
pattern = r"```python\n(.*?)\n```" pattern = r"```python\n(.*?)\n```"
matches = re.findall(pattern, text, re.DOTALL) matches = re.findall(pattern, text, re.DOTALL)
@@ -55,21 +62,20 @@ def extract_python_code_blocks(text):
matches = re.findall(pattern, text, re.DOTALL) matches = re.findall(pattern, text, re.DOTALL)
if len(matches) == 0: if len(matches) == 0:
return [text] return [text]
else:
matches = [remove_last_line(matches[0])]
matches = [remove_last_line(matches[0])]


return matches return matches




def remove_last_line(python_code): def remove_last_line(python_code):
"""
Removes the last line from a given string of Python code.
"""Remove the last line from a given string of Python code.


Parameters:
- python_code: A string representing Python source code.
Args:
python_code: A string representing Python source code.


Returns: Returns:
- A string with the last line removed.
str: A string with the last line removed.

""" """
lines = python_code.split("\n") # Split the string into lines lines = python_code.split("\n") # Split the string into lines
if lines: # Check if there are any lines to remove if lines: # Check if there are any lines to remove
@@ -78,9 +84,17 @@ def remove_last_line(python_code):




def calculate_similarity(source, target): def calculate_similarity(source, target):
"""
Calculate a similarity score between the source and target strings.
"""Calculate a similarity score between the source and target strings.
This uses the edit distance relative to the length of the strings. This uses the edit distance relative to the length of the strings.

Args:
source: First string to compare
target: Second string to compare

Returns:
float: Similarity score between 0 and 1

""" """
edit_distance = pylcs.edit_distance(source, target) edit_distance = pylcs.edit_distance(source, target)
max_length = max(len(source), len(target)) max_length = max(len(source), len(target))
@@ -90,9 +104,17 @@ def calculate_similarity(source, target):




def find_best_match_location(source_code, target_block): def find_best_match_location(source_code, target_block):
"""
Find the best match for the target_block within the source_code by searching line by line,
considering blocks of varying lengths.
"""Find the best match for the target_block within the source_code.

This function searches line by line, considering blocks of varying lengths.

Args:
source_code: The source code to search within
target_block: The code block to find a match for

Returns:
tuple: (start_index, end_index) of the best matching location

""" """
source_lines = source_code.split("\n") source_lines = source_code.split("\n")
target_lines = target_block.split("\n") target_lines = target_block.split("\n")
@@ -121,8 +143,15 @@ def find_best_match_location(source_code, target_block):




def replace_code_in_source(source_code, replacement_block: str): def replace_code_in_source(source_code, replacement_block: str):
"""
Replace the best matching block in the source_code with the replacement_block, considering variable block lengths.
"""Replace the best matching block in the source_code with the replacement_block.

Args:
source_code: The original source code
replacement_block: The new code block to insert

Returns:
str: The modified source code

""" """
replacement_block = extract_python_code_blocks(replacement_block)[0] replacement_block = extract_python_code_blocks(replacement_block)[0]
start_index, end_index = find_best_match_location(source_code, replacement_block) start_index, end_index = find_best_match_location(source_code, replacement_block)
@@ -132,12 +161,18 @@ def replace_code_in_source(source_code, replacement_block: str):
source_code[:start_index] + replacement_block + source_code[end_index:] source_code[:start_index] + replacement_block + source_code[end_index:]
) )
return new_source return new_source
else:
return source_code
return source_code




class Operator: class Operator:
"""A class for managing LLM-based code modifications.
This class handles the process of using LLMs to modify code based on
natural language instructions and managing the modification workflow.
"""

def __init__(self) -> None: def __init__(self) -> None:
"""Initialize the operator with policy initialization flag."""
self.policy_init = False self.policy_init = False


def on_event( def on_event(
@@ -145,6 +180,16 @@ class Operator:
dora_event, dora_event,
send_output, send_output,
) -> DoraStatus: ) -> DoraStatus:
"""Handle incoming events and process LLM-based code modifications.

Args:
dora_event: Dictionary containing event information
send_output: Function to send output to the dataflow

Returns:
DoraStatus: Status indicating whether to continue processing

"""
global model, tokenizer global model, tokenizer
if dora_event["type"] == "INPUT" and dora_event["id"] == "text": if dora_event["type"] == "INPUT" and dora_event["id"] == "text":
input = dora_event["value"][0].as_py() input = dora_event["value"][0].as_py()
@@ -155,14 +200,14 @@ class Operator:
current_directory = os.path.dirname(current_file_path) current_directory = os.path.dirname(current_file_path)
path = current_directory + "/policy.py" path = current_directory + "/policy.py"


with open(path, "r", encoding="utf8") as f:
with open(path, encoding="utf8") as f:
code = f.read() code = f.read()


user_message = input user_message = input
start_llm = time.time() start_llm = time.time()


output = self.ask_llm( output = self.ask_llm(
CODE_MODIFIER_TEMPLATE.format(code=code, user_message=user_message)
CODE_MODIFIER_TEMPLATE.format(code=code, user_message=user_message),
) )


source_code = replace_code_in_source(code, output) source_code = replace_code_in_source(code, output)
@@ -178,7 +223,15 @@ class Operator:
return DoraStatus.CONTINUE return DoraStatus.CONTINUE


def ask_llm(self, prompt): def ask_llm(self, prompt):
"""Send a prompt to the LLM and get its response.

Args:
prompt: The prompt to send to the LLM

Returns:
str: The LLM's response


"""
# Generate output # Generate output
# prompt = PROMPT_TEMPLATE.format(system_message=system_message, prompt=prompt)) # prompt = PROMPT_TEMPLATE.format(system_message=system_message, prompt=prompt))
input = tokenizer(prompt, return_tensors="pt") input = tokenizer(prompt, return_tensors="pt")
@@ -213,7 +266,7 @@ if __name__ == "__main__":
current_directory = os.path.dirname(current_file_path) current_directory = os.path.dirname(current_file_path)


path = current_directory + "/policy.py" path = current_directory + "/policy.py"
with open(path, "r", encoding="utf8") as f:
with open(path, encoding="utf8") as f:
raw = f.read() raw = f.read()


op.on_event( op.on_event(
@@ -226,7 +279,7 @@ if __name__ == "__main__":
"path": path, "path": path,
"user_message": "When I say suit up, get the hat and the get the food.", "user_message": "When I say suit up, get the hat and the get the food.",
}, },
]
],
), ),
"metadata": [], "metadata": [],
}, },


+ 3
- 1
examples/aloha/nodes/plot_node.py View File

@@ -1,8 +1,10 @@
"""TODO: Add docstring."""

import os import os

import cv2 import cv2
from dora import Node from dora import Node



IMAGE_WIDTH = int(os.getenv("IMAGE_WIDTH", "1280")) IMAGE_WIDTH = int(os.getenv("IMAGE_WIDTH", "1280"))
IMAGE_HEIGHT = int(os.getenv("IMAGE_HEIGHT", "720")) IMAGE_HEIGHT = int(os.getenv("IMAGE_HEIGHT", "720"))
FONT = cv2.FONT_HERSHEY_SIMPLEX FONT = cv2.FONT_HERSHEY_SIMPLEX


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

@@ -1,12 +1,34 @@
import pyarrow as pa
"""Module for implementing robot control policies.

This module provides functionality for implementing and managing robot control policies,
including action selection and state management.
"""

from dora import DoraStatus from dora import DoraStatus




class Operator: class Operator:
"""A class for implementing robot control policies.
This class handles the selection and execution of robot actions based on
input events and current state.
"""

def __init__(self): def __init__(self):
"""Initialize the operator with available actions."""
self.actions = ["get_food", "get_hat"] self.actions = ["get_food", "get_hat"]


def on_event(self, event: dict, send_output) -> DoraStatus: def on_event(self, event: dict, send_output) -> DoraStatus:
"""Handle incoming events and generate appropriate actions.

Args:
event: Dictionary containing event information
send_output: Function to send output to the dataflow

Returns:
DoraStatus: Status indicating whether to continue processing

"""
if event["type"] == "INPUT": if event["type"] == "INPUT":
id = event["id"] id = event["id"]
# On initialization # On initialization


+ 11
- 4
examples/aloha/nodes/realsense_node.py View File

@@ -1,9 +1,16 @@
import pyrealsense2 as rs
import numpy as np
from dora import Node
import pyarrow as pa
"""Module for handling Intel RealSense camera data.

This module provides functionality for capturing and processing data from
Intel RealSense cameras, including depth and color images.
"""

import os import os

import cv2 import cv2
import numpy as np
import pyarrow as pa
import pyrealsense2 as rs
from dora import Node


IMAGE_WIDTH = int(os.getenv("IMAGE_WIDTH", "640")) IMAGE_WIDTH = int(os.getenv("IMAGE_WIDTH", "640"))
IMAGE_HEIGHT = int(os.getenv("IMAGE_HEIGHT", "480")) IMAGE_HEIGHT = int(os.getenv("IMAGE_HEIGHT", "480"))


+ 5
- 3
examples/aloha/nodes/replay.py View File

@@ -1,8 +1,10 @@
from dora import Node
import pandas as pd
import pyarrow as pa
"""TODO: Add docstring."""

import time import time


import pandas as pd
import pyarrow as pa
from dora import Node


TOPIC = "puppet_goal_position" TOPIC = "puppet_goal_position"




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

@@ -1,12 +1,16 @@
"""Module for handling webcam input and processing.

This module provides functionality for capturing and processing video input
from webcam devices for robot vision applications.
"""

#!/usr/bin/env python3 #!/usr/bin/env python3
# -*- coding: utf-8 -*-


import os import os
import time
import numpy as np

import cv2 import cv2
import numpy as np
import pyarrow as pa import pyarrow as pa

from dora import Node from dora import Node


node = Node() node = Node()
@@ -26,8 +30,8 @@ for event in node:
frame = np.zeros((CAMERA_HEIGHT, CAMERA_WIDTH, 3), dtype=np.uint8) frame = np.zeros((CAMERA_HEIGHT, CAMERA_WIDTH, 3), dtype=np.uint8)
cv2.putText( cv2.putText(
frame, frame,
"No Webcam was found at index %d" % (CAMERA_ID),
(int(30), int(30)),
f"No Webcam was found at index {CAMERA_ID}",
(30, 30),
font, font,
0.75, 0.75,
(255, 255, 255), (255, 255, 255),


+ 18
- 8
examples/aloha/nodes/whisper_node.py View File

@@ -1,14 +1,16 @@
import pyarrow as pa
import whisper
from pynput import keyboard
from pynput.keyboard import Key, Events
from dora import Node
"""Module for speech recognition using Whisper.
This module provides functionality for capturing audio input and converting
it to text using the Whisper speech recognition model.
"""


import torch
import numpy as np import numpy as np
import pyarrow as pa import pyarrow as pa
import sounddevice as sd import sounddevice as sd
import gc # garbage collect library
import whisper
from dora import Node
from pynput import keyboard
from pynput.keyboard import Events, Key


model = whisper.load_model("base") model = whisper.load_model("base")


@@ -18,7 +20,15 @@ node = Node()




def get_text(duration) -> str: def get_text(duration) -> str:
"""Capture audio and convert it to text using Whisper.

Args:
duration: Duration of audio to capture in seconds

Returns:
str: Transcribed text from the audio input


"""
## Microphone ## Microphone
audio_data = sd.rec( audio_data = sd.rec(
int(SAMPLE_RATE * duration), int(SAMPLE_RATE * duration),
@@ -48,7 +58,7 @@ with keyboard.Events() as events:
if event.key == Key.alt_r: if event.key == Key.alt_r:
result = get_text(5) result = get_text(5)
node.send_output( node.send_output(
"text_llm", pa.array([result["text"]]), dora_event["metadata"]
"text_llm", pa.array([result["text"]]), dora_event["metadata"],
) )
elif event.key == Key.ctrl_r: elif event.key == Key.ctrl_r:
result = get_text(3) result = get_text(3)


+ 6
- 3
examples/aloha/tests/test_realsense.py View File

@@ -1,7 +1,10 @@
import pyrealsense2 as rs
"""TODO: Add docstring."""

import os

import cv2 import cv2
import numpy as np import numpy as np
import os
import pyrealsense2 as rs


CAMERA_ID = os.getenv("CAMERA_ID") CAMERA_ID = os.getenv("CAMERA_ID")
pipe = rs.pipeline() pipe = rs.pipeline()
@@ -12,7 +15,7 @@ profile = pipe.start(config)




try: try:
for i in range(0, 1000):
for i in range(1000):
frames = pipe.wait_for_frames() frames = pipe.wait_for_frames()
color_frame = frames.get_color_frame() color_frame = frames.get_color_frame()
color_images = np.asanyarray(color_frame.get_data()) color_images = np.asanyarray(color_frame.get_data())


+ 10
- 5
examples/lebai/nodes/interpolation.py View File

@@ -1,14 +1,19 @@
from dora import Node
import pyarrow as pa
"""Module for interpolating between robot configurations.

This module provides functionality for creating smooth transitions between
robot configurations and movements.
"""

from enum import Enum from enum import Enum


import pyarrow as pa
from dora import Node

node = Node() node = Node()




class Action(Enum): class Action(Enum):
"""
Action abstraction
"""
"""Action abstraction for robot movement commands."""


YAW_RIGHT = ("yaw right", "movej", [0, 0, 0, 0, -0.1, 0, 0.1]) YAW_RIGHT = ("yaw right", "movej", [0, 0, 0, 0, -0.1, 0, 0.1])
YAW_LEFT = ("yaw left", "movej", [0, 0, 0, 0, 0.1, 0, 0.1]) YAW_LEFT = ("yaw left", "movej", [0, 0, 0, 0, 0.1, 0, 0.1])


+ 7
- 1
examples/lebai/nodes/key_interpolation.py View File

@@ -1,5 +1,11 @@
from dora import Node
"""Module for interpolating between keyframe positions.

This module provides functionality for creating smooth transitions between
keyframe positions in robot motion sequences.
"""

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


node = Node() node = Node()




+ 5
- 3
examples/lebai/nodes/prompt_interpolation.py View File

@@ -1,5 +1,7 @@
from dora import Node
"""TODO: Add docstring."""

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


node = Node() node = Node()


@@ -15,7 +17,7 @@ for event in node:
pa.array( pa.array(
[ [
"Respond with left, right, forward, back, up, down or go home in order for the robotic arm to " "Respond with left, right, forward, back, up, down or go home in order for the robotic arm to "
+ text
]
+ text,
],
), ),
) )

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

+ 7
- 1
examples/lebai/nodes/voice_interpolation.py View File

@@ -1,5 +1,11 @@
from dora import Node
"""Module for interpolating between voice commands.

This module provides functionality for processing and interpolating between
voice commands used for robot control.
"""

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


node = Node() node = Node()




+ 49
- 4
examples/reachy/nodes/gym_dora_node.py View File

@@ -1,20 +1,33 @@
"""TODO: Add docstring."""

import time import time
from pathlib import Path


import gym_dora # noqa: F401
import gymnasium as gym import gymnasium as gym
import pandas as pd import pandas as pd
from lerobot.common.datasets.lerobot_dataset import LeRobotDataset from lerobot.common.datasets.lerobot_dataset import LeRobotDataset


import gym_dora # noqa: F401

env = gym.make( env = gym.make(
"gym_dora/DoraReachy2-v0", disable_env_checker=True, max_episode_steps=10000
"gym_dora/DoraReachy2-v0", disable_env_checker=True, max_episode_steps=10000,
) )
observation = env.reset() observation = env.reset()




class ReplayPolicy: class ReplayPolicy:
"""A policy class for replaying recorded robot actions.
This class handles loading and replaying recorded actions from a dataset,
maintaining timing between actions to match the original recording.
"""

def __init__(self, example_path, epidode=0): def __init__(self, example_path, epidode=0):
"""Initialize the replay policy.

Args:
example_path: Path to the directory containing recorded actions
epidode: Index of the episode to replay

"""
df_action = pd.read_parquet(example_path / "action.parquet") df_action = pd.read_parquet(example_path / "action.parquet")
df_episode_index = pd.read_parquet(example_path / "episode_index.parquet") df_episode_index = pd.read_parquet(example_path / "episode_index.parquet")
self.df = pd.merge_asof( self.df = pd.merge_asof(
@@ -31,6 +44,16 @@ class ReplayPolicy:
self.finished = False self.finished = False


def select_action(self, obs): def select_action(self, obs):
"""Select the next action to replay.

Args:
obs: Current observation from the environment (unused)

Returns:
tuple: (action, finished) where action is the next action to take
and finished indicates if all actions have been replayed

"""
if self.index < len(self.df): if self.index < len(self.df):
self.index += 1 self.index += 1
else: else:
@@ -44,7 +67,19 @@ class ReplayPolicy:




class ReplayLeRobotPolicy: class ReplayLeRobotPolicy:
"""A policy class for replaying actions from the LeRobot dataset.
This class handles loading and replaying actions from the LeRobot dataset,
maintaining the sequence of actions from the original recording.
"""

def __init__(self, episode=21): def __init__(self, episode=21):
"""Initialize the LeRobot replay policy.

Args:
episode: Index of the episode to replay from the dataset

"""
self.index = 0 self.index = 0
self.finished = False self.finished = False
# episode = 1 # episode = 1
@@ -55,6 +90,16 @@ class ReplayLeRobotPolicy:
self.actions = dataset.hf_dataset["action"][from_index:to_index] self.actions = dataset.hf_dataset["action"][from_index:to_index]


def select_action(self, obs): def select_action(self, obs):
"""Select the next action to replay from the LeRobot dataset.

Args:
obs: Current observation from the environment (unused)

Returns:
tuple: (action, finished) where action is the next action to take
and finished indicates if all actions have been replayed

"""
if self.index < len(self.actions): if self.index < len(self.actions):
self.index += 1 self.index += 1
else: else:


+ 10
- 5
examples/reachy/nodes/keyboard_node.py View File

@@ -1,8 +1,13 @@
from pynput import keyboard
from pynput.keyboard import Key, Events
"""Module for handling keyboard input in the Reachy robot control system.

This module provides functionality for capturing and processing keyboard input
to control the Reachy robot's movements and actions.
"""

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

from pynput import keyboard
from pynput.keyboard import Events, Key


node = Node() node = Node()
buffer_text = "" buffer_text = ""
@@ -17,7 +22,7 @@ with keyboard.Events() as events:
cursor += 1 cursor += 1
node.send_output("space", pa.array([cursor])) node.send_output("space", pa.array([cursor]))
space = True space = True


elif event is not None and isinstance(event, Events.Release): elif event is not None and isinstance(event, Events.Release):
if event.key == Key.space: if event.key == Key.space:
@@ -25,7 +30,7 @@ with keyboard.Events() as events:
space = False space = False
elif event.key == Key.backspace: elif event.key == Key.backspace:
node.send_output("failed", pa.array([cursor])) node.send_output("failed", pa.array([cursor]))


if node.next(0.001) is None: if node.next(0.001) is None:
break break

+ 8
- 8
examples/reachy/nodes/lerobot_webcam_saver.py View File

@@ -1,14 +1,14 @@
"""TODO: Add docstring."""

#!/usr/bin/env python3 #!/usr/bin/env python3
# -*- coding: utf-8 -*-


import os import os
import time
import numpy as np
import subprocess
from pathlib import Path

import cv2 import cv2
import pyarrow as pa import pyarrow as pa
from pathlib import Path
from dora import Node from dora import Node
import subprocess


node = Node() node = Node()


@@ -40,12 +40,12 @@ for event in node:
f"ffmpeg -r {FPS} " f"ffmpeg -r {FPS} "
"-f image2 " "-f image2 "
"-loglevel error " "-loglevel error "
f"-i {str(out_dir / 'frame_%06d.png')} "
f"-i {out_dir / 'frame_%06d.png'!s} "
"-vcodec libx264 " "-vcodec libx264 "
"-g 2 " "-g 2 "
"-pix_fmt yuv444p " "-pix_fmt yuv444p "
f"{str(video_path)} &&"
f"rm -r {str(out_dir)}"
f"{video_path!s} &&"
f"rm -r {out_dir!s}"
) )
print(ffmpeg_cmd, flush=True) print(ffmpeg_cmd, flush=True)
subprocess.Popen([ffmpeg_cmd], start_new_session=True, shell=True) subprocess.Popen([ffmpeg_cmd], start_new_session=True, shell=True)


+ 7
- 1
examples/reachy/nodes/plot_node.py View File

@@ -1,8 +1,14 @@
"""Module for visualizing robot data and states.

This module provides functionality for plotting and displaying various aspects
of the robot's state, including positions, movements, and sensor data.
"""

import os import os

import cv2 import cv2
from dora import Node from dora import Node



IMAGE_WIDTH = int(os.getenv("IMAGE_WIDTH", "1280")) IMAGE_WIDTH = int(os.getenv("IMAGE_WIDTH", "1280"))
IMAGE_HEIGHT = int(os.getenv("IMAGE_HEIGHT", "720")) IMAGE_HEIGHT = int(os.getenv("IMAGE_HEIGHT", "720"))
FONT = cv2.FONT_HERSHEY_SIMPLEX FONT = cv2.FONT_HERSHEY_SIMPLEX


+ 14
- 15
examples/reachy/nodes/reachy_client.py View File

@@ -1,7 +1,6 @@
import argparse
import os
"""TODO: Add docstring."""
import time import time
from pathlib import Path


# import h5py # import h5py
import numpy as np import numpy as np
@@ -83,7 +82,7 @@ for event in node:
# min(100, max(0, action[7] / 2.26 * 100)) # min(100, max(0, action[7] / 2.26 * 100))
# ) # replay true action value # ) # replay true action value
reachy.l_arm.gripper.set_opening( reachy.l_arm.gripper.set_opening(
0 if action[7] < 2.0 else 100
0 if action[7] < 2.0 else 100,
) # trick to force the gripper to close fully ) # trick to force the gripper to close fully


reachy.r_arm.shoulder.pitch.goal_position = np.rad2deg(action[8]) reachy.r_arm.shoulder.pitch.goal_position = np.rad2deg(action[8])
@@ -97,7 +96,7 @@ for event in node:
# min(100, max(0, action[15] / 2.26 * 100)) # min(100, max(0, action[15] / 2.26 * 100))
# ) # replay true action value # ) # replay true action value
reachy.r_arm.gripper.set_opening( reachy.r_arm.gripper.set_opening(
0 if action[15] < 2.0 else 100
0 if action[15] < 2.0 else 100,
) # trick to force the gripper to close fully ) # trick to force the gripper to close fully
reachy.mobile_base.set_speed(action[16], action[17], np.rad2deg(action[18])) reachy.mobile_base.set_speed(action[16], action[17], np.rad2deg(action[18]))
reachy.head.neck.roll.goal_position = np.rad2deg(action[19]) reachy.head.neck.roll.goal_position = np.rad2deg(action[19])
@@ -110,38 +109,38 @@ for event in node:
mobile_base_pos = {"vx": 0, "vy": 0, "vtheta": 0} mobile_base_pos = {"vx": 0, "vy": 0, "vtheta": 0}
qpos = { qpos = {
"l_arm_shoulder_pitch": np.deg2rad( "l_arm_shoulder_pitch": np.deg2rad(
reachy.l_arm.shoulder.pitch.present_position
reachy.l_arm.shoulder.pitch.present_position,
), ),
"l_arm_shoulder_roll": np.deg2rad( "l_arm_shoulder_roll": np.deg2rad(
reachy.l_arm.shoulder.roll.present_position
reachy.l_arm.shoulder.roll.present_position,
), ),
"l_arm_elbow_yaw": np.deg2rad(reachy.l_arm.elbow.yaw.present_position), "l_arm_elbow_yaw": np.deg2rad(reachy.l_arm.elbow.yaw.present_position),
"l_arm_elbow_pitch": np.deg2rad( "l_arm_elbow_pitch": np.deg2rad(
reachy.l_arm.elbow.pitch.present_position
reachy.l_arm.elbow.pitch.present_position,
), ),
"l_arm_wrist_roll": np.deg2rad( "l_arm_wrist_roll": np.deg2rad(
reachy.l_arm.wrist.roll.present_position
reachy.l_arm.wrist.roll.present_position,
), ),
"l_arm_wrist_pitch": np.deg2rad( "l_arm_wrist_pitch": np.deg2rad(
reachy.l_arm.wrist.pitch.present_position
reachy.l_arm.wrist.pitch.present_position,
), ),
"l_arm_wrist_yaw": np.deg2rad(reachy.l_arm.wrist.yaw.present_position), "l_arm_wrist_yaw": np.deg2rad(reachy.l_arm.wrist.yaw.present_position),
"l_gripper": reachy.l_arm.gripper._present_position, "l_gripper": reachy.l_arm.gripper._present_position,
"r_arm_shoulder_pitch": np.deg2rad( "r_arm_shoulder_pitch": np.deg2rad(
reachy.r_arm.shoulder.pitch.present_position
reachy.r_arm.shoulder.pitch.present_position,
), ),
"r_arm_shoulder_roll": np.deg2rad( "r_arm_shoulder_roll": np.deg2rad(
reachy.r_arm.shoulder.roll.present_position
reachy.r_arm.shoulder.roll.present_position,
), ),
"r_arm_elbow_yaw": np.deg2rad(reachy.r_arm.elbow.yaw.present_position), "r_arm_elbow_yaw": np.deg2rad(reachy.r_arm.elbow.yaw.present_position),
"r_arm_elbow_pitch": np.deg2rad( "r_arm_elbow_pitch": np.deg2rad(
reachy.r_arm.elbow.pitch.present_position
reachy.r_arm.elbow.pitch.present_position,
), ),
"r_arm_wrist_roll": np.deg2rad( "r_arm_wrist_roll": np.deg2rad(
reachy.r_arm.wrist.roll.present_position
reachy.r_arm.wrist.roll.present_position,
), ),
"r_arm_wrist_pitch": np.deg2rad( "r_arm_wrist_pitch": np.deg2rad(
reachy.r_arm.wrist.pitch.present_position
reachy.r_arm.wrist.pitch.present_position,
), ),
"r_arm_wrist_yaw": np.deg2rad(reachy.r_arm.wrist.yaw.present_position), "r_arm_wrist_yaw": np.deg2rad(reachy.r_arm.wrist.yaw.present_position),
"r_gripper": reachy.r_arm.gripper._present_position, "r_gripper": reachy.r_arm.gripper._present_position,


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

@@ -1,7 +1,6 @@
import argparse
import os
"""TODO: Add docstring."""
import time import time
from pathlib import Path


import numpy as np import numpy as np
import pyarrow as pa import pyarrow as pa
@@ -21,7 +20,6 @@ cam = SDKWrapper(get_config_file_path("CONFIG_SR"), fps=freq)
# ret, image = cap.read() # ret, image = cap.read()


import cv2 import cv2
import numpy as np


episode = 1 episode = 1
dataset = LeRobotDataset("cadene/reachy2_teleop_remi") dataset = LeRobotDataset("cadene/reachy2_teleop_remi")
@@ -41,8 +39,6 @@ images = images.astype(np.uint8)
images = cv2.cvtColor(images, cv2.COLOR_RGB2BGR) images = cv2.cvtColor(images, cv2.COLOR_RGB2BGR)


# start = time.time() # start = time.time()
import PIL
import torch


# frame_hwc = (images.permute((1, 2, 0)) * 255).type(torch.uint8).cpu().numpy() # frame_hwc = (images.permute((1, 2, 0)) * 255).type(torch.uint8).cpu().numpy()




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

@@ -1,7 +1,10 @@
from lerobot.common.datasets.lerobot_dataset import LeRobotDataset
"""TODO: Add docstring."""

import time import time
from dora import Node
import pyarrow as pa import pyarrow as pa
from dora import Node
from lerobot.common.datasets.lerobot_dataset import LeRobotDataset


episode = 1 episode = 1
dataset = LeRobotDataset("cadene/reachy2_teleop_remi") dataset = LeRobotDataset("cadene/reachy2_teleop_remi")


+ 12
- 4
examples/reachy1/nodes/gym_dora_node.py View File

@@ -1,20 +1,23 @@
"""TODO: Add docstring."""

import time import time
from pathlib import Path


import gym_dora # noqa: F401
import gymnasium as gym import gymnasium as gym
import pandas as pd import pandas as pd
from lerobot.common.datasets.lerobot_dataset import LeRobotDataset from lerobot.common.datasets.lerobot_dataset import LeRobotDataset


import gym_dora # noqa: F401

env = gym.make( env = gym.make(
"gym_dora/DoraReachy2-v0", disable_env_checker=True, max_episode_steps=10000
"gym_dora/DoraReachy2-v0", disable_env_checker=True, max_episode_steps=10000,
) )
observation = env.reset() observation = env.reset()




class ReplayPolicy: class ReplayPolicy:
"""TODO: Add docstring."""

def __init__(self, example_path, epidode=0): def __init__(self, example_path, epidode=0):
"""TODO: Add docstring."""
df_action = pd.read_parquet(example_path / "action.parquet") df_action = pd.read_parquet(example_path / "action.parquet")
df_episode_index = pd.read_parquet(example_path / "episode_index.parquet") df_episode_index = pd.read_parquet(example_path / "episode_index.parquet")
self.df = pd.merge_asof( self.df = pd.merge_asof(
@@ -31,6 +34,7 @@ class ReplayPolicy:
self.finished = False self.finished = False


def select_action(self, obs): def select_action(self, obs):
"""TODO: Add docstring."""
if self.index < len(self.df): if self.index < len(self.df):
self.index += 1 self.index += 1
else: else:
@@ -44,7 +48,10 @@ class ReplayPolicy:




class ReplayLeRobotPolicy: class ReplayLeRobotPolicy:
"""TODO: Add docstring."""

def __init__(self, episode=21): def __init__(self, episode=21):
"""TODO: Add docstring."""
self.index = 0 self.index = 0
self.finished = False self.finished = False
# episode = 1 # episode = 1
@@ -55,6 +62,7 @@ class ReplayLeRobotPolicy:
self.actions = dataset.hf_dataset["action"][from_index:to_index] self.actions = dataset.hf_dataset["action"][from_index:to_index]


def select_action(self, obs): def select_action(self, obs):
"""TODO: Add docstring."""
if self.index < len(self.actions): if self.index < len(self.actions):
self.index += 1 self.index += 1
else: else:


+ 3
- 1
examples/reachy1/nodes/key_interpolation.py View File

@@ -1,5 +1,7 @@
from dora import Node
"""TODO: Add docstring."""

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


node = Node() node = Node()




+ 6
- 5
examples/reachy1/nodes/keyboard_node.py View File

@@ -1,8 +1,9 @@
from pynput import keyboard
from pynput.keyboard import Key, Events
"""TODO: Add docstring."""
import pyarrow as pa import pyarrow as pa
from dora import Node from dora import Node

from pynput import keyboard
from pynput.keyboard import Events, Key


node = Node() node = Node()
buffer_text = "" buffer_text = ""
@@ -17,7 +18,7 @@ with keyboard.Events() as events:
cursor += 1 cursor += 1
node.send_output("space", pa.array([cursor])) node.send_output("space", pa.array([cursor]))
space = True space = True


elif event is not None and isinstance(event, Events.Release): elif event is not None and isinstance(event, Events.Release):
if event.key == Key.space: if event.key == Key.space:
@@ -25,7 +26,7 @@ with keyboard.Events() as events:
space = False space = False
elif event.key == Key.backspace: elif event.key == Key.backspace:
node.send_output("failed", pa.array([cursor])) node.send_output("failed", pa.array([cursor]))


if node.next(0.001) is None: if node.next(0.001) is None:
break break

+ 12
- 8
examples/reachy1/nodes/lerobot_webcam_saver.py View File

@@ -1,14 +1,18 @@
"""Module for saving webcam feed data from LeRobot experiments.

This module provides functionality for capturing and saving webcam feed data
during LeRobot experiments with the Reachy robot.
"""

#!/usr/bin/env python3 #!/usr/bin/env python3
# -*- coding: utf-8 -*-


import os import os
import time
import numpy as np
import subprocess
from pathlib import Path

import cv2 import cv2
import pyarrow as pa import pyarrow as pa
from pathlib import Path
from dora import Node from dora import Node
import subprocess


node = Node() node = Node()


@@ -40,12 +44,12 @@ for event in node:
f"ffmpeg -r {FPS} " f"ffmpeg -r {FPS} "
"-f image2 " "-f image2 "
"-loglevel error " "-loglevel error "
f"-i {str(out_dir / 'frame_%06d.png')} "
f"-i {out_dir / 'frame_%06d.png'!s} "
"-vcodec libx264 " "-vcodec libx264 "
"-g 2 " "-g 2 "
"-pix_fmt yuv444p " "-pix_fmt yuv444p "
f"{str(video_path)} &&"
f"rm -r {str(out_dir)}"
f"{video_path!s} &&"
f"rm -r {out_dir!s}"
) )
print(ffmpeg_cmd, flush=True) print(ffmpeg_cmd, flush=True)
subprocess.Popen([ffmpeg_cmd], start_new_session=True, shell=True) subprocess.Popen([ffmpeg_cmd], start_new_session=True, shell=True)


+ 7
- 1
examples/reachy1/nodes/plot_node.py View File

@@ -1,8 +1,14 @@
"""Module for visualizing robot data and states.

This module provides functionality for creating real-time visualizations of
robot states, movements, and sensor data during operation.
"""

import os import os

import cv2 import cv2
from dora import Node from dora import Node



IMAGE_WIDTH = int(os.getenv("IMAGE_WIDTH", "1280")) IMAGE_WIDTH = int(os.getenv("IMAGE_WIDTH", "1280"))
IMAGE_HEIGHT = int(os.getenv("IMAGE_HEIGHT", "720")) IMAGE_HEIGHT = int(os.getenv("IMAGE_HEIGHT", "720"))
FONT = cv2.FONT_HERSHEY_SIMPLEX FONT = cv2.FONT_HERSHEY_SIMPLEX


+ 18
- 15
examples/reachy1/nodes/reachy_client.py View File

@@ -1,7 +1,10 @@
import argparse
import os
"""Module for client-side communication with the Reachy robot.

This module provides functionality for establishing and managing communication
with the Reachy robot, including sending commands and receiving sensor data.
"""

import time import time
from pathlib import Path


# import h5py # import h5py
import numpy as np import numpy as np
@@ -83,7 +86,7 @@ for event in node:
# min(100, max(0, action[7] / 2.26 * 100)) # min(100, max(0, action[7] / 2.26 * 100))
# ) # replay true action value # ) # replay true action value
reachy.l_arm.gripper.set_opening( reachy.l_arm.gripper.set_opening(
0 if action[7] < 2.0 else 100
0 if action[7] < 2.0 else 100,
) # trick to force the gripper to close fully ) # trick to force the gripper to close fully


reachy.r_arm.shoulder.pitch.goal_position = np.rad2deg(action[8]) reachy.r_arm.shoulder.pitch.goal_position = np.rad2deg(action[8])
@@ -97,7 +100,7 @@ for event in node:
# min(100, max(0, action[15] / 2.26 * 100)) # min(100, max(0, action[15] / 2.26 * 100))
# ) # replay true action value # ) # replay true action value
reachy.r_arm.gripper.set_opening( reachy.r_arm.gripper.set_opening(
0 if action[15] < 2.0 else 100
0 if action[15] < 2.0 else 100,
) # trick to force the gripper to close fully ) # trick to force the gripper to close fully
reachy.mobile_base.set_speed(action[16], action[17], np.rad2deg(action[18])) reachy.mobile_base.set_speed(action[16], action[17], np.rad2deg(action[18]))
reachy.head.neck.roll.goal_position = np.rad2deg(action[19]) reachy.head.neck.roll.goal_position = np.rad2deg(action[19])
@@ -110,38 +113,38 @@ for event in node:
mobile_base_pos = {"vx": 0, "vy": 0, "vtheta": 0} mobile_base_pos = {"vx": 0, "vy": 0, "vtheta": 0}
qpos = { qpos = {
"l_arm_shoulder_pitch": np.deg2rad( "l_arm_shoulder_pitch": np.deg2rad(
reachy.l_arm.shoulder.pitch.present_position
reachy.l_arm.shoulder.pitch.present_position,
), ),
"l_arm_shoulder_roll": np.deg2rad( "l_arm_shoulder_roll": np.deg2rad(
reachy.l_arm.shoulder.roll.present_position
reachy.l_arm.shoulder.roll.present_position,
), ),
"l_arm_elbow_yaw": np.deg2rad(reachy.l_arm.elbow.yaw.present_position), "l_arm_elbow_yaw": np.deg2rad(reachy.l_arm.elbow.yaw.present_position),
"l_arm_elbow_pitch": np.deg2rad( "l_arm_elbow_pitch": np.deg2rad(
reachy.l_arm.elbow.pitch.present_position
reachy.l_arm.elbow.pitch.present_position,
), ),
"l_arm_wrist_roll": np.deg2rad( "l_arm_wrist_roll": np.deg2rad(
reachy.l_arm.wrist.roll.present_position
reachy.l_arm.wrist.roll.present_position,
), ),
"l_arm_wrist_pitch": np.deg2rad( "l_arm_wrist_pitch": np.deg2rad(
reachy.l_arm.wrist.pitch.present_position
reachy.l_arm.wrist.pitch.present_position,
), ),
"l_arm_wrist_yaw": np.deg2rad(reachy.l_arm.wrist.yaw.present_position), "l_arm_wrist_yaw": np.deg2rad(reachy.l_arm.wrist.yaw.present_position),
"l_gripper": reachy.l_arm.gripper._present_position, "l_gripper": reachy.l_arm.gripper._present_position,
"r_arm_shoulder_pitch": np.deg2rad( "r_arm_shoulder_pitch": np.deg2rad(
reachy.r_arm.shoulder.pitch.present_position
reachy.r_arm.shoulder.pitch.present_position,
), ),
"r_arm_shoulder_roll": np.deg2rad( "r_arm_shoulder_roll": np.deg2rad(
reachy.r_arm.shoulder.roll.present_position
reachy.r_arm.shoulder.roll.present_position,
), ),
"r_arm_elbow_yaw": np.deg2rad(reachy.r_arm.elbow.yaw.present_position), "r_arm_elbow_yaw": np.deg2rad(reachy.r_arm.elbow.yaw.present_position),
"r_arm_elbow_pitch": np.deg2rad( "r_arm_elbow_pitch": np.deg2rad(
reachy.r_arm.elbow.pitch.present_position
reachy.r_arm.elbow.pitch.present_position,
), ),
"r_arm_wrist_roll": np.deg2rad( "r_arm_wrist_roll": np.deg2rad(
reachy.r_arm.wrist.roll.present_position
reachy.r_arm.wrist.roll.present_position,
), ),
"r_arm_wrist_pitch": np.deg2rad( "r_arm_wrist_pitch": np.deg2rad(
reachy.r_arm.wrist.pitch.present_position
reachy.r_arm.wrist.pitch.present_position,
), ),
"r_arm_wrist_yaw": np.deg2rad(reachy.r_arm.wrist.yaw.present_position), "r_arm_wrist_yaw": np.deg2rad(reachy.r_arm.wrist.yaw.present_position),
"r_gripper": reachy.r_arm.gripper._present_position, "r_gripper": reachy.r_arm.gripper._present_position,


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

@@ -1,7 +1,10 @@
import argparse
import os
"""Module for handling vision-related communication with the Reachy robot.

This module provides functionality for processing and managing visual data
from the Reachy robot's cameras and vision sensors.
"""

import time import time
from pathlib import Path


import numpy as np import numpy as np
import pyarrow as pa import pyarrow as pa
@@ -21,7 +24,6 @@ cam = SDKWrapper(get_config_file_path("CONFIG_SR"), fps=freq)
# ret, image = cap.read() # ret, image = cap.read()


import cv2 import cv2
import numpy as np


episode = 1 episode = 1
dataset = LeRobotDataset("cadene/reachy2_teleop_remi") dataset = LeRobotDataset("cadene/reachy2_teleop_remi")
@@ -41,8 +43,6 @@ images = images.astype(np.uint8)
images = cv2.cvtColor(images, cv2.COLOR_RGB2BGR) images = cv2.cvtColor(images, cv2.COLOR_RGB2BGR)


# start = time.time() # start = time.time()
import PIL
import torch


# frame_hwc = (images.permute((1, 2, 0)) * 255).type(torch.uint8).cpu().numpy() # frame_hwc = (images.permute((1, 2, 0)) * 255).type(torch.uint8).cpu().numpy()




+ 9
- 2
examples/reachy1/nodes/replay_node.py View File

@@ -1,7 +1,14 @@
from lerobot.common.datasets.lerobot_dataset import LeRobotDataset
"""Module for replaying recorded robot actions and movements.

This module provides functionality for loading and replaying previously recorded
robot actions and movements, allowing for demonstration and analysis of robot behavior.
"""

import time import time
from dora import Node
import pyarrow as pa import pyarrow as pa
from dora import Node
from lerobot.common.datasets.lerobot_dataset import LeRobotDataset


episode = 1 episode = 1
dataset = LeRobotDataset("cadene/reachy2_teleop_remi") dataset = LeRobotDataset("cadene/reachy2_teleop_remi")


+ 15
- 10
examples/reachy1/nodes/text_interpolation.py View File

@@ -1,6 +1,11 @@
from dora import Node
"""Module for interpolating between text-based robot commands.

This module provides functionality for processing and interpolating between
text-based commands to create smooth transitions in robot behavior.
"""

import pyarrow as pa import pyarrow as pa
import numpy as np
from dora import Node


node = Node() node = Node()


@@ -19,19 +24,19 @@ for event in node:
node.send_output("head_action", pa.array([0, head_step, 0])) node.send_output("head_action", pa.array([0, head_step, 0]))
elif text == "look up": elif text == "look up":
node.send_output( node.send_output(
"head_action", pa.array([head_step / 2, 0, -head_step / 2])
"head_action", pa.array([head_step / 2, 0, -head_step / 2]),
) )
elif text == "look down": elif text == "look down":
node.send_output( node.send_output(
"head_action", pa.array([-head_step / 2, 0, head_step / 2])
"head_action", pa.array([-head_step / 2, 0, head_step / 2]),
) )
elif text == "look up": elif text == "look up":
node.send_output( node.send_output(
"head_action", pa.array([head_step / 2, 0, -head_step / 2])
"head_action", pa.array([head_step / 2, 0, -head_step / 2]),
) )
elif text == "look down": elif text == "look down":
node.send_output( node.send_output(
"head_action", pa.array([-head_step / 2, 0, head_step / 2])
"head_action", pa.array([-head_step / 2, 0, head_step / 2]),
) )
elif text == "smile": elif text == "smile":
node.send_output("antenna_action", pa.array(["smile"])) node.send_output("antenna_action", pa.array(["smile"]))
@@ -54,8 +59,8 @@ for event in node:
"question", "question",
pa.array( pa.array(
[ [
"Respond with right, left, forward, backward, open, or close to grab the trash"
]
"Respond with right, left, forward, backward, open, or close to grab the trash",
],
), ),
) )


@@ -65,8 +70,8 @@ for event in node:
"question", "question",
pa.array( pa.array(
[ [
"Respond with right, left, forward, backward, open, or close to put the trash in your hand in the right bin"
]
"Respond with right, left, forward, backward, open, or close to put the trash in your hand in the right bin",
],
), ),
) )
node.send_output("gripper_action", pa.array([100])) node.send_output("gripper_action", pa.array([100]))

+ 118
- 14
examples/so100/bus.py View File

@@ -1,17 +1,24 @@
import enum
"""Module for managing Feetech servo motor communication.


import pyarrow as pa
This module provides functionality for communicating with and controlling
Feetech servo motors through a serial bus interface.
"""


import enum
from typing import Union from typing import Union


import pyarrow as pa
from scservo_sdk import ( from scservo_sdk import (
PacketHandler,
PortHandler,
COMM_SUCCESS, COMM_SUCCESS,
SCS_HIBYTE,
SCS_HIWORD,
SCS_LOBYTE,
SCS_LOWORD,
GroupSyncRead, GroupSyncRead,
GroupSyncWrite, GroupSyncWrite,
PacketHandler,
PortHandler,
) )
from scservo_sdk import SCS_HIBYTE, SCS_HIWORD, SCS_LOBYTE, SCS_LOWORD


PROTOCOL_VERSION = 0 PROTOCOL_VERSION = 0
BAUD_RATE = 1_000_000 BAUD_RATE = 1_000_000
@@ -22,6 +29,16 @@ def wrap_joints_and_values(
joints: Union[list[str], pa.Array], joints: Union[list[str], pa.Array],
values: Union[list[int], pa.Array], values: Union[list[int], pa.Array],
) -> pa.StructArray: ) -> pa.StructArray:
"""Create a structured array from joint names and their values.

Args:
joints: List of joint names or Arrow array of joint names
values: List of values or Arrow array of values

Returns:
pa.StructArray: Structured array containing joint names and values

"""
return pa.StructArray.from_arrays( return pa.StructArray.from_arrays(
arrays=[joints, values], arrays=[joints, values],
names=["joints", "values"], names=["joints", "values"],
@@ -29,11 +46,15 @@ def wrap_joints_and_values(




class TorqueMode(enum.Enum): class TorqueMode(enum.Enum):
"""Enumeration of torque control modes for servo motors."""

ENABLED = pa.scalar(1, pa.uint32()) ENABLED = pa.scalar(1, pa.uint32())
DISABLED = pa.scalar(0, pa.uint32()) DISABLED = pa.scalar(0, pa.uint32())




class OperatingMode(enum.Enum): class OperatingMode(enum.Enum):
"""Enumeration of operating modes for servo motors."""

ONE_TURN = pa.scalar(0, pa.uint32()) ONE_TURN = pa.scalar(0, pa.uint32())




@@ -91,15 +112,23 @@ MODEL_CONTROL_TABLE = {




class FeetechBus: class FeetechBus:
"""A class for managing communication with Feetech servo motors.
This class handles the low-level communication with Feetech servo motors
through a serial bus interface, providing methods for reading and writing
motor parameters.
"""


def __init__(self, port: str, description: dict[str, (np.uint8, str)]): def __init__(self, port: str, description: dict[str, (np.uint8, str)]):
"""
"""Initialize the Feetech bus interface.

Args: Args:
port: the serial port to connect to the Feetech bus
description: a dictionary containing the description of the motors connected to the bus. The keys are the
motor names and the values are tuples containing the motor id and the motor model.
"""
port: The serial port to connect to the Feetech bus
description: A dictionary containing the description of the motors
connected to the bus. The keys are the motor names and
the values are tuples containing the motor id and model.


"""
self.port = port self.port = port
self.descriptions = description self.descriptions = description
self.motor_ctrl = {} self.motor_ctrl = {}
@@ -130,9 +159,17 @@ class FeetechBus:
self.group_writers = {} self.group_writers = {}


def close(self): def close(self):
"""Close the serial port connection."""
self.port_handler.closePort() self.port_handler.closePort()


def write(self, data_name: str, data: pa.StructArray): def write(self, data_name: str, data: pa.StructArray):
"""Write data to the specified motor parameters.

Args:
data_name: Name of the parameter to write
data: Structured array containing the data to write

"""
motor_ids = [ motor_ids = [
self.motor_ctrl[motor_name.as_py()]["id"] self.motor_ctrl[motor_name.as_py()]["id"]
for motor_name in data.field("joints") for motor_name in data.field("joints")
@@ -183,7 +220,7 @@ class FeetechBus:
else: else:
raise NotImplementedError( raise NotImplementedError(
f"Value of the number of bytes to be sent is expected to be in [1, 2, 4], but {packet_bytes_size} " f"Value of the number of bytes to be sent is expected to be in [1, 2, 4], but {packet_bytes_size} "
f"is provided instead."
f"is provided instead.",
) )


if init_group: if init_group:
@@ -195,10 +232,20 @@ class FeetechBus:
if comm != COMM_SUCCESS: if comm != COMM_SUCCESS:
raise ConnectionError( raise ConnectionError(
f"Write failed due to communication error on port {self.port} for group_key {group_key}: " f"Write failed due to communication error on port {self.port} for group_key {group_key}: "
f"{self.packet_handler.getTxRxResult(comm)}"
f"{self.packet_handler.getTxRxResult(comm)}",
) )


def read(self, data_name: str, motor_names: pa.Array) -> pa.StructArray: def read(self, data_name: str, motor_names: pa.Array) -> pa.StructArray:
"""Read data from the specified motor parameters.

Args:
data_name: Name of the parameter to read
motor_names: Array of motor names to read from

Returns:
pa.StructArray: Structured array containing the read data

"""
motor_ids = [ motor_ids = [
self.motor_ctrl[motor_name.as_py()]["id"] for motor_name in motor_names self.motor_ctrl[motor_name.as_py()]["id"] for motor_name in motor_names
] ]
@@ -225,13 +272,13 @@ class FeetechBus:
if comm != COMM_SUCCESS: if comm != COMM_SUCCESS:
raise ConnectionError( raise ConnectionError(
f"Read failed due to communication error on port {self.port} for group_key {group_key}: " f"Read failed due to communication error on port {self.port} for group_key {group_key}: "
f"{self.packet_handler.getTxRxResult(comm)}"
f"{self.packet_handler.getTxRxResult(comm)}",
) )


values = pa.array( values = pa.array(
[ [
self.group_readers[group_key].getData( self.group_readers[group_key].getData(
idx, packet_address, packet_bytes_size
idx, packet_address, packet_bytes_size,
) )
for idx in motor_ids for idx in motor_ids
], ],
@@ -249,25 +296,82 @@ class FeetechBus:
return wrap_joints_and_values(motor_names, values) return wrap_joints_and_values(motor_names, values)


def write_torque_enable(self, torque_mode: pa.StructArray): def write_torque_enable(self, torque_mode: pa.StructArray):
"""Enable or disable torque for the specified motors.

Args:
torque_mode: Structured array containing torque enable/disable values

"""
self.write("Torque_Enable", torque_mode) self.write("Torque_Enable", torque_mode)


def write_operating_mode(self, operating_mode: pa.StructArray): def write_operating_mode(self, operating_mode: pa.StructArray):
"""Set the operating mode for the specified motors.

Args:
operating_mode: Structured array containing operating mode values

"""
self.write("Mode", operating_mode) self.write("Mode", operating_mode)


def read_position(self, motor_names: pa.Array) -> pa.StructArray: def read_position(self, motor_names: pa.Array) -> pa.StructArray:
"""Read current position from the specified motors.

Args:
motor_names: Array of motor names to read positions from

Returns:
pa.StructArray: Structured array containing position values

"""
return self.read("Present_Position", motor_names) return self.read("Present_Position", motor_names)


def read_velocity(self, motor_names: pa.Array) -> pa.StructArray: def read_velocity(self, motor_names: pa.Array) -> pa.StructArray:
"""Read current velocity from the specified motors.

Args:
motor_names: Array of motor names to read velocities from

Returns:
pa.StructArray: Structured array containing velocity values

"""
return self.read("Present_Velocity", motor_names) return self.read("Present_Velocity", motor_names)


def read_current(self, motor_names: pa.Array) -> pa.StructArray: def read_current(self, motor_names: pa.Array) -> pa.StructArray:
"""Read current current from the specified motors.

Args:
motor_names: Array of motor names to read currents from

Returns:
pa.StructArray: Structured array containing current values

"""
return self.read("Present_Current", motor_names) return self.read("Present_Current", motor_names)


def write_goal_position(self, goal_position: pa.StructArray): def write_goal_position(self, goal_position: pa.StructArray):
"""Set goal positions for the specified motors.

Args:
goal_position: Structured array containing goal position values

"""
self.write("Goal_Position", goal_position) self.write("Goal_Position", goal_position)


def write_max_angle_limit(self, max_angle_limit: pa.StructArray): def write_max_angle_limit(self, max_angle_limit: pa.StructArray):
"""Set maximum angle limits for the specified motors.

Args:
max_angle_limit: Structured array containing maximum angle limit values

"""
self.write("Max_Angle_Limit", max_angle_limit) self.write("Max_Angle_Limit", max_angle_limit)


def write_min_angle_limit(self, min_angle_limit: pa.StructArray): def write_min_angle_limit(self, min_angle_limit: pa.StructArray):
"""Set minimum angle limits for the specified motors.

Args:
min_angle_limit: Structured array containing minimum angle limit values

"""
self.write("Min_Angle_Limit", min_angle_limit) self.write("Min_Angle_Limit", min_angle_limit)

+ 28
- 21
examples/so100/configure.py View File

@@ -1,5 +1,7 @@
"""
SO100 Auto Configure: This program is used to automatically configure the SO-ARM100 (SO100) for the user.
"""Module for configuring and setting up the SO100 robot hardware.

This module provides functionality for initializing and configuring the SO100 robot's
servo motors and other hardware components.


The program will: The program will:
1. Disable all torque motors of provided SO100. 1. Disable all torque motors of provided SO100.
@@ -14,20 +16,17 @@ It will also enable all appropriate operating modes for the SO100.
""" """


import argparse import argparse
import time
import json import json
import time


import pyarrow as pa import pyarrow as pa

from bus import FeetechBus, TorqueMode, OperatingMode
from pwm_position_control.transform import pwm_to_logical_arrow, wrap_joints_and_values

from bus import FeetechBus, OperatingMode, TorqueMode
from pwm_position_control.functions import construct_control_table
from pwm_position_control.tables import ( from pwm_position_control.tables import (
construct_logical_to_pwm_conversion_table_arrow, construct_logical_to_pwm_conversion_table_arrow,
construct_pwm_to_logical_conversion_table_arrow, construct_pwm_to_logical_conversion_table_arrow,
) )

from pwm_position_control.functions import construct_control_table
from pwm_position_control.transform import pwm_to_logical_arrow, wrap_joints_and_values


FULL_ARM = pa.array( FULL_ARM = pa.array(
[ [
@@ -50,35 +49,43 @@ GRIPPER = pa.array(["gripper"], type=pa.string())




def pause(): def pause():
"""Pause execution and wait for user input."""
input("Press Enter to continue...") input("Press Enter to continue...")




def configure_servos(bus: FeetechBus): def configure_servos(bus: FeetechBus):
"""Configure the servos with default settings.

Args:
bus: The FeetechBus instance to configure.

"""
bus.write_torque_enable( bus.write_torque_enable(
wrap_joints_and_values(FULL_ARM, [TorqueMode.DISABLED.value] * 6)
wrap_joints_and_values(FULL_ARM, [TorqueMode.DISABLED.value] * 6),
) )


bus.write_operating_mode( bus.write_operating_mode(
wrap_joints_and_values(FULL_ARM, [OperatingMode.ONE_TURN.value] * 6)
wrap_joints_and_values(FULL_ARM, [OperatingMode.ONE_TURN.value] * 6),
) )


bus.write_max_angle_limit( bus.write_max_angle_limit(
wrap_joints_and_values(FULL_ARM, [pa.scalar(0, pa.uint32())] * 6)
wrap_joints_and_values(FULL_ARM, [pa.scalar(0, pa.uint32())] * 6),
) )


bus.write_min_angle_limit( bus.write_min_angle_limit(
wrap_joints_and_values(FULL_ARM, [pa.scalar(0, pa.uint32())] * 6)
wrap_joints_and_values(FULL_ARM, [pa.scalar(0, pa.uint32())] * 6),
) )




def main(): def main():
"""Run the servo configuration process."""
parser = argparse.ArgumentParser( parser = argparse.ArgumentParser(
description="SO100 Auto Configure: This program is used to automatically configure the Low Cost Robot (SO100) " description="SO100 Auto Configure: This program is used to automatically configure the Low Cost Robot (SO100) "
"for the user."
"for the user.",
) )


parser.add_argument( parser.add_argument(
"--port", type=str, required=True, help="The port of the SO100."
"--port", type=str, required=True, help="The port of the SO100.",
) )
parser.add_argument( parser.add_argument(
"--right", "--right",
@@ -130,10 +137,10 @@ def main():
pwm_positions = (pwm_position_1, pwm_position_2) pwm_positions = (pwm_position_1, pwm_position_2)


pwm_to_logical_conversion_table = construct_pwm_to_logical_conversion_table_arrow( pwm_to_logical_conversion_table = construct_pwm_to_logical_conversion_table_arrow(
pwm_positions, targets
pwm_positions, targets,
) )
logical_to_pwm_conversion_table = construct_logical_to_pwm_conversion_table_arrow( logical_to_pwm_conversion_table = construct_logical_to_pwm_conversion_table_arrow(
pwm_positions, targets
pwm_positions, targets,
) )


control_table_json = {} control_table_json = {}
@@ -149,7 +156,7 @@ def main():
left = "left" if args.left else "right" left = "left" if args.left else "right"
path = ( path = (
input( input(
f"Please enter the path of the configuration file (default is ./examples/so100/configs/follower.{left}.json): "
f"Please enter the path of the configuration file (default is ./examples/so100/configs/follower.{left}.json): ",
) )
or f"./examples/so100/configs/follower.{left}.json" or f"./examples/so100/configs/follower.{left}.json"
) )
@@ -158,21 +165,21 @@ def main():
json.dump(control_table_json, file) json.dump(control_table_json, file)


control_table = construct_control_table( control_table = construct_control_table(
pwm_to_logical_conversion_table, logical_to_pwm_conversion_table
pwm_to_logical_conversion_table, logical_to_pwm_conversion_table,
) )


while True: while True:
try: try:
pwm_position = arm.read_position(FULL_ARM) pwm_position = arm.read_position(FULL_ARM)
logical_position = pwm_to_logical_arrow( logical_position = pwm_to_logical_arrow(
pwm_position, control_table, ranged=True
pwm_position, control_table, ranged=True,
).field("values") ).field("values")


print(f"Logical Position: {logical_position}") print(f"Logical Position: {logical_position}")


except ConnectionError: except ConnectionError:
print( print(
"Connection error occurred. Please check the connection and try again."
"Connection error occurred. Please check the connection and try again.",
) )


time.sleep(0.5) time.sleep(0.5)


+ 14
- 13
examples/so100/nodes/interpolate_lcr_to_so100.py View File

@@ -1,25 +1,26 @@
import os
"""TODO: Add docstring."""

import argparse import argparse
import json import json
import os


import pyarrow as pa import pyarrow as pa
import pyarrow.compute as pc import pyarrow.compute as pc

from dora import Node from dora import Node
from pwm_position_control.load import load_control_table_from_json_conversion_tables
from pwm_position_control.transform import ( from pwm_position_control.transform import (
wrap_joints_and_values,
pwm_to_logical_arrow,
logical_to_pwm_with_offset_arrow, logical_to_pwm_with_offset_arrow,
pwm_to_logical_arrow,
wrap_joints_and_values,
) )
from pwm_position_control.load import load_control_table_from_json_conversion_tables




def main(): def main():
"""Handle interpolation between LCR and SO100 robot configurations."""
# Handle dynamic nodes, ask for the name of the node in the dataflow # Handle dynamic nodes, ask for the name of the node in the dataflow
parser = argparse.ArgumentParser( parser = argparse.ArgumentParser(
description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the "
"LCR followers knowing a Leader position and Follower position."
"LCR followers knowing a Leader position and Follower position.",
) )


parser.add_argument( parser.add_argument(
@@ -48,19 +49,19 @@ def main():
if not os.environ.get("LEADER_CONTROL") and args.leader_control is None: if not os.environ.get("LEADER_CONTROL") and args.leader_control is None:
raise ValueError( raise ValueError(
"The leader control is not set. Please set the configuration of the leader in the environment variables or " "The leader control is not set. Please set the configuration of the leader in the environment variables or "
"as an argument."
"as an argument.",
) )


if not os.environ.get("FOLLOWER_CONTROL") and args.follower_control is None: if not os.environ.get("FOLLOWER_CONTROL") and args.follower_control is None:
raise ValueError( raise ValueError(
"The follower control is not set. Please set the configuration of the follower in the environment " "The follower control is not set. Please set the configuration of the follower in the environment "
"variables or as an argument."
"variables or as an argument.",
) )


with open( with open(
os.environ.get("LEADER_CONTROL") os.environ.get("LEADER_CONTROL")
if args.leader_control is None if args.leader_control is None
else args.leader_control
else args.leader_control,
) as file: ) as file:
leader_control = json.load(file) leader_control = json.load(file)
load_control_table_from_json_conversion_tables(leader_control, leader_control) load_control_table_from_json_conversion_tables(leader_control, leader_control)
@@ -68,11 +69,11 @@ def main():
with open( with open(
os.environ.get("FOLLOWER_CONTROL") os.environ.get("FOLLOWER_CONTROL")
if args.follower_control is None if args.follower_control is None
else args.follower_control
else args.follower_control,
) as file: ) as file:
follower_control = json.load(file) follower_control = json.load(file)
load_control_table_from_json_conversion_tables( load_control_table_from_json_conversion_tables(
follower_control, follower_control
follower_control, follower_control,
) )


initial_mask = [ initial_mask = [
@@ -132,7 +133,7 @@ def main():
) )


pwm_goal = logical_to_pwm_with_offset_arrow( pwm_goal = logical_to_pwm_with_offset_arrow(
follower_position, logical_goal, follower_control
follower_position, logical_goal, follower_control,
) )


node.send_output("follower_goal", pwm_goal, event["metadata"]) node.send_output("follower_goal", pwm_goal, event["metadata"])


+ 17
- 12
examples/so100/nodes/interpolate_lcr_x_so100_to_record.py View File

@@ -1,23 +1,28 @@
import os
"""Module for recording and interpolating between LCR and SO100 configurations.

This module provides functionality for recording robot configurations and
interpolating between LCR and SO100 robots to create smooth motion sequences.
"""

import argparse import argparse
import json import json
import os


import pyarrow as pa import pyarrow as pa
import pyarrow.compute as pc import pyarrow.compute as pc

from dora import Node from dora import Node

from pwm_position_control.load import load_control_table_from_json_conversion_tables from pwm_position_control.load import load_control_table_from_json_conversion_tables
from pwm_position_control.transform import ( from pwm_position_control.transform import (
wrap_joints_and_values,
pwm_to_logical_arrow, pwm_to_logical_arrow,
wrap_joints_and_values,
) )




def main(): def main():
"""Handle interpolation and recording between LCR and SO100 configurations."""
parser = argparse.ArgumentParser( parser = argparse.ArgumentParser(
description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the "
"LCR followers knowing a Leader position and Follower position."
"LCR followers knowing a Leader position and Follower position.",
) )


parser.add_argument( parser.add_argument(
@@ -45,19 +50,19 @@ def main():
if not os.environ.get("LEADER_CONTROL") and args.leader_control is None: if not os.environ.get("LEADER_CONTROL") and args.leader_control is None:
raise ValueError( raise ValueError(
"The leader control is not set. Please set the configuration of the leader in the environment variables or " "The leader control is not set. Please set the configuration of the leader in the environment variables or "
"as an argument."
"as an argument.",
) )


if not os.environ.get("FOLLOWER_CONTROL") and args.follower_control is None: if not os.environ.get("FOLLOWER_CONTROL") and args.follower_control is None:
raise ValueError( raise ValueError(
"The follower control is not set. Please set the configuration of the follower in the environment " "The follower control is not set. Please set the configuration of the follower in the environment "
"variables or as an argument."
"variables or as an argument.",
) )


with open( with open(
os.environ.get("LEADER_CONTROL") os.environ.get("LEADER_CONTROL")
if args.leader_control is None if args.leader_control is None
else args.leader_control
else args.leader_control,
) as file: ) as file:
leader_control = json.load(file) leader_control = json.load(file)
load_control_table_from_json_conversion_tables(leader_control, leader_control) load_control_table_from_json_conversion_tables(leader_control, leader_control)
@@ -65,11 +70,11 @@ def main():
with open( with open(
os.environ.get("FOLLOWER_CONTROL") os.environ.get("FOLLOWER_CONTROL")
if args.follower_control is None if args.follower_control is None
else args.follower_control
else args.follower_control,
) as file: ) as file:
follower_control = json.load(file) follower_control = json.load(file)
load_control_table_from_json_conversion_tables( load_control_table_from_json_conversion_tables(
follower_control, follower_control
follower_control, follower_control,
) )


node = Node(args.name) node = Node(args.name)
@@ -98,11 +103,11 @@ def main():
follower_position = event["value"] follower_position = event["value"]


follower_position = pwm_to_logical_arrow( follower_position = pwm_to_logical_arrow(
follower_position, follower_control
follower_position, follower_control,
) )


node.send_output( node.send_output(
"logical_position", follower_position, event["metadata"]
"logical_position", follower_position, event["metadata"],
) )


elif event_type == "ERROR": elif event_type == "ERROR":


+ 13
- 9
examples/so100/nodes/interpolate_replay_to_so100.py View File

@@ -1,20 +1,24 @@
import os
"""Module for interpolating between replay and SO100 robot configurations.

This module provides functionality for interpolating between recorded robot actions
and SO100 robot configurations to create smooth motion sequences.
"""

import argparse import argparse
import json import json

import pyarrow as pa
import os


from dora import Node from dora import Node

from pwm_position_control.load import load_control_table_from_json_conversion_tables from pwm_position_control.load import load_control_table_from_json_conversion_tables
from pwm_position_control.transform import logical_to_pwm_with_offset_arrow from pwm_position_control.transform import logical_to_pwm_with_offset_arrow




def main(): def main():
"""Handle replay interpolation for SO100 robot configurations."""
# Handle dynamic nodes, ask for the name of the node in the dataflow # Handle dynamic nodes, ask for the name of the node in the dataflow
parser = argparse.ArgumentParser( parser = argparse.ArgumentParser(
description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the "
"LCR followers knowing a Leader position and Follower position."
"LCR followers knowing a Leader position and Follower position.",
) )


parser.add_argument( parser.add_argument(
@@ -36,17 +40,17 @@ def main():
if not os.environ.get("FOLLOWER_CONTROL") and args.follower_control is None: if not os.environ.get("FOLLOWER_CONTROL") and args.follower_control is None:
raise ValueError( raise ValueError(
"The follower control is not set. Please set the configuration of the follower in the environment " "The follower control is not set. Please set the configuration of the follower in the environment "
"variables or as an argument."
"variables or as an argument.",
) )


with open( with open(
os.environ.get("FOLLOWER_CONTROL") os.environ.get("FOLLOWER_CONTROL")
if args.follower_control is None if args.follower_control is None
else args.follower_control
else args.follower_control,
) as file: ) as file:
follower_control = json.load(file) follower_control = json.load(file)
load_control_table_from_json_conversion_tables( load_control_table_from_json_conversion_tables(
follower_control, follower_control
follower_control, follower_control,
) )


node = Node(args.name) node = Node(args.name)
@@ -68,7 +72,7 @@ def main():
continue continue


physical_goal = logical_to_pwm_with_offset_arrow( physical_goal = logical_to_pwm_with_offset_arrow(
follower_position, leader_position, follower_control
follower_position, leader_position, follower_control,
) )


node.send_output("follower_goal", physical_goal, event["metadata"]) node.send_output("follower_goal", physical_goal, event["metadata"])


+ 2
- 1
node-hub/dora-argotranslate/pyproject.toml View File

@@ -22,5 +22,6 @@ dora-argotranslate = "dora_argotranslate.main:main"
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP",
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

+ 1
- 1
node-hub/dora-distil-whisper/dora_distil_whisper/main.py View File

@@ -239,5 +239,5 @@ def main():
if text.strip() == "" or text.strip() == ".": if text.strip() == "" or text.strip() == ".":
continue continue
node.send_output( node.send_output(
"text", pa.array([text]), {"language": TARGET_LANGUAGE}
"text", pa.array([text]), {"language": TARGET_LANGUAGE},
) )

+ 3
- 2
node-hub/dora-distil-whisper/pyproject.toml View File

@@ -29,6 +29,7 @@ dora-distil-whisper = "dora_distil_whisper.main:main"


[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle
"UP",
"D", # pydocstyle
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

+ 2
- 1
node-hub/dora-echo/pyproject.toml View File

@@ -21,5 +21,6 @@ dora-echo = "dora_echo.main:main"
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP"
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

+ 2
- 1
node-hub/dora-internvl/pyproject.toml View File

@@ -34,5 +34,6 @@ dora-internvl = "dora_internvl.main:main"
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP",
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

+ 2
- 1
node-hub/dora-ios-lidar/pyproject.toml View File

@@ -18,5 +18,6 @@ dora-ios-lidar = "dora_ios_lidar.main:main"
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP",
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

+ 2
- 1
node-hub/dora-keyboard/pyproject.toml View File

@@ -27,5 +27,6 @@ dora-keyboard = "dora_keyboard.main:main"
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP",
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

+ 2
- 1
node-hub/dora-kit-car/pyproject.toml View File

@@ -19,5 +19,6 @@ features = ["python", "pyo3/extension-module"]
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP",
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

+ 2
- 1
node-hub/dora-kokoro-tts/pyproject.toml View File

@@ -18,5 +18,6 @@ dora-kokoro-tts = "dora_kokoro_tts.main:main"
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP",
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

+ 0
- 1
node-hub/dora-llama-cpp-python/dora_llama_cpp_python/__main__.py View File

@@ -2,6 +2,5 @@


from .main import main from .main import main



if __name__ == "__main__": if __name__ == "__main__":
main() main()

+ 11
- 10
node-hub/dora-llama-cpp-python/dora_llama_cpp_python/main.py View File

@@ -1,10 +1,11 @@
"""TODO: Add docstring.""" """TODO: Add docstring."""


import logging
import os import os
from pathlib import Path

import pyarrow as pa import pyarrow as pa
from dora import Node from dora import Node
from pathlib import Path
import logging


# Configure logging # Configure logging
logging.basicConfig(level=logging.INFO) logging.basicConfig(level=logging.INFO)
@@ -24,7 +25,7 @@ CONTEXT_SIZE = int(os.getenv("CONTEXT_SIZE", "4096"))
def get_model(): def get_model():
"""Load a GGUF model using llama-cpp-python with optional GPU acceleration.""" """Load a GGUF model using llama-cpp-python with optional GPU acceleration."""
from llama_cpp import Llama from llama_cpp import Llama
try: try:
# Check if path exists locally # Check if path exists locally
model_path = Path(MODEL_NAME_OR_PATH) model_path = Path(MODEL_NAME_OR_PATH)
@@ -35,7 +36,7 @@ def get_model():
n_gpu_layers=N_GPU_LAYERS, n_gpu_layers=N_GPU_LAYERS,
n_ctx=CONTEXT_SIZE, n_ctx=CONTEXT_SIZE,
n_threads=N_THREADS, n_threads=N_THREADS,
verbose=False
verbose=False,
) )
else: else:
# Load from HuggingFace # Load from HuggingFace
@@ -46,14 +47,14 @@ def get_model():
n_gpu_layers=N_GPU_LAYERS, n_gpu_layers=N_GPU_LAYERS,
n_ctx=CONTEXT_SIZE, n_ctx=CONTEXT_SIZE,
n_threads=N_THREADS, n_threads=N_THREADS,
verbose=False
verbose=False,
) )
logging.info("Model loaded successfully") logging.info("Model loaded successfully")
return llm return llm
except Exception as e: except Exception as e:
logging.error(f"Error loading model: {e}")
logging.exception(f"Error loading model: {e}")
raise raise




@@ -79,9 +80,9 @@ def main():
max_tokens=MAX_TOKENS, max_tokens=MAX_TOKENS,
stop=["Q:", "\n"], stop=["Q:", "\n"],
)["choices"][0]["text"] )["choices"][0]["text"]
node.send_output( node.send_output(
output_id="text", data=pa.array([response]), metadata={}
output_id="text", data=pa.array([response]), metadata={},
) )






+ 2
- 1
node-hub/dora-llama-cpp-python/pyproject.toml View File

@@ -43,5 +43,6 @@ dora-llama-cpp-python = "dora_llama_cpp_python.main:main"
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP"
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

+ 1
- 1
node-hub/dora-magma/dora_magma/__init__.py View File

@@ -16,4 +16,4 @@ except FileNotFoundError:




submodule_path = Path(__file__).resolve().parent / "Magma" submodule_path = Path(__file__).resolve().parent / "Magma"
sys.path.insert(0, str(submodule_path))
sys.path.insert(0, str(submodule_path))

+ 6
- 6
node-hub/dora-magma/dora_magma/main.py View File

@@ -27,7 +27,7 @@ def load_magma_models():
DEFAULT_PATH = str(magma_dir.parent) DEFAULT_PATH = str(magma_dir.parent)
if not os.path.exists(DEFAULT_PATH): if not os.path.exists(DEFAULT_PATH):
logger.warning( logger.warning(
"Warning: Magma submodule not found, falling back to HuggingFace version"
"Warning: Magma submodule not found, falling back to HuggingFace version",
) )
DEFAULT_PATH = "microsoft/Magma-8B" DEFAULT_PATH = "microsoft/Magma-8B"


@@ -42,7 +42,7 @@ def load_magma_models():
device_map="auto", device_map="auto",
) )
processor = AutoProcessor.from_pretrained( processor = AutoProcessor.from_pretrained(
MODEL_NAME_OR_PATH, trust_remote_code=True
MODEL_NAME_OR_PATH, trust_remote_code=True,
) )
except Exception as e: except Exception as e:
logger.error(f"Failed to load model: {e}") logger.error(f"Failed to load model: {e}")
@@ -72,7 +72,7 @@ def generate(
] ]


prompt = processor.tokenizer.apply_chat_template( prompt = processor.tokenizer.apply_chat_template(
convs, tokenize=False, add_generation_prompt=True
convs, tokenize=False, add_generation_prompt=True,
) )


try: try:
@@ -102,7 +102,7 @@ def generate(


# Parse the trajectories using the same approach as in `https://github.com/microsoft/Magma/blob/main/agents/robot_traj/app.py` # Parse the trajectories using the same approach as in `https://github.com/microsoft/Magma/blob/main/agents/robot_traj/app.py`
traces_dict = ast.literal_eval( traces_dict = ast.literal_eval(
"{" + traces_str.strip().replace("\n\n", ",") + "}"
"{" + traces_str.strip().replace("\n\n", ",") + "}",
) )
for mark_id, trace in traces_dict.items(): for mark_id, trace in traces_dict.items():
trajectories[mark_id] = ast.literal_eval(trace) trajectories[mark_id] = ast.literal_eval(trace)
@@ -153,7 +153,7 @@ def main():
frame = cv2.imdecode(storage, cv2.IMREAD_COLOR) frame = cv2.imdecode(storage, cv2.IMREAD_COLOR)
if frame is None: if frame is None:
raise ValueError( raise ValueError(
f"Failed to decode image with encoding {encoding}"
f"Failed to decode image with encoding {encoding}",
) )
frame = frame[:, :, ::-1] # Convert BGR to RGB frame = frame[:, :, ::-1] # Convert BGR to RGB
else: else:
@@ -181,7 +181,7 @@ def main():
image = frames[image_id] image = frames[image_id]
response, trajectories = generate(image, task_description) response, trajectories = generate(image, task_description)
node.send_output( node.send_output(
"text", pa.array([response]), {"image_id": image_id}
"text", pa.array([response]), {"image_id": image_id},
) )


# Send trajectory data if available # Send trajectory data if available


+ 2
- 1
node-hub/dora-magma/pyproject.toml View File

@@ -43,5 +43,6 @@ extend.exclude = "dora_magma/Magma"
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP",
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

+ 1
- 1
node-hub/dora-magma/tests/test_magma_node.py View File

@@ -3,4 +3,4 @@


def test_import_main(): def test_import_main():
"""TODO: Add docstring.""" """TODO: Add docstring."""
pass # Model is too big for the CI/CD
# Model is too big for the CI/CD

+ 2
- 1
node-hub/dora-microphone/pyproject.toml View File

@@ -27,5 +27,6 @@ dora-microphone = "dora_microphone.main:main"
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP",
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

+ 2
- 1
node-hub/dora-object-to-pose/pyproject.toml View File

@@ -18,5 +18,6 @@ features = ["python", "pyo3/extension-module"]
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP",
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

+ 2
- 2
node-hub/dora-openai-server/pyproject.toml View File

@@ -30,6 +30,6 @@ dora-openai-server = "dora_openai_server.main:main"
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP"
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]


+ 0
- 1
node-hub/dora-openai-server/tests/test_dora_openai_server.py View File

@@ -2,4 +2,3 @@


def test_import_main(): def test_import_main():
"""TODO: Add docstring.""" """TODO: Add docstring."""
pass

+ 2
- 1
node-hub/dora-opus/pyproject.toml View File

@@ -31,5 +31,6 @@ dora-opus = "dora_opus.main:main"
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP",
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

+ 1
- 0
node-hub/dora-outtetts/dora_outtetts/tests/test_main.py View File

@@ -3,6 +3,7 @@
import os import os


import pytest import pytest

from dora_outtetts.main import load_interface, main from dora_outtetts.main import load_interface, main


CI = os.getenv("CI", "false") in ["True", "true"] CI = os.getenv("CI", "false") in ["True", "true"]


+ 2
- 1
node-hub/dora-outtetts/pyproject.toml View File

@@ -31,5 +31,6 @@ dora-outtetts = "dora_outtetts.main:main"
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP",
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

+ 2
- 1
node-hub/dora-parler/pyproject.toml View File

@@ -33,5 +33,6 @@ dora-parler = "dora_parler.main:main"
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP",
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

+ 3
- 3
node-hub/dora-phi4/dora_phi4/main.py View File

@@ -27,7 +27,7 @@ else:
MODEL_PATH = "microsoft/Phi-4-multimodal-instruct" MODEL_PATH = "microsoft/Phi-4-multimodal-instruct"


processor = AutoProcessor.from_pretrained( processor = AutoProcessor.from_pretrained(
MODEL_PATH, trust_remote_code=True, use_fast=True
MODEL_PATH, trust_remote_code=True, use_fast=True,
) )


# Define model config # Define model config
@@ -42,12 +42,12 @@ MODEL_CONFIG = {


# Infer device map without full initialization # Infer device map without full initialization
device_map = infer_auto_device_map( device_map = infer_auto_device_map(
AutoModelForCausalLM.from_pretrained(MODEL_PATH, **MODEL_CONFIG)
AutoModelForCausalLM.from_pretrained(MODEL_PATH, **MODEL_CONFIG),
) )


# Load the model directly with the inferred device map # Load the model directly with the inferred device map
model = AutoModelForCausalLM.from_pretrained( model = AutoModelForCausalLM.from_pretrained(
MODEL_PATH, **MODEL_CONFIG, device_map=device_map
MODEL_PATH, **MODEL_CONFIG, device_map=device_map,
) )


generation_config = GenerationConfig.from_pretrained(MODEL_PATH) generation_config = GenerationConfig.from_pretrained(MODEL_PATH)


+ 2
- 1
node-hub/dora-phi4/pyproject.toml View File

@@ -34,5 +34,6 @@ dora-phi4 = "dora_phi4.main:main"
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP"
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

+ 2
- 1
node-hub/dora-piper/pyproject.toml View File

@@ -18,5 +18,6 @@ dora-piper = "dora_piper.main:main"
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP",
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

+ 2
- 1
node-hub/dora-pyaudio/pyproject.toml View File

@@ -23,5 +23,6 @@ dora-pyaudio = "dora_pyaudio.main:main"
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP",
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

+ 2
- 1
node-hub/dora-pyorbbecksdk/pyproject.toml View File

@@ -21,5 +21,6 @@ dora-pyorbbecksdk = "dora_pyorbbecksdk.main:main"
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP",
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

+ 2
- 1
node-hub/dora-pyrealsense/pyproject.toml View File

@@ -24,5 +24,6 @@ dora-pyrealsense = "dora_pyrealsense.main:main"
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP",
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

+ 2
- 1
node-hub/dora-qwen/pyproject.toml View File

@@ -45,5 +45,6 @@ dora-qwen = "dora_qwen.main:main"
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP",
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

+ 2
- 1
node-hub/dora-qwen2-5-vl/pyproject.toml View File

@@ -51,5 +51,6 @@ build-backend = "setuptools.build_meta"
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP",
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

+ 2
- 1
node-hub/dora-qwenvl/pyproject.toml View File

@@ -38,5 +38,6 @@ dora-qwenvl = "dora_qwenvl.main:main"
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP",
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

+ 2
- 1
node-hub/dora-rdt-1b/pyproject.toml View File

@@ -48,5 +48,6 @@ build-backend = "poetry.core.masonry.api"
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP",
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

+ 4
- 4
node-hub/dora-reachy1/dora_reachy1/main.py View File

@@ -16,7 +16,7 @@ def r_arm_inverse_kinematics(reachy, pose, action) -> list:
[0, 1, 0, pose[1] + action[1]], [0, 1, 0, pose[1] + action[1]],
[1, 0, 0, pose[2] + action[2]], [1, 0, 0, pose[2] + action[2]],
[0, 0, 0, 1], [0, 0, 0, 1],
]
],
) )
return reachy.r_arm.inverse_kinematics(A) return reachy.r_arm.inverse_kinematics(A)


@@ -70,7 +70,7 @@ def main():
default_pose = r_arm_inverse_kinematics(reachy, r_arm_pose, [0, 0, 0]) default_pose = r_arm_inverse_kinematics(reachy, r_arm_pose, [0, 0, 0])


goto( goto(
{joint: pos for joint, pos in zip(reachy.r_arm.joints.values(), default_pose)},
{joint: pos for joint, pos in zip(reachy.r_arm.joints.values(), default_pose, strict=False)},
duration=3, duration=3,
) )


@@ -84,7 +84,7 @@ def main():
goto( goto(
{ {
joint: pos joint: pos
for joint, pos in zip(reachy.r_arm.joints.values(), joint_pose)
for joint, pos in zip(reachy.r_arm.joints.values(), joint_pose, strict=False)
}, },
duration=0.200, duration=0.200,
) )
@@ -119,7 +119,7 @@ def main():
{ {
joint: pos joint: pos
for joint, pos in zip( for joint, pos in zip(
reachy.r_arm.joints.values(), default_pose
reachy.r_arm.joints.values(), default_pose, strict=False,
) )
}, },
duration=3, duration=3,


+ 3
- 2
node-hub/dora-reachy1/dora_reachy1_vision/main.py View File

@@ -1,9 +1,10 @@
"""TODO: Add docstring.""" """TODO: Add docstring."""


from reachy_sdk import ReachySDK
import os import os
from dora import Node
import pyarrow as pa import pyarrow as pa
from dora import Node
from reachy_sdk import ReachySDK




def main(): def main():


+ 2
- 1
node-hub/dora-reachy1/pyproject.toml View File

@@ -30,5 +30,6 @@ build-backend = "poetry.core.masonry.api"
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP"
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

+ 0
- 1
node-hub/dora-reachy1/tests/test_dora_reachy1.py View File

@@ -5,7 +5,6 @@


def test_pass(): def test_pass():
"""TODO: Add docstring.""" """TODO: Add docstring."""
pass




# def test_import_main(): # def test_import_main():


+ 2
- 1
node-hub/dora-reachy2/pyproject.toml View File

@@ -27,5 +27,6 @@ dora-reachy2-head = "dora_reachy2.head:main"
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP",
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

+ 0
- 1
node-hub/dora-reachy2/tests/test_dora_reachy2.py View File

@@ -5,7 +5,6 @@


def test_pass(): def test_pass():
"""TODO: Add docstring.""" """TODO: Add docstring."""
pass




# def test_import_camera_main(): # def test_import_camera_main():


+ 2
- 1
node-hub/dora-rerun/pyproject.toml View File

@@ -22,5 +22,6 @@ features = ["python", "pyo3/extension-module"]
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP",
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

+ 2
- 1
node-hub/dora-sam2/pyproject.toml View File

@@ -23,5 +23,6 @@ dora-sam2 = "dora_sam2.main:main"
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP",
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

+ 3
- 1
node-hub/dora-transformers/dora_transformers/__init__.py View File

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

import os import os


# Define the path to the README file relative to the package directory # Define the path to the README file relative to the package directory
@@ -5,7 +7,7 @@ readme_path = os.path.join(os.path.dirname(os.path.dirname(__file__)), "README.m


# Read the content of the README file # Read the content of the README file
try: try:
with open(readme_path, "r", encoding="utf-8") as f:
with open(readme_path, encoding="utf-8") as f:
__doc__ = f.read() __doc__ = f.read()
except FileNotFoundError: except FileNotFoundError:
__doc__ = "README file not found." __doc__ = "README file not found."

+ 2
- 1
node-hub/dora-transformers/dora_transformers/__main__.py View File

@@ -1,5 +1,6 @@
from .main import main
"""TODO: Add docstring."""


from .main import main


if __name__ == "__main__": if __name__ == "__main__":
main() main()

+ 25
- 22
node-hub/dora-transformers/dora_transformers/main.py View File

@@ -1,9 +1,12 @@
"""TODO: Add docstring."""

import logging
import os import os

import pyarrow as pa import pyarrow as pa
import torch
from dora import Node from dora import Node
from transformers import AutoModelForCausalLM, AutoTokenizer from transformers import AutoModelForCausalLM, AutoTokenizer
import logging
import torch


# Configure logging # Configure logging
logging.basicConfig(level=logging.INFO) logging.basicConfig(level=logging.INFO)
@@ -31,25 +34,24 @@ ACTIVATION_WORDS = os.getenv("ACTIVATION_WORDS", "what how who where you").split


def load_model(): def load_model():
"""Load the transformer model and tokenizer.""" """Load the transformer model and tokenizer."""

logging.info(f"Loading model {MODEL_NAME} on {DEVICE}") logging.info(f"Loading model {MODEL_NAME} on {DEVICE}")
# Memory efficient loading # Memory efficient loading
model_kwargs = { model_kwargs = {
"torch_dtype": TORCH_DTYPE, "torch_dtype": TORCH_DTYPE,
"device_map": DEVICE, "device_map": DEVICE,
} }
if ENABLE_MEMORY_EFFICIENT and DEVICE == "cuda": if ENABLE_MEMORY_EFFICIENT and DEVICE == "cuda":
model_kwargs.update({ model_kwargs.update({
"low_cpu_mem_usage": True, "low_cpu_mem_usage": True,
"offload_folder": "offload", "offload_folder": "offload",
"load_in_8bit": True
"load_in_8bit": True,
}) })
model = AutoModelForCausalLM.from_pretrained( model = AutoModelForCausalLM.from_pretrained(
MODEL_NAME, MODEL_NAME,
**model_kwargs
**model_kwargs,
) )
tokenizer = AutoTokenizer.from_pretrained(MODEL_NAME) tokenizer = AutoTokenizer.from_pretrained(MODEL_NAME)
logging.info("Model loaded successfully") logging.info("Model loaded successfully")
@@ -60,40 +62,41 @@ def load_model():
def generate_response(model, tokenizer, text: str, history) -> tuple[str, list]: def generate_response(model, tokenizer, text: str, history) -> tuple[str, list]:
"""Generate text using the transformer model.""" """Generate text using the transformer model."""
history += [{"role": "user", "content": text}] history += [{"role": "user", "content": text}]
prompt = tokenizer.apply_chat_template( prompt = tokenizer.apply_chat_template(
history, tokenize=False, add_generation_prompt=True
history, tokenize=False, add_generation_prompt=True,
) )
model_inputs = tokenizer([prompt], return_tensors="pt").to(DEVICE) model_inputs = tokenizer([prompt], return_tensors="pt").to(DEVICE)
with torch.inference_mode(): with torch.inference_mode():
generated_ids = model.generate( generated_ids = model.generate(
**model_inputs,
**model_inputs,
max_new_tokens=MAX_TOKENS, max_new_tokens=MAX_TOKENS,
pad_token_id=tokenizer.pad_token_id, pad_token_id=tokenizer.pad_token_id,
do_sample=True, do_sample=True,
temperature=0.7, temperature=0.7,
top_p=0.9,
top_p=0.9,
repetition_penalty=1.2, repetition_penalty=1.2,
length_penalty=0.5, length_penalty=0.5,
) )
generated_ids = [ generated_ids = [
output_ids[len(input_ids):] output_ids[len(input_ids):]
for input_ids, output_ids in zip(model_inputs.input_ids, generated_ids) for input_ids, output_ids in zip(model_inputs.input_ids, generated_ids)
] ]
response = tokenizer.batch_decode(generated_ids, skip_special_tokens=True)[0] response = tokenizer.batch_decode(generated_ids, skip_special_tokens=True)[0]
history += [{"role": "assistant", "content": response}] history += [{"role": "assistant", "content": response}]
# Clear CUDA cache after successful generation if enabled # Clear CUDA cache after successful generation if enabled
if ENABLE_MEMORY_EFFICIENT and DEVICE == "cuda": if ENABLE_MEMORY_EFFICIENT and DEVICE == "cuda":
torch.cuda.empty_cache() torch.cuda.empty_cache()
return response, history return response, history


def main(): def main():
"""TODO: Add docstring."""
# Initialize model and conversation history # Initialize model and conversation history
model, tokenizer = load_model() model, tokenizer = load_model()
# Initialize history with system prompt # Initialize history with system prompt
@@ -109,11 +112,11 @@ def main():
logging.info(f"Processing input: {text}") logging.info(f"Processing input: {text}")
response, history = generate_response(model, tokenizer, text, history) response, history = generate_response(model, tokenizer, text, history)
logging.info(f"Generated response: {response}") logging.info(f"Generated response: {response}")
node.send_output( node.send_output(
output_id="text",
data=pa.array([response]),
metadata={}
output_id="text",
data=pa.array([response]),
metadata={},
) )


if __name__ == "__main__": if __name__ == "__main__":


+ 8
- 0
node-hub/dora-transformers/pyproject.toml View File

@@ -24,3 +24,11 @@ dev = ["pytest >=8.1.1", "ruff >=0.9.1"]


[project.scripts] [project.scripts]
dora-transformer = "dora_transformer.main:main" dora-transformer = "dora_transformer.main:main"


[tool.ruff.lint]
extend-select = [
"D", # pydocstyle
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
]

+ 3
- 0
node-hub/dora-transformers/tests/test_dora_transformers.py View File

@@ -1,7 +1,10 @@
"""TODO: Add docstring."""

import pytest import pytest




def test_import_main(): def test_import_main():
"""TODO: Add docstring."""
from dora_transformers.main import main from dora_transformers.main import main


# Check that everything is working, and catch dora Runtime Exception as we're not running in a dora dataflow. # Check that everything is working, and catch dora Runtime Exception as we're not running in a dora dataflow.


+ 2
- 1
node-hub/dora-ugv/pyproject.toml View File

@@ -18,5 +18,6 @@ dora-ugv = "dora_ugv.main:main"
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP",
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

+ 2
- 1
node-hub/dora-vad/pyproject.toml View File

@@ -18,5 +18,6 @@ dora-vad = "dora_vad.main:main"
[tool.ruff.lint] [tool.ruff.lint]
extend-select = [ extend-select = [
"D", # pydocstyle "D", # pydocstyle
"UP",
"UP", # Ruff's UP rule
"PERF" # Ruff's PERF rule
] ]

Some files were not shown because too many files changed in this diff

Loading…
Cancel
Save