style nit

- Introduced `LeaderFollowerProcessor` for managing leader-follower teleoperation logic, including position tracking and end-effector action computation.
- Added `SO101LeaderFollower` class to extend the SO101 leader functionality, enabling both leading and following modes with keyboard event handling for intervention control.
- updated docs

docs/source/hilserl.mdx

@@ -127,11 +127,11 @@ class RewardClassifierConfig:
 # Dataset configuration
 class DatasetConfig:
     repo_id: str    # LeRobot dataset repository ID
-    dataset_root: str    # Local dataset root directory
     task: str    # Task identifier
-    num_episodes: int    # Number of episodes for recording
-    episode: int    # Episode index for replay
-    push_to_hub: bool    # Whether to push datasets to Hub
+    root: str | None = None    # Local dataset root directory
+    num_episodes_to_record: int = 5    # Number of episodes for recording
+    replay_episode: int | None = None    # Episode index for replay
+    push_to_hub: bool = False    # Whether to push datasets to Hub
 ```
 <!-- prettier-ignore-end -->
 
@@ -320,6 +320,8 @@ python -m lerobot.scripts.find_joint_limits \
     --teleop.id=blue
 ```
 
+Note: You can also use `so101_leader` as the teleop type if you have the SO101 leader arm with reduced gears for smoother teleoperation.
+
 **Workflow**
 
 1. Run the script and move the robot through the space that solves the task
@@ -351,7 +353,7 @@ Create a configuration file for recording demonstrations (or edit an existing on
 
 1. Set `mode` to `"record"` at the root level
 2. Specify a unique `repo_id` for your dataset in the `dataset` section (e.g., "username/task_name")
-3. Set `num_episodes` in the `dataset` section to the number of demonstrations you want to collect
+3. Set `num_episodes_to_record` in the `dataset` section to the number of demonstrations you want to collect
 4. Set `env.processor.image_preprocessing.crop_params_dict` to `{}` initially (we'll determine crops later)
 5. Configure `env.robot`, `env.teleop`, and other hardware settings in the `env` section
 
@@ -390,10 +392,10 @@ Example configuration section:
   },
   "dataset": {
     "repo_id": "username/pick_lift_cube",
-    "dataset_root": null,
+    "root": null,
     "task": "pick_and_lift",
-    "num_episodes": 15,
-    "episode": 0,
+    "num_episodes_to_record": 15,
+    "replay_episode": 0,
     "push_to_hub": true
   },
   "mode": "record",
@@ -476,7 +478,7 @@ To setup the gamepad, you need to set the `control_mode` to `"gamepad"` and defi
 
 The SO101 leader arm has reduced gears that allows it to move and track the follower arm during exploration. Therefore, taking over is much smoother than the gearless SO100.
 
-To setup the SO101 leader, you need to set the `control_mode` to `"leader"` and define the `teleop` section in the configuration file.
+To setup the SO101 leader, you need to set the `control_mode` to `"leader"` and define the `teleop` section in the configuration file with `leader_follower_mode` enabled:
 
 ```json
 {
@@ -484,7 +486,8 @@ To setup the SO101 leader, you need to set the `control_mode` to `"leader"` and
     "teleop": {
       "type": "so101_leader",
       "port": "/dev/tty.usbmodem585A0077921",
-      "use_degrees": true
+      "use_degrees": true,
+      "leader_follower_mode": true
     },
     "processor": {
       "control_mode": "leader",
@@ -496,6 +499,11 @@ To setup the SO101 leader, you need to set the `control_mode` to `"leader"` and
 }
 ```
 
+The `leader_follower_mode` enables the leader arm to automatically track the follower's position when you're not intervening. This creates a seamless teleoperation experience where:
+
+- When not intervening: the leader arm follows the follower arm's position
+- When intervening (press `space`): you control the leader arm, and the follower tracks it in end-effector space
+
 In order to annotate the success/failure of the episode, **you will need** to use a keyboard to press `s` for success, `esc` for failure.
 During the online training, press `space` to take over the policy and `space` again to give the control back to the policy.
 
@@ -626,7 +634,7 @@ python -m lerobot.scripts.rl.gym_manipulator --config_path src/lerobot/configs/r
 
 - **mode**: set it to `"record"` to collect a dataset (at root level)
 - **dataset.repo_id**: `"hf_username/dataset_name"`, name of the dataset and repo on the hub
-- **dataset.num_episodes**: Number of episodes to record
+- **dataset.num_episodes_to_record**: Number of episodes to record
 - **env.processor.reset.terminate_on_success**: Whether to automatically terminate episodes when success is detected (default: `true`)
 - **env.fps**: Number of frames per second to record
 - **dataset.push_to_hub**: Whether to push the dataset to the hub
@@ -664,8 +672,8 @@ Example configuration section for data collection:
     "repo_id": "hf_username/dataset_name",
     "dataset_root": "data/your_dataset",
     "task": "reward_classifier_task",
-    "num_episodes": 20,
-    "episode": 0,
+    "num_episodes_to_record": 20,
+    "replay_episode": null,
     "push_to_hub": true
   },
   "mode": "record",

docs/source/hilserl_sim.mdx

@@ -107,10 +107,10 @@ To collect a dataset, set the mode to `record` whilst defining the repo_id and n
   },
   "dataset": {
     "repo_id": "username/sim_dataset",
-    "dataset_root": null,
+    "root": null,
     "task": "pick_cube",
-    "num_episodes": 10,
-    "episode": 0,
+    "num_episodes_to_record": 10,
+    "replay_episode": null,
     "push_to_hub": true
   },
   "mode": "record"

docs/source/il_sim.mdx

@@ -36,10 +36,10 @@ To teleoperate and collect a dataset, we need to modify this config file. Here's
   },
   "dataset": {
     "repo_id": "your_username/il_gym",
-    "dataset_root": null,
+    "root": null,
     "task": "pick_cube",
-    "num_episodes": 30,
-    "episode": 0,
+    "num_episodes_to_record": 30,
+    "replay_episode": null,
     "push_to_hub": true
   },
   "mode": "record",
