diff --git a/config/gello/xarm7_gello_cartesian_record_config.yaml b/config/gello/xarm7_gello_cartesian_record_config.yaml deleted file mode 100644 index 12d9c1a..0000000 --- a/config/gello/xarm7_gello_cartesian_record_config.yaml +++ /dev/null @@ -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 diff --git a/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py b/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py index 4990357..e5378af 100644 --- a/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py +++ b/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 - # FK request on every GELLO servo cycle. + # Cartesian observations and the joint-mode TCP z guard use the + # 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) - 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}") - target = np.asarray(states[0][:self._dof], dtype=np.float64) - if not np.all(np.isfinite(target)): - raise RuntimeError("Unable to initialize TCP z guard from non-finite joint state") - self._last_safe_joint_target = target - if self._tcp_z_guard_backend == "local_projection": - if self._local_kinematics is None: - raise RuntimeError("Local xArm7 kinematics has not been initialized") - current_z = float(self._local_kinematics.tcp_position(target)[2]) - if current_z < self._min_tcp_z_mm: - raise RuntimeError( - f"Current TCP z {current_z:.2f} mm is below the hard floor " - 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 - + 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"Unable to initialize TCP z guard from joint state, code={code}") + target = np.asarray(states[0][:self._dof], dtype=np.float64) + if not np.all(np.isfinite(target)): + raise RuntimeError("Unable to initialize TCP z guard from non-finite joint state") + self._last_safe_joint_target = target + if self._tcp_z_guard_backend == "local_projection": + if self._local_kinematics is None: + raise RuntimeError("Local xArm7 kinematics has not been initialized") + current_z = float(self._local_kinematics.tcp_position(target)[2]) + if current_z < self._min_tcp_z_mm: + raise RuntimeError( + f"Current TCP z {current_z:.2f} mm is below the hard floor " + f"{self._min_tcp_z_mm:.2f} mm" + ) 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) - 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_aa(axis_angle_pose=cmd_list, 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: diff --git a/src/lerobot_robot_ufactory/scripts/uf_lerobot_eval.py b/src/lerobot_robot_ufactory/scripts/uf_lerobot_eval.py index f698f20..b3ea4f8 100644 --- a/src/lerobot_robot_ufactory/scripts/uf_lerobot_eval.py +++ b/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: diff --git a/src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py b/src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py index 936d3d4..a9a29d5 100644 --- a/src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py +++ b/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) diff --git a/src/lerobot_robot_ufactory/scripts/uf_robot_teleop.py b/src/lerobot_robot_ufactory/scripts/uf_robot_teleop.py index dcc0f5a..2d68619 100644 --- a/src/lerobot_robot_ufactory/scripts/uf_robot_teleop.py +++ b/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, ) diff --git a/tests/test_tcp_z_safety.py b/tests/test_tcp_z_safety.py index a885ad1..d9a9364 100644 --- a/tests/test_tcp_z_safety.py +++ b/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