Remove redundant code

config/gello/xarm7_gello_cartesian_record_config.yaml

@@ -1,43 +0,0 @@
-robot:
-  type: uf::robot
-  id: "uf_robot_cartesian"
-  robot_dof: 7
-  control_space: "cartesian"
-  robot_ip: "192.168.1.245"
-  gripper_type: 1
-  enable_logs: false
-  max_linear_velocity: 200
-  min_tcp_z_mm: 50
-  gripper_error_log_path: "logs/xarm7_gripper_errors.log"
-  cameras:
-    camera:
-      type: intelrealsense
-      serial_number_or_name: "242622070583"
-      width: 640
-      height: 480
-      fps: 30
-    camera2:
-      type: intelrealsense
-      serial_number_or_name: "148522072685"
-      width: 640
-      height: 480
-      fps: 30
-
-teleop:
-  type: uf::gello_teleop
-  id: "gello_teleop"
-  port: "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FTB9HYVD-if00-port0"
-  joint_ids: [1, 2, 3, 4, 5, 6, 7]
-  joint_signs: [1, 1, 1, 1, 1, 1, 1]
-  gripper_id: 8
-  gripper_open_deg: 198.28125
-  gripper_close_deg: 155.75
-
-dataset:
-  root: "datasets/xarm7_gello_cartesian_datas"
-  repo_id: "ufactory/xarm7_gello_cartesian_datas"
-  single_task: "Pick up the purple grape and drop into the box on the left."
-  fps: 60
-  episode_time_s: 60
-  reset_time_s: 20
-  push_to_hub: False

src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py

@@ -130,7 +130,6 @@ class UFRobot(Robot, Thread):
         self._local_joint_origins = None
         self._local_model_fault_count = 0
         self._last_safe_joint_target = None
-        self._last_safe_cartesian_target = None
         self._last_guard_path = "not_run"
         self._tcp_z_is_clamped = False
         self._tcp_z_last_log_time = 0.0
@@ -139,8 +138,8 @@ class UFRobot(Robot, Thread):
         self.report_stop_event = Event()
         self._rt_report_normal = False
         self._update_lock = Lock()
-        # The TCP z guard uses the asynchronous RT report to avoid a blocking
+        # Cartesian observations and the joint-mode TCP z guard use the
-        # FK request on every GELLO servo cycle.
+        # asynchronous RT report.
         self._use_rt_report = (
             self._control_space == "cartesian" or self._min_tcp_z_mm is not None
         )
@@ -310,35 +309,25 @@ class UFRobot(Robot, Thread):
 
     def _initialize_tcp_z_guard(self) -> None:
         """Initialize guard state from the robot's current physical target."""
-        if self._min_tcp_z_mm is None or self.real_arm is None:
+        if self._control_space != "joint" or self._min_tcp_z_mm is None or self.real_arm is None:
             return
 
