Browse Source

Fixed node-hub linting issues

tags/v0.3.11-rc1
7SOMAY 10 months ago
parent
commit
d12b232199
87 changed files with 584 additions and 570 deletions
  1. +1
    -2
      apis/python/node/generate_stubs.py
  2. +1
    -0
      binaries/cli/src/template/python/listener/listener-template.py
  3. +1
    -0
      binaries/cli/src/template/python/talker/talker-template.py
  4. +127
    -34
      examples/alexk-lcr/bus.py
  5. +18
    -23
      examples/alexk-lcr/configure.py
  6. +11
    -13
      examples/alexk-lcr/nodes/interpolate_lcr_to_lcr.py
  7. +10
    -12
      examples/alexk-lcr/nodes/interpolate_lcr_to_record.py
  8. +7
    -9
      examples/alexk-lcr/nodes/interpolate_lcr_to_simu_lcr.py
  9. +6
    -7
      examples/alexk-lcr/nodes/interpolate_replay_to_lcr.py
  10. +40
    -43
      examples/aloha/benchmark/python/dynamixel.py
  11. +18
    -25
      examples/aloha/benchmark/python/robot.py
  12. +3
    -3
      examples/aloha/benchmark/python/teleoperate_real_robot.py
  13. +11
    -12
      examples/aloha/benchmark/ros2/teleop.py
  14. +6
    -6
      examples/aloha/nodes/gym_dora_node.py
  15. +4
    -5
      examples/aloha/nodes/keyboard_node.py
  16. +6
    -8
      examples/aloha/nodes/lerobot_webcam_saver.py
  17. +28
    -29
      examples/aloha/nodes/llm_op.py
  18. +1
    -1
      examples/aloha/nodes/plot_node.py
  19. +0
    -1
      examples/aloha/nodes/policy.py
  20. +5
    -4
      examples/aloha/nodes/realsense_node.py
  21. +3
    -3
      examples/aloha/nodes/replay.py
  22. +4
    -6
      examples/aloha/nodes/webcam.py
  23. +5
    -9
      examples/aloha/nodes/whisper_node.py
  24. +4
    -3
      examples/aloha/tests/test_realsense.py
  25. +4
    -5
      examples/lebai/nodes/interpolation.py
  26. +1
    -1
      examples/lebai/nodes/key_interpolation.py
  27. +3
    -3
      examples/lebai/nodes/prompt_interpolation.py
  28. +1
    -1
      examples/lebai/nodes/voice_interpolation.py
  29. +2
    -4
      examples/reachy/nodes/gym_dora_node.py
  30. +4
    -5
      examples/reachy/nodes/keyboard_node.py
  31. +6
    -8
      examples/reachy/nodes/lerobot_webcam_saver.py
  32. +1
    -1
      examples/reachy/nodes/plot_node.py
  33. +12
    -15
      examples/reachy/nodes/reachy_client.py
  34. +0
    -6
      examples/reachy/nodes/reachy_vision_client.py
  35. +3
    -2
      examples/reachy/nodes/replay_node.py
  36. +2
    -4
      examples/reachy1/nodes/gym_dora_node.py
  37. +1
    -1
      examples/reachy1/nodes/key_interpolation.py
  38. +4
    -5
      examples/reachy1/nodes/keyboard_node.py
  39. +6
    -8
      examples/reachy1/nodes/lerobot_webcam_saver.py
  40. +1
    -1
      examples/reachy1/nodes/plot_node.py
  41. +12
    -15
      examples/reachy1/nodes/reachy_client.py
  42. +0
    -6
      examples/reachy1/nodes/reachy_vision_client.py
  43. +3
    -2
      examples/reachy1/nodes/replay_node.py
  44. +9
    -10
      examples/reachy1/nodes/text_interpolation.py
  45. +16
    -16
      examples/so100/bus.py
  46. +24
    -21
      examples/so100/configure.py
  47. +12
    -13
      examples/so100/nodes/interpolate_lcr_to_so100.py
  48. +11
    -12
      examples/so100/nodes/interpolate_lcr_x_so100_to_record.py
  49. +7
    -9
      examples/so100/nodes/interpolate_replay_to_so100.py
  50. +1
    -1
      node-hub/dora-distil-whisper/dora_distil_whisper/main.py
  51. +0
    -1
      node-hub/dora-llama-cpp-python/dora_llama_cpp_python/__main__.py
  52. +11
    -10
      node-hub/dora-llama-cpp-python/dora_llama_cpp_python/main.py
  53. +1
    -1
      node-hub/dora-magma/dora_magma/__init__.py
  54. +6
    -6
      node-hub/dora-magma/dora_magma/main.py
  55. +1
    -1
      node-hub/dora-magma/tests/test_magma_node.py
  56. +0
    -1
      node-hub/dora-openai-server/tests/test_dora_openai_server.py
  57. +1
    -0
      node-hub/dora-outtetts/dora_outtetts/tests/test_main.py
  58. +3
    -3
      node-hub/dora-phi4/dora_phi4/main.py
  59. +4
    -4
      node-hub/dora-reachy1/dora_reachy1/main.py
  60. +3
    -2
      node-hub/dora-reachy1/dora_reachy1_vision/main.py
  61. +0
    -1
      node-hub/dora-reachy1/tests/test_dora_reachy1.py
  62. +0
    -1
      node-hub/dora-reachy2/tests/test_dora_reachy2.py
  63. +1
    -1
      node-hub/dora-transformers/dora_transformers/__init__.py
  64. +0
    -1
      node-hub/dora-transformers/dora_transformers/__main__.py
  65. +22
    -21
      node-hub/dora-transformers/dora_transformers/main.py
  66. +12
    -11
      node-hub/dynamixel-client/dynamixel_client/bus.py
  67. +7
    -8
      node-hub/dynamixel-client/dynamixel_client/main.py
  68. +0
    -1
      node-hub/dynamixel-client/tests/test_dynamixel_client.py
  69. +1
    -1
      node-hub/feetech-client/feetech_client/__init__.py
  70. +4
    -4
      node-hub/feetech-client/feetech_client/bus.py
  71. +5
    -6
      node-hub/feetech-client/feetech_client/main.py
  72. +0
    -1
      node-hub/feetech-client/tests/test_feetech_client.py
  73. +6
    -5
      node-hub/lebai-client/lebai_client/main.py
  74. +1
    -2
      node-hub/lebai-client/test/test.py
  75. +0
    -1
      node-hub/lebai-client/tests/test_lebai_client.py
  76. +1
    -1
      node-hub/lerobot-dashboard/lerobot_dashboard/__init__.py
  77. +7
    -9
      node-hub/lerobot-dashboard/lerobot_dashboard/main.py
  78. +0
    -1
      node-hub/lerobot-dashboard/tests/test_lerobot_dashboard.py
  79. +1
    -1
      node-hub/mujoco-client/mujoco_client/__init__.py
  80. +9
    -14
      node-hub/mujoco-client/mujoco_client/main.py
  81. +0
    -1
      node-hub/mujoco-client/tests/test_mujoco_client.py
  82. +1
    -1
      node-hub/replay-client/replay_client/__init__.py
  83. +3
    -4
      node-hub/replay-client/replay_client/main.py
  84. +0
    -1
      node-hub/replay-client/tests/test_replay_client.py
  85. +0
    -1
      node-hub/video-encoder/tests/test_video_encoder.py
  86. +1
    -1
      node-hub/video-encoder/video_encoder/__init__.py
  87. +6
    -8
      node-hub/video-encoder/video_encoder/main.py

