From fb0c15a6aaee4f2ebde4068c07f5656c50f398d7 Mon Sep 17 00:00:00 2001 From: dylan Date: Sun, 28 Sep 2025 12:51:18 -0400 Subject: [PATCH 1/2] setup robot abstractions and teleop --- cameras.py | 48 ++++ src/lerobot/robots/meca/collect.py | 134 +++++++++++ src/lerobot/robots/meca/inference.py | 109 +++++++++ src/lerobot/robots/meca/meca.py | 234 +++++++++++++++++++ src/lerobot/robots/meca/mecaconfig.py | 36 +++ src/lerobot/robots/meca/replay.py | 43 ++++ src/lerobot/robots/meca/run.py | 43 ++++ src/lerobot/scripts/lerobot_record.py | 2 + src/lerobot/teleoperators/omni/__init__.py | 0 src/lerobot/teleoperators/omni/controller.py | 127 ++++++++++ src/lerobot/teleoperators/omni/omni.py | 127 ++++++++++ 11 files changed, 903 insertions(+) create mode 100644 cameras.py create mode 100644 src/lerobot/robots/meca/collect.py create mode 100644 src/lerobot/robots/meca/inference.py create mode 100644 src/lerobot/robots/meca/meca.py create mode 100644 src/lerobot/robots/meca/mecaconfig.py create mode 100644 src/lerobot/robots/meca/replay.py create mode 100644 src/lerobot/robots/meca/run.py create mode 100644 src/lerobot/teleoperators/omni/__init__.py create mode 100644 src/lerobot/teleoperators/omni/controller.py create mode 100644 src/lerobot/teleoperators/omni/omni.py diff --git a/cameras.py b/cameras.py new file mode 100644 index 0000000000..59637653e7 --- /dev/null +++ b/cameras.py @@ -0,0 +1,48 @@ +import cv2 +from lerobot.cameras.opencv.configuration_opencv import OpenCVCameraConfig +from lerobot.cameras.opencv.camera_opencv import OpenCVCamera +from lerobot.cameras.configs import ColorMode, Cv2Rotation + +# First camera +config1 = OpenCVCameraConfig( + index_or_path=0, + width=1280, + height=720, + color_mode=ColorMode.RGB, # good for ML + rotation=Cv2Rotation.NO_ROTATION +) +camera1 = OpenCVCamera(config1) +camera1.connect() + +# Second camera +config2 = OpenCVCameraConfig( + index_or_path=3, + width=1280, + height=720, + color_mode=ColorMode.RGB, # also RGB + rotation=Cv2Rotation.ROTATE_180 +) +camera2 = OpenCVCamera(config2) +camera2.connect() + +try: + while True: + # Grab latest available frames (non-blocking) + frame1 = camera1.async_read() + frame2 = camera2.async_read() + + if frame1 is not None: + # convert for OpenCV display (expects BGR) + cv2.imshow("Camera 0", cv2.cvtColor(frame1, cv2.COLOR_RGB2BGR)) + + if frame2 is not None: + cv2.imshow("Camera 3", cv2.cvtColor(frame2, cv2.COLOR_RGB2BGR)) + + # Needed for display refresh + quit on 'q' + if cv2.waitKey(1) & 0xFF == ord("q"): + break + +finally: + camera1.disconnect() + camera2.disconnect() + cv2.destroyAllWindows() diff --git a/src/lerobot/robots/meca/collect.py b/src/lerobot/robots/meca/collect.py new file mode 100644 index 0000000000..aea334c333 --- /dev/null +++ b/src/lerobot/robots/meca/collect.py @@ -0,0 +1,134 @@ +from lerobot.cameras.opencv.configuration_opencv import OpenCVCameraConfig +from lerobot.datasets.lerobot_dataset import LeRobotDataset +from lerobot.datasets.utils import hw_to_dataset_features +from lerobot.utils.control_utils import init_keyboard_listener +from lerobot.utils.utils import log_say +from lerobot.utils.visualization_utils import init_rerun +from lerobot.scripts.lerobot_record import record_loop + +# Import your robot + teleop +from lerobot.robots.meca.meca import Meca +from lerobot.robots.meca.mecaconfig import MecaConfig +from lerobot.teleoperators.omni.omni import OmniTeleoperator, OmniConfig +from lerobot.policies.factory import make_policy, make_pre_post_processors + +# ------------------------ +# Experiment parameters +# ------------------------ +NUM_EPISODES = 5 +FPS = 30 +EPISODE_TIME_SEC = 60 +RESET_TIME_SEC = 10 +TASK_DESCRIPTION = "Microsurgery teleop task" + +# ------------------------ +# Configurations +# ------------------------ + +# Cameras (adapt indices to your setup) +camera_config = { + "top": OpenCVCameraConfig(index_or_path=0, width=640, height=480, fps=FPS), + "bottom": OpenCVCameraConfig(index_or_path=3, width=1280, height=720, fps=260), +} + +# Meca robot config +meca_cfg = MecaConfig( + ip="192.168.0.100", # <-- Replace with your robot IP + id="meca500", + cameras=camera_config, +) + +# Omni haptic device config +omni_cfg = OmniConfig() + +# ------------------------ +# Instantiate +# ------------------------ +robot = Meca(meca_cfg) +teleop = OmniTeleoperator(omni_cfg) + +# Dataset features +action_features = hw_to_dataset_features(robot.action_features, "action") +obs_features = hw_to_dataset_features(robot.observation_features, "observation") +dataset_features = {**action_features, **obs_features} + + + +try: + dataset = LeRobotDataset("dylanmcguir3/meca-needle-pick") + print("šŸ“‚ Loaded existing dataset") +except Exception: + # Otherwise, create it fresh + dataset = LeRobotDataset.create( + repo_id="dylanmcguir3/meca-needle-pick", + fps=FPS, + features=dataset_features, + robot_type=robot.name, + use_videos=True, + image_writer_threads=4, + ) + print("šŸ“‚ Created new dataset") + +# ------------------------ +# Setup utils +# ------------------------ +_, events = init_keyboard_listener() +init_rerun(session_name="meca_teleop_recording") + +# ------------------------ +# Main recording loop +# ------------------------ +robot.connect() +teleop.connect() + +episode_idx = 0 +while episode_idx < NUM_EPISODES and not events["stop_recording"]: + log_say(f"Recording episode {episode_idx + 1} of {NUM_EPISODES}") + + record_loop( + robot=robot, + events=events, + fps=FPS, + teleop=teleop, + dataset=dataset, + control_time_s=EPISODE_TIME_SEC, + single_task=TASK_DESCRIPTION, + display_data=True, + teleop_action_processor=robot.teleop_action_processor, + robot_action_processor=robot.robot_action_processor, + robot_observation_processor=robot.robot_observation_processor, + ) + + # Reset environment if needed + if not events["stop_recording"] and (episode_idx < NUM_EPISODES - 1 or events["rerecord_episode"]): + log_say("Resetting environment...") + record_loop( + robot=robot, + events=events, + fps=FPS, + teleop=teleop, + control_time_s=RESET_TIME_SEC, + single_task=TASK_DESCRIPTION, + display_data=True, + robot_observation_processor=robot.robot_observation_processor, + teleop_action_processor=robot.teleop_action_processor, + robot_action_processor=robot.robot_action_processor, + ) + + if events["rerecord_episode"]: + log_say("Re-recording episode") + events["rerecord_episode"] = False + events["exit_early"] = False + dataset.clear_episode_buffer() + continue + + dataset.save_episode() + episode_idx += 1 + +# ------------------------ +# Cleanup +# ------------------------ +log_say("Stopping recording...") +robot.disconnect() +teleop.disconnect() +dataset.push_to_hub() diff --git a/src/lerobot/robots/meca/inference.py b/src/lerobot/robots/meca/inference.py new file mode 100644 index 0000000000..85e5ab8a20 --- /dev/null +++ b/src/lerobot/robots/meca/inference.py @@ -0,0 +1,109 @@ +from pathlib import Path +import time + +from lerobot.cameras.opencv.configuration_opencv import OpenCVCameraConfig +from lerobot.datasets.lerobot_dataset import LeRobotDataset +from lerobot.datasets.utils import hw_to_dataset_features +from lerobot.policies.diffusion.modeling_diffusion import DiffusionPolicy +from lerobot.robots.meca.mecaconfig import MecaConfig +from lerobot.robots.meca.meca import Meca +from lerobot.utils.control_utils import init_keyboard_listener +from lerobot.utils.utils import log_say +from lerobot.utils.visualization_utils import init_rerun +from lerobot.scripts.lerobot_record import record_loop +from lerobot.policies.factory import make_policy, make_pre_post_processors +import cv2 + + +# ------------------------- +# User settings +# ------------------------- +NUM_EPISODES = 5 +FPS = 30 +EPISODE_TIME_SEC = 60 +TASK_DESCRIPTION = "Microsurgery teleop task" + +# šŸ‘‡ Fill in your actual repos +HF_USER = "dylanmcguir3" +HF_MODEL_ID = f"{HF_USER}/needle-pick" # trained diffusion policy +HF_DATASET_ID = f"{HF_USER}/meca-needle-pick" # dataset to store evaluation rollouts + +# ------------------------- +# Robot + camera config +# ------------------------- +camera_config = { + "top": OpenCVCameraConfig(index_or_path=0, width=640, height=480, fps=FPS), + "bottom": OpenCVCameraConfig(index_or_path=3, width=1280, height=720, fps=260), +} + +robot_config = MecaConfig(ip="192.168.0.100", id="meca_eval_robot", cameras=camera_config) +robot = Meca(robot_config) + +# ------------------------- +# Load trained policy +# ------------------------- +policy = DiffusionPolicy.from_pretrained(Path("/home/dylan/LeRobot/lerobot/outputs/train/needle-pick-diffusion-test/checkpoints/100000/pretrained_model")) + +# ------------------------- +# Dataset features +# ------------------------- +action_features = hw_to_dataset_features(robot.action_features, "action") +obs_features = hw_to_dataset_features(robot.observation_features, "observation") +dataset_features = {**action_features, **obs_features} + +dataset = LeRobotDataset.create( + repo_id=HF_DATASET_ID + "-eval", + fps=FPS, + features=dataset_features, + robot_type=robot.name, + use_videos=True, + image_writer_threads=4, +) + +# ------------------------- +# Keyboard + visualization +# ------------------------- +_, events = init_keyboard_listener() +init_rerun(session_name="policy_eval") + +# ------------------------- +# Connect robot +# ------------------------- +robot.connect() + +# Pre/post processors for policy I/O +preprocessor, postprocessor = make_pre_post_processors(policy, pretrained_path=Path("/home/dylan/LeRobot/lerobot/outputs/train/needle-pick-diffusion-test/checkpoints/100000/pretrained_model"), dataset_stats=dataset.meta.stats) + +# ------------------------- +# Evaluation loop +# ------------------------- +for episode_idx in range(NUM_EPISODES): + log_say(f"Running inference, recording eval episode {episode_idx + 1}/{NUM_EPISODES}") + + record_loop( + robot=robot, + events=events, + fps=FPS, + policy=policy, + preprocessor=preprocessor, + postprocessor=postprocessor, + dataset=dataset, + control_time_s=EPISODE_TIME_SEC, + single_task=TASK_DESCRIPTION, + display_data=True, + teleop_action_processor=robot.teleop_action_processor, + robot_action_processor=robot.robot_action_processor, + robot_observation_processor=robot.robot_observation_processor, + ) + + if cv2.waitKey(1) & 0xFF == ord("r"): + robot.reset() + + + dataset.save_episode() + +# ------------------------- +# Cleanup +# ------------------------- +robot.disconnect() +dataset.push_to_hub() diff --git a/src/lerobot/robots/meca/meca.py b/src/lerobot/robots/meca/meca.py new file mode 100644 index 0000000000..d7756d167c --- /dev/null +++ b/src/lerobot/robots/meca/meca.py @@ -0,0 +1,234 @@ +from matplotlib import pyplot as plt +import numpy as np +from lerobot.robots.meca.mecaconfig import MecaConfig +from mecademicpy.robot import Robot as MecademicRobot +from lerobot.robots import Robot +from lerobot.cameras import make_cameras_from_configs +from typing import Any +import cv2 + + +class Meca(Robot): + config_class = MecaConfig + name = "meca" + + def __init__(self, config: MecaConfig): + super().__init__(config) + self.robot = MecademicRobot() + self.cameras = make_cameras_from_configs(config.cameras) + self.connected = False + self.gripper_state = 0 # Assume gripper starts open + self.ip = config.ip + + + @property + def _ee_pose(self) -> dict[str, type]: + return { + "x" : float, + "y" : float, + "z" : float, + "a" : float, + "b" : float, + "g" : float, + "gripper" : float + } + + @property + def _ee_pose_delta(self) -> dict[str, type]: + return { + "dx" : float, + "dy" : float, + "dz" : float, + "da" : float, + "db" : float, + "dg" : float, + "gripper" : float + } + + @property + def _cameras_ft(self) -> dict[str, tuple]: + return { + cam: (224, 224, 3) for cam in self.cameras + } + + @property + def observation_features(self) -> dict: + return {**self._ee_pose, **self._cameras_ft} + + @property + def action_features(self) -> dict: + return self._ee_pose_delta + + def connect(self, calibrate: bool = True) -> None: + self.robot.Connect(self.ip) + self.robot.ResetError() + + if self.robot.GetStatusRobot(True).homing_state == 0: + print("āš ļø Robot not homed, homing now...") + self.robot.ActivateAndHome() + self.robot.WaitHomed() + else: + self.robot.ResumeMotion() + self.robot.SetJointVel(10) + self.robot.SetJointAcc(50) + self.robot.SetCartAcc(100) + self.robot.MoveJoints(1, 44, 17, 1, -30, 0) + print("šŸ¤– Robot ready.") + + + # šŸ”‘ Connect all cameras here + for cam in self.cameras.values(): + try: + cam.connect() + print(f"šŸ“· Connected {cam}") + except Exception as e: + print(f"āš ļø Failed to connect {cam}: {e}") + + self.connected = True + + + + def disconnect(self) -> None: + try: + self.robot.Disconnect() + except Exception as e: + print(f"āš ļø Robot disconnect error: {e}") + + for cam in self.cameras.values(): + if getattr(cam, "is_connected", False): + try: + cam.disconnect() + except Exception as e: + print(f"āš ļø Camera disconnect error: {e}") + self.connected = False + + def reset(self) -> None: + if not self.connected: + raise RuntimeError("Robot not connected. Call connect() before resetting.") + self.robot.ResetError() + self.robot.ActivateAndHome() + self.robot.WaitHomed() + self.robot.ResumeMotion() + self.robot.MoveJoints(1, 44, 17, 1, -30, 0) + self.gripper_state = 0 # Assume gripper starts open + print("šŸ¤– Robot reset and ready.") + + @property + def is_connected(self) -> bool: + return self.connected + + @property + def is_calibrated(self) -> bool: + return True # No calibration needed for Meca500 + + def calibrate(self) -> None: + pass # No calibration needed for Meca500 + + def configure(self) -> None: + """No-op configuration for Meca500 (already configured on connect).""" + pass + + def get_observation(self) -> dict: + obs = {} + pose = np.array(self.robot.GetPose(), dtype=np.float32) + obs.update({ + "x": pose[0], + "y": pose[1], + "z": pose[2], + "a": pose[3], + "b": pose[4], + "g": pose[5], + "gripper": self.gripper_state + }) + for cam_name, cam in self.cameras.items(): + frame = cam.async_read(timeout_ms=100) + if frame is not None: + obs[cam_name] = frame + return obs + + def send_action(self, action: dict[str, Any]) -> dict[str, Any]: + if not self.connected: + raise RuntimeError("Robot not connected. Call connect() before sending actions.") + + dx = action.get("dx", 0.0) + dy = action.get("dy", 0.0) + dz = action.get("dz", 0.0) + da = action.get("da", 0.0) + db = action.get("db", 0.0) + dg = action.get("dg", 0.0) + gripper = action.get("gripper", 0) + + # āš”ļø Non-blocking relative motion + self.robot.MoveLinRelWRF(-dz, -dx, dy, -dg, da, -db) + + # Gripper toggle (edge detection recommended) + if gripper is not None: + if gripper and self.gripper_state == 0: + self.robot.GripperClose() + self.gripper_state = 1 + elif not gripper and self.gripper_state == 1: + self.robot.GripperOpen() + self.gripper_state = 0 + + return self.get_observation() + + def center_crop(self, img, output_size=(224, 224)): + """ + Center crop + resize an image. + Args: + img: numpy array (H, W, C) + output_size: tuple (h, w) for final size + Returns: + Cropped and resized numpy array + """ + h, w = img.shape[:2] + new_h, new_w = min(h, w), min(h, w) # square crop + + # Top-left corner of the crop + top = (h - new_h) // 2 + left = (w - new_w) // 2 + + # Crop and resize + cropped = img[top:top + new_h, left:left + new_w] + return cv2.resize(cropped, output_size) + + + def teleop_action_processor(self, action: tuple) -> dict: + # Already in the right shape from OmniTeleoperator + return action[0] + + def robot_observation_processor(self, obs: tuple) -> dict: + processed = dict(obs) # shallow copy + for cam in ["top", "bottom"]: + if cam in processed: + img = processed[cam] + + # Try explicit color conversion (MJPEG/YUYV ā†’ BGR) + try: + img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) + # Debug visualization + except cv2.error: + # If conversion fails, just leave as-is + pass + + # Crop + processed[cam] = self.center_crop(img, (224, 224)) + + # Flip bottom + if cam == "bottom": + processed[cam] = cv2.flip(processed[cam], -1) + else: + cv2.imshow("flipped", processed[cam]) + cv2.waitKey(1) + + + + return processed + + + def robot_action_processor(self, action: tuple) -> dict: + # No processing needed for Meca500 + return action[0] + + + diff --git a/src/lerobot/robots/meca/mecaconfig.py b/src/lerobot/robots/meca/mecaconfig.py new file mode 100644 index 0000000000..99a889cee3 --- /dev/null +++ b/src/lerobot/robots/meca/mecaconfig.py @@ -0,0 +1,36 @@ +from dataclasses import dataclass, field + +from lerobot.cameras import CameraConfig +from lerobot.cameras.opencv import OpenCVCameraConfig +from lerobot.robots import RobotConfig +from lerobot.cameras.configs import ColorMode, Cv2Rotation + + +@RobotConfig.register_subclass("meca") +@dataclass +@RobotConfig.register_subclass("meca") +@dataclass +class MecaConfig(RobotConfig): + cameras: dict[str, CameraConfig] = field( + default_factory=lambda: { + "cam_1": OpenCVCameraConfig( + index_or_path=0, + width=640, + height=360, + fps=30, + color_mode=ColorMode.RGB, + rotation=Cv2Rotation.NO_ROTATION, + ), + "cam_2": OpenCVCameraConfig( + index_or_path=3, + width=640, + height=360, + fps=260, + color_mode=ColorMode.RGB, + rotation=Cv2Rotation.ROTATE_180, + ), + } + ) + ip: str = "192.168.0.100" # default Meca500 IP + port: int = 3000 # default control port + diff --git a/src/lerobot/robots/meca/replay.py b/src/lerobot/robots/meca/replay.py new file mode 100644 index 0000000000..9276a8290d --- /dev/null +++ b/src/lerobot/robots/meca/replay.py @@ -0,0 +1,43 @@ +from lerobot.datasets.lerobot_dataset import LeRobotDataset +from lerobot.utils.robot_utils import busy_wait +from lerobot.utils.utils import log_say +from lerobot.robots.meca.mecaconfig import MecaConfig +from lerobot.robots.meca.meca import Meca + +import time + +# Setup your Meca +meca_cfg = MecaConfig(ip="192.168.0.100") +robot = Meca(meca_cfg) +robot.connect() + +# Load dataset +dataset = LeRobotDataset("dylanmcguir3/meca-needle-pick") +actions = dataset.hf_dataset.select_columns("action") + +# Choose which episodes to replay +episodes_to_replay = [0, 1] # put indices here +RESET_TIME_SEC = 5 # how long to wait during reset + +for episode_idx in episodes_to_replay: + log_say(f"Replaying episode {episode_idx}") + episode = dataset.hf_dataset.filter(lambda e: e["episode_index"] == episode_idx) + + for idx in range(len(episode)): + t0 = time.perf_counter() + + action = { + name: float(episode[idx]["action"][i]) + for i, name in enumerate(dataset.features["action"]["names"]) + } + print(f"Step {idx+1}/{len(episode)}, Action: {action}") + robot.send_action(action) + + busy_wait(1.0 / dataset.fps - (time.perf_counter() - t0)) + + # Reset between episodes + robot.reset() + # Optionally send robot to home pose + # robot.reset() # uncomment if you have a reset method + +robot.disconnect() diff --git a/src/lerobot/robots/meca/run.py b/src/lerobot/robots/meca/run.py new file mode 100644 index 0000000000..6278fdab0c --- /dev/null +++ b/src/lerobot/robots/meca/run.py @@ -0,0 +1,43 @@ +""" +run.py + +Demo: Teleoperate the Meca500 with a Phantom Omni haptic device +""" + +import time +import mecaconfig +from meca import Meca +from lerobot.teleoperators.omni.omni import OmniTeleoperator, OmniConfig + + + +def main(): + # --- Configs --- + meca_cfg = mecaconfig.MecaConfig(ip="192.168.0.100") # adjust IP if different + omni_cfg = OmniConfig(scale_translation=.4, scale_rotation=1) + + # --- Init devices --- + robot = Meca(meca_cfg) + teleop = OmniTeleoperator(omni_cfg) + + try: + robot.connect() + teleop.connect() + + print("šŸ¤– Teleop loop started. Press Ctrl+C to quit.") + + while True: + observation = robot.get_observation() + action = teleop.get_action() + robot.send_action(action) + + + except KeyboardInterrupt: + print("šŸ›‘ Stopping teleoperation.") + finally: + teleop.disconnect() + robot.disconnect() + + +if __name__ == "__main__": + main() diff --git a/src/lerobot/scripts/lerobot_record.py b/src/lerobot/scripts/lerobot_record.py index d097a9d2f4..ecf4ed2c59 100644 --- a/src/lerobot/scripts/lerobot_record.py +++ b/src/lerobot/scripts/lerobot_record.py @@ -64,6 +64,7 @@ from pathlib import Path from pprint import pformat from typing import Any +import cv2 from lerobot.cameras import ( # noqa: F401 CameraConfig, # noqa: F401 @@ -357,6 +358,7 @@ def record_loop( # Action can eventually be clipped using `max_relative_target`, # so action actually sent is saved in the dataset. action = postprocessor.process(action) # TODO(steven, pepijn, adil): we should use a pipeline step to clip the action, so the sent action is the action that we input to the robot. + print(f"Sending action to robot: {robot_action_to_send}") _sent_action = robot.send_action(robot_action_to_send) # Write to dataset diff --git a/src/lerobot/teleoperators/omni/__init__.py b/src/lerobot/teleoperators/omni/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/lerobot/teleoperators/omni/controller.py b/src/lerobot/teleoperators/omni/controller.py new file mode 100644 index 0000000000..c540fb284a --- /dev/null +++ b/src/lerobot/teleoperators/omni/controller.py @@ -0,0 +1,127 @@ +""" +controller.py + +Manages input from a haptic device (via pyOpenHaptics). +Provides a DeviceState object with latest transform + button state. +""" + +import time +import numpy as np +from dataclasses import dataclass +from pyOpenHaptics.hd_device import HapticDevice +from pyOpenHaptics.hd_callback import hd_callback +from pyOpenHaptics.hd import get_transform, get_buttons +from scipy.spatial.transform import Rotation as R + + +# ------------------------ +# Shared Device State +# ------------------------ + +@dataclass +class DeviceState: + transform: object = None + buttons: int = 0 + + +device_state = DeviceState() # global state container + + +# ------------------------ +# Utilities +# ------------------------ + +def transform_to_numpy(matrix): + """Convert transform matrix (4x4) into np.array.""" + return np.array([[matrix[i][j] for j in range(4)] for i in range(4)], dtype=float) + + +def extract_position(matrix): + T = np.array([[matrix[i][j] for j in range(4)] for i in range(4)]) + pos = T[3, :3] # If position is in the last row return pos, T + return pos, T + + +def extract_deltas(matrix, prev_hand, prev_R, sr, st, max_step=10.0, max_angle=10.0): + """Compute relative translation and orientation deltas from haptic transform.""" + T = np.array([[matrix[i][j] for j in range(4)] for i in range(4)]) + pos = T[3, :3] # last row = position + Rmat = T[:3, :3] # 3x3 rotation + + if prev_hand is None or prev_R is None: + return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0), pos.copy(), Rmat.copy() + + # --- translation delta --- + dh = (pos - prev_hand) * st + dx = dh[0] + dy = dh[1] + dz = dh[2] + + # --- orientation delta --- + dR = Rmat @ prev_R.T + rotvec = R.from_matrix(dR).as_rotvec(degrees=True) * sr + dĪ±, dĪ², dĪ³ = rotvec # roll, pitch, yaw changes + + return (dx, dy, dz, dĪ±, dĪ², dĪ³), pos.copy(), Rmat.copy() + + + +# ------------------------ +# Callback +# ------------------------ + +@hd_callback +def device_callback(): + """Callback executed by pyOpenHaptics scheduler.""" + global device_state + device_state.transform = get_transform() + device_state.buttons = get_buttons() + return True + + +# ------------------------ +# Interface +# ------------------------ + +class Controller: + """ + Wrapper around HapticDevice that provides latest state via .get_state(). + """ + + def __init__(self): + self.device = HapticDevice(callback=device_callback, scheduler_type="async") + + def get_state(self): + """Return (position[3], transform[4x4], buttons).""" + if device_state.transform is None: + return None + pos, T = extract_position(device_state.transform) + return pos, T, device_state.buttons + + def close(self): + self.device.close() + +device = HapticDevice(callback=device_callback, scheduler_type="async") + + + +# ------------------------ +# Standalone test +# ------------------------ + +if __name__ == "__main__": + ctrl = Controller() + try: + while True: + state = ctrl.get_state() + if state: + pos, T, buttons = state + print("Position:", pos) + print("Transform:\n", T) + print("Buttons:", buttons) + print("-" * 40) + time.sleep(0.1) + except KeyboardInterrupt: + print("Shutting down...") + finally: + ctrl.close() diff --git a/src/lerobot/teleoperators/omni/omni.py b/src/lerobot/teleoperators/omni/omni.py new file mode 100644 index 0000000000..060154d75a --- /dev/null +++ b/src/lerobot/teleoperators/omni/omni.py @@ -0,0 +1,127 @@ +from pathlib import Path +from attrs import field +import numpy as np +from dataclasses import dataclass +from typing import Any +from lerobot.teleoperators import Teleoperator + +# Import your existing controller +from .controller import Controller, extract_deltas + + +@dataclass +class OmniConfig: + device_name: str = "PhantomOmni" + # scaling factors for translation (mm ā†’ robot mm) and rotation (deg ā†’ robot deg) + scale_translation: float = 1.0 + scale_rotation: float = .5 + max_step: float = 2.0 + max_angle: float = 2.0 + id: int = 0 # device ID if multiple devices connected + calibration_dir: Path = Path("~/.lerobot/calibration/omni").expanduser() + + + +class OmniTeleoperator(Teleoperator): + config_class = OmniConfig + name = "omni" + + def __init__(self, config: OmniConfig): + super().__init__(config) + self.ctrl = Controller() + self.connected = True + self.prev_hand = None + self.prev_R = None + self.prev_buttons = 0 + self.motion_scale = 1.0 + self.gripper_state = False # toggle state + self.config = config + # ------------------------ + # Required abstract stubs + # ------------------------ + def configure(self) -> None: + """No configuration needed for Omni.""" + pass + + def calibrate(self) -> None: + """No calibration needed for Omni.""" + pass + + @property + def is_calibrated(self) -> bool: + return True + + @property + def feedback_features(self) -> dict: + """No haptic feedback features exposed yet.""" + return {} + + # ------------------------ + # Core interface + # ------------------------ + @property + def action_features(self) -> dict[str, type]: + return { + "dx": float, + "dy": float, + "dz": float, + "da": float, + "db": float, + "dg": float, + "gripper": bool, + } + + def connect(self) -> None: + self.connected = True + + def disconnect(self) -> None: + if self.connected: + try: + self.ctrl.close() + except Exception as e: + print(f"āš ļø Error while closing haptic device: {e}") + self.connected = False + + @property + def is_connected(self) -> bool: + return self.connected + + def get_action(self) -> dict[str, Any]: + state = self.ctrl.get_state() + if not state: + return {} + pos, T, buttons = state + + # --- Button edge detection --- + if buttons != self.prev_buttons: + self.prev_buttons = buttons + if buttons & 1: # Button 1 toggles gripper + self.gripper_state = not self.gripper_state + print("Toggled gripper:", self.gripper_state) + if buttons & 2: # Button 2 toggles motion scaling + self.motion_scale = 0.1 if self.motion_scale == 1 else 1 + print("Motion scale set to", self.motion_scale) + + # deltas + deltas, self.prev_hand, self.prev_R = extract_deltas( + T, self.prev_hand, self.prev_R, + sr=self.config.scale_rotation, + st=self.config.scale_translation, + max_step=self.config.max_step, + max_angle=self.config.max_angle, + ) + dx, dy, dz, da, db, dg = deltas + + return { + "dx": dx * self.motion_scale, + "dy": dy * self.motion_scale, + "dz": dz * self.motion_scale, + "da": da * self.motion_scale, + "db": db * self.motion_scale, + "dg": dg * self.motion_scale, + "gripper": self.gripper_state, + } + + def send_feedback(self, feedback: dict[str, Any]) -> None: + # Optionally implement force feedback if pyOpenHaptics exposes it + pass From 2903dcb7f308f6a3bc7dea4a1ccbb873fe83be83 Mon Sep 17 00:00:00 2001 From: dylan Date: Sun, 28 Sep 2025 16:24:53 -0400 Subject: [PATCH 2/2] data collection infra --- src/lerobot/robots/meca/act_inference.py | 112 +++++++++++++++++++++++ src/lerobot/robots/meca/collect.py | 30 +++--- src/lerobot/robots/meca/inference.py | 6 +- src/lerobot/robots/meca/meca.py | 24 +++-- src/lerobot/scripts/lerobot_record.py | 1 + 5 files changed, 146 insertions(+), 27 deletions(-) create mode 100644 src/lerobot/robots/meca/act_inference.py diff --git a/src/lerobot/robots/meca/act_inference.py b/src/lerobot/robots/meca/act_inference.py new file mode 100644 index 0000000000..de0a5ac3aa --- /dev/null +++ b/src/lerobot/robots/meca/act_inference.py @@ -0,0 +1,112 @@ +from pathlib import Path +import time + +from lerobot.cameras.opencv.configuration_opencv import OpenCVCameraConfig +from lerobot.datasets.lerobot_dataset import LeRobotDataset +from lerobot.datasets.utils import hw_to_dataset_features +from lerobot.policies.act.modeling_act import ACTPolicy # šŸ‘ˆ change here +from lerobot.robots.meca.mecaconfig import MecaConfig +from lerobot.robots.meca.meca import Meca +from lerobot.utils.control_utils import init_keyboard_listener +from lerobot.utils.utils import log_say +from lerobot.utils.visualization_utils import init_rerun +from lerobot.scripts.lerobot_record import record_loop +from lerobot.policies.factory import make_policy, make_pre_post_processors +import cv2 + + +# ------------------------- +# User settings +# ------------------------- +NUM_EPISODES = 5 +FPS = 30 +EPISODE_TIME_SEC = 60 +TASK_DESCRIPTION = "Microsurgery teleop task" + +# šŸ‘‡ Fill in your actual repos +HF_USER = "dylanmcguir3" +HF_MODEL_ID = f"{HF_USER}/needle-pick-act" # ACT policy model +HF_DATASET_ID = f"{HF_USER}/meca-needle-pick" # dataset to store evaluation rollouts + +# ------------------------- +# Robot + camera config +# ------------------------- +camera_config = { + "top": OpenCVCameraConfig(index_or_path=0, width=640, height=480, fps=FPS), + "bottom": OpenCVCameraConfig(index_or_path=2, width=1280, height=720, fps=260), +} + +robot_config = MecaConfig(ip="192.168.0.100", id="meca_eval_robot", cameras=camera_config) +robot = Meca(robot_config) + +# ------------------------- +# Load trained ACT policy +# ------------------------- +policy = ACTPolicy.from_pretrained(Path("/home/dylan/LeRobot/lerobot/outputs/train/needle-pick-act-test/checkpoints/100000/pretrained_model")) + +# ------------------------- +# Dataset features +# ------------------------- +action_features = hw_to_dataset_features(robot.action_features, "action") +obs_features = hw_to_dataset_features(robot.observation_features, "observation") +dataset_features = {**action_features, **obs_features} + +dataset = LeRobotDataset.create( + repo_id=HF_DATASET_ID + "-act-eval", + fps=FPS, + features=dataset_features, + robot_type=robot.name, + use_videos=True, + image_writer_threads=4, +) + +# ------------------------- +# Keyboard + visualization +# ------------------------- +_, events = init_keyboard_listener() +init_rerun(session_name="policy_eval_act") + +# ------------------------- +# Connect robot +# ------------------------- +robot.connect() + +# Pre/post processors for ACT policy +preprocessor, postprocessor = make_pre_post_processors( + policy, + pretrained_path=Path("/home/dylan/LeRobot/lerobot/outputs/train/needle-pick-act-test/checkpoints/100000/pretrained_model"), + dataset_stats=dataset.meta.stats +) + +# ------------------------- +# Evaluation loop +# ------------------------- +for episode_idx in range(NUM_EPISODES): + log_say(f"Running inference, recording ACT eval episode {episode_idx + 1}/{NUM_EPISODES}") + + record_loop( + robot=robot, + events=events, + fps=FPS, + policy=policy, + preprocessor=preprocessor, + postprocessor=postprocessor, + dataset=dataset, + control_time_s=EPISODE_TIME_SEC, + single_task=TASK_DESCRIPTION, + display_data=True, + teleop_action_processor=robot.teleop_action_processor, + robot_action_processor=robot.robot_action_processor, + robot_observation_processor=robot.robot_observation_processor, + ) + + if cv2.waitKey(1) & 0xFF == ord("r"): + robot.reset() + + dataset.save_episode() + +# ------------------------- +# Cleanup +# ------------------------- +robot.disconnect() +dataset.push_to_hub() diff --git a/src/lerobot/robots/meca/collect.py b/src/lerobot/robots/meca/collect.py index aea334c333..c0e9dbe5f8 100644 --- a/src/lerobot/robots/meca/collect.py +++ b/src/lerobot/robots/meca/collect.py @@ -15,7 +15,7 @@ # ------------------------ # Experiment parameters # ------------------------ -NUM_EPISODES = 5 +NUM_EPISODES = 10 FPS = 30 EPISODE_TIME_SEC = 60 RESET_TIME_SEC = 10 @@ -28,7 +28,7 @@ # Cameras (adapt indices to your setup) camera_config = { "top": OpenCVCameraConfig(index_or_path=0, width=640, height=480, fps=FPS), - "bottom": OpenCVCameraConfig(index_or_path=3, width=1280, height=720, fps=260), + "bottom": OpenCVCameraConfig(index_or_path=2, width=1280, height=720, fps=260), } # Meca robot config @@ -54,20 +54,20 @@ -try: - dataset = LeRobotDataset("dylanmcguir3/meca-needle-pick") - print("šŸ“‚ Loaded existing dataset") -except Exception: +# try: +# dataset = LeRobotDataset("dylanmcguir3/meca-needle-pick-lr") +# print("šŸ“‚ Loaded existing dataset") +# except Exception: # Otherwise, create it fresh - dataset = LeRobotDataset.create( - repo_id="dylanmcguir3/meca-needle-pick", - fps=FPS, - features=dataset_features, - robot_type=robot.name, - use_videos=True, - image_writer_threads=4, - ) - print("šŸ“‚ Created new dataset") +dataset = LeRobotDataset.create( + repo_id="dylanmcguir3/meca-needle-pick-lr", + fps=FPS, + features=dataset_features, + robot_type=robot.name, + use_videos=True, + image_writer_threads=4, +) +print("šŸ“‚ Created new dataset") # ------------------------ # Setup utils diff --git a/src/lerobot/robots/meca/inference.py b/src/lerobot/robots/meca/inference.py index 85e5ab8a20..53bd26e4ff 100644 --- a/src/lerobot/robots/meca/inference.py +++ b/src/lerobot/robots/meca/inference.py @@ -33,7 +33,7 @@ # ------------------------- camera_config = { "top": OpenCVCameraConfig(index_or_path=0, width=640, height=480, fps=FPS), - "bottom": OpenCVCameraConfig(index_or_path=3, width=1280, height=720, fps=260), + "bottom": OpenCVCameraConfig(index_or_path=2, width=1280, height=720, fps=260), } robot_config = MecaConfig(ip="192.168.0.100", id="meca_eval_robot", cameras=camera_config) @@ -42,7 +42,7 @@ # ------------------------- # Load trained policy # ------------------------- -policy = DiffusionPolicy.from_pretrained(Path("/home/dylan/LeRobot/lerobot/outputs/train/needle-pick-diffusion-test/checkpoints/100000/pretrained_model")) +policy = DiffusionPolicy.from_pretrained(Path("/home/dylan/LeRobot/lerobot/outputs/train/needle-pick-diffusion-test/checkpoints/040000/pretrained_model")) # ------------------------- # Dataset features @@ -72,7 +72,7 @@ robot.connect() # Pre/post processors for policy I/O -preprocessor, postprocessor = make_pre_post_processors(policy, pretrained_path=Path("/home/dylan/LeRobot/lerobot/outputs/train/needle-pick-diffusion-test/checkpoints/100000/pretrained_model"), dataset_stats=dataset.meta.stats) +preprocessor, postprocessor = make_pre_post_processors(policy, pretrained_path=Path("/home/dylan/LeRobot/lerobot/outputs/train/needle-pick-diffusion-test/checkpoints/040000/pretrained_model"), dataset_stats=dataset.meta.stats) # ------------------------- # Evaluation loop diff --git a/src/lerobot/robots/meca/meca.py b/src/lerobot/robots/meca/meca.py index d7756d167c..9075a73e89 100644 --- a/src/lerobot/robots/meca/meca.py +++ b/src/lerobot/robots/meca/meca.py @@ -17,6 +17,7 @@ def __init__(self, config: MecaConfig): self.robot = MecademicRobot() self.cameras = make_cameras_from_configs(config.cameras) self.connected = False + self.resetting = False self.gripper_state = 0 # Assume gripper starts open self.ip = config.ip @@ -60,6 +61,7 @@ def action_features(self) -> dict: return self._ee_pose_delta def connect(self, calibrate: bool = True) -> None: + self.resetting = True self.robot.Connect(self.ip) self.robot.ResetError() @@ -72,10 +74,12 @@ def connect(self, calibrate: bool = True) -> None: self.robot.SetJointVel(10) self.robot.SetJointAcc(50) self.robot.SetCartAcc(100) + + self.robot.GripperOpen() self.robot.MoveJoints(1, 44, 17, 1, -30, 0) + print("šŸ¤– Robot ready.") - # šŸ”‘ Connect all cameras here for cam in self.cameras.values(): try: @@ -85,6 +89,7 @@ def connect(self, calibrate: bool = True) -> None: print(f"āš ļø Failed to connect {cam}: {e}") self.connected = True + self.resetting = False @@ -113,6 +118,13 @@ def reset(self) -> None: self.gripper_state = 0 # Assume gripper starts open print("šŸ¤– Robot reset and ready.") + def move_rest_position(self) -> dict: + self.resetting = True + self.robot.MoveJoints(1, 44, 17, 1, -30, 0) + self.robot.GripperOpen() + self.gripper_state = 0 + self.resetting = False + @property def is_connected(self) -> bool: return self.connected @@ -149,6 +161,8 @@ def get_observation(self) -> dict: def send_action(self, action: dict[str, Any]) -> dict[str, Any]: if not self.connected: raise RuntimeError("Robot not connected. Call connect() before sending actions.") + if self.resetting: + return self.get_observation() # Ignore actions while resetting dx = action.get("dx", 0.0) dy = action.get("dy", 0.0) @@ -203,13 +217,6 @@ def robot_observation_processor(self, obs: tuple) -> dict: if cam in processed: img = processed[cam] - # Try explicit color conversion (MJPEG/YUYV ā†’ BGR) - try: - img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) - # Debug visualization - except cv2.error: - # If conversion fails, just leave as-is - pass # Crop processed[cam] = self.center_crop(img, (224, 224)) @@ -218,7 +225,6 @@ def robot_observation_processor(self, obs: tuple) -> dict: if cam == "bottom": processed[cam] = cv2.flip(processed[cam], -1) else: - cv2.imshow("flipped", processed[cam]) cv2.waitKey(1) diff --git a/src/lerobot/scripts/lerobot_record.py b/src/lerobot/scripts/lerobot_record.py index ecf4ed2c59..aa75fdddb4 100644 --- a/src/lerobot/scripts/lerobot_record.py +++ b/src/lerobot/scripts/lerobot_record.py @@ -296,6 +296,7 @@ def record_loop( if events["exit_early"]: events["exit_early"] = False + robot.move_rest_position() break # Get robot observation