-        if self._control_space == "joint":
+        code, states = self.real_arm.get_joint_states(is_radian=True, num=1)
-            code, states = self.real_arm.get_joint_states(is_radian=True, num=1)
+        if code != 0 or not states or len(states[0]) < self._dof:
-            if code != 0 or not states or len(states[0]) < self._dof:
+            raise RuntimeError(f"Unable to initialize TCP z guard from joint state, code={code}")
-                raise RuntimeError(f"Unable to initialize TCP z guard from joint state, code={code}")
+        target = np.asarray(states[0][:self._dof], dtype=np.float64)
-            target = np.asarray(states[0][:self._dof], dtype=np.float64)
+        if not np.all(np.isfinite(target)):
-            if not np.all(np.isfinite(target)):
+            raise RuntimeError("Unable to initialize TCP z guard from non-finite joint state")
-                raise RuntimeError("Unable to initialize TCP z guard from non-finite joint state")
+        self._last_safe_joint_target = target
-            self._last_safe_joint_target = target
+        if self._tcp_z_guard_backend == "local_projection":
-            if self._tcp_z_guard_backend == "local_projection":
+            if self._local_kinematics is None:
-                if self._local_kinematics is None:
+                raise RuntimeError("Local xArm7 kinematics has not been initialized")
-                    raise RuntimeError("Local xArm7 kinematics has not been initialized")
+            current_z = float(self._local_kinematics.tcp_position(target)[2])
-                current_z = float(self._local_kinematics.tcp_position(target)[2])
+            if current_z < self._min_tcp_z_mm:
-                if current_z < self._min_tcp_z_mm:
+                raise RuntimeError(
-                    raise RuntimeError(
+                    f"Current TCP z {current_z:.2f} mm is below the hard floor "
-                        f"Current TCP z {current_z:.2f} mm is below the hard floor "
+                    f"{self._min_tcp_z_mm:.2f} mm"
-                        f"{self._min_tcp_z_mm:.2f} mm"
+                )
-                    )
-        else:
-            code, pose = self.real_arm.get_position_aa(is_radian=True)
-            if code != 0 or len(pose) < 6:
-                raise RuntimeError(f"Unable to initialize TCP z guard from TCP pose, code={code}")
-            target = np.asarray(pose[:6], dtype=np.float64)
-            if not np.all(np.isfinite(target)):
-                raise RuntimeError("Unable to initialize TCP z guard from non-finite TCP pose")
-            self._last_safe_cartesian_target = target
-
         self._tcp_z_is_clamped = False
         self._tcp_z_last_log_time = 0.0
         self._tcp_z_last_error_log_time = 0.0
@@ -673,30 +662,6 @@ class UFRobot(Robot, Thread):
             and actual_z > self._min_tcp_z_mm + activation_margin
         )
 
-    def _guard_cartesian_target(self, command: list[float]) -> np.ndarray | None:
-        """Clamp a Cartesian target without changing its other five components."""
-        target = np.asarray(command, dtype=np.float64)
-        if self._min_tcp_z_mm is None:
-            return target
-
-        fallback = self._last_safe_cartesian_target
-        try:
-            if target.shape != (6,) or not np.all(np.isfinite(target)):
-                raise ValueError("Cartesian target has invalid shape or contains NaN/Inf")
-            requested_z = float(target[2])
-            if requested_z < self._min_tcp_z_mm:
-                target[2] = self._min_tcp_z_mm
-                self._log_tcp_z_clamp(True, requested_z)
-            else:
-                self._log_tcp_z_clamp(False)
-            self._last_safe_cartesian_target = target.copy()
-            return target
-        except Exception as exc:
-            self._log_tcp_z_guard_error(str(exc))
-            if fallback is None:
-                return None
-            return np.asarray(fallback, dtype=np.float64).copy()
-
     def configure(self) -> None:
         self.real_arm.motion_enable()
         self.real_arm.clean_error()
@@ -804,21 +769,6 @@ class UFRobot(Robot, Thread):
         self._is_calibrated = True
         pass # CHECK! currently No-op
 
-    def get_joint_observation(self) -> dict[str, float]:
-        """Read joint positions for teleoperator alignment in Cartesian mode."""
-        if self.real_arm is None or not self._is_connected:
-            raise ConnectionError("UF Robot is not connected")
-        code, states = self.real_arm.get_joint_states(is_radian=True, num=1)
-        if code != 0 or not states or len(states[0]) < self._dof:
-            raise RuntimeError(f"Failed to read xArm joint states, code={code}")
-        positions = np.asarray(states[0][:self._dof], dtype=np.float64)
-        if not np.all(np.isfinite(positions)):
-            raise RuntimeError("xArm joint states contain NaN or Inf")
-        return {
-            f"{self.prefix}J{i + 1}.pos": float(position)
-            for i, position in enumerate(positions)
-        }
-
     def get_observation(self) -> dict[str, np.ndarray]:
         obs_dict = {}
         self._log_controller_error_if_changed("get_observation")
@@ -1097,48 +1047,6 @@ class UFRobot(Robot, Thread):
         if code is not None and code != 0:
             raise RuntimeError(f"{command} failed, code={code}, {self._motion_status()}")
 