+ 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()


+ 127
- 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,37 @@ TIMEOUT_MS = 1000


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


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


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


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


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


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


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


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

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


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


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




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

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




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

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




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


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

Args:
port: The serial port to connect to the Dynamixel bus.
description: A dictionary containing the description of the motors connected to the bus.
The keys are the motor names and the values are tuples containing the motor id
and the motor model.
"""
self.port = port self.port = port
self.descriptions = description self.descriptions = description
self.motor_ctrl = {} self.motor_ctrl = {}
@@ -184,9 +202,16 @@ class DynamixelBus:
self.group_writers = {} self.group_writers = {}


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


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

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


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


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

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

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


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


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

Args:
torque_mode: Structured array containing the torque mode for each servo.
"""
self.write("Torque_Enable", torque_mode) self.write("Torque_Enable", torque_mode)


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

Args:
operating_mode: Structured array containing the operating mode for each servo.
"""
self.write("Operating_Mode", operating_mode) self.write("Operating_Mode", operating_mode)


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

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

Returns:
Structured array containing the current positions.
"""
return self.read("Present_Position", motor_names) return self.read("Present_Position", motor_names)


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

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

Returns:
Structured array containing the current velocities.
"""
return self.read("Present_Velocity", motor_names) return self.read("Present_Velocity", motor_names)


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

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

Returns:
Structured array containing the current currents.
"""
return self.read("Present_Current", motor_names) return self.read("Present_Current", motor_names)


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

Args:
goal_position: Structured array containing the goal positions.
"""
self.write("Goal_Position", goal_position) self.write("Goal_Position", goal_position)


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

Args:
goal_current: Structured array containing the goal currents.
"""
self.write("Goal_Current", goal_current) self.write("Goal_Current", goal_current)


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

Args:
position_p_gain: Structured array containing the position P gains.
"""
self.write("Position_P_Gain", position_p_gain) self.write("Position_P_Gain", position_p_gain)


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

Args:
position_i_gain: Structured array containing the position I gains.
"""
self.write("Position_I_Gain", position_i_gain) self.write("Position_I_Gain", position_i_gain)


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