@@ -50,7 +50,7 @@ To teleoperate and collect a dataset, we need to modify this config file. Here's
 Key configuration points:
 
 - Set your `repo_id` in the `dataset` section: `"repo_id": "your_username/il_gym"`
-- Set `num_episodes: 30` to collect 30 demonstration episodes
+- Set `num_episodes_to_record: 30` to collect 30 demonstration episodes
 - Ensure `mode` is set to `"record"`
 - If you don't have an NVIDIA GPU, change `"device": "cuda"` to `"mps"` for macOS or `"cpu"`
 - To use keyboard instead of gamepad, change `"task"` to `"PandaPickCubeKeyboard-v0"`

src/lerobot/processor/__init__.py

@@ -15,20 +15,20 @@
 # limitations under the License.
 
 from .batch_processor import ToBatchProcessor
-from .delta_action_processor import MapDeltaActionToRobotAction
+from .delta_action_processor import MapDeltaActionToRobotAction, MapTensorToDeltaActionDict
 from .device_processor import DeviceProcessor
+from .gym_action_processor import Numpy2TorchActionProcessor, Torch2NumpyActionProcessor
 from .hil_processor import (
     AddTeleopActionAsComplimentaryData,
     AddTeleopEventsAsInfo,
     GripperPenaltyProcessor,
     ImageCropResizeProcessor,
     InterventionActionProcessor,
-    Numpy2TorchActionProcessor,
     RewardClassifierProcessor,
     TimeLimitProcessor,
-    Torch2NumpyActionProcessor,
 )
 from .joint_observations_processor import JointVelocityProcessor, MotorCurrentProcessor
+from .leader_follower_processor import LeaderFollowerProcessor
 from .normalize_processor import NormalizerProcessor, UnnormalizerProcessor, hotswap_stats
 from .observation_processor import VanillaObservationProcessor
 from .pipeline import (
@@ -55,6 +55,7 @@ __all__ = [
     "DeviceProcessor",
     "DoneProcessor",
     "MapDeltaActionToRobotAction",
+    "MapTensorToDeltaActionDict",
     "EnvTransition",
     "GripperPenaltyProcessor",
     "IdentityProcessor",
@@ -62,6 +63,7 @@ __all__ = [
     "InfoProcessor",
     "InterventionActionProcessor",
     "JointVelocityProcessor",
+    "LeaderFollowerProcessor",
     "MapDeltaActionToRobotAction",
     "MotorCurrentProcessor",
     "NormalizerProcessor",

src/lerobot/processor/delta_action_processor.py

@@ -14,7 +14,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-from dataclasses import dataclass, field
+from dataclasses import dataclass
 
 from torch import Tensor
 
@@ -22,6 +22,30 @@ from lerobot.configs.types import FeatureType, PolicyFeature
 from lerobot.processor.pipeline import ActionProcessor, ProcessorStepRegistry
 
 
+@ProcessorStepRegistry.register("map_tensor_to_delta_action_dict")
+@dataclass
+class MapTensorToDeltaActionDict(ActionProcessor):
+    """
+    Map a tensor to a delta action dictionary.
+    """
+
+    def action(self, action: Tensor) -> dict:
+        if isinstance(action, dict):
+            return action
+        if action.dim() > 1:
+            action = action.squeeze(0)
+
+        # TODO (maractingi): add rotation
+        delta_action = {
+            "action.delta_x": action[0],
+            "action.delta_y": action[1],
+            "action.delta_z": action[2],
+        }
+        if action.shape[0] > 3:
+            delta_action["action.gripper"] = action[3]
+        return delta_action
+
+
 @ProcessorStepRegistry.register("map_delta_action_to_robot_action")
 @dataclass
 class MapDeltaActionToRobotAction(ActionProcessor):
@@ -53,25 +77,17 @@ class MapDeltaActionToRobotAction(ActionProcessor):
     # Scale factors for delta movements
     position_scale: float = 1.0
     rotation_scale: float = 0.0  # No rotation deltas for gamepad/keyboard
-    gripper_deadzone: float = 0.1  # Threshold for gripper activation
-    _prev_enabled: bool = field(default=False, init=False, repr=False)
 
-    def action(self, action: dict | Tensor | None) -> dict:
+    def action(self, action: dict | None) -> dict:
         if action is None:
             return {}
 
         # NOTE (maractingi): Action can be a dict from the teleop_devices or a tensor from the policy
         # TODO (maractingi): changing this target_xyz naming convention from the teleop_devices
-        if isinstance(action, dict):
-            delta_x = action.pop("action.delta_x", 0.0)
-            delta_y = action.pop("action.delta_y", 0.0)
-            delta_z = action.pop("action.delta_z", 0.0)
-            gripper = action.pop("action.gripper", 1.0)  # Default to "stay" (1.0)
-        else:
-            delta_x = action[0].item()
-            delta_y = action[1].item()
-            delta_z = action[2].item()
-            gripper = action[3].item()
+        delta_x = action.pop("action.delta_x", 0.0)
+        delta_y = action.pop("action.delta_y", 0.0)
+        delta_z = action.pop("action.delta_z", 0.0)
+        gripper = action.pop("action.gripper", 1.0)  # Default to "stay" (1.0)
 
         # Determine if the teleoperator is actively providing input
         # Consider enabled if any significant movement delta is detected
@@ -101,7 +117,6 @@ class MapDeltaActionToRobotAction(ActionProcessor):
             "action.gripper": float(gripper),
         }
 
-        self._prev_enabled = enabled
         return action
 
     def transform_features(self, features: dict[str, PolicyFeature]) -> dict[str, PolicyFeature]:
@@ -120,6 +135,3 @@ class MapDeltaActionToRobotAction(ActionProcessor):
             }
         )
         return features