-    def joint_action_to_cartesian(self, action: dict) -> dict:
-        """Convert an absolute joint action to an xArm axis-angle pose.
-
-        GELLO reports absolute joint positions while Cartesian control expects
-        ``pose.x/y/z/rx/ry/rz``. The xArm controller's FK is used so the
-        configured robot model and tool frame stay authoritative. The returned
-        action is passed through ``send_action`` for Cartesian safety checks.
-        """
-        if self.real_arm is None or not self._is_connected:
-            raise ConnectionError("UF Robot is not connected")
-
-        joint_keys = [f"{self.prefix}J{i}.pos" for i in range(1, self._dof + 1)]
-        try:
-            joints = [float(action[key]) for key in joint_keys]
-        except (KeyError, TypeError, ValueError) as exc:
-            raise ValueError(f"Invalid joint action; expected keys {joint_keys}") from exc
-
-        if not np.all(np.isfinite(joints)):
-            raise ValueError("Joint action contains NaN or Inf")
-
-        code, pose = self.real_arm.get_forward_kinematics(
-            joints,
-            input_is_radian=True,
-            return_is_radian=True,
-        )
-        pose = np.asarray(pose, dtype=np.float64)
-        if code != 0 or pose.shape[0] < 6 or not np.all(np.isfinite(pose[:6])):
-            raise RuntimeError(f"xArm forward kinematics failed, code={code}, pose={pose}")
-
-        cartesian = {
-            f"{self.prefix}pose.x": float(pose[0]),
-            f"{self.prefix}pose.y": float(pose[1]),
-            f"{self.prefix}pose.z": float(pose[2]),
-            f"{self.prefix}pose.rx": float(pose[3]),
-            f"{self.prefix}pose.ry": float(pose[4]),
-            f"{self.prefix}pose.rz": float(pose[5]),
-        }
-        gripper_key = f"{self.prefix}gripper.pos"
-        if gripper_key in action:
-            cartesian[gripper_key] = float(action[gripper_key])
-        return cartesian
-
     def send_action(self, action: dict) -> np.ndarray:
         if not self._is_connected:
             raise ConnectionError()
@@ -1228,12 +1136,7 @@ class UFRobot(Robot, Thread):
             if not self._rt_report_normal:
                 raise ConnectionError("RT Report for target robot NOT READY! ")
             cmd_list = [action[f"{self.prefix}pose.x"], action[f"{self.prefix}pose.y"], action[f"{self.prefix}pose.z"], action[f"{self.prefix}pose.rx"], action[f"{self.prefix}pose.ry"], action[f"{self.prefix}pose.rz"]]
-            safe_cmd = self._guard_cartesian_target(cmd_list)
+            self.real_arm.set_position_aa(axis_angle_pose=cmd_list, speed=lin_spd, is_radian=True, wait=False)
-            if safe_cmd is not None:
-                safe_cmd = safe_cmd.tolist()
-                for i, key in enumerate(("x", "y", "z", "rx", "ry", "rz")):
-                    safe_action[f"{self.prefix}pose.{key}"] = float(safe_cmd[i])
-                self.real_arm.set_position_aa(axis_angle_pose=safe_cmd, speed=lin_spd, is_radian=True, wait=False)
             # self.real_arm.set_position(*cmd_list, radius=0, speed=lin_spd, is_radian=True, wait=False)
 
         if self._cmd_cnt < 99999:

src/lerobot_robot_ufactory/scripts/uf_lerobot_eval.py

@@ -8,7 +8,6 @@ from dataclasses import asdict, dataclass
 from pprint import pformat
 import numpy as np
 import lerobot_robot_ufactory # patch
-from lerobot.scripts.lerobot_record import register_third_party_plugins
 from lerobot.datasets.pipeline_features import aggregate_pipeline_dataset_features, create_initial_features
 from lerobot.datasets.utils import build_dataset_frame, combine_feature_dicts
 from lerobot.policies.utils import make_robot_action
@@ -56,15 +55,6 @@ def continuous_rotvec(new_rv, prev_rv):
             new_rv = -(2 * np.pi - angle) * axis
     return new_rv
 
