From 1f873f977f4b7f3fbf69db3506fbb57d64cd535e Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Wed, 16 Jul 2025 15:27:05 +0200 Subject: [PATCH 1/5] Add LeRobot integration --- teleop/leteleop/README.md | 5 ++ teleop/leteleop/__init__.py | 91 +++++++++++++++++++++++++++++++++++++ teleop/leteleop/config.py | 11 +++++ 3 files changed, 107 insertions(+) create mode 100644 teleop/leteleop/README.md create mode 100644 teleop/leteleop/__init__.py create mode 100644 teleop/leteleop/config.py diff --git a/teleop/leteleop/README.md b/teleop/leteleop/README.md new file mode 100644 index 0000000..5d63e21 --- /dev/null +++ b/teleop/leteleop/README.md @@ -0,0 +1,5 @@ +python -m lerobot.teleoperate \ + --robot.type=lerobot_xarm.xarm.Xarm \ + --robot.id=black \ + --teleop.type=teleop.leteleop.LeTeleop \ + --fps=60 diff --git a/teleop/leteleop/__init__.py b/teleop/leteleop/__init__.py new file mode 100644 index 0000000..6120e85 --- /dev/null +++ b/teleop/leteleop/__init__.py @@ -0,0 +1,91 @@ +from typing import Any +from lerobot.teleoperators.teleoperator import Teleoperator +from .config import LeTeleopConfig +from teleop import Teleop +import numpy as np +import threading + + +class LeTeleop(Teleoperator): + config_class = LeTeleopConfig + name = "teleop" + + def __init__(self, config: LeTeleopConfig): + super().__init__(config) + self.config = config + self._connected = False + self._calibrated = True + self._home = False + + self._server = Teleop(host=config.host, port=int(config.port)) + self._server.subscribe(self._on_teleop_callback) + self._gripper_state = 0.0 + self._last_pose = np.eye(4) + self._mutex = threading.Lock() + + def _on_teleop_callback(self, pose, message): + with self._mutex: + if message["move"]: + self._last_pose = pose + else: + self._last_pose = None + + if message["gripper"] is not None: + self._gripper_state = 2.0 if message["gripper"] == "open" else 0.0 + self._home = ( + message["reservedButtonA"] is not None and message["reservedButtonA"] + ) + + @property + def action_features(self) -> dict[str, type]: + if self.config.use_gripper: + return {"pose_from_initial": np.ndarray, "gripper": float, "home": bool} + else: + return { + "pose_from_initial": np.ndarray, + } + + @property + def feedback_features(self) -> dict[str, type]: + return {} + + @property + def is_connected(self) -> bool: + return self._connected + + def connect(self, calibrate: bool = True) -> None: + self._connected = True + threading.Thread(target=self._server.run, daemon=True).start() + + @property + def is_calibrated(self) -> bool: + return self._calibrated + + def calibrate(self) -> None: + pass + + def configure(self) -> None: + pass + + def get_action(self) -> dict[str, Any]: + last_pose = None + with self._mutex: + if self._last_pose is not None: + last_pose = self._last_pose.copy() + + action = {} + if last_pose is not None: + action["pose_from_initial"] = last_pose + if self.config.use_gripper: + action["gripper"] = self._gripper_state + if self._home: + action["home"] = True + return action + + def send_feedback(self, feedback: dict[str, float]) -> None: + # TODO(rcadene, aliberts): Implement force feedback + raise NotImplementedError + + def disconnect(self) -> None: + self._connected = False + self._server.stop() diff --git a/teleop/leteleop/config.py b/teleop/leteleop/config.py new file mode 100644 index 0000000..2c4a596 --- /dev/null +++ b/teleop/leteleop/config.py @@ -0,0 +1,11 @@ +from dataclasses import dataclass + +from lerobot.teleoperators.config import TeleoperatorConfig + + +@TeleoperatorConfig.register_subclass("teleop.leteleop.LeTeleop") +@dataclass +class LeTeleopConfig(TeleoperatorConfig): + port: str = "4443" + host: str = "0.0.0.0" + use_gripper: bool = True From 6feb20ba611ed5c1bd3224baba173038d84b0039 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Mon, 28 Jul 2025 14:23:31 +0200 Subject: [PATCH 2/5] fix scaling --- README.md | 18 ++++---- teleop/__init__.py | 6 ++- teleop/leteleop/README.md | 5 -- teleop/leteleop/__init__.py | 91 ------------------------------------- teleop/leteleop/config.py | 11 ----- teleop/teleop-ui.js | 21 ++++++++- 6 files changed, 33 insertions(+), 119 deletions(-) delete mode 100644 teleop/leteleop/README.md delete mode 100644 teleop/leteleop/__init__.py delete mode 100644 teleop/leteleop/config.py diff --git a/README.md b/README.md index b556c03..7d5dc39 100644 --- a/README.md +++ b/README.md @@ -20,7 +20,7 @@ The web application leverages the WebXR API, which combines your phone’s senso The package is available on [PyPI](https://pypi.org/project/teleop/). You can install it using pip: ```bash -pip3 install teleop +pip install teleop ``` ## Usage @@ -32,7 +32,7 @@ We provide some ready-to-use robot arm interfaces, but you can also create your A simple interface that prints the teleop responses. You can use it as a reference to build your own interface. ```bash -python3 -m teleop.basic +python -m teleop.basic ``` ### xArm @@ -41,7 +41,7 @@ Interface to teleoperate the [uFactory Lite 6](https://www.ufactory.cc/lite-6-co Minor changes are probably necessary to support other xArm robots. ```bash -python3 -m teleop.xarm +python -m teleop.xarm ``` Note that the interface is very simple, it doesn't implement any kind of filtering. @@ -53,7 +53,7 @@ Smart phones are typically 30fps while VR joysticks 90fps which is much more pre The ROS 2 interface is designed primarily for use with the [cartesian_controllers](https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers) package, but it can also be adapted for [MoveIt Servo](https://moveit.picknik.ai/main/doc/examples/realtime_servo/realtime_servo_tutorial.html) or other packages. ```bash -python3 -m teleop.ros2 +python -m teleop.ros2 ``` **Published topics:** @@ -66,7 +66,7 @@ python3 -m teleop.ros2 You can override the default topic names using standard ROS 2 arguments: ```bash -python3 -m teleop.ros2 --ros-args -r target_frame:=/some_other_topic_name +python -m teleop.ros2 --ros-args -r target_frame:=/some_other_topic_name ``` ### ROS 2 Interface with IK @@ -76,7 +76,7 @@ No servoing support, no problem. Panda arm usage example: ```bash -python3 -m teleop.ros2_ik \ +python -m teleop.ros2_ik \ --joint-names panda_joint1 panda_joint2 panda_joint3 panda_joint4 panda_joint5 panda_joint6 panda_joint7 \ --ee-link panda_hand \ --ros-args -r /joint_trajectory:=/panda_arm_controller/joint_trajectory @@ -84,7 +84,7 @@ python3 -m teleop.ros2_ik \ xArm usage example: ```bash -python3 -m teleop.ros2_ik \ +python -m teleop.ros2_ik \ --joint-names joint1 joint2 joint3 joint4 joint5 joint6 \ --ee-link link6 \ --ros-args -r /joint_trajectory:=/joint_trajectory_controller/joint_trajectory @@ -191,8 +191,8 @@ If you’d like to contribute, install the package in editable mode: # Install the package in editable mode git clone https://github.com/SpesRobotics/teleop.git cd teleop -pip3 install -e . +pip install -e . # Run the tests -python3 -m pytest +python -m pytest ``` diff --git a/teleop/__init__.py b/teleop/__init__.py index eca0836..edc11c4 100644 --- a/teleop/__init__.py +++ b/teleop/__init__.py @@ -260,12 +260,16 @@ def __update(self, message): relative_orientation = received_pose[:3, :3] @ np.linalg.inv( self.__relative_pose_init[:3, :3] ) + + if scale > 1.0: + relative_position *= scale + self.__pose = np.eye(4) self.__pose[:3, 3] = self.__absolute_pose_init[:3, 3] + relative_position self.__pose[:3, :3] = relative_orientation @ self.__absolute_pose_init[:3, :3] # Apply scale - if scale != 1.0: + if scale < 1.0: self.__pose = interpolate_transforms( self.__absolute_pose_init, self.__pose, scale ) diff --git a/teleop/leteleop/README.md b/teleop/leteleop/README.md deleted file mode 100644 index 5d63e21..0000000 --- a/teleop/leteleop/README.md +++ /dev/null @@ -1,5 +0,0 @@ -python -m lerobot.teleoperate \ - --robot.type=lerobot_xarm.xarm.Xarm \ - --robot.id=black \ - --teleop.type=teleop.leteleop.LeTeleop \ - --fps=60 diff --git a/teleop/leteleop/__init__.py b/teleop/leteleop/__init__.py deleted file mode 100644 index 6120e85..0000000 --- a/teleop/leteleop/__init__.py +++ /dev/null @@ -1,91 +0,0 @@ -from typing import Any -from lerobot.teleoperators.teleoperator import Teleoperator -from .config import LeTeleopConfig -from teleop import Teleop -import numpy as np -import threading - - -class LeTeleop(Teleoperator): - config_class = LeTeleopConfig - name = "teleop" - - def __init__(self, config: LeTeleopConfig): - super().__init__(config) - self.config = config - self._connected = False - self._calibrated = True - self._home = False - - self._server = Teleop(host=config.host, port=int(config.port)) - self._server.subscribe(self._on_teleop_callback) - self._gripper_state = 0.0 - self._last_pose = np.eye(4) - self._mutex = threading.Lock() - - def _on_teleop_callback(self, pose, message): - with self._mutex: - if message["move"]: - self._last_pose = pose - else: - self._last_pose = None - - if message["gripper"] is not None: - self._gripper_state = 2.0 if message["gripper"] == "open" else 0.0 - self._home = ( - message["reservedButtonA"] is not None and message["reservedButtonA"] - ) - - @property - def action_features(self) -> dict[str, type]: - if self.config.use_gripper: - return {"pose_from_initial": np.ndarray, "gripper": float, "home": bool} - else: - return { - "pose_from_initial": np.ndarray, - } - - @property - def feedback_features(self) -> dict[str, type]: - return {} - - @property - def is_connected(self) -> bool: - return self._connected - - def connect(self, calibrate: bool = True) -> None: - self._connected = True - threading.Thread(target=self._server.run, daemon=True).start() - - @property - def is_calibrated(self) -> bool: - return self._calibrated - - def calibrate(self) -> None: - pass - - def configure(self) -> None: - pass - - def get_action(self) -> dict[str, Any]: - last_pose = None - with self._mutex: - if self._last_pose is not None: - last_pose = self._last_pose.copy() - - action = {} - if last_pose is not None: - action["pose_from_initial"] = last_pose - if self.config.use_gripper: - action["gripper"] = self._gripper_state - if self._home: - action["home"] = True - return action - - def send_feedback(self, feedback: dict[str, float]) -> None: - # TODO(rcadene, aliberts): Implement force feedback - raise NotImplementedError - - def disconnect(self) -> None: - self._connected = False - self._server.stop() diff --git a/teleop/leteleop/config.py b/teleop/leteleop/config.py deleted file mode 100644 index 2c4a596..0000000 --- a/teleop/leteleop/config.py +++ /dev/null @@ -1,11 +0,0 @@ -from dataclasses import dataclass - -from lerobot.teleoperators.config import TeleoperatorConfig - - -@TeleoperatorConfig.register_subclass("teleop.leteleop.LeTeleop") -@dataclass -class LeTeleopConfig(TeleoperatorConfig): - port: str = "4443" - host: str = "0.0.0.0" - use_gripper: bool = True diff --git a/teleop/teleop-ui.js b/teleop/teleop-ui.js index e01bbe3..3debb6d 100644 --- a/teleop/teleop-ui.js +++ b/teleop/teleop-ui.js @@ -186,6 +186,21 @@ class TeleopUI extends HTMLElement { .info-box { flex: 1; } + + .auxilary-section { + display: flex; + flex-direction: row; + justify-content: center; + gap: 20px; + } + + .scale-section { + width: 300px; + } + + .reserved-section { + width: 200px; + } } @@ -207,9 +222,10 @@ class TeleopUI extends HTMLElement { +
+ min="0" max="5" step="1" value="3">
scale 1.0
@@ -217,6 +233,7 @@ class TeleopUI extends HTMLElement {
A
B
+