Add gello and robot reset pose logic.

config/gello/xarm5_gello_record_config.yaml

@@ -5,7 +5,6 @@ robot:
   control_space: "joint"
   robot_ip: "192.168.1.245"
   gripper_type: 1
-  start_joints: [0, 0, -90, 90, 0]
 
 # make sure to edit with your correct configurations!
 teleop:
@@ -15,7 +14,6 @@ teleop:
   joint_signs: [1, 1, -1, 1, 1]
   start_joints: [0, 0, -90, 90, 0]
   gripper_id: 8
-  torque_joint_ids: [3, 5]
 
 dataset:
   # root of local repo: /home/<user_name>/.cache/huggingface/lerobot (default)

config/gello/xarm6_gello_record_config.yaml

@@ -5,7 +5,6 @@ robot:
   control_space: "joint"
   robot_ip: "192.168.1.245"
   gripper_type: 1
-  start_joints: [0, 0, -90, 0, 90, 0]
 
 # make sure to edit with your correct configurations!
 teleop:
@@ -15,7 +14,6 @@ teleop:
   joint_signs: [1, 1, -1, 1, 1, 1]
   start_joints: [0, 0, -90, 0, 90, 0]
   gripper_id: 8
-  torque_joint_ids: [3]
 
 dataset:
   # root of local repo: /home/<user_name>/.cache/huggingface/lerobot (default)

config/gello/xarm7_gello_record_config.yaml

@@ -5,14 +5,16 @@ robot:
   control_space: "joint"
   robot_ip: "192.168.1.245"
   gripper_type: 1
-  start_joints: [0, 0, 0, 90, 0, 90, 0]
+  # Redundant args, indicating the initial pose of xarm7. Set by 192.168.1.245:18333
+  # start_joints: [0, -30, 0, 0, 0, 30, 0]
 
 # make sure to edit with your correct configurations!
 teleop:
   type: uf::gello_teleop
   id: "gello_teleop"
   port: "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FTAJZYC7-if00-port0"
-  start_joints: [0, 0, 0, 90, 0, 90, 0]
+  # GELLO calibration reference; matches the xArm SDK initial point.
+  start_joints: [0, -30, 0, 0, 0, 30, 0]
 
 dataset:
   # root of local repo: /home/<user_name>/.cache/huggingface/lerobot (default)

config/pika/xarm7_pika_record_config.yaml

@@ -6,8 +6,6 @@ robot:
   robot_ip: "192.168.1.85"
   gripper_type: 10
   max_linear_velocity: 200
-  start_joints: [-0.2, -13.9, -0.3, 52.6, 0.9, 66.6, 0.2]
-  start_tcp_pose: [400, 0, 400, 180, 0, 0]
   cameras:
     fisheye:
       type: opencv

config/umi/dual_xarm6_umi_record_config.yaml

@@ -13,8 +13,6 @@ robot:
       robot_ip: "192.168.1.29"
       gripper_type: 2
       max_linear_velocity: 200
-      start_joints: [-14.7, 25.6, -49.6, -282.5, 95.9, -65.9]
-      start_tcp_pose: [400, 0, 150, -90, 0, 0]
       cameras:
         fisheye:
           type: opencv
@@ -31,8 +29,6 @@ robot:
       robot_ip: "192.168.1.83"
       gripper_type: 2
       max_linear_velocity: 200
-      start_joints: [14.5, 25.5, -49.6, -76.8, 95.9, 66.6]
-      start_tcp_pose: [400, 0, 150, 90, 0, 0]
       cameras:
         fisheye:
           type: opencv
@@ -75,4 +71,3 @@ dataset:
   num_episodes: 100
   push_to_hub: False
 
-

config/umi/xarm6_umi_record_config.yaml

@@ -6,8 +6,6 @@ robot:
   robot_ip: "192.168.1.83"
   gripper_type: 2
   max_linear_velocity: 250
-  start_joints: [0, -14.8, -52.6, 0, 67.4, 0]
-  start_tcp_pose: [400, 0, 400, 180, 0, 0]
   cameras:
     fisheye:
       type: opencv