-
-    def reset(self):
-        self._prev_enabled = False

src/lerobot/processor/gym_action_processor.py

@@ -0,0 +1,68 @@
+#! /usr/bin/env python
+
+# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+
+from dataclasses import dataclass
+
+import numpy as np
+import torch
+
+from lerobot.processor.pipeline import ActionProcessor, ProcessorStepRegistry
+
+
+@ProcessorStepRegistry.register("torch2numpy_action_processor")
+@dataclass
+class Torch2NumpyActionProcessor(ActionProcessor):
+    """Convert PyTorch tensor actions to NumPy arrays."""
+
+    squeeze_batch_dim: bool = True
+
+    def action(self, action: torch.Tensor | None) -> np.ndarray | None:
+        if action is None:
+            return None
+
+        if not isinstance(action, torch.Tensor):
+            raise TypeError(
+                f"Expected torch.Tensor or None, got {type(action).__name__}. "
+                "Use appropriate processor for non-tensor actions."
+            )
+
+        numpy_action = action.detach().cpu().numpy()
+
+        # Remove batch dimensions but preserve action dimensions
+        # Only squeeze if there's a batch dimension (first dim == 1)
+        if (
+            self.squeeze_batch_dim
+            and numpy_action.shape
+            and len(numpy_action.shape) > 1
+            and numpy_action.shape[0] == 1
+        ):
+            numpy_action = numpy_action.squeeze(0)
+
+        return numpy_action
+
+
+@ProcessorStepRegistry.register("numpy2torch_action_processor")
+@dataclass
+class Numpy2TorchActionProcessor(ActionProcessor):
+    """Convert NumPy array action to PyTorch tensor."""
+
+    def action(self, action: np.ndarray | None) -> torch.Tensor | None:
+        if action is None:
+            return None
+        if not isinstance(action, np.ndarray):
+            raise TypeError(
+                f"Expected np.ndarray or None, got {type(action).__name__}. "
+                "Use appropriate processor for non-tensor actions."
+            )
+        torch_action = torch.from_numpy(action)
+        return torch_action

src/lerobot/processor/hil_processor.py