Args:
position_d_gain: Structured array containing the position D gains.
"""
self.write("Position_D_Gain", position_d_gain) self.write("Position_D_Gain", position_d_gain)

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

@@ -1,5 +1,4 @@
"""
LCR Configuration Tool: This program is used to automatically configure the Low Cost Robot (LCR) for the user.
"""LCR Configuration Tool: This program is used to automatically configure the Low Cost Robot (LCR) for the user.


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


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


import pyarrow as pa import pyarrow as pa

from bus import DynamixelBus, TorqueMode, OperatingMode

from pwm_position_control.transform import pwm_to_logical_arrow, wrap_joints_and_values

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

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


FULL_ARM = pa.array( FULL_ARM = pa.array(
[ [
@@ -56,26 +51,26 @@ def pause():


def configure_servos(bus: DynamixelBus): def configure_servos(bus: DynamixelBus):
bus.write_torque_enable( bus.write_torque_enable(
wrap_joints_and_values(FULL_ARM, [TorqueMode.DISABLED.value] * 6)
wrap_joints_and_values(FULL_ARM, [TorqueMode.DISABLED.value] * 6),
) )


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


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




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


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


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


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


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


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


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


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


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


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


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


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

@@ -1,24 +1,22 @@
import os
import argparse import argparse
import json import json
import os


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

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




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


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


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


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


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


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


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


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

@@ -1,23 +1,21 @@
import os
import argparse import argparse
import json import json
import os


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

from dora import Node from dora import Node

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




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


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


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


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


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


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


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


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


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

@@ -1,25 +1,23 @@
import os
import argparse import argparse
import json import json
import os


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

from dora import Node from dora import Node

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




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


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


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


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

@@ -1,9 +1,8 @@
import os
import argparse import argparse
import json import json
import os


from dora import Node from dora import Node

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


@@ -11,7 +10,7 @@ from pwm_position_control.transform import logical_to_pwm_with_offset_arrow
def main(): def main():
parser = argparse.ArgumentParser( parser = argparse.ArgumentParser(
description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the " description="Interpolation LCR Node: This Dora node is used to calculates appropriate goal positions for the "
"LCR followers knowing a Leader position and Follower position."
"LCR followers knowing a Leader position and Follower position.",
) )


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


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


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


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


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


+ 40
- 43
examples/aloha/benchmark/python/dynamixel.py View File

@@ -1,10 +1,12 @@
from __future__ import annotations from __future__ import annotations

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

from dynamixel_sdk import * # Uses Dynamixel SDK library




class ReadAttribute(enum.Enum): class ReadAttribute(enum.Enum):
@@ -87,7 +89,7 @@ class Dynamixel:


# self._enable_torque(motor_id) # self._enable_torque(motor_id)
dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx( dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx(
self.portHandler, motor_id, self.ADDR_GOAL_POSITION, goal_position
self.portHandler, motor_id, self.ADDR_GOAL_POSITION, goal_position,
) )
# self._process_response(dxl_comm_result, dxl_error) # self._process_response(dxl_comm_result, dxl_error)
# print(f'set position of motor {motor_id} to {goal_position}') # print(f'set position of motor {motor_id} to {goal_position}')
@@ -101,24 +103,23 @@ class Dynamixel:
self._enable_torque(motor_id) self._enable_torque(motor_id)
# print(f'enabling torque') # print(f'enabling torque')
dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx(
self.portHandler, motor_id, self.ADDR_GOAL_PWM, pwm_value
self.portHandler, motor_id, self.ADDR_GOAL_PWM, pwm_value,
) )
# self._process_response(dxl_comm_result, dxl_error) # self._process_response(dxl_comm_result, dxl_error)
# print(f'set pwm of motor {motor_id} to {pwm_value}') # print(f'set pwm of motor {motor_id} to {pwm_value}')
if dxl_comm_result != COMM_SUCCESS: if dxl_comm_result != COMM_SUCCESS:
if tries <= 1: if tries <= 1:
raise ConnectionError( raise ConnectionError(
f"dxl_comm_result: {self.packetHandler.getTxRxResult(dxl_comm_result)}"
)
else:
print(
f"dynamixel pwm setting failure trying again with {tries - 1} tries"
f"dxl_comm_result: {self.packetHandler.getTxRxResult(dxl_comm_result)}",
) )
self.set_pwm_value(motor_id, pwm_value, tries=tries - 1)
print(
f"dynamixel pwm setting failure trying again with {tries - 1} tries",
)
self.set_pwm_value(motor_id, pwm_value, tries=tries - 1)
elif dxl_error != 0: elif dxl_error != 0:
print(f"dxl error {dxl_error}") print(f"dxl error {dxl_error}")
raise ConnectionError( raise ConnectionError(
f"dynamixel error: {self.packetHandler.getTxRxResult(dxl_error)}"
f"dynamixel error: {self.packetHandler.getTxRxResult(dxl_error)}",
) )


def read_temperature(self, motor_id: int): def read_temperature(self, motor_id: int):
@@ -157,8 +158,7 @@ class Dynamixel:
return self._read_value(motor_id, ReadAttribute.HARDWARE_ERROR_STATUS, 1) return self._read_value(motor_id, ReadAttribute.HARDWARE_ERROR_STATUS, 1)


def set_id(self, old_id, new_id, use_broadcast_id: bool = False): def set_id(self, old_id, new_id, use_broadcast_id: bool = False):
"""
sets the id of the dynamixel servo
"""Sets the id of the dynamixel servo
@param old_id: current id of the servo @param old_id: current id of the servo
@param new_id: new id @param new_id: new id
@param use_broadcast_id: set ids of all connected dynamixels if True. @param use_broadcast_id: set ids of all connected dynamixels if True.
@@ -170,21 +170,21 @@ class Dynamixel:
else: else:
current_id = old_id current_id = old_id
dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx( dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx(
self.portHandler, current_id, self.ADDR_ID, new_id
self.portHandler, current_id, self.ADDR_ID, new_id,
) )
self._process_response(dxl_comm_result, dxl_error, old_id) self._process_response(dxl_comm_result, dxl_error, old_id)
self.config.id = id self.config.id = id


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


def _disable_torque(self, motor_id): def _disable_torque(self, motor_id):
dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx( dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx(
self.portHandler, motor_id, self.ADDR_TORQUE_ENABLE, 0
self.portHandler, motor_id, self.ADDR_TORQUE_ENABLE, 0,
) )
self._process_response(dxl_comm_result, dxl_error, motor_id) self._process_response(dxl_comm_result, dxl_error, motor_id)
self.torque_enabled[motor_id] = False self.torque_enabled[motor_id] = False
@@ -192,42 +192,42 @@ class Dynamixel:
def _process_response(self, dxl_comm_result: int, dxl_error: int, motor_id: int): def _process_response(self, dxl_comm_result: int, dxl_error: int, motor_id: int):
if dxl_comm_result != COMM_SUCCESS: if dxl_comm_result != COMM_SUCCESS:
raise ConnectionError( raise ConnectionError(
f"dxl_comm_result for motor {motor_id}: {self.packetHandler.getTxRxResult(dxl_comm_result)}"
f"dxl_comm_result for motor {motor_id}: {self.packetHandler.getTxRxResult(dxl_comm_result)}",
) )
elif dxl_error != 0:
if dxl_error != 0:
print(f"dxl error {dxl_error}") print(f"dxl error {dxl_error}")
raise ConnectionError( raise ConnectionError(
f"dynamixel error for motor {motor_id}: {self.packetHandler.getTxRxResult(dxl_error)}"
f"dynamixel error for motor {motor_id}: {self.packetHandler.getTxRxResult(dxl_error)}",
) )


def set_operating_mode(self, motor_id: int, operating_mode: OperatingMode): def set_operating_mode(self, motor_id: int, operating_mode: OperatingMode):
dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx(
self.portHandler, motor_id, self.OPERATING_MODE_ADDR, operating_mode.value
self.portHandler, motor_id, self.OPERATING_MODE_ADDR, operating_mode.value,
) )
self._process_response(dxl_comm_result, dxl_error, motor_id) self._process_response(dxl_comm_result, dxl_error, motor_id)
self.operating_modes[motor_id] = operating_mode self.operating_modes[motor_id] = operating_mode


def set_pwm_limit(self, motor_id: int, limit: int): def set_pwm_limit(self, motor_id: int, limit: int):
dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx(
self.portHandler, motor_id, 36, limit
self.portHandler, motor_id, 36, limit,
) )
self._process_response(dxl_comm_result, dxl_error, motor_id) self._process_response(dxl_comm_result, dxl_error, motor_id)


def set_velocity_limit(self, motor_id: int, velocity_limit): def set_velocity_limit(self, motor_id: int, velocity_limit):
dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx( dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx(
self.portHandler, motor_id, self.ADDR_VELOCITY_LIMIT, velocity_limit
self.portHandler, motor_id, self.ADDR_VELOCITY_LIMIT, velocity_limit,
) )
self._process_response(dxl_comm_result, dxl_error, motor_id) self._process_response(dxl_comm_result, dxl_error, motor_id)


def set_P(self, motor_id: int, P: int): def set_P(self, motor_id: int, P: int):
dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx(
self.portHandler, motor_id, self.POSITION_P, P
self.portHandler, motor_id, self.POSITION_P, P,
) )
self._process_response(dxl_comm_result, dxl_error, motor_id) self._process_response(dxl_comm_result, dxl_error, motor_id)


def set_I(self, motor_id: int, I: int): def set_I(self, motor_id: int, I: int):
dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx( dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx(
self.portHandler, motor_id, self.POSITION_I, I
self.portHandler, motor_id, self.POSITION_I, I,
) )
self._process_response(dxl_comm_result, dxl_error, motor_id) self._process_response(dxl_comm_result, dxl_error, motor_id)


@@ -243,7 +243,7 @@ class Dynamixel:
def set_home_offset(self, motor_id: int, home_position: int): def set_home_offset(self, motor_id: int, home_position: int):
self._disable_torque(motor_id) self._disable_torque(motor_id)
dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx( dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx(
self.portHandler, motor_id, ReadAttribute.HOMING_OFFSET.value, home_position
self.portHandler, motor_id, ReadAttribute.HOMING_OFFSET.value, home_position,
) )
self._process_response(dxl_comm_result, dxl_error, motor_id) self._process_response(dxl_comm_result, dxl_error, motor_id)
self._enable_torque(motor_id) self._enable_torque(motor_id)
@@ -265,7 +265,7 @@ class Dynamixel:


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


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


def set_home_position(self, motor_id: int): def set_home_position(self, motor_id: int):


+ 18
- 25
examples/aloha/benchmark/python/robot.py View File

@@ -1,16 +1,17 @@
import numpy as np
from dynamixel import Dynamixel, OperatingMode, ReadAttribute
import time import time
from enum import Enum, auto
from typing import Union

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




class MotorControlType(Enum): class MotorControlType(Enum):
@@ -65,8 +66,7 @@ class Robot:
self.motor_control_state = MotorControlType.DISABLED self.motor_control_state = MotorControlType.DISABLED


def read_position(self, tries=2): def read_position(self, tries=2):
"""
Reads the joint positions of the robot. 2048 is the center position. 0 and 4096 are 180 degrees in each direction.
"""Reads the joint positions of the robot. 2048 is the center position. 0 and 4096 are 180 degrees in each direction.
:param tries: maximum number of tries to read the position :param tries: maximum number of tries to read the position
:return: list of joint positions in range [0, 4096] :return: list of joint positions in range [0, 4096]
""" """
@@ -74,8 +74,7 @@ class Robot:
if result != 0: if result != 0:
if tries > 0: if tries > 0:
return self.read_position(tries=tries - 1) return self.read_position(tries=tries - 1)
else:
print(f"failed to read position!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
print("failed to read position!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
positions = [] positions = []
for id in self.servo_ids: for id in self.servo_ids:
position = self.position_reader.getData(id, ReadAttribute.POSITION.value, 4) position = self.position_reader.getData(id, ReadAttribute.POSITION.value, 4)
@@ -85,8 +84,7 @@ class Robot:
return positions return positions


def read_velocity(self): def read_velocity(self):
"""
Reads the joint velocities of the robot.
"""Reads the joint velocities of the robot.
:return: list of joint velocities, :return: list of joint velocities,
""" """
self.velocity_reader.txRxPacket() self.velocity_reader.txRxPacket()
@@ -99,11 +97,9 @@ class Robot:
return velocties return velocties


def set_goal_pos(self, action): def set_goal_pos(self, action):
""":param action: list or numpy array of target joint positions in range [0, 4096]
""" """

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


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


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


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


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