src/lerobot_robot_ufactory/robots/uf_mock_robot/multiple_uf_mock_robot.py

@@ -57,6 +57,10 @@ class MultipleUFMockRobot(Robot):
         for robot in self.robots.values():
             robot.configure()
 
+    def reset_to_initial(self) -> None:
+        for robot in self.robots.values():
+            robot.reset_to_initial()
+
     def disconnect(self) -> None:
         for robot in self.robots.values():
             robot.disconnect()

src/lerobot_robot_ufactory/robots/uf_mock_robot/uf_mock_robot.py

@@ -93,6 +93,9 @@ class UFMockRobot(Robot):
     def configure(self) -> None:
         pass
 
+    def reset_to_initial(self) -> None:
+        pass
+
     def calibrate(self) -> None:
         self._is_calibrated = True
         pass # CHECK! currently No-op
@@ -130,4 +133,3 @@ class UFMockRobot(Robot):
         return self._is_connected
 
 
-

src/lerobot_robot_ufactory/robots/uf_robot/multiple_uf_robot.py

@@ -101,6 +101,30 @@ class MultipleUFRobot(Robot):
             for robot in self.robots.values():
                 robot.configure()
 
+    def reset_to_initial(self) -> None:
+        """Reset every arm to its SDK-configured initial point."""
+        if self._is_async_configure:
+            errors = []
+
+            def reset_robot(robot):
+                try:
+                    robot.reset_to_initial()
+                except BaseException as exc:
+                    errors.append(exc)
+
+            threads = []
+            for robot in self.robots.values():
+                thread = threading.Thread(target=reset_robot, args=(robot,), daemon=True)
+                threads.append(thread)
+                thread.start()
+            for thread in threads:
+                thread.join()
+            if errors:
+                raise errors[0]
+        else:
+            for robot in self.robots.values():
+                robot.reset_to_initial()
+
     def disconnect(self) -> None:
         for robot in self.robots.values():
             robot.disconnect()

src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py

@@ -17,6 +17,7 @@ from xarm.core.utils import convert
 
 ## Configurations:
 INIT_SYNC_JOINT_VELOCITY_RAD = 0.2