@@ -8,7 +8,6 @@ import torchvision.transforms.functional as F  # noqa: N812
 
 from lerobot.configs.types import PolicyFeature
 from lerobot.processor.pipeline import (
-    ActionProcessor,
     ComplementaryDataProcessor,
     EnvTransition,
     InfoProcessor,
@@ -49,55 +48,6 @@ class AddTeleopEventsAsInfo(InfoProcessor):
         return info
 
 
-@ProcessorStepRegistry.register("torch2numpy_action_processor")
-@dataclass
-class Torch2NumpyActionProcessor(ActionProcessor):
-    """Convert PyTorch tensor actions to NumPy arrays."""
-
-    squeeze_batch_dim: bool = True
-
-    def action(self, action: torch.Tensor | None) -> np.ndarray | None:
-        if action is None:
-            return None
-
-        if not isinstance(action, torch.Tensor):
-            raise TypeError(
-                f"Expected torch.Tensor or None, got {type(action).__name__}. "
-                "Use appropriate processor for non-tensor actions."
-            )
-
-        numpy_action = action.detach().cpu().numpy()
-
-        # Remove batch dimensions but preserve action dimensions
-        # Only squeeze if there's a batch dimension (first dim == 1)
-        if (
-            self.squeeze_batch_dim
-            and numpy_action.shape
-            and len(numpy_action.shape) > 1
-            and numpy_action.shape[0] == 1
-        ):
-            numpy_action = numpy_action.squeeze(0)
-
-        return numpy_action
-
-
-@ProcessorStepRegistry.register("numpy2torch_action_processor")
-@dataclass
-class Numpy2TorchActionProcessor(ActionProcessor):
-    """Convert NumPy array action to PyTorch tensor."""
-
-    def action(self, action: np.ndarray | None) -> torch.Tensor | None:
-        if action is None:
-            return None
-        if not isinstance(action, np.ndarray):
-            raise TypeError(
-                f"Expected np.ndarray or None, got {type(action).__name__}. "
-                "Use appropriate processor for non-tensor actions."
-            )
-        torch_action = torch.from_numpy(action)
-        return torch_action
-
-
 @ProcessorStepRegistry.register("image_crop_resize_processor")
 @dataclass
 class ImageCropResizeProcessor(ObservationProcessor):
@@ -271,7 +221,8 @@ class InterventionActionProcessor:
 
         # Get intervention signals from complementary data
         info = transition.get(TransitionKey.INFO, {})
-        teleop_action = info.get("teleop_action", {})
+        complementary_data = transition.get(TransitionKey.COMPLEMENTARY_DATA, {})
+        teleop_action = complementary_data.get("teleop_action", {})
         is_intervention = info.get(TeleopEvents.IS_INTERVENTION, False)
         terminate_episode = info.get(TeleopEvents.TERMINATE_EPISODE, False)
         success = info.get(TeleopEvents.SUCCESS, False)

src/lerobot/processor/joint_observations_processor.py

@@ -13,7 +13,7 @@ from lerobot.robots import Robot
 
 @dataclass
 @ProcessorStepRegistry.register("joint_velocity_processor")
-class JointVelocityProcessor:
+class JointVelocityProcessor(ObservationProcessor):
     """Add joint velocity information to observations."""
 
     joint_velocity_limits: float = 100.0

src/lerobot/processor/leader_follower_processor.py

@@ -0,0 +1,120 @@
+#!/usr/bin/env python
+
+# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from dataclasses import dataclass
+
+import numpy as np
+import torch
+
+from lerobot.model.kinematics import RobotKinematics
+from lerobot.processor.pipeline import EnvTransition, ProcessorStepRegistry, TransitionKey
+from lerobot.robots import Robot
+from lerobot.teleoperators import Teleoperator
+from lerobot.teleoperators.utils import TeleopEvents
+
+
+@ProcessorStepRegistry.register("leader_follower_processor")
+@dataclass
+class LeaderFollowerProcessor:
+    """
+    Processor for leader-follower teleoperation mode.
+
+    This processor:
+    1. Sends follower positions to leader arm when not intervening
+    2. Computes EE delta actions from leader when intervening
+    3. Handles teleop events from the leader device
+    """
+
+    leader_device: Teleoperator
+    motor_names: list[str]
+    robot: Robot
+    kinematics: RobotKinematics
+    end_effector_step_sizes: np.ndarray | None = None
+    use_gripper: bool = True
+    prev_leader_gripper: float | None = None
+    max_gripper_pos: float = 100.0
+
+    def __call__(self, transition: EnvTransition) -> EnvTransition:
+        """Process transition with leader-follower logic."""
+        # Get current follower position from complementary data
+        raw_joint_pos = transition.get(TransitionKey.COMPLEMENTARY_DATA, {}).get("raw_joint_positions")
+        if raw_joint_pos is not None:
+            # Send follower position to leader (for follow mode)
+            follower_action = {f"{motor}.pos": float(raw_joint_pos[motor]) for motor in self.motor_names}
+            self.leader_device.send_action(follower_action)
+
+        # Only compute EE action if intervention is active
+        # (AddTeleopEventsAsInfo already added IS_INTERVENTION to info)
+        info = transition.get(TransitionKey.INFO, {})
+        if info.get(TeleopEvents.IS_INTERVENTION, False):
+            # Get leader joint positions from teleop_action
+            # (AddTeleopActionAsComplimentaryData already got the action)
+            complementary = transition.get(TransitionKey.COMPLEMENTARY_DATA, {})
+            teleop_action = complementary.get("teleop_action", {})
+
+            if isinstance(teleop_action, dict) and raw_joint_pos is not None:
+                # Extract leader positions from teleop action dict
+                leader_pos = np.array([teleop_action.get(f"{motor}.pos", 0) for motor in self.motor_names])
+                follower_pos = np.array([raw_joint_pos[motor] for motor in self.motor_names])
+
+                # Compute EE positions
+                leader_ee = self.kinematics.forward_kinematics(leader_pos)[:3, 3]
+                follower_ee = self.kinematics.forward_kinematics(follower_pos)[:3, 3]
+
+                # Compute normalized EE delta
+                if self.end_effector_step_sizes is not None:
+                    ee_delta = np.clip(
+                        leader_ee - follower_ee, -self.end_effector_step_sizes, self.end_effector_step_sizes
+                    )
+                    ee_delta_normalized = ee_delta / self.end_effector_step_sizes
+                else:
+                    ee_delta_normalized = leader_ee - follower_ee
+
+                # Handle gripper
+                if self.use_gripper and len(leader_pos) > 3:
+                    if self.prev_leader_gripper is None:
+                        self.prev_leader_gripper = np.clip(leader_pos[-1], 0, self.max_gripper_pos)
+
+                    leader_gripper = leader_pos[-1]
+                    gripper_delta = leader_gripper - self.prev_leader_gripper
+                    normalized_delta = gripper_delta / self.max_gripper_pos
+
+                    # Quantize gripper action
+                    if normalized_delta >= 0.3:
+                        gripper_action = 2
+                    elif normalized_delta <= -0.1:
+                        gripper_action = 0
+                    else:
+                        gripper_action = 1
+
+                    self.prev_leader_gripper = leader_gripper
+
+                    # Create intervention action
+                    intervention_action = np.append(ee_delta_normalized, gripper_action)
+                else:
+                    intervention_action = ee_delta_normalized
+
+                # Override teleop_action with computed EE action
+                complementary["teleop_action"] = torch.from_numpy(intervention_action).float()
+                transition[TransitionKey.COMPLEMENTARY_DATA] = complementary  # type: ignore[misc]
+
+        return transition
+
+    def reset(self) -> None:
+        """Reset leader-follower state."""
+        self.prev_leader_gripper = None
+        if hasattr(self.leader_device, "reset"):
+            self.leader_device.reset()

src/lerobot/replay.py

@@ -45,9 +45,11 @@ from dataclasses import asdict, dataclass
 from pathlib import Path
 from pprint import pformat
 
-import draccus
-
+from lerobot.configs import parser
 from lerobot.datasets.lerobot_dataset import LeRobotDataset
+from lerobot.processor import RobotProcessor
+from lerobot.processor.converters import to_output_robot_action, to_transition_teleop_action
+from lerobot.processor.pipeline import IdentityProcessor
 from lerobot.robots import (  # noqa: F401
     Robot,
     RobotConfig,
@@ -83,13 +85,25 @@ class ReplayConfig:
     dataset: DatasetReplayConfig
     # Use vocal synthesis to read events.
     play_sounds: bool = True
+    # Optional processor for actions before sending to robot
+    robot_action_processor: RobotProcessor | None = None
 
 
-@draccus.wrap()
+@parser.wrap()
 def replay(cfg: ReplayConfig):
     init_logging()
     logging.info(pformat(asdict(cfg)))
 
+    # Initialize robot action processor with default if not provided
+    robot_action_processor = cfg.robot_action_processor or RobotProcessor(
+        steps=[IdentityProcessor()],
+        to_transition=lambda tr: tr,
+        to_output=to_output_robot_action,  # type: ignore[arg-type]
+    )
+
+    # Reset processor
+    robot_action_processor.reset()
+
     robot = make_robot_from_config(cfg.robot)
     dataset = LeRobotDataset(cfg.dataset.repo_id, root=cfg.dataset.root, episodes=[cfg.dataset.episode])
     actions = dataset.hf_dataset.select_columns("action")
@@ -104,7 +118,12 @@ def replay(cfg: ReplayConfig):
         for i, name in enumerate(dataset.features["action"]["names"]):
             action[name] = action_array[i]
 
-        robot.send_action(action)
+        # Process action through robot action processor
+        # Note: We need to convert the action dict to a transition format first
+        action_transition = to_transition_teleop_action(action)
+        processed_action = robot_action_processor(action_transition)
+
+        robot.send_action(processed_action)  # type: ignore[arg-type]
 
         dt_s = time.perf_counter() - start_episode_t
         busy_wait(1 / dataset.fps - dt_s)

src/lerobot/robots/so100_follower/robot_kinematic_processor.py

@@ -140,6 +140,11 @@ class EEReferenceAndDelta:
     def transform_features(self, features: dict[str, PolicyFeature]) -> dict[str, PolicyFeature]:
         return features
 
+    def reset(self):
+        self._prev_enabled = False
+        self.reference_ee_pose = None
+        self._command_when_disabled = None
+
 
 @ProcessorStepRegistry.register("ee_bounds_and_safety")
 @dataclass
@@ -162,6 +167,7 @@ class EEBoundsAndSafety(ActionProcessor):
     max_ee_step_m: float = 0.05
     max_ee_twist_step_rad: float = 0.20
     _last_pos: np.ndarray | None = field(default=None, init=False, repr=False)
+    _last_twist: np.ndarray | None = field(default=None, init=False, repr=False)
 
     def action(self, act: dict | None) -> dict:
         x = act.pop("action.ee.x", None)
@@ -205,6 +211,7 @@ class EEBoundsAndSafety(ActionProcessor):
 
     def reset(self):
         self._last_pos = None
+        self._last_twist = None
 
     def transform_features(self, features: dict[str, PolicyFeature]) -> dict[str, PolicyFeature]:
         # Because this is last step we specify the dataset features of this step that we want to be stored in the dataset

src/lerobot/scripts/rl/actor.py

@@ -98,7 +98,6 @@ from lerobot.utils.utils import (
 
 ACTOR_SHUTDOWN_TIMEOUT = 30
 
-
 #################################################
 # Main entry point #
 #################################################
@@ -288,7 +287,9 @@ def act_with_policy(
             logging.info("[ACTOR] Shutting down act_with_policy")
             return
 
-        observation = transition[TransitionKey.OBSERVATION]
+        observation = {
+            k: v for k, v in transition[TransitionKey.OBSERVATION].items() if k in cfg.policy.input_features
+        }
 
         # Time policy inference and check if it meets FPS requirement
         with policy_timer:
@@ -308,8 +309,16 @@ def act_with_policy(
         )
 
         # Extract values from processed transition
-        next_observation = new_transition[TransitionKey.OBSERVATION]
-        executed_action = new_transition[TransitionKey.ACTION]
+        next_observation = {
+            k: v
+            for k, v in new_transition[TransitionKey.OBSERVATION].items()
+            if k in cfg.policy.input_features
+        }
+
+        # Teleop action is the action that was executed in the environment
+        # It is either the action from the teleop device or the action from the policy
+        executed_action = new_transition[TransitionKey.COMPLEMENTARY_DATA]["teleop_action"]
+
         reward = new_transition[TransitionKey.REWARD]
         done = new_transition.get(TransitionKey.DONE, False)
         truncated = new_transition.get(TransitionKey.TRUNCATED, False)

src/lerobot/scripts/rl/gym_manipulator.py

@@ -36,7 +36,9 @@ from lerobot.processor import (
     ImageCropResizeProcessor,
     InterventionActionProcessor,
     JointVelocityProcessor,
+    LeaderFollowerProcessor,
     MapDeltaActionToRobotAction,
+    MapTensorToDeltaActionDict,
     MotorCurrentProcessor,
     Numpy2TorchActionProcessor,
     RewardClassifierProcessor,
@@ -51,6 +53,7 @@ from lerobot.robots import (  # noqa: F401
     RobotConfig,
     make_robot_from_config,
     so100_follower,
+    so101_follower,
 )
 from lerobot.robots.robot import Robot
 from lerobot.robots.so100_follower.robot_kinematic_processor import (
@@ -67,6 +70,7 @@ from lerobot.teleoperators import (
     make_teleoperator_from_config,
     so101_leader,  # noqa: F401
 )
+from lerobot.teleoperators.so101_leader.so101_leader_follower import SO101LeaderFollower
 from lerobot.teleoperators.teleoperator import Teleoperator
 from lerobot.teleoperators.utils import TeleopEvents
 from lerobot.utils.robot_utils import busy_wait
@@ -80,11 +84,11 @@ class DatasetConfig:
     """Configuration for dataset creation and management."""
 
     repo_id: str
-    dataset_root: str
     task: str
-    num_episodes: int
-    episode: int
-    push_to_hub: bool
+    root: str | None = None
+    num_episodes_to_record: int = 5
+    replay_episode: int | None = None
+    push_to_hub: bool = False
 
 
 @dataclass
@@ -112,7 +116,7 @@ def create_transition(
     }
 
 
-def reset_follower_position(robot_arm: Robot, target_position: np.ndarray) -> None:
+def reset_follower_position(robot_arm: Robot | SO101LeaderFollower, target_position: np.ndarray) -> None:
     """Reset robot arm to target position using smooth trajectory."""
     current_position_dict = robot_arm.bus.sync_read("Present_Position")
     current_position = np.array(
@@ -401,9 +405,7 @@ def make_processors(
             joint_names=motor_names,
         )
 
-    env_pipeline_steps = [
-        VanillaObservationProcessor(),
-    ]
+    env_pipeline_steps = [VanillaObservationProcessor()]
 
     if cfg.processor.observation is not None:
         if cfg.processor.observation.add_joint_velocity_to_observation:
@@ -459,20 +461,48 @@ def make_processors(
     env_pipeline_steps.append(ToBatchProcessor())
     env_pipeline_steps.append(DeviceProcessor(device=device))
 
+    # Get control mode
+    control_mode = cfg.processor.control_mode if cfg.processor is not None else "gamepad"
+
     action_pipeline_steps = [
         AddTeleopActionAsComplimentaryData(teleop_device=teleop_device),
         AddTeleopEventsAsInfo(teleop_device=teleop_device),
         AddRobotObservationAsComplimentaryData(robot=env.robot),
+    ]
+    # Check for leader control mode
+    if control_mode == "leader":
+        assert isinstance(teleop_device, SO101LeaderFollower), (
+            "Leader control mode requires SO101LeaderFollower teleop device"
+        )
+
+        action_pipeline_steps.append(
+            LeaderFollowerProcessor(
+                leader_device=teleop_device,
+                motor_names=motor_names,
+                robot=env.robot,
+                kinematics=kinematics_solver,
+                end_effector_step_sizes=np.array(
+                    list(cfg.processor.inverse_kinematics.end_effector_step_sizes.values())
+                ),
+                use_gripper=cfg.processor.gripper.use_gripper if cfg.processor.gripper is not None else False,
+                max_gripper_pos=cfg.processor.max_gripper_pos
+                if cfg.processor.max_gripper_pos is not None
+                else 100.0,
+            )
+        )
+        # Standard teleop mode (gamepad, keyboard, etc.)
+    action_pipeline_steps.append(
         InterventionActionProcessor(
             use_gripper=cfg.processor.gripper.use_gripper if cfg.processor.gripper is not None else False,
             terminate_on_success=terminate_on_success,
         ),
-    ]
+    )
 
     # Replace InverseKinematicsProcessor with new kinematic processors
     if cfg.processor.inverse_kinematics is not None and kinematics_solver is not None:
         # Add EE bounds and safety processor
         inverse_kinematics_steps = [
+            MapTensorToDeltaActionDict(),
             MapDeltaActionToRobotAction(),
             EEReferenceAndDelta(
                 kinematics=kinematics_solver,
@@ -572,10 +602,6 @@ def control_loop(
     dt = 1.0 / cfg.env.fps
 
     print(f"Starting control loop at {cfg.env.fps} FPS")
-    print("Controls:")
-    print("- Use gamepad/teleop device for intervention")
-    print("- When not intervening, robot will stay still")
-    print("- Press Ctrl+C to exit")
 
     # Reset environment and processors
     obs, info = env.reset()
@@ -625,7 +651,7 @@ def control_loop(
         dataset = LeRobotDataset.create(
             cfg.dataset.repo_id,
             cfg.env.fps,
-            root=cfg.dataset.dataset_root,
+            root=cfg.dataset.root,
             use_videos=True,
             image_writer_threads=4,
             image_writer_processes=0,
@@ -636,7 +662,7 @@ def control_loop(
     episode_step = 0
     episode_start_time = time.perf_counter()
 
-    while episode_idx < cfg.dataset.num_episodes:
+    while episode_idx < cfg.dataset.num_episodes_to_record:
         step_start_time = time.perf_counter()
 
         # Create a neutral action (no movement)
@@ -711,10 +737,12 @@ def control_loop(
 
 def replay_trajectory(env: gym.Env, action_processor: RobotProcessor, cfg: GymManipulatorConfig) -> None:
     """Replay recorded trajectory on robot environment."""
+    assert cfg.dataset.replay_episode is not None, "Replay episode must be provided for replay"
+
     dataset = LeRobotDataset(
         cfg.dataset.repo_id,
-        root=cfg.dataset.dataset_root,
-        episodes=[cfg.dataset.episode],
+        root=cfg.dataset.root,
+        episodes=[cfg.dataset.replay_episode],
         download_videos=False,
     )
     dataset_actions = dataset.hf_dataset.select_columns(["action"])

src/lerobot/teleoperate.py

@@ -56,11 +56,18 @@ import time
 from dataclasses import asdict, dataclass
 from pprint import pformat
 
-import draccus
 import rerun as rr
 
 from lerobot.cameras.opencv.configuration_opencv import OpenCVCameraConfig  # noqa: F401
 from lerobot.cameras.realsense.configuration_realsense import RealSenseCameraConfig  # noqa: F401
+from lerobot.configs import parser
+from lerobot.processor import RobotProcessor
+from lerobot.processor.converters import (
+    to_output_robot_action,
+    to_transition_robot_observation,
+    to_transition_teleop_action,
+)
+from lerobot.processor.pipeline import IdentityProcessor
 from lerobot.robots import (  # noqa: F401
     Robot,
     RobotConfig,
@@ -97,21 +104,67 @@ class TeleoperateConfig:
     teleop_time_s: float | None = None
     # Display all cameras on screen
     display_data: bool = False
+    # Optional processors for data transformation
+    teleop_action_processor: RobotProcessor | None = None  # runs after teleop
+    robot_action_processor: RobotProcessor | None = None  # runs before robot
+    robot_observation_processor: RobotProcessor | None = None  # runs after robot
 
 
 def teleop_loop(
-    teleop: Teleoperator, robot: Robot, fps: int, display_data: bool = False, duration: float | None = None
+    teleop: Teleoperator,
+    robot: Robot,
+    fps: int,
+    display_data: bool = False,
+    duration: float | None = None,
+    teleop_action_processor: RobotProcessor | None = None,
+    robot_action_processor: RobotProcessor | None = None,
+    robot_observation_processor: RobotProcessor | None = None,
 ):
+    # Initialize processors with defaults if not provided
+    teleop_action_processor = teleop_action_processor or RobotProcessor(
+        steps=[IdentityProcessor()], to_transition=to_transition_teleop_action, to_output=lambda tr: tr
+    )
+    robot_action_processor = robot_action_processor or RobotProcessor(
+        steps=[IdentityProcessor()],
+        to_transition=lambda tr: tr,
+        to_output=to_output_robot_action,  # type: ignore[arg-type]
+    )
+    robot_observation_processor = robot_observation_processor or RobotProcessor(
+        steps=[IdentityProcessor()], to_transition=to_transition_robot_observation, to_output=lambda tr: tr
+    )
+
+    # Reset processors
+    teleop_action_processor.reset()
+    robot_action_processor.reset()
+    robot_observation_processor.reset()
+
     display_len = max(len(key) for key in robot.action_features)
     start = time.perf_counter()
+
     while True:
         loop_start = time.perf_counter()
-        action = teleop.get_action()
-        if display_data:
-            observation = robot.get_observation()
-            log_rerun_data(observation=observation, action=action)
 
-        robot.send_action(action)
+        # Get robot observation
+        obs = robot.get_observation()
+
+        # Process robot observation through pipeline
+        obs_transition = robot_observation_processor(obs)
+
+        # Get teleop action
+        raw_action = teleop.get_action()
+
+        # Process teleop action through pipeline
+        teleop_transition = teleop_action_processor(raw_action)
+
+        # Process action for robot through pipeline
+        robot_action_to_send = robot_action_processor(teleop_transition)
+
+        # Send processed action to robot (robot_action_processor.to_output should return dict[str, Any])
+        robot.send_action(robot_action_to_send)  # type: ignore[arg-type]
+
+        if display_data:
+            log_rerun_data([obs_transition, teleop_transition])
+
         dt_s = time.perf_counter() - loop_start
         busy_wait(1 / fps - dt_s)
 
@@ -119,17 +172,18 @@ def teleop_loop(
 
         print("\n" + "-" * (display_len + 10))
         print(f"{'NAME':<{display_len}} | {'NORM':>7}")
-        for motor, value in action.items():
+        # Display the final robot action that was sent
+        for motor, value in robot_action_to_send.items():
             print(f"{motor:<{display_len}} | {value:>7.2f}")
         print(f"\ntime: {loop_s * 1e3:.2f}ms ({1 / loop_s:.0f} Hz)")
 
         if duration is not None and time.perf_counter() - start >= duration:
             return
 
-        move_cursor_up(len(action) + 5)
+        move_cursor_up(len(robot_action_to_send) + 5)
 
 
-@draccus.wrap()
+@parser.wrap()
 def teleoperate(cfg: TeleoperateConfig):
     init_logging()
     logging.info(pformat(asdict(cfg)))
@@ -143,7 +197,16 @@ def teleoperate(cfg: TeleoperateConfig):
     robot.connect()
 
     try:
-        teleop_loop(teleop, robot, cfg.fps, display_data=cfg.display_data, duration=cfg.teleop_time_s)
+        teleop_loop(
+            teleop=teleop,
+            robot=robot,
+            fps=cfg.fps,
+            display_data=cfg.display_data,
+            duration=cfg.teleop_time_s,
+            teleop_action_processor=cfg.teleop_action_processor,
+            robot_action_processor=cfg.robot_action_processor,
+            robot_observation_processor=cfg.robot_observation_processor,
+        )
     except KeyboardInterrupt:
         pass
     finally:

src/lerobot/teleoperators/so101_leader/__init__.py

@@ -16,3 +16,4 @@
 
 from .config_so101_leader import SO101LeaderConfig
 from .so101_leader import SO101Leader
+from .so101_leader_follower import SO101LeaderFollower

src/lerobot/teleoperators/so101_leader/config_so101_leader.py

@@ -26,3 +26,6 @@ class SO101LeaderConfig(TeleoperatorConfig):
     port: str
 
     use_degrees: bool = False
+
+    # Enable leader-follower mode where leader can both lead and follow
+    leader_follower_mode: bool = False

src/lerobot/teleoperators/so101_leader/so101_leader_follower.py

@@ -0,0 +1,208 @@
+#!/usr/bin/env python
+
+# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import logging
+import time
+from collections import deque
+from threading import Event, Thread
+
+import numpy as np
+from pynput import keyboard
+
+from lerobot.teleoperators.so101_leader.so101_leader import SO101Leader
+from lerobot.teleoperators.utils import TeleopEvents
+
+logger = logging.getLogger(__name__)
+
+
+class SO101LeaderFollower(SO101Leader):
+    """
+    Extended SO101 Leader that can both lead (human control) and follow (mimic follower).
+
+    This class adds leader-follower functionality where:
+    - In follow mode: The leader arm mimics the follower's position (torque enabled)
+    - In lead mode: Human controls the leader (torque disabled) and provides actions
+    """
+
+    def __init__(self, config):
+        super().__init__(config)
+
+        # Leader-follower state
+        self.is_intervening = False
+        self.leader_torque_enabled = True
+
+        # Tracking error for automatic intervention detection
+        self.leader_tracking_error_queue = deque(maxlen=4)
+
+        # Keyboard event handling
+        self.keyboard_events = {
+            "intervention": False,
+            "success": False,
+            "failure": False,
+            "rerecord": False,
+        }
+        self.keyboard_thread = None
+        self.stop_event = Event()
+
+        # Store last follower position for action computation
+        self.last_follower_pos = None
+
+    def connect(self, calibrate: bool = True) -> None:
+        """Connect and configure for leader-follower mode."""
+        super().connect(calibrate)
+
+        # Configure for leader-follower mode with lower gains
+        # Lower gains allow manual intervention without injury risk
+        self.bus.sync_write("Torque_Enable", 1)
+        for motor in self.bus.motors:
+            self.bus.write("P_Coefficient", motor, 16)
+            self.bus.write("I_Coefficient", motor, 0)
+            self.bus.write("D_Coefficient", motor, 16)
+
+        # Start keyboard listener
+        self._start_keyboard_listener()
+
+        print("- Leader-Follower Mode:")
+        print("  - Press SPACE to toggle intervention (leader control)")
+        print("  - When not intervening, leader follows follower position")
+        print("  - When intervening, follower follows leader in end-effector space")
+        print("  - Press 's' to mark episode as success")
+        print("  - Press ESC to end episode as failure")
+        print("  - Press 'r' to re-record episode")
+
+    def _start_keyboard_listener(self):
+        """Start keyboard listener thread for intervention control."""
+
+        def on_press(key):
+            try:
+                if key == keyboard.Key.space:
+                    self.keyboard_events["intervention"] = not self.keyboard_events["intervention"]
+                    self.is_intervening = self.keyboard_events["intervention"]
+                    state = "INTERVENTION MODE" if self.is_intervening else "FOLLOWING MODE"
+                    logger.info(f"Toggled to {state}")
+                elif key == keyboard.Key.esc:
+                    self.keyboard_events["failure"] = True
+                elif hasattr(key, "char"):
+                    if key.char == "s":
+                        self.keyboard_events["success"] = True
+                    elif key.char == "r":
+                        self.keyboard_events["rerecord"] = True
+            except Exception as e:
+                logger.error(f"Error handling key press: {e}")
+
+        def listen():
+            with keyboard.Listener(on_press=on_press) as listener:
+                while not self.stop_event.is_set():
+                    time.sleep(0.1)
+                listener.stop()
+
+        self.keyboard_thread = Thread(target=listen, daemon=True)
+        self.keyboard_thread.start()
+
+    def send_action(self, action: dict[str, float]) -> None:
+        """
+        Send position commands to leader arm (follow mode).
+
+        Args:
+            action: Dictionary of motor positions to command
+        """
+        # Store follower position for later use
+        self.last_follower_pos = np.array([action.get(f"{motor}.pos", 0) for motor in self.bus.motors])
+
+        if not self.is_intervening:
+            # Follow mode: enable torque and track follower
+            if not self.leader_torque_enabled:
+                self.bus.sync_write("Torque_Enable", 1)
+                self.leader_torque_enabled = True
+
+            # Send follower positions to leader
+            goal_pos = {motor: action[f"{motor}.pos"] for motor in self.bus.motors}
+            self.bus.sync_write("Goal_Position", goal_pos)
+
+            # Track error for automatic intervention detection
+            current_pos = self.bus.sync_read("Present_Position")
+            current_array = np.array([current_pos[motor] for motor in self.bus.motors])
+            error = np.linalg.norm(self.last_follower_pos[:-1] - current_array[:-1])
+            self.leader_tracking_error_queue.append(error)
+
+    def get_action(self) -> dict[str, float]:
+        """
+        Get action from leader arm.
+
+        In follow mode: Returns neutral/current positions
+        In lead mode: Returns actual leader positions for follower to track
+        """
+        start = time.perf_counter()
+
+        if self.is_intervening:
+            # Lead mode: disable torque if needed and return leader positions
+            if self.leader_torque_enabled:
+                self.bus.sync_write("Torque_Enable", 0)
+                self.leader_torque_enabled = False
+
+            # Get current leader position
+            action = self.bus.sync_read("Present_Position")
+            action = {f"{motor}.pos": val for motor, val in action.items()}
+
+            # Track error
+            if self.last_follower_pos is not None:
+                current_array = np.array([action[f"{motor}.pos"] for motor in self.bus.motors])
+                error = np.linalg.norm(self.last_follower_pos[:-1] - current_array[:-1])
+                self.leader_tracking_error_queue.append(error)
+        else:
+            # Follow mode: return current/neutral positions
+            action = self.bus.sync_read("Present_Position")
+            action = {f"{motor}.pos": val for motor, val in action.items()}
+
+        dt_ms = (time.perf_counter() - start) * 1e3
+        logger.debug(f"{self} read action: {dt_ms:.1f}ms")
+        return action
+
+    def get_teleop_events(self) -> dict[TeleopEvents, bool]:
+        """Get current keyboard events."""
+        events = {}
+
+        # Map keyboard events to TeleopEvents
+        if self.keyboard_events["success"]:
+            events[TeleopEvents.SUCCESS] = True
+            self.keyboard_events["success"] = False
+        if self.keyboard_events["failure"]:
+            events[TeleopEvents.FAILURE] = True
+            events[TeleopEvents.TERMINATE_EPISODE] = True
+            self.keyboard_events["failure"] = False
+        if self.keyboard_events["rerecord"]:
+            events[TeleopEvents.RERECORD_EPISODE] = True
+            events[TeleopEvents.TERMINATE_EPISODE] = True
+            self.keyboard_events["rerecord"] = False
+
+        # Always report intervention state
+        events[TeleopEvents.IS_INTERVENTION] = self.is_intervening
+
+        return events
+
+    def disconnect(self) -> None:
+        """Disconnect and cleanup."""
+        self.stop_event.set()
+        if self.keyboard_thread:
+            self.keyboard_thread.join(timeout=1.0)
+        super().disconnect()
+
+    def reset(self) -> None:
+        """Reset leader-follower state."""
+        self.is_intervening = False
+        self.leader_torque_enabled = True
+        self.leader_tracking_error_queue.clear()
+        self.keyboard_events = {"intervention": False, "success": False, "failure": False, "rerecord": False}

src/lerobot/teleoperators/utils.py

@@ -42,7 +42,10 @@ def make_teleoperator_from_config(config: TeleoperatorConfig) -> Teleoperator:
 
         return SO100Leader(config)
     elif config.type == "so101_leader":
-        from .so101_leader import SO101Leader
+        from .so101_leader import SO101Leader, SO101LeaderFollower
+
+        if getattr(config, "leader_follower_mode", False):
+            return SO101LeaderFollower(config)
 
         return SO101Leader(config)
     elif config.type == "stretch3":