@@ -1,11 +1,11 @@
from robot import Robot
from dynamixel import Dynamixel from dynamixel import Dynamixel
from robot import Robot


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


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

@@ -1,8 +1,7 @@
import dora import dora
from dora import Node
import pyarrow as pa
import numpy as np import numpy as np

import pyarrow as pa
from dora import Node


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


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


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


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


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

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

@@ -1,12 +1,12 @@
import gymnasium as gym
import time
from pathlib import Path


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


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


@@ -49,8 +49,8 @@ class ReplayPolicy:


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






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

@@ -1,8 +1,7 @@
from pynput import keyboard
from pynput.keyboard import Key, Events
import pyarrow as pa import pyarrow as pa
from dora import Node from dora import Node

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


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


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


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

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

@@ -1,14 +1,12 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
# -*- coding: utf-8 -*-


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

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


node = Node() node = Node()


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


+ 28
- 29
examples/aloha/nodes/llm_op.py View File

@@ -1,14 +1,14 @@
from dora import DoraStatus
import pylcs
import os
import pyarrow as pa
from transformers import AutoModelForCausalLM, AutoTokenizer
import torch

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


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

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


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




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


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


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

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


return matches return matches




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


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


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

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




def calculate_similarity(source, target): def calculate_similarity(source, target):
"""
Calculate a similarity score between the source and target strings.
"""Calculate a similarity score between the source and target strings.
This uses the edit distance relative to the length of the strings. This uses the edit distance relative to the length of the strings.
""" """
edit_distance = pylcs.edit_distance(source, target) edit_distance = pylcs.edit_distance(source, target)
@@ -90,8 +92,7 @@ def calculate_similarity(source, target):




def find_best_match_location(source_code, target_block): def find_best_match_location(source_code, target_block):
"""
Find the best match for the target_block within the source_code by searching line by line,
"""Find the best match for the target_block within the source_code by searching line by line,
considering blocks of varying lengths. considering blocks of varying lengths.
""" """
source_lines = source_code.split("\n") source_lines = source_code.split("\n")
@@ -121,8 +122,7 @@ def find_best_match_location(source_code, target_block):




def replace_code_in_source(source_code, replacement_block: str): def replace_code_in_source(source_code, replacement_block: str):
"""
Replace the best matching block in the source_code with the replacement_block, considering variable block lengths.
"""Replace the best matching block in the source_code with the replacement_block, considering variable block lengths.
""" """
replacement_block = extract_python_code_blocks(replacement_block)[0] replacement_block = extract_python_code_blocks(replacement_block)[0]
start_index, end_index = find_best_match_location(source_code, replacement_block) start_index, end_index = find_best_match_location(source_code, replacement_block)
@@ -132,8 +132,7 @@ def replace_code_in_source(source_code, replacement_block: str):
source_code[:start_index] + replacement_block + source_code[end_index:] source_code[:start_index] + replacement_block + source_code[end_index:]
) )
return new_source return new_source
else:
return source_code
return source_code




class Operator: class Operator:
@@ -155,14 +154,14 @@ class Operator:
current_directory = os.path.dirname(current_file_path) current_directory = os.path.dirname(current_file_path)
path = current_directory + "/policy.py" path = current_directory + "/policy.py"


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


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


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


source_code = replace_code_in_source(code, output) source_code = replace_code_in_source(code, output)
@@ -213,7 +212,7 @@ if __name__ == "__main__":
current_directory = os.path.dirname(current_file_path) current_directory = os.path.dirname(current_file_path)


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


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


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

@@ -1,8 +1,8 @@
import os import os

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



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


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

@@ -1,4 +1,3 @@
import pyarrow as pa
from dora import DoraStatus from dora import DoraStatus






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

@@ -1,9 +1,10 @@
import pyrealsense2 as rs
import numpy as np
from dora import Node
import pyarrow as pa
import os import os

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


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


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

@@ -1,8 +1,8 @@
from dora import Node
import pandas as pd
import pyarrow as pa
import time import time


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


TOPIC = "puppet_goal_position" TOPIC = "puppet_goal_position"




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

@@ -1,12 +1,10 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
# -*- coding: utf-8 -*-


import os import os
import time
import numpy as np

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

from dora import Node from dora import Node


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


+ 5
- 9
examples/aloha/nodes/whisper_node.py View File

@@ -1,14 +1,10 @@
import pyarrow as pa
import whisper
from pynput import keyboard
from pynput.keyboard import Key, Events
from dora import Node

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


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


@@ -48,7 +44,7 @@ with keyboard.Events() as events:
if event.key == Key.alt_r: if event.key == Key.alt_r:
result = get_text(5) result = get_text(5)
node.send_output( node.send_output(
"text_llm", pa.array([result["text"]]), dora_event["metadata"]
"text_llm", pa.array([result["text"]]), dora_event["metadata"],
) )
elif event.key == Key.ctrl_r: elif event.key == Key.ctrl_r:
result = get_text(3) result = get_text(3)


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

@@ -1,7 +1,8 @@
import pyrealsense2 as rs
import os

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


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




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


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

@@ -1,14 +1,13 @@
from dora import Node
import pyarrow as pa
from enum import Enum from enum import Enum


import pyarrow as pa
from dora import Node

node = Node() node = Node()




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


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


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

@@ -1,5 +1,5 @@
from dora import Node
import pyarrow as pa import pyarrow as pa
from dora import Node


node = Node() node = Node()




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

@@ -1,5 +1,5 @@
from dora import Node
import pyarrow as pa import pyarrow as pa
from dora import Node


node = Node() node = Node()


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

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

@@ -1,5 +1,5 @@
from dora import Node
import pyarrow as pa import pyarrow as pa
from dora import Node