+ROBOT_RESET_SPEED_DEG = 60
 
 CARTESIAN_OBS_KEYS = [
     "pose.x", "pose.y", "pose.z", "pose.rx", "pose.ry", "pose.rz",
@@ -75,6 +76,7 @@ class UFRobot(Robot, Thread):
         self._control_space = self.config.control_space
 
         self.real_arm = None
+        self._initial_point = None
         cameras_args = self.config.cameras_args or {}
         self.camera_width = cameras_args.get('w', 0)
         self.camera_height = cameras_args.get('h', 0)
@@ -90,15 +92,6 @@ class UFRobot(Robot, Thread):
         self._max_joint_velocity = math.radians(self.config.max_joint_velocity)
         self._max_linear_velocity = self.config.max_linear_velocity
 
-        if self.config.start_tcp_pose and len(self.config.start_tcp_pose) >= 6:
-            self._start_tcp_pose = list(self.config.start_tcp_pose[:3]) + list(map(math.radians, self.config.start_tcp_pose[3:6]))
-        else:
-            self._start_tcp_pose = None
-        if self.config.start_joints:
-            self._start_joints = list(map(math.radians, self.config.start_joints))
-        else:
-            self._start_joints = None
-
         self.report_stop_event = Event()
         self._rt_report_normal = False
         self._update_lock = Lock()
@@ -193,6 +186,13 @@ class UFRobot(Robot, Thread):
             self._is_connected = False
             raise ConnectionError()
 
+        code, initial_point = self.real_arm.get_initial_point()
+        if code != 0:
+            raise RuntimeError(f"get_initial_point failed, code={code}")
+        if initial_point is None or len(initial_point) < self._dof:
+            raise RuntimeError(f"Invalid initial point returned by xArm: {initial_point}")
+        self._initial_point = list(initial_point[:self._dof])
+
         for cam in self.cameras.values():
             cam.connect()
             self._is_connected = self._is_connected and cam.is_connected
@@ -206,7 +206,10 @@ class UFRobot(Robot, Thread):
         #         print('Could not connect to pika gripper.')
         #         raise ConnectionError()
 
+        if self.config.manual_mode:
             self.configure()
+        else:
+            self.reset_to_initial()
         if calibrate:  
             self.calibrate()
 
@@ -214,6 +217,25 @@ class UFRobot(Robot, Thread):
 
         self._is_connected = True
 
+    def reset_to_initial(self) -> None:
+        if not self._is_connected or self.real_arm is None:
+            raise ConnectionError("UF Robot is not connected")
+        if self._initial_point is None:
+            raise RuntimeError("xArm initial point has not been loaded")
+
+        self.real_arm.set_mode(0)
+        self.real_arm.set_state(0)
+        code = self.real_arm.set_servo_angle(
+            angle=self._initial_point,
+            speed=ROBOT_RESET_SPEED_DEG,
+            is_radian=False,
+            wait=True,
+        )
+        if code != 0:
+            raise RuntimeError(f"Failed to move to xArm initial point, code={code}")
+
+        self.configure()
+
     def configure(self) -> None:
         self.real_arm.motion_enable()
         self.real_arm.clean_error()
@@ -277,13 +299,6 @@ class UFRobot(Robot, Thread):
             if err_warn[0] != 0:
                 raise RuntimeError(f"Failed to set correct state to Gripper! Controller Error code: {err_warn[0]} !")
         
-        if self._start_joints is not None:
-            self.real_arm.set_servo_angle(angle=self._start_joints, is_radian=True, wait=True)
-        if self._start_tcp_pose is not None:
-            self.real_arm.set_position(*self._start_tcp_pose, speed=100, is_radian=True, wait=True)
-            _, self._start_joints = self.real_arm.get_servo_angle(is_radian=True)
-            self._start_tcp_pose = None
-
         if self._control_space == "joint":
             self.real_arm.set_mode(6)
         elif self._control_space == "cartesian":

src/lerobot_robot_ufactory/robots/uf_robot/uf_robot_config.py

@@ -1,5 +1,4 @@
 from dataclasses import dataclass, field
-from typing import Tuple
 from lerobot.cameras import CameraConfig
 from lerobot.robots import RobotConfig
 
@@ -20,8 +19,8 @@ class UFRobotConfig(RobotConfig):
     observe_joint_vel: bool = False # only effective in joint control mode
     manual_mode: bool = False  # xArm joint teaching mode; records state without sending actions
     teach_sensitivity: int | None = None  # xArm teaching sensitivity, valid range: 1-5
-    start_joints: Tuple[float, ...] = (0, 0, 0, 90, 0, 90, 0) # °
+    # start_joints and start_tcp_pose are intentionally disabled.
-    start_tcp_pose: Tuple[float, ...] = None # [x, y, z, roll(°), pitch(°), yaw(°)]
+    # Reset uses the xArm SDK initial_point instead of configuration poses.
     max_joint_velocity: int = 90   # °/s, only effective in joint control mode
     max_linear_velocity: int = 200 # mm/s, only effective in cartesian control mode
     no_action: bool = False # only for debug

src/lerobot_robot_ufactory/scripts/uf_lerobot_eval.py

@@ -232,7 +232,10 @@ def eval_loop(cfg: EvalConfig, relative=False, rx_continuous=False):
 
     # with torch.no_grad(), torch.autocast(device_type=device.type) if cfg.policy.use_amp else nullcontext():
     while True:
-        robot.configure()
+        reset = getattr(robot, "reset_to_initial", None)
+        if reset is None:
+            reset = robot.configure
+        reset()
         policy.reset()
         preprocessor.reset()
         postprocessor.reset()

src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py

@@ -408,6 +408,23 @@ def record_loop(
         timestamp = time.perf_counter() - start_episode_t
 
 
+def _prepare_recording_episode(robot, teleop, is_uf_teleop, manual_mode):
+    if is_uf_teleop:
+        # Stop teleop output before handing control to the xArm reset motion.
+        teleop.set_teleop_enabled(False)
+
+    if is_uf_teleop or manual_mode:
+        reset = getattr(robot, "reset_to_initial", None)
+        if reset is None:
+            reset = robot.configure
+        reset()
+
+    if is_uf_teleop:
+        obs = robot.get_observation()
+        teleop.reset_to_robot_observation(obs)
+        teleop.set_teleop_enabled(True, obs)
+
+
 def record(cfg: UFRecordConfig, async_save: bool = False) -> LeRobotDataset:
     init_logging()
     logging.info(pformat(asdict(cfg)))
@@ -540,8 +557,6 @@ def record(cfg: UFRecordConfig, async_save: bool = False) -> LeRobotDataset:
         print('⌨   [ESC] Exit  [Space] Start  [←] Reset  [→] Save')
     else:
         input('⌨   Press Enter to start record >>> ')
-        if is_uf_teleop:
-            teleop.set_teleop_enabled(True)
         is_recorded = True
         print('\n********** Episode Record Loop Start **********')
 
@@ -575,10 +590,7 @@ def record(cfg: UFRecordConfig, async_save: bool = False) -> LeRobotDataset:
                 events["rerecord_episode"] = False
                 events["exit_early"] = False
                 if is_uf_teleop or manual_mode:
-                    robot.configure()
+                    _prepare_recording_episode(robot, teleop, is_uf_teleop, manual_mode)
-                    if is_uf_teleop:
-                        obs = robot.get_observation()
-                        teleop.set_teleop_enabled(True, obs)
                 log_say(f"Recording episode {_current_episode_index(dataset)}", cfg.play_sounds)
                 record_loop(
                     robot=robot,

src/lerobot_robot_ufactory/scripts/uf_robot_teleop.py

@@ -61,7 +61,20 @@ def teleop_loop(cfg: TeleopConfig):
     is_evt = not is_headless()
     is_uf_teleop = isinstance(teleop, UFBaseTeleop)
 
-    is_reset = False
+    def reset_uf_control():
+        if is_uf_teleop:
+            # Stop teleop output before handing control to the xArm reset motion.
+            teleop.set_teleop_enabled(False)
+        reset = getattr(robot, "reset_to_initial", None)
+        if reset is None:
+            reset = robot.configure
+        reset()
+        if is_uf_teleop:
+            obs = robot.get_observation()
+            teleop.reset_to_robot_observation(obs)
+            teleop.set_teleop_enabled(True, obs)
+
+    is_reset = is_uf_teleop
     is_paused = True
     events = {"exit": False}
     listener = None
@@ -105,12 +118,14 @@ def teleop_loop(cfg: TeleopConfig):
 
         listener, events = init_keyboard_listener(events=events, on_press=on_press, on_release=on_release)
         print("\n********** Teleop Control Loop Start **********")
+        if is_uf_teleop:
+            print('⌨   [ESC] Exit  [Space] Reset / Start  [←] Reset')
+        else:
             print('⌨   [ESC] Exit  [Space] Start  [←] Reset')
     else:
         input('⌨   Press Enter to start teleop >>> ')
         if is_uf_teleop:
-            obs = robot.get_observation()
+            reset_uf_control()
-            teleop.set_teleop_enabled(True, obs)
         is_paused = False
         is_reset = False
         print("\n********** Teleop Control Loop Start **********")
@@ -143,10 +158,10 @@ def teleop_loop(cfg: TeleopConfig):
                     print('⌨   [ESC] Exit  [Space] Start  [←] Reset')
                 else:
                     if is_reset:
+                        reset_uf_control()
                         is_reset = False
-                        robot.configure()
                     # print('========== Teleop is start ==========')
-                    if is_uf_teleop:
+                    elif is_uf_teleop:
                         obs = robot.get_observation()
                         teleop.set_teleop_enabled(True, obs)
                     print('⌨   [ESC] Exit  [Space] Pause  [←] Reset')

src/lerobot_robot_ufactory/teleoperators/base_teleop/base_teleop.py

@@ -22,3 +22,7 @@ class UFBaseTeleop(Teleoperator):
         当enabled为True且obs不为None时, 顺便设置机械臂初始位置映射
         """
         pass
+
+    def reset_to_robot_observation(self, obs):
+        """Align a physical teleoperator with the robot after robot reset."""
+        pass

src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_teleop.py

@@ -10,6 +10,11 @@ from .gello_teleop_config import GelloTeleopConfig
 
 logger = logging.getLogger(__name__)
 
+GELLO_RESET_SPEED_DEG = 30.0
+GELLO_RESET_TOLERANCE_DEG = 2.0
+GELLO_RESET_CONTROL_HZ = 50.0
+GELLO_RESET_TIMEOUT_MARGIN_S = 5.0
+
 class GelloTeleop(UFBaseTeleop):
     """
     GELLO for xArm tele-op, ref: https://wuphilipp.github.io/gello_site/
@@ -22,6 +27,7 @@ class GelloTeleop(UFBaseTeleop):
         super().__init__(config)
         self.config = config
         self._is_connected = False
+        self._teleop_enabled = False
         self._is_calibrated = True # CHECK!!
 
         from gello.dynamixel.driver import DynamixelDriver
@@ -57,11 +63,6 @@ class GelloTeleop(UFBaseTeleop):
         print(self._dynamixel_robo_config)
         self.dof = len(start_joints)
 
-        if self.config.torque_joint_ids:
-            driver = DynamixelDriver(self.config.torque_joint_ids, port=self.config.port, baudrate=57600)
-            driver.set_torque_mode(True)
-            driver.close()
-
     @property
     def action_features(self) -> dict:
         # Add one more dof for gripper
@@ -95,6 +96,7 @@ class GelloTeleop(UFBaseTeleop):
         from gello.agents.gello_agent import GelloAgent
 
         self.gello_agent = GelloAgent(port=self.config.port, dynamixel_config=self._dynamixel_robo_config)
+        self.gello_agent._robot.set_torque_mode(False)
         if not self._is_calibrated and calibrate:
             logger.info(
                 "Mismatch between calibration values in the motor and the calibration file or no calibration file found"
@@ -117,7 +119,73 @@ class GelloTeleop(UFBaseTeleop):
         # TODO: Go to sync position slowly? Can not 
         pass
 
+    def reset_to_robot_observation(self, obs):
+        """Move the physical Gello to the robot's post-reset joint state."""
+        if not self._is_connected:
+            raise DeviceNotConnectedError("Gello teleop is not connected")
+
+        self._teleop_enabled = False
+        gello_robot = self.gello_agent._robot
+        driver = gello_robot._driver
+        current_raw = np.asarray(driver.get_joints(), dtype=float)
+        target_raw = current_raw.copy()
+        signs = np.asarray(gello_robot._joint_signs, dtype=float)
+        offsets = np.asarray(gello_robot._joint_offsets, dtype=float)
+
+        target_robot_joints = np.asarray(
+            [obs[f"J{i + 1}.pos"] for i in range(self.dof)], dtype=float
+        )
+        target_raw[: self.dof] = target_robot_joints * signs[: self.dof] + offsets[: self.dof]
+
+        if gello_robot.gripper_open_close is not None and len(target_raw) > self.dof:
+            gripper_pos = float(obs.get("gripper.pos", 0.0))
+            gripper_open, gripper_close = gello_robot.gripper_open_close
+            gripper_pos = min(max(gripper_pos, 0.0), 1.0)
+            target_raw[self.dof] = gripper_open + gripper_pos * (gripper_close - gripper_open)
+
+        arm_delta = np.max(np.abs(target_raw[: self.dof] - current_raw[: self.dof]))
+        reset_speed_rad_s = math.radians(GELLO_RESET_SPEED_DEG)
+        duration_s = max(0.5, float(arm_delta / reset_speed_rad_s))
+        deadline = time.perf_counter() + duration_s + GELLO_RESET_TIMEOUT_MARGIN_S
+        success = False
+
+        try:
+            gello_robot.set_torque_mode(True)
+            start_t = time.perf_counter()
+            while True:
+                elapsed_s = time.perf_counter() - start_t
+                progress = min(elapsed_s / duration_s, 1.0)
+                command = current_raw + (target_raw - current_raw) * progress
+                driver.set_joints(command.tolist())
+                if progress >= 1.0:
+                    break
+                time.sleep(1.0 / GELLO_RESET_CONTROL_HZ)
+
+            while time.perf_counter() < deadline:
+                measured_raw = np.asarray(driver.get_joints(), dtype=float)
+                if np.max(np.abs(measured_raw - target_raw)) <= math.radians(GELLO_RESET_TOLERANCE_DEG):
+                    success = True
+                    break
+                driver.set_joints(target_raw.tolist())
+                time.sleep(1.0 / GELLO_RESET_CONTROL_HZ)
+
+            if not success:
+                raise RuntimeError("Gello did not reach the robot initial point before timeout")
+        finally:
+            gello_robot.set_torque_mode(False)
+            gello_robot._last_pos = None
+
+    def set_teleop_enabled(self, enabled: bool, obs=None):
+        if enabled and not self._is_connected:
+            raise DeviceNotConnectedError("Gello teleop is not connected")
+        if not enabled and self._is_connected and hasattr(self, "gello_agent"):
+            self.gello_agent._robot.set_torque_mode(False)
+        self._teleop_enabled = enabled
+        logger.info("Gello teleoperation %s", "enabled" if enabled else "disabled")
+
     def get_action(self) -> dict[str, np.ndarray]:
+        if not self._teleop_enabled:
+            raise RuntimeError("Gello teleop is disabled")
         start = time.perf_counter()
         fake_obs = dict({"joint_state": np.array([0.0]*(self.dof+1))}) # for agent.act() argument, actually no use
         action_array = self.gello_agent.act(fake_obs) # current gello joint pos as np.ndarray
@@ -134,8 +202,8 @@ class GelloTeleop(UFBaseTeleop):
         raise NotImplementedError
 
     def disconnect(self) -> None:
-        if not self._is_connected:
+        if hasattr(self, "gello_agent"):
-            DeviceNotConnectedError(f"{self} is not connected.")
+            self.gello_agent._robot.set_torque_mode(False)
-
         self._is_connected = False
+        self._teleop_enabled = False
         logger.info(f"{self} disconnected.")

src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_teleop_config.py

@@ -14,9 +14,10 @@ class GelloTeleopConfig(TeleoperatorConfig):
     # Others: Calibration angles, joint directions etc
     joint_ids: Tuple[int, ...] = (1, 2, 3, 4, 5, 6, 7)
     joint_signs: Tuple[int, ...] = (1, 1, 1, 1, 1, 1, 1) # if follow the original open-sourced gello xarm7 setup
+    # GELLO encoder calibration reference; this is not the xArm reset target.
     start_joints: Tuple[float, ...] = (0, 0, 0, 90, 0, 90, 0)  # °
     gripper_id: int = 8  # -1: no gripper
-    torque_joint_ids: Tuple[int, ...] = None  # the joints will activate torque mode.
+    torque_joint_ids: Tuple[int, ...] = None  # deprecated; reset controls all GELLO joints.
 
     def __post_init__(self):
         self.id = 'gello_teleop' if self.id is None else self.id

src/lerobot_robot_ufactory/teleoperators/pika_teleop/pika_teleop.py

@@ -111,6 +111,7 @@ class PikaTeleop(UFBaseTeleop, Thread):
                 self._teleop_enabled = True
                 print(f'[{self.prefix}PIKA] Teleoperation is start')
             else:
+                if self._last_action is not None:
                     obs = self._last_action
                     self._last_robot_pose = [obs[f"{self.prefix}pose.x"], obs[f"{self.prefix}pose.y"], obs[f"{self.prefix}pose.z"], obs[f"{self.prefix}pose.rx"], obs[f"{self.prefix}pose.ry"], obs[f"{self.prefix}pose.rz"]]
                     if self.config.use_gripper:

tests/test_gello_reset.py

@@ -0,0 +1,110 @@
+import numpy as np
+import pytest
+
+from lerobot_robot_ufactory.teleoperators.gello_teleop import gello_teleop as gello_module
+from lerobot_robot_ufactory.scripts.uf_lerobot_record import _prepare_recording_episode
+
+
+class FakeDriver:
+    def __init__(self, positions, follow_commands=True):
+        self.positions = np.asarray(positions, dtype=float)
+        self.follow_commands = follow_commands
+        self.commands = []
+
+    def get_joints(self):
+        return self.positions.copy()
+
+    def set_joints(self, positions):
+        self.commands.append(np.asarray(positions, dtype=float))
+        if self.follow_commands:
+            self.positions = self.commands[-1].copy()
+
+
+class FakeGelloRobot:
+    def __init__(self, follow_commands=True):
+        self._driver = FakeDriver([0.0, 0.0, 0.0], follow_commands=follow_commands)
+        self._joint_signs = np.array([1.0, -1.0, 1.0])
+        self._joint_offsets = np.array([0.1, 0.2, 0.0])
+        self.gripper_open_close = (0.0, 1.0)
+        self._last_pos = object()
+        self.torque_calls = []
+
+    def set_torque_mode(self, enabled):
+        self.torque_calls.append(enabled)
+
+
+def make_teleop(robot):
+    teleop = gello_module.GelloTeleop.__new__(gello_module.GelloTeleop)
+    teleop._is_connected = True
+    teleop._teleop_enabled = False
+    teleop.dof = 2
+    teleop.gello_agent = type("FakeAgent", (), {"_robot": robot})()
+    return teleop
+
+
+def patch_clock(monkeypatch):
+    clock = [0.0]
+    monkeypatch.setattr(gello_module.time, "perf_counter", lambda: clock[0])
+    monkeypatch.setattr(
+        gello_module.time,
+        "sleep",
+        lambda seconds: clock.__setitem__(0, clock[0] + seconds),
+    )
+
+
+def test_gello_reset_moves_to_robot_observation_and_disables_torque(monkeypatch):
+    patch_clock(monkeypatch)
+    robot = FakeGelloRobot()
+    teleop = make_teleop(robot)
+
+    teleop.reset_to_robot_observation(
+        {"J1.pos": 0.3, "J2.pos": -0.4, "gripper.pos": 0.5}
+    )
+
+    assert robot.torque_calls == [True, False]
+    assert np.allclose(robot._driver.positions, [0.4, 0.6, 0.5])
+    assert robot._last_pos is None
+    assert teleop._teleop_enabled is False
+
+
+def test_gello_reset_failure_leaves_torque_off_and_teleop_disabled(monkeypatch):
+    patch_clock(monkeypatch)
+    robot = FakeGelloRobot(follow_commands=False)
+    teleop = make_teleop(robot)
+
+    with pytest.raises(RuntimeError, match="did not reach"):
+        teleop.reset_to_robot_observation(
+            {"J1.pos": 0.3, "J2.pos": -0.4, "gripper.pos": 0.5}
+        )
+
+    assert robot.torque_calls == [True, False]
+    assert teleop._teleop_enabled is False
+
+
+def test_recording_reset_disables_before_robot_and_enables_after_alignment():
+    calls = []
+
+    class FakeRobot:
+        def reset_to_initial(self):
+            calls.append("robot_reset")
+
+        def get_observation(self):
+            calls.append("observation")
+            return {"J1.pos": 0.0}
+
+    class FakeTeleop:
+        def set_teleop_enabled(self, enabled, obs=None):
+            calls.append(f"teleop_{enabled}")
+
+        def reset_to_robot_observation(self, obs):
+            calls.append("gello_alignment")
+
+    _prepare_recording_episode(FakeRobot(), FakeTeleop(), True, False)
+
+    assert calls == [
+        "teleop_False",
+        "robot_reset",
+        "observation",
+        "gello_alignment",
+        "teleop_True",
+    ]

tests/test_manual_mode.py

@@ -9,6 +9,7 @@ from lerobot_robot_ufactory.robots.uf_robot.uf_robot_config import UFRobotConfig
 from lerobot_robot_ufactory.scripts import uf_lerobot_record as record_module
 from lerobot_robot_ufactory.scripts.uf_lerobot_record import (
     _manual_action_from_observation,
+    _prepare_recording_episode,
     get_cfg,
 )
 
@@ -20,6 +21,7 @@ class FakeXArm:
         self.axis = 6
         self.error_code = 0
         self.mode = 0
+        self.initial_point = [0.0, -30.0, 0.0, 0.0, 0.0, 30.0]
         self.calls = []
 
     def motion_enable(self, **kwargs):
@@ -41,6 +43,14 @@ class FakeXArm:
         self.calls.append(("set_state", state))
         return 0
 
+    def get_initial_point(self):
+        self.calls.append(("get_initial_point",))
+        return 0, self.initial_point
+
+    def set_servo_angle(self, **kwargs):
+        self.calls.append(("set_servo_angle", kwargs))
+        return 0
+
     def get_err_warn_code(self):
         return 0, [0, 0]
 
@@ -72,7 +82,6 @@ def test_manual_mode_robot_enters_teaching_mode_without_sending_actions(monkeypa
         robot_dof=6,
         control_space="joint",
         gripper_type=0,
-        start_joints=(),
         manual_mode=True,
         teach_sensitivity=4,
     )
@@ -81,10 +90,26 @@ def test_manual_mode_robot_enters_teaching_mode_without_sending_actions(monkeypa
     robot.connect()
     assert arm.mode == 2
     assert ("set_teach_sensitivity", 4) in arm.calls
+    assert robot._initial_point == arm.initial_point
+    assert not any(call[0] == "set_servo_angle" for call in arm.calls)
+
+    robot.reset_to_initial()
+    reset_calls = [call for call in arm.calls if call[0] == "set_servo_angle"]
+    assert reset_calls == [
+        (
+            "set_servo_angle",
+            {
+                "angle": arm.initial_point,
+                "speed": 60,
+                "is_radian": False,
+                "wait": True,
+            },
+        )
+    ]
+    assert arm.mode == 2
 
     action = {"J1.pos": 1.0}
     assert robot.send_action(action) is action
-    assert not any(call[0] == "set_servo_angle" for call in arm.calls)
 
     observation = robot.get_observation()
     assert observation["J1.pos"] == 0.0
@@ -95,6 +120,42 @@ def test_manual_mode_robot_enters_teaching_mode_without_sending_actions(monkeypa
     assert ("disconnect",) in arm.calls
 
 
+def test_robot_reset_uses_sdk_initial_point_in_normal_mode(monkeypatch, tmp_path):
+    from lerobot_robot_ufactory.robots.uf_robot import uf_robot as uf_robot_module
+
+    arm = FakeXArm("192.168.1.245")
+    monkeypatch.setattr(uf_robot_module, "XArmAPI", lambda robot_ip: arm)
+    monkeypatch.setattr(uf_robot_module.time, "sleep", lambda _: None)
+
+    config = UFRobotConfig(
+        id="test_normal_robot",
+        calibration_dir=tmp_path,
+        robot_ip=arm.robot_ip,
+        robot_dof=6,
+        control_space="joint",
+        gripper_type=0,
+    )
+    robot = uf_robot_module.UFRobot(config)
+    assert not hasattr(config, "start_joints")
+    assert not hasattr(config, "start_tcp_pose")
+    robot.connect()
+
+    reset_calls = [call for call in arm.calls if call[0] == "set_servo_angle"]
+    assert reset_calls == [
+        (
+            "set_servo_angle",
+            {
+                "angle": arm.initial_point,
+                "speed": 60,
+                "is_radian": False,
+                "wait": True,
+            },
+        )
+    ]
+
+    robot.disconnect()
+
+
 def test_manual_mode_config_rejects_cartesian_control(tmp_path):
     with pytest.raises(ValueError, match="control_space='joint'"):
         UFRobotConfig(
@@ -201,3 +262,18 @@ def test_manual_record_loop_writes_actual_state_as_action(tmp_path):
 
     dataset.save_episode()
     dataset.finalize()
+
+
+def test_manual_recording_episode_resets_before_recording():
+    class FakeRobot:
+        def __init__(self):
+            self.calls = []
+
+        def reset_to_initial(self):
+            self.calls.append("reset_to_initial")
+
+    robot = FakeRobot()
+
+    _prepare_recording_episode(robot, teleop=None, is_uf_teleop=False, manual_mode=True)
+
+    assert robot.calls == ["reset_to_initial"]