-def blend_poses(pose_a, pose_b, alpha):
-    """位姿混合: (1-alpha)*A + alpha*B, 旋转用 SO(3) 插值。
-    先将 pose_b 的 rotvec 归一化到与 pose_a 同符号半球，避免 ±π 跳变破坏线性混合。"""
-    blended_pos = (1 - alpha) * np.array(pose_a[:3]) + alpha * np.array(pose_b[:3])
-    # 旋转用线性混合 rotvec (delta 很小时近似 SLERP)
-    rot_b = continuous_rotvec(np.array(pose_b[3:6]), np.array(pose_a[3:6]))
-    blended_rot = (1 - alpha) * np.array(pose_a[3:6]) + alpha * rot_b
-    return np.concatenate([blended_pos, blended_rot]).tolist()
-
 def compute_relative_axis_angle(rot_prev, rot_curr):
     """
     计算两个轴角之间的相对旋转。
@@ -212,24 +202,6 @@ def eval_loop(cfg: EvalConfig, relative=False, rx_continuous=False, safety_guard
     device = get_safe_torch_device(policy.config.device, log=True)
     sleep_time_s = 1 / dataset_metadata.fps
 
-    # Gripper look-ahead: denormalization stats for peeking into the action queue
-    _gripper_mean = dataset_metadata.stats['action']['mean'][-1].item()
-    _gripper_std = dataset_metadata.stats['action']['std'][-1].item()
-    _gripper_min = dataset_metadata.stats['action']['min'][-1].item()
-    _gripper_max = dataset_metadata.stats['action']['max'][-1].item()
-    is_act_policy = hasattr(policy.config, 'chunk_size')
-    # ACT: lookahead 30 (~1s) 补偿 chunk 慢启动; DP: 队列仅 8 步，lookahead 4
-    GRIPPER_LOOKAHEAD = 0 if is_act_policy else 4
-
-    # =====================================================
-    # Chunk boundary smoothing: only damp large discontinuities at action chunk
-    # boundaries (mean ~6mm jump) while preserving smooth within-chunk motion (~1mm).
-    # When step-to-step cmd change exceeds SMOOTH_THRESHOLD, clamp it to that limit.
-    # =====================================================
-    SMOOTH_THRESHOLD = 0  # mm: smooth chunk boundary jumps while preserving trajectory
-    SMOOTH_ROT_THRESHOLD = 0.05  # rad: max allowed rotation jump per step
-    prev_smoothed_pose = None
-
     print("\n********** Policy Eval Episode Loop Start **********")
     print(f'relative: {relative}')
 
@@ -248,10 +220,8 @@ def eval_loop(cfg: EvalConfig, relative=False, rx_continuous=False, safety_guard
         prev_robot_dict = {}
         prev_action_dict = {}
 
-        is_multiple_robot = False
         if hasattr(cfg.robot, 'robots'):
             keys = cfg.robot.robots.keys()
-            is_multiple_robot = True
         else:
             keys = ['']
         for key in keys:
@@ -284,7 +254,6 @@ def eval_loop(cfg: EvalConfig, relative=False, rx_continuous=False, safety_guard
             obs = robot.get_observation()
 
             curr_robot_dict = {}
-            curr_action_dict = {}
             for key in keys:
                 prefix = f'.{key}' if key else ''
                 is_tcp = f'{prefix}pose.x' in obs and f'{prefix}pose.y' in obs and f'{prefix}pose.z' in obs and f'{prefix}pose.rx' in obs and f'{prefix}pose.ry' in obs and f'{prefix}pose.rz' in obs
@@ -348,67 +317,13 @@ def eval_loop(cfg: EvalConfig, relative=False, rx_continuous=False, safety_guard
 
                 # robot_action_to_send[f'{prefix}pose.z'] = max(robot_action_to_send[f'{prefix}pose.z'], 199)
 
-                # Rate-limited smoothing: cap position velocity to reduce chunk boundary jerks
-                # Uses vector-norm clamping to preserve motion direction
-                if SMOOTH_THRESHOLD > 0:
-                    pos_keys = [f'{prefix}pose.x', f'{prefix}pose.y', f'{prefix}pose.z']
-                    rot_keys = [f'{prefix}pose.rx', f'{prefix}pose.ry', f'{prefix}pose.rz']
-                    if prev_smoothed_pose is None:
-                        prev_smoothed_pose = {k: robot_action_to_send[k] for k in pos_keys + rot_keys}
-                    else:
-                        # Vector-norm clamp on position (preserves direction)
-                        delta_pos = np.array([robot_action_to_send[k] - prev_smoothed_pose[k] for k in pos_keys])
-                        norm = np.linalg.norm(delta_pos)
-                        if norm > SMOOTH_THRESHOLD:
-                            delta_pos = delta_pos * (SMOOTH_THRESHOLD / norm)
-                        for i, k in enumerate(pos_keys):
-                            prev_smoothed_pose[k] = prev_smoothed_pose[k] + delta_pos[i]
-                            robot_action_to_send[k] = prev_smoothed_pose[k]
-                        # Per-axis clamp on rotation
-                        for k in rot_keys:
-                            delta = robot_action_to_send[k] - prev_smoothed_pose[k]
-                            if abs(delta) > SMOOTH_ROT_THRESHOLD:
-                                delta = SMOOTH_ROT_THRESHOLD * (1 if delta > 0 else -1)
-                            prev_smoothed_pose[k] = prev_smoothed_pose[k] + delta
-                            robot_action_to_send[k] = prev_smoothed_pose[k]
-
                 if relative:
                     curr_action_pose = np.array([
                         robot_action_to_send[f'{prefix}pose.x'], robot_action_to_send[f'{prefix}pose.y'], robot_action_to_send[f'{prefix}pose.z'],
                         robot_action_to_send[f'{prefix}pose.rx'], robot_action_to_send[f'{prefix}pose.ry'], robot_action_to_send[f'{prefix}pose.rz']
                     ])
-                    # 相对增量模式: 漂移修正，将指令位姿温和拉回实际位姿
-                    # prev_action_pose = blend_poses(curr_action_pose, curr_robot_pose, 0.05)
                     prev_action_dict[key]['pose'] = curr_action_pose
 
-            # # Gripper look-ahead: peek ahead in the action queue to compensate
-            # # for the slow ramp in the ACT chunk (eliminates 1-2s gripper delay)
-            # gripper_raw = robot_action_to_send.get('left.gripper.pos', 0)
-            # if hasattr(policy, '_action_queue') and len(policy._action_queue) > 0:
-            #     # ACT: 队列为 deque of tensors, 归一化方式 MEAN_STD
-            #     lookahead_idx = min(GRIPPER_LOOKAHEAD, len(policy._action_queue) - 1)
-            #     future_gripper_norm = policy._action_queue[lookahead_idx][0, -1].item()
-            #     gripper_raw = future_gripper_norm * _gripper_std + _gripper_mean
-            # elif hasattr(policy, '_queues') and 'action' in policy._queues and len(policy._queues['action']) > 0:
-            #     # DP: 队列结构不同, 归一化方式 MIN_MAX → [-1,1] → [min,max]
-            #     lookahead_idx = min(GRIPPER_LOOKAHEAD, len(policy._queues['action']) - 1)
-            #     future_gripper_norm = policy._queues['action'][lookahead_idx][0, -1].item()
-            #     gripper_raw = (future_gripper_norm + 1) / 2 * (_gripper_max - _gripper_min) + _gripper_min
-            # robot_action_to_send['left.gripper.pos'] = 1.0 if gripper_raw > 0.4 else 0.0
-
-            # gripper_raw = robot_action_to_send.get('right.gripper.pos', 0)
-            # if hasattr(policy, '_action_queue') and len(policy._action_queue) > 0:
-            #     # ACT: 队列为 deque of tensors, 归一化方式 MEAN_STD
-            #     lookahead_idx = min(GRIPPER_LOOKAHEAD, len(policy._action_queue) - 1)
-            #     future_gripper_norm = policy._action_queue[lookahead_idx][0, -1].item()
-            #     gripper_raw = future_gripper_norm * _gripper_std + _gripper_mean
-            # elif hasattr(policy, '_queues') and 'action' in policy._queues and len(policy._queues['action']) > 0:
-            #     # DP: 队列结构不同, 归一化方式 MIN_MAX → [-1,1] → [min,max]
-            #     lookahead_idx = min(GRIPPER_LOOKAHEAD, len(policy._queues['action']) - 1)
-            #     future_gripper_norm = policy._queues['action'][lookahead_idx][0, -1].item()
-            #     gripper_raw = (future_gripper_norm + 1) / 2 * (_gripper_max - _gripper_min) + _gripper_min
-            # robot_action_to_send['right.gripper.pos'] = 1.0 if gripper_raw > 0.4 else 0.0
-
             # Safety check: any violation triggers an e-stop; the action is
             # NOT sent and the loop waits for the operator to press right arrow
             if safety_guard is not None:

src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py

@@ -484,23 +484,6 @@ def record_loop(
             else:
                 act = teleop.get_action()
 
-            # GELLO reports absolute joint positions. In Cartesian robot mode,
-            # convert them with the xArm FK before the normal action pipeline;
-            # send_action() then applies the Cartesian safety guard and sends
-            # the resulting pose with set_position_aa().
-            joint_action_keys = [
-                f"{getattr(robot, 'prefix', '')}J{i}.pos"
-                for i in range(1, getattr(robot, "_dof", 0) + 1)
-            ]
-            if (
-                getattr(robot, "_control_space", None) == "cartesian"
-                and hasattr(robot, "joint_action_to_cartesian")
-                and joint_action_keys
-                and act is not None
-                and all(key in act for key in joint_action_keys)
-            ):
-                act = robot.joint_action_to_cartesian(act)
-
             # (space mouse) from delta Cartesian cmd to absolute command
             if act is not None and "pose.dx" in act:
                 last_robot_cmd.update({"pose.x": last_robot_cmd["pose.x"] + act["pose.dx"], "pose.y": last_robot_cmd["pose.y"] + act["pose.dy"], "pose.z": last_robot_cmd["pose.z"] + act["pose.dz"]})
@@ -602,11 +585,6 @@ def _prepare_recording_episode(robot, teleop, is_uf_teleop, manual_mode):
 
     if is_uf_teleop:
         obs = robot.get_observation()
-        # Cartesian observations expose TCP pose, while GELLO alignment needs
-        # the current absolute joint positions.
-        joint_observation = getattr(robot, "get_joint_observation", None)
-        if joint_observation is not None:
-            obs.update(joint_observation())
         teleop.set_teleop_enabled(True, obs)
 
 

src/lerobot_robot_ufactory/scripts/uf_robot_teleop.py

@@ -10,7 +10,6 @@ from dataclasses import asdict, dataclass
 from datetime import datetime
 from pprint import pformat
 import lerobot_robot_ufactory # patch
-from lerobot.scripts.lerobot_record import register_third_party_plugins
 from lerobot.processor import (
     make_default_processors,
 )

tests/test_tcp_z_safety.py

@@ -43,7 +43,6 @@ def make_guard_robot(arm, min_tcp_z_mm=100.0, control_space="joint"):
     robot._control_space = control_space
     robot._min_tcp_z_mm = min_tcp_z_mm
     robot._last_safe_joint_target = np.asarray([0.25] * 7)
-    robot._last_safe_cartesian_target = np.asarray([250.0, 0.0, 120.0, 0.0, 0.0, 0.0])
     robot._tcp_z_is_clamped = False
     robot._tcp_z_last_log_time = 0.0
     robot._tcp_z_last_error_log_time = 0.0
@@ -145,14 +144,6 @@ def test_joint_guard_rejects_discontinuous_ik_solution():
     assert np.allclose(result, [0.25] * 7)
 
 
-def test_cartesian_guard_preserves_other_axes_and_clamps_z():
-    robot = make_guard_robot(object(), control_space="cartesian")
-
-    result = robot._guard_cartesian_target([300.0, 20.0, 90.0, 0.1, 0.2, 0.3])
-
-    assert result == pytest.approx([300.0, 20.0, 100.0, 0.1, 0.2, 0.3])
-
-
 def test_send_action_sends_and_returns_clamped_joint_target():
     arm = FakeKinematicsArm()
     arm.error_code = 0