node = Node() node = Node()




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

@@ -1,14 +1,12 @@
import time import time
from pathlib import Path


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


import gym_dora # noqa: F401

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




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

@@ -1,8 +1,7 @@
from pynput import keyboard
from pynput.keyboard import Key, Events
import pyarrow as pa import pyarrow as pa
from dora import Node from dora import Node

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


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


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


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

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

@@ -1,14 +1,12 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
# -*- coding: utf-8 -*-


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

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


node = Node() node = Node()


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


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

@@ -1,8 +1,8 @@
import os import os

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



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


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

@@ -1,7 +1,4 @@
import argparse
import os
import time import time
from pathlib import Path


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


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


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

@@ -1,7 +1,4 @@
import argparse
import os
import time import time
from pathlib import Path


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


import cv2 import cv2
import numpy as np


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


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


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




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

@@ -1,7 +1,8 @@
from lerobot.common.datasets.lerobot_dataset import LeRobotDataset
import time import time
from dora import Node
import pyarrow as pa import pyarrow as pa
from dora import Node
from lerobot.common.datasets.lerobot_dataset import LeRobotDataset


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


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

@@ -1,14 +1,12 @@
import time import time
from pathlib import Path


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


import gym_dora # noqa: F401

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




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

@@ -1,5 +1,5 @@
from dora import Node
import pyarrow as pa import pyarrow as pa
from dora import Node


node = Node() node = Node()




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

@@ -1,8 +1,7 @@
from pynput import keyboard
from pynput.keyboard import Key, Events
import pyarrow as pa import pyarrow as pa
from dora import Node from dora import Node

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


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


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


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

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

@@ -1,14 +1,12 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
# -*- coding: utf-8 -*-


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

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


node = Node() node = Node()


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


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

@@ -1,8 +1,8 @@
import os import os

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



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


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

@@ -1,7 +1,4 @@
import argparse
import os
import time import time
from pathlib import Path


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


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


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

@@ -1,7 +1,4 @@
import argparse
import os
import time import time
from pathlib import Path


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


import cv2 import cv2
import numpy as np


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


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


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




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

@@ -1,7 +1,8 @@
from lerobot.common.datasets.lerobot_dataset import LeRobotDataset
import time import time
from dora import Node
import pyarrow as pa import pyarrow as pa
from dora import Node
from lerobot.common.datasets.lerobot_dataset import LeRobotDataset


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


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

@@ -1,6 +1,5 @@
from dora import Node
import pyarrow as pa import pyarrow as pa
import numpy as np
from dora import Node


node = Node() node = Node()


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


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

+ 16
- 16
examples/so100/bus.py View File

@@ -1,17 +1,18 @@
import enum import enum

import pyarrow as pa

from typing import Union from typing import Union


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


PROTOCOL_VERSION = 0 PROTOCOL_VERSION = 0
BAUD_RATE = 1_000_000 BAUD_RATE = 1_000_000
@@ -93,13 +94,12 @@ MODEL_CONTROL_TABLE = {
class FeetechBus: class FeetechBus:


def __init__(self, port: str, description: dict[str, (np.uint8, str)]): def __init__(self, port: str, description: dict[str, (np.uint8, str)]):
"""
Args:
port: the serial port to connect to the Feetech bus
description: a dictionary containing the description of the motors connected to the bus. The keys are the
motor names and the values are tuples containing the motor id and the motor model.
"""
"""Args:
port: the serial port to connect to the Feetech bus
description: a dictionary containing the description of the motors connected to the bus. The keys are the
motor names and the values are tuples containing the motor id and the motor model.


"""
self.port = port self.port = port
self.descriptions = description self.descriptions = description
self.motor_ctrl = {} self.motor_ctrl = {}
@@ -183,7 +183,7 @@ class FeetechBus:
else: else:
raise NotImplementedError( raise NotImplementedError(
f"Value of the number of bytes to be sent is expected to be in [1, 2, 4], but {packet_bytes_size} " f"Value of the number of bytes to be sent is expected to be in [1, 2, 4], but {packet_bytes_size} "
f"is provided instead."
f"is provided instead.",
) )


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


def read(self, data_name: str, motor_names: pa.Array) -> pa.StructArray: def read(self, data_name: str, motor_names: pa.Array) -> pa.StructArray:
@@ -225,13 +225,13 @@ class FeetechBus:
if comm != COMM_SUCCESS: if comm != COMM_SUCCESS:
raise ConnectionError( raise ConnectionError(
f"Read failed due to communication error on port {self.port} for group_key {group_key}: " f"Read failed due to communication error on port {self.port} for group_key {group_key}: "
f"{self.packet_handler.getTxRxResult(comm)}"
f"{self.packet_handler.getTxRxResult(comm)}",
) )


values = pa.array( values = pa.array(
[ [
self.group_readers[group_key].getData( self.group_readers[group_key].getData(
idx, packet_address, packet_bytes_size
idx, packet_address, packet_bytes_size,
) )
for idx in motor_ids for idx in motor_ids
], ],


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

@@ -1,5 +1,4 @@
"""
SO100 Auto Configure: This program is used to automatically configure the SO-ARM100 (SO100) for the user.
"""SO100 Auto Configure: This program is used to automatically configure the SO-ARM100 (SO100) for the user.


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


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


import pyarrow as pa import pyarrow as pa

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

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

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


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




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




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

Args:
bus: The FeetechBus instance to configure.
"""
bus.write_torque_enable( bus.write_torque_enable(
wrap_joints_and_values(FULL_ARM, [TorqueMode.DISABLED.value] * 6)
wrap_joints_and_values(FULL_ARM, [TorqueMode.DISABLED.value] * 6),
) )


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


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


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




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


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


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


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


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


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


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


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


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


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

@@ -1,25 +1,24 @@
import os
import argparse import argparse
import json import json
import os


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

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




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


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


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


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


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


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


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


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

@@ -1,23 +1,22 @@
import os
import argparse import argparse
import json import json
import os


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

from dora import Node from dora import Node

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




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


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


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


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


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


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


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


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


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

@@ -1,20 +1,18 @@
import os
import argparse import argparse
import json import json

import pyarrow as pa
import os


from dora import Node from dora import Node

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




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


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


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


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


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


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


+ 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},
) )

+ 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={},
) )






+ 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


+ 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

+ 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

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


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


+ 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():


+ 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():


+ 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():


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

@@ -7,7 +7,7 @@ readme_path = os.path.join(os.path.dirname(os.path.dirname(__file__)), "README.m


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

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

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


from .main import main from .main import main



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

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

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


import logging
import os import os

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


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


def main(): def main():
@@ -111,11 +112,11 @@ def main():
logging.info(f"Processing input: {text}") logging.info(f"Processing input: {text}")
response, history = generate_response(model, tokenizer, text, history) response, history = generate_response(model, tokenizer, text, history)
logging.info(f"Generated response: {response}") logging.info(f"Generated response: {response}")
node.send_output( node.send_output(
output_id="text",
data=pa.array([response]),
metadata={}
output_id="text",
data=pa.array([response]),
metadata={},
) )


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


+ 12
- 11
node-hub/dynamixel-client/dynamixel_client/bus.py View File

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


import enum import enum

import pyarrow as pa

from typing import Union from typing import Union


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


PROTOCOL_VERSION = 2.0 PROTOCOL_VERSION = 2.0
BAUD_RATE = 1_000_000 BAUD_RATE = 1_000_000
@@ -70,7 +71,7 @@ def wrap_joints_and_values(
mask = values.is_null() mask = values.is_null()


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




@@ -248,7 +249,7 @@ class DynamixelBus:
else: else:
raise NotImplementedError( raise NotImplementedError(
f"Value of the number of bytes to be sent is expected to be in [1, 2, 4], but {packet_bytes_size} " f"Value of the number of bytes to be sent is expected to be in [1, 2, 4], but {packet_bytes_size} "
f"is provided instead."
f"is provided instead.",
) )


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


def read(self, data_name: str, motor_names: pa.Array) -> pa.StructArray: def read(self, data_name: str, motor_names: pa.Array) -> pa.StructArray:
@@ -291,13 +292,13 @@ class DynamixelBus:
if comm != COMM_SUCCESS: if comm != COMM_SUCCESS:
raise ConnectionError( raise ConnectionError(
f"Read failed due to communication error on port {self.port} for group_key {group_key}: " f"Read failed due to communication error on port {self.port} for group_key {group_key}: "
f"{self.packet_handler.getTxRxResult(comm)}"
f"{self.packet_handler.getTxRxResult(comm)}",
) )


values = pa.array( values = pa.array(
[ [
self.group_readers[group_key].getData( self.group_readers[group_key].getData(
idx, packet_address, packet_bytes_size
idx, packet_address, packet_bytes_size,
) )
for idx in motor_ids for idx in motor_ids
], ],


+ 7
- 8
node-hub/dynamixel-client/dynamixel_client/main.py View File

@@ -3,13 +3,12 @@
It can be used to read positions, velocities, currents, and set goal positions and currents. It can be used to read positions, velocities, currents, and set goal positions and currents.
""" """


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


import pyarrow as pa import pyarrow as pa

from dora import Node from dora import Node


from .bus import DynamixelBus, TorqueMode, wrap_joints_and_values from .bus import DynamixelBus, TorqueMode, wrap_joints_and_values
@@ -76,7 +75,7 @@ class Client:
wrap_joints_and_values( wrap_joints_and_values(
self.config["joints"], self.config["joints"],
[TorqueMode.DISABLED.value] * len(self.config["joints"]), [TorqueMode.DISABLED.value] * len(self.config["joints"]),
)
),
) )


def pull_position(self, node, metadata): def pull_position(self, node, metadata):
@@ -132,7 +131,7 @@ def main():
"""Handle dynamic nodes, ask for the name of the node in the dataflow.""" """Handle dynamic nodes, ask for the name of the node in the dataflow."""
parser = argparse.ArgumentParser( parser = argparse.ArgumentParser(
description="Dynamixel Client: This node is used to represent a chain of dynamixel motors. It can be used to " description="Dynamixel Client: This node is used to represent a chain of dynamixel motors. It can be used to "
"read positions, velocities, currents, and set goal positions and currents."
"read positions, velocities, currents, and set goal positions and currents.",
) )


parser.add_argument( parser.add_argument(
@@ -162,7 +161,7 @@ def main():
if not os.environ.get("PORT") and args.port is None: if not os.environ.get("PORT") and args.port is None:
raise ValueError( raise ValueError(
"The port is not set. Please set the port of the dynamixel motors in the environment variables or as an " "The port is not set. Please set the port of the dynamixel motors in the environment variables or as an "
"argument."
"argument.",
) )


port = os.environ.get("PORT") if args.port is None else args.port port = os.environ.get("PORT") if args.port is None else args.port
@@ -171,7 +170,7 @@ def main():
if not os.environ.get("CONFIG") and args.config is None: if not os.environ.get("CONFIG") and args.config is None:
raise ValueError( raise ValueError(
"The configuration is not set. Please set the configuration of the dynamixel motors in the environment " "The configuration is not set. Please set the configuration of the dynamixel motors in the environment "
"variables or as an argument."
"variables or as an argument.",
) )


with open(os.environ.get("CONFIG") if args.config is None else args.config) as file: with open(os.environ.get("CONFIG") if args.config is None else args.config) as file:
@@ -203,7 +202,7 @@ def main():
"goal_current": wrap_joints_and_values( "goal_current": wrap_joints_and_values(
pa.array(config.keys(), pa.string()), pa.array(config.keys(), pa.string()),
pa.array( pa.array(
[config[joint]["goal_current"] for joint in joints], type=pa.uint32()
[config[joint]["goal_current"] for joint in joints], type=pa.uint32(),
), ),
), ),
"P": wrap_joints_and_values( "P": wrap_joints_and_values(


+ 0
- 1
node-hub/dynamixel-client/tests/test_dynamixel_client.py View File

@@ -4,7 +4,6 @@


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




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


+ 1
- 1
node-hub/feetech-client/feetech_client/__init__.py View File

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

+ 4
- 4
node-hub/feetech-client/feetech_client/bus.py View File

@@ -194,7 +194,7 @@ class FeetechBus:
else: else:
raise NotImplementedError( raise NotImplementedError(
f"Value of the number of bytes to be sent is expected to be in [1, 2, 4], but {packet_bytes_size} " f"Value of the number of bytes to be sent is expected to be in [1, 2, 4], but {packet_bytes_size} "
f"is provided instead."
f"is provided instead.",
) )


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


def read(self, data_name: str, motor_names: pa.Array) -> pa.StructArray: def read(self, data_name: str, motor_names: pa.Array) -> pa.StructArray:
@@ -237,13 +237,13 @@ class FeetechBus:
if comm != COMM_SUCCESS: if comm != COMM_SUCCESS:
raise ConnectionError( raise ConnectionError(
f"Read failed due to communication error on port {self.port} for group_key {group_key}: " f"Read failed due to communication error on port {self.port} for group_key {group_key}: "
f"{self.packet_handler.getTxRxResult(comm)}"
f"{self.packet_handler.getTxRxResult(comm)}",
) )


values = pa.array( values = pa.array(
[ [
self.group_readers[group_key].getData( self.group_readers[group_key].getData(
idx, packet_address, packet_bytes_size
idx, packet_address, packet_bytes_size,
) )
for idx in motor_ids for idx in motor_ids
], ],


+ 5
- 6
node-hub/feetech-client/feetech_client/main.py View File

@@ -1,11 +1,10 @@
"""Feetech Client: This node is used to represent a chain of feetech motors. It can be used to read positions, velocities, currents, and set goal positions and currents.""" """Feetech Client: This node is used to represent a chain of feetech motors. It can be used to read positions, velocities, currents, and set goal positions and currents."""


import os
import argparse import argparse
import json import json
import os


import pyarrow as pa import pyarrow as pa

from dora import Node from dora import Node


from .bus import FeetechBus, TorqueMode, wrap_joints_and_values from .bus import FeetechBus, TorqueMode, wrap_joints_and_values
@@ -60,7 +59,7 @@ class Client:
wrap_joints_and_values( wrap_joints_and_values(
self.config["joints"], self.config["joints"],
[TorqueMode.DISABLED.value] * len(self.config["joints"]), [TorqueMode.DISABLED.value] * len(self.config["joints"]),
)
),
) )


def pull_position(self, node, metadata): def pull_position(self, node, metadata):
@@ -110,7 +109,7 @@ def main():
parser = argparse.ArgumentParser( parser = argparse.ArgumentParser(
description="Feetech Client: This node is used to represent a chain of feetech motors. " description="Feetech Client: This node is used to represent a chain of feetech motors. "
"It can be used to read " "It can be used to read "
"positions, velocities, currents, and set goal positions and currents."
"positions, velocities, currents, and set goal positions and currents.",
) )


parser.add_argument( parser.add_argument(
@@ -140,7 +139,7 @@ def main():
if not os.environ.get("PORT") and args.port is None: if not os.environ.get("PORT") and args.port is None:
raise ValueError( raise ValueError(
"The port is not set. Please set the port of the feetech motors in the environment variables or as an " "The port is not set. Please set the port of the feetech motors in the environment variables or as an "
"argument."
"argument.",
) )


port = os.environ.get("PORT") if args.port is None else args.port port = os.environ.get("PORT") if args.port is None else args.port
@@ -149,7 +148,7 @@ def main():
if not os.environ.get("CONFIG") and args.config is None: if not os.environ.get("CONFIG") and args.config is None:
raise ValueError( raise ValueError(
"The configuration is not set. Please set the configuration of the feetech motors in the environment " "The configuration is not set. Please set the configuration of the feetech motors in the environment "
"variables or as an argument."
"variables or as an argument.",
) )


with open(os.environ.get("CONFIG") if args.config is None else args.config) as file: with open(os.environ.get("CONFIG") if args.config is None else args.config) as file:


+ 0
- 1
node-hub/feetech-client/tests/test_feetech_client.py View File

@@ -4,7 +4,6 @@


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




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


+ 6
- 5
node-hub/lebai-client/lebai_client/main.py View File

@@ -1,11 +1,12 @@
"""TODO: Add docstring.""" """TODO: Add docstring."""
import lebai_sdk
import numpy as np
from dora import Node
import json import json
import os import os
import time import time


import lebai_sdk
import numpy as np
from dora import Node



def load_json_file(file_path): def load_json_file(file_path):
"""Load JSON file and return the dictionary.""" """Load JSON file and return the dictionary."""
@@ -28,7 +29,7 @@ SAVED_POSE_PATH = "pose_library.json"


lebai_sdk.init() lebai_sdk.init()
ROBOT_IP = os.getenv( ROBOT_IP = os.getenv(
"LEBAI_IP", "10.42.0.253"
"LEBAI_IP", "10.42.0.253",
) # 设定机器人ip地址,需要根据机器人实际ip地址修改 ) # 设定机器人ip地址,需要根据机器人实际ip地址修改




@@ -180,7 +181,7 @@ def main():
"duration": time.time() - start_time, "duration": time.time() - start_time,
"joint_position": joint_position, "joint_position": joint_position,
"t": t * 2 if t == 0.1 else t, "t": t * 2 if t == 0.1 else t,
}
},
] ]
start_time = time.time() start_time = time.time()




+ 1
- 2
node-hub/lebai-client/test/test.py View File

@@ -1,7 +1,6 @@
"""TODO: Add docstring.""" """TODO: Add docstring."""
import lebai_sdk import lebai_sdk



lebai_sdk.init() lebai_sdk.init()




@@ -26,7 +25,7 @@ def main():
r = 0 # 交融半径 (m)。用于指定路径的平滑效果 r = 0 # 交融半径 (m)。用于指定路径的平滑效果


lebai.movel( lebai.movel(
cartesian_pose, a, v, t, r
cartesian_pose, a, v, t, r,
) # 直线运动 https://help.lebai.ltd/sdk/motion.html#%E7%9B%B4%E7%BA%BF%E8%BF%90%E5%8A%A8 ) # 直线运动 https://help.lebai.ltd/sdk/motion.html#%E7%9B%B4%E7%BA%BF%E8%BF%90%E5%8A%A8
lebai.wait_move() # 等待运动完成 lebai.wait_move() # 等待运动完成
# scene_number = "10000" #需要调用的场景编号 # scene_number = "10000" #需要调用的场景编号


+ 0
- 1
node-hub/lebai-client/tests/test_lebai_client.py View File

@@ -4,7 +4,6 @@


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




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


+ 1
- 1
node-hub/lerobot-dashboard/lerobot_dashboard/__init__.py View File

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

+ 7
- 9
node-hub/lerobot-dashboard/lerobot_dashboard/main.py View File

@@ -1,19 +1,17 @@
"""Following Dora node is a minimalistic interface that shows two images and text in a Pygame window.""" """Following Dora node is a minimalistic interface that shows two images and text in a Pygame window."""


import os
import argparse import argparse

import pygame
import os


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




def main(): def main():
"""Handle dynamic nodes, ask for the name of the node in the dataflow.""" """Handle dynamic nodes, ask for the name of the node in the dataflow."""
parser = argparse.ArgumentParser( parser = argparse.ArgumentParser(
description="LeRobot Record: This node is used to record episodes of a robot interacting with the environment."
description="LeRobot Record: This node is used to record episodes of a robot interacting with the environment.",
) )


parser.add_argument( parser.add_argument(
@@ -66,7 +64,7 @@ def main():
if event_type == "STOP": if event_type == "STOP":
break break


elif event_type == "INPUT":
if event_type == "INPUT":
event_id = event["id"] event_id = event["id"]


if event_id == "image_left": if event_id == "image_left":
@@ -80,7 +78,7 @@ def main():
} }


image_left = pygame.image.frombuffer( image_left = pygame.image.frombuffer(
image["data"], (image["width"], image["height"]), "BGR"
image["data"], (image["width"], image["height"]), "BGR",
) )


elif event_id == "image_right": elif event_id == "image_right":
@@ -93,7 +91,7 @@ def main():
} }


image_right = pygame.image.frombuffer( image_right = pygame.image.frombuffer(
image["data"], (image["width"], image["height"]), "BGR"
image["data"], (image["width"], image["height"]), "BGR",
) )


elif event_id == "tick": elif event_id == "tick":
@@ -104,7 +102,7 @@ def main():
if pygame_event.type == pygame.QUIT: if pygame_event.type == pygame.QUIT:
running = False running = False
break break
elif pygame_event.type == pygame.KEYDOWN:
if pygame_event.type == pygame.KEYDOWN:
key = pygame.key.name(pygame_event.key) key = pygame.key.name(pygame_event.key)


if key == "space": if key == "space":


+ 0
- 1
node-hub/lerobot-dashboard/tests/test_lerobot_dashboard.py View File

@@ -4,7 +4,6 @@


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




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


+ 1
- 1
node-hub/mujoco-client/mujoco_client/__init__.py View File

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

+ 9
- 14
node-hub/mujoco-client/mujoco_client/main.py View File

@@ -1,16 +1,14 @@
"""Mujoco Client: This node is used to represent simulated robot, it can be used to read virtual positions, or can be controlled.""" """Mujoco Client: This node is used to represent simulated robot, it can be used to read virtual positions, or can be controlled."""


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

import pyarrow as pa

from dora import Node
import os
import time


import mujoco import mujoco
import mujoco.viewer import mujoco.viewer
import pyarrow as pa
from dora import Node




class Client: class Client:
@@ -66,22 +64,19 @@ class Client:


elif event_type == "ERROR": elif event_type == "ERROR":
raise ValueError( raise ValueError(
"An error occurred in the dataflow: " + event["error"]
"An error occurred in the dataflow: " + event["error"],
) )


self.node.send_output("end", pa.array([])) self.node.send_output("end", pa.array([]))


def pull_position(self, node, metadata): def pull_position(self, node, metadata):
"""TODO: Add docstring.""" """TODO: Add docstring."""
pass


def pull_velocity(self, node, metadata): def pull_velocity(self, node, metadata):
"""TODO: Add docstring.""" """TODO: Add docstring."""
pass


def pull_current(self, node, metadata): def pull_current(self, node, metadata):
"""TODO: Add docstring.""" """TODO: Add docstring."""
pass


def write_goal_position(self, goal_position_with_joints): def write_goal_position(self, goal_position_with_joints):
"""TODO: Add docstring.""" """TODO: Add docstring."""
@@ -96,7 +91,7 @@ def main():
"""Handle dynamic nodes, ask for the name of the node in the dataflow.""" """Handle dynamic nodes, ask for the name of the node in the dataflow."""
parser = argparse.ArgumentParser( parser = argparse.ArgumentParser(
description="MujoCo Client: This node is used to represent a MuJoCo simulation. It can be used instead of a " description="MujoCo Client: This node is used to represent a MuJoCo simulation. It can be used instead of a "
"follower arm to test the dataflow."
"follower arm to test the dataflow.",
) )


parser.add_argument( parser.add_argument(
@@ -114,14 +109,14 @@ def main():
) )


parser.add_argument( parser.add_argument(
"--config", type=str, help="The configuration of the joints.", default=None
"--config", type=str, help="The configuration of the joints.", default=None,
) )


args = parser.parse_args() args = parser.parse_args()


if not os.getenv("SCENE") and args.scene is None: if not os.getenv("SCENE") and args.scene is None:
raise ValueError( raise ValueError(
"Please set the SCENE environment variable or pass the --scene argument."
"Please set the SCENE environment variable or pass the --scene argument.",
) )


scene = os.getenv("SCENE", args.scene) scene = os.getenv("SCENE", args.scene)
@@ -130,7 +125,7 @@ def main():
if not os.environ.get("CONFIG") and args.config is None: if not os.environ.get("CONFIG") and args.config is None:
raise ValueError( raise ValueError(
"The configuration is not set. Please set the configuration of the simulated motors in the environment " "The configuration is not set. Please set the configuration of the simulated motors in the environment "
"variables or as an argument."
"variables or as an argument.",
) )


with open(os.environ.get("CONFIG") if args.config is None else args.config) as file: with open(os.environ.get("CONFIG") if args.config is None else args.config) as file:


+ 0
- 1
node-hub/mujoco-client/tests/test_mujoco_client.py View File

@@ -4,7 +4,6 @@


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




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


+ 1
- 1
node-hub/replay-client/replay_client/__init__.py View File

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

+ 3
- 4
node-hub/replay-client/replay_client/main.py View File

@@ -3,12 +3,11 @@
reading a dataset of actions and joints from a specific episode. reading a dataset of actions and joints from a specific episode.
""" """


import os
import argparse import argparse
import os


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




@@ -78,7 +77,7 @@ class Client:
def main(): def main():
"""Handle dynamic nodes, ask for the name of the node in the dataflow.""" """Handle dynamic nodes, ask for the name of the node in the dataflow."""
parser = argparse.ArgumentParser( parser = argparse.ArgumentParser(
description="Replay Client: This node is used to replay a sequence of goals for a followee robot."
description="Replay Client: This node is used to replay a sequence of goals for a followee robot.",
) )


parser.add_argument( parser.add_argument(


+ 0
- 1
node-hub/replay-client/tests/test_replay_client.py View File

@@ -4,7 +4,6 @@


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




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


+ 0
- 1
node-hub/video-encoder/tests/test_video_encoder.py View File

@@ -4,7 +4,6 @@


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




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


+ 1
- 1
node-hub/video-encoder/video_encoder/__init__.py View File

@@ -1 +1 @@
"""TODO: Doc String."""
"""TODO: Doc String."""

+ 6
- 8
node-hub/video-encoder/video_encoder/main.py View File

@@ -1,13 +1,11 @@
"""TODO : Doc String.""" """TODO : Doc String."""
import argparse
import os import os
from pathlib import Path from pathlib import Path


import cv2 import cv2
import argparse

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

from dora import Node from dora import Node
from ffmpeg import FFmpeg from ffmpeg import FFmpeg


@@ -15,7 +13,7 @@ from ffmpeg import FFmpeg
def main(): def main():
"""Handle dynamic nodes, ask for the name of the node in the dataflow.""" """Handle dynamic nodes, ask for the name of the node in the dataflow."""
parser = argparse.ArgumentParser( parser = argparse.ArgumentParser(
description="Video Encoder: This node is used to record episodes of a robot interacting with the environment."
description="Video Encoder: This node is used to record episodes of a robot interacting with the environment.",
) )


parser.add_argument( parser.add_argument(
@@ -77,8 +75,8 @@ def main():
{ {
"path": f"videos/{name}", "path": f"videos/{name}",
"timestamp": float(frame_count) / fps, "timestamp": float(frame_count) / fps,
}
]
},
],
), ),
event["metadata"], event["metadata"],
) )
@@ -92,7 +90,7 @@ def main():
} }


data = image["data"].reshape( data = image["data"].reshape(
(image["height"], image["width"], image["channels"])
(image["height"], image["width"], image["channels"]),
) )


path = str(out_dir / f"frame_{frame_count:06d}.png") path = str(out_dir / f"frame_{frame_count:06d}.png")
@@ -118,7 +116,7 @@ def main():
.option("y") .option("y")
.input(str(out_dir / "frame_%06d.png"), f="image2", r=fps) .input(str(out_dir / "frame_%06d.png"), f="image2", r=fps)
.output( .output(
str(video_path), vcodec="libx264", g=2, pix_fmt="yuv444p"
str(video_path), vcodec="libx264", g=2, pix_fmt="yuv444p",
) )
) )




Loading…
Cancel
Save