From 3a87382ba4ca2dd214373aa051fd54f110173e9c Mon Sep 17 00:00:00 2001 From: Elvin <223015152@link.cuhk.edu.cn> Date: Wed, 4 Mar 2026 11:02:06 +0800 Subject: [PATCH 01/26] chore(ci): make quality checks pass across repo - fix ruff violations in dataset, recording, and value modules - fix typos findings and markdown/workflow lint issues - update pages workflow checkout to disable credential persistence - apply pre-commit auto-formatting and EOF normalization - include requested repo housekeeping updates (.gitignore and README) --- .github/workflows/deploy-website-pages.yml | 2 + .gitignore | 1 + AGENTS.md | 2 +- README.md | 15 +++++- src/lerobot/configs/value_train.py | 10 ++-- src/lerobot/datasets/dataset_tools.py | 6 ++- src/lerobot/datasets/lerobot_dataset.py | 7 ++- src/lerobot/rl/acp_dataset_stats.py | 5 +- src/lerobot/scripts/lerobot_dataset_report.py | 18 +++++-- .../scripts/lerobot_human_inloop_record.py | 8 +-- src/lerobot/scripts/lerobot_record.py | 24 ++++----- src/lerobot/scripts/lerobot_value_infer.py | 12 ++--- src/lerobot/scripts/lerobot_value_train.py | 4 +- src/lerobot/scripts/recording_hil.py | 5 +- src/lerobot/scripts/recording_loop.py | 52 +++++++++++-------- src/lerobot/scripts/value_infer_viz.py | 14 +++-- .../bi_so_leader/bi_so_leader.py | 8 +-- .../teleoperators/so_leader/so_leader.py | 4 +- src/lerobot/utils/recording_annotations.py | 1 - src/lerobot/values/__init__.py | 2 +- .../values/pistar06/modeling_pistar06.py | 33 ++++++------ .../values/pistar06/processor_pistar06.py | 6 +-- tests/datasets/test_dataset_tools.py | 4 +- tests/test_control_robot.py | 11 ++-- .../value/test_lerobot_value_infer_sampler.py | 8 ++- tests/value/test_lerobot_value_infer_viz.py | 3 +- tests/value/test_pistar06_value_stack.py | 7 +-- 27 files changed, 152 insertions(+), 120 deletions(-) diff --git a/.github/workflows/deploy-website-pages.yml b/.github/workflows/deploy-website-pages.yml index d349a28..4ffcd1f 100644 --- a/.github/workflows/deploy-website-pages.yml +++ b/.github/workflows/deploy-website-pages.yml @@ -29,6 +29,8 @@ jobs: steps: - name: Checkout uses: actions/checkout@v4 + with: + persist-credentials: false - name: Setup Pages uses: actions/configure-pages@v5 diff --git a/.gitignore b/.gitignore index b47e22c..d7e9f43 100644 --- a/.gitignore +++ b/.gitignore @@ -177,3 +177,4 @@ outputs/ *.urdf *.xml *.part +AGENTS.md diff --git a/AGENTS.md b/AGENTS.md index ed81e11..f4fddd2 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -22,7 +22,7 @@ If using `conda activate /llm_jzm/cache/conda_env/lerobot`: - Treat it as shared machine mode. - Hugging Face cache is not in the default location. - Set Hugging Face token to the shared token for runs on this machine. -- Set Wandb token to hte shared token for runs on this machine +- Set Wandb token to the shared token for runs on this machine Use: diff --git a/README.md b/README.md index 871422f..390fd86 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@

Evo-RL

- project website + project website lerobot version wechat post wechat group join us @@ -84,6 +84,7 @@ For SO101 setup, please follow the [official SO101 tutorial](https://wiki.seeeds #### Device path recommendation Recommended path strategy: + - **Robot serial:** use `/dev/serial/by-id/` (stable across reboots). - **Cameras:** prefer `/dev/v4l/by-id/`; if IDs are not unique, use `/dev/v4l/by-path/`. - In examples below: robot ports use `by-id`, camera paths use `by-path`. @@ -213,6 +214,7 @@ lerobot-human-inloop-record \ Recommendation: use **`fourcc: "MJPG"`** for OpenCV and **`warmup_s`** for RealSense. In this example `front` uses RealSense, but you can switch it to OpenCV with the same structure. Hotkeys: + - `i`: toggle intervention mode (policy <-> teleop takeover) - `s`: mark success and end current episode - `f`: mark failure and end current episode @@ -230,7 +232,7 @@ This prints: dataset meta, totals, episode-length stats/histogram, success/inter ### 4) Value Function Training -Train the value function on the current dataset. Current default: [Pi*0.6](https://www.pi.website/blog/pistar06) (`--value.type=pistar06`). +Train the value function on the current dataset. Current default: [Pi\*0.6](https://www.pi.website/blog/pistar06) (`--value.type=pistar06`). Single-GPU template: @@ -260,6 +262,7 @@ CUDA_VISIBLE_DEVICES= accelerate launch \ ``` To plug in a different value function, minimal path in this repo: + - Add `src/lerobot/values//configuration_.py` with `@PreTrainedConfig.register_subclass("")`. - Add `src/lerobot/values//modeling_.py` with `Policy(PreTrainedPolicy)` (implement at least `forward`, `predict_value`, and `build_training_raw_batch_hook` for `lerobot-value-train`). - Add `src/lerobot/values//processor_.py` with `make__pre_post_processors(...)`. @@ -268,6 +271,7 @@ To plug in a different value function, minimal path in this repo: ### 5) Value Inference Infer value signals and write value/advantage/indicator back to the dataset: + - `value`: estimated return-to-go of the current frame. - `advantage`: relative improvement signal (higher means better-than-baseline trajectory quality). - `indicator`: binarized training tag derived from advantage. @@ -302,10 +306,12 @@ CUDA_VISIBLE_DEVICES= accelerate launch \ ``` Parameter notes: + - `--acp.n_step`: n-step advantage horizon. - `--acp.positive_ratio`: positive label ratio after advantage binarization (e.g., `0.3` = top 30% per task). Expected new columns: + - `complementary_info.value_` - `complementary_info.advantage_` - `complementary_info.acp_indicator_` @@ -313,6 +319,7 @@ Expected new columns: These columns are written back to the original dataset specified by `--dataset.repo_id`. Visualization (optional): + - Enable with `--viz.enable=true`. - Output videos are saved to `outputs/value_infer//value/viz/`. - Each video overlays a value curve and per-frame text (`advantage`, `acp_indicator`) on top of camera frames. @@ -323,6 +330,7 @@ Visualization (optional): - `--viz.vcodec=` and `--viz.frame_storage_mode=memory|disk` Value overlay examples (SO101, smooth=11): + - GIF assets are stored under `website/assets/gifs/`. ### 6) Policy Training @@ -354,6 +362,7 @@ lerobot-train \ `--acp.indicator_dropout_prob` controls tag drop rate in task text; `0.3` helps learn both tagged and untagged conditions. Important checks: + - `--acp.indicator_field` must exist in the dataset and be **binary (`0/1`)**. Multi-GPU template: @@ -396,6 +405,7 @@ lerobot-human-inloop-record \ ``` Dataset continuation options: + - **Append in place:** keep `--resume=true` and continue recording into the same dataset. - **Merge multiple rounds:** use the official dataset editor to merge separate datasets. @@ -407,6 +417,7 @@ lerobot-edit-dataset \ ``` Additional data attributes vs default `lerobot-record` behavior: + - `complementary_info.policy_action`: policy output action at each step. - `complementary_info.is_intervention`: whether current step is in intervention. - `complementary_info.state`: intervention state-machine state. diff --git a/src/lerobot/configs/value_train.py b/src/lerobot/configs/value_train.py index 0636abc..1569331 100644 --- a/src/lerobot/configs/value_train.py +++ b/src/lerobot/configs/value_train.py @@ -41,7 +41,9 @@ class ValueTargetsConfig: if self.c_fail_coef < 0: raise ValueError("'targets.c_fail_coef' must be non-negative.") if not self.target_field.startswith("observation."): - raise ValueError("'targets.target_field' must start with 'observation.' to survive processor conversion.") + raise ValueError( + "'targets.target_field' must start with 'observation.' to survive processor conversion." + ) @dataclass @@ -113,7 +115,7 @@ class ValueTrainPipelineConfig(HubMixin): self.targets.validate() if hasattr(self.value, "target_key"): - setattr(self.value, "target_key", self.targets.target_field) + self.value.target_key = self.targets.target_field if not self.job_name: self.job_name = f"{self.value.type}_value" @@ -136,7 +138,9 @@ class ValueTrainPipelineConfig(HubMixin): self.scheduler = self.value.get_scheduler_preset() if self.value.push_to_hub and not self.value.repo_id: - raise ValueError("'value.repo_id' argument missing. Please specify it to push the model to the hub.") + raise ValueError( + "'value.repo_id' argument missing. Please specify it to push the model to the hub." + ) @classmethod def __get_path_fields__(cls) -> list[str]: diff --git a/src/lerobot/datasets/dataset_tools.py b/src/lerobot/datasets/dataset_tools.py index c9a285c..27e67ae 100644 --- a/src/lerobot/datasets/dataset_tools.py +++ b/src/lerobot/datasets/dataset_tools.py @@ -836,7 +836,11 @@ def _copy_and_reindex_episodes_metadata( for key in schema.names: if key in {"episode_index", "tasks", "length"}: continue - if key.startswith("stats/") or key.startswith("__index_level_") or key.startswith("meta/episodes/"): + if ( + key.startswith("stats/") + or key.startswith("__index_level_") + or key.startswith("meta/episodes/") + ): continue if key not in seen_preserved_keys: seen_preserved_keys.add(key) diff --git a/src/lerobot/datasets/lerobot_dataset.py b/src/lerobot/datasets/lerobot_dataset.py index 6538b56..a4c3a4c 100644 --- a/src/lerobot/datasets/lerobot_dataset.py +++ b/src/lerobot/datasets/lerobot_dataset.py @@ -729,15 +729,14 @@ class LeRobotDataset(torch.utils.data.Dataset): # Check if cached dataset contains all requested episodes if not self._check_cached_episodes_sufficient(): raise FileNotFoundError("Cached dataset doesn't contain all requested episodes") - except (AssertionError, FileNotFoundError, NotADirectoryError): + except (AssertionError, FileNotFoundError, NotADirectoryError) as err: if is_valid_version(self.revision): self.revision = get_safe_version(self.repo_id, self.revision) self.download(download_videos) self.hf_dataset = self.load_hf_dataset() if not self._check_cached_episodes_sufficient(): missing_videos_hint = ( - " Missing video files are required by this dataset. " - "Retry with `download_videos=True`." + " Missing video files are required by this dataset. Retry with `download_videos=True`." if len(self.meta.video_keys) > 0 and not download_videos else "" ) @@ -745,7 +744,7 @@ class LeRobotDataset(torch.utils.data.Dataset): "Cached dataset is incomplete after download. " "Some requested episodes or files are missing." f"{missing_videos_hint}" - ) + ) from err # Create mapping from absolute indices to relative indices when only a subset of the episodes are loaded # Build a mapping: absolute_index -> relative_index_in_filtered_dataset diff --git a/src/lerobot/rl/acp_dataset_stats.py b/src/lerobot/rl/acp_dataset_stats.py index 46ba3e0..89cc0e2 100644 --- a/src/lerobot/rl/acp_dataset_stats.py +++ b/src/lerobot/rl/acp_dataset_stats.py @@ -60,10 +60,7 @@ def _from_meta_stats(dataset: Any, indicator_field: str) -> ACPIndicatorStats | count_raw = _stats_entry_to_scalar(field_stats, "count") total_count = int(round(count_raw)) if count_raw is not None else -1 - if total_count >= 0: - positive_count = int(round(ratio * total_count)) - else: - positive_count = -1 + positive_count = int(round(ratio * total_count)) if total_count >= 0 else -1 return ACPIndicatorStats( indicator_field=indicator_field, diff --git a/src/lerobot/scripts/lerobot_dataset_report.py b/src/lerobot/scripts/lerobot_dataset_report.py index ff1a00d..2b87231 100644 --- a/src/lerobot/scripts/lerobot_dataset_report.py +++ b/src/lerobot/scripts/lerobot_dataset_report.py @@ -136,7 +136,9 @@ def build_report(dataset_root: Path) -> dict[str, Any]: episodes_df = episodes_ds.to_pandas() actual_episode_count = int(len(episodes_df)) - episode_success_labels = episodes_df["episode_success"].tolist() if "episode_success" in episodes_df else [] + episode_success_labels = ( + episodes_df["episode_success"].tolist() if "episode_success" in episodes_df else [] + ) normalized_labels = [_normalize_label(v) for v in episode_success_labels] success_count = sum(1 for v in normalized_labels if v == "success") @@ -153,7 +155,11 @@ def build_report(dataset_root: Path) -> dict[str, Any]: intervention_frames = 0 intervention_episode_count = 0 - if intervention_col in data_schema_names and episode_index_col in data_schema_names and actual_frame_count > 0: + if ( + intervention_col in data_schema_names + and episode_index_col in data_schema_names + and actual_frame_count > 0 + ): table = data_dataset.to_table(columns=[episode_index_col, intervention_col]) episode_indices = table[episode_index_col].to_pylist() intervention_values = table[intervention_col].to_pylist() @@ -190,10 +196,14 @@ def build_report(dataset_root: Path) -> dict[str, Any]: fps = float(info.get("fps", 0) or 0) episode_lengths_frames = ( - episodes_df["length"].astype(int).tolist() if "length" in episodes_df and actual_episode_count > 0 else [] + episodes_df["length"].astype(int).tolist() + if "length" in episodes_df and actual_episode_count > 0 + else [] ) episode_lengths_seconds = [frames / fps for frames in episode_lengths_frames] if fps > 0 else [] - length_mean = float(sum(episode_lengths_seconds) / len(episode_lengths_seconds)) if episode_lengths_seconds else 0.0 + length_mean = ( + float(sum(episode_lengths_seconds) / len(episode_lengths_seconds)) if episode_lengths_seconds else 0.0 + ) length_min = float(min(episode_lengths_seconds)) if episode_lengths_seconds else 0.0 length_max = float(max(episode_lengths_seconds)) if episode_lengths_seconds else 0.0 length_histogram = _build_episode_length_histogram(episode_lengths_seconds, bins=20) diff --git a/src/lerobot/scripts/lerobot_human_inloop_record.py b/src/lerobot/scripts/lerobot_human_inloop_record.py index e7a9398..da85708 100644 --- a/src/lerobot/scripts/lerobot_human_inloop_record.py +++ b/src/lerobot/scripts/lerobot_human_inloop_record.py @@ -22,16 +22,16 @@ Phase A/B goals: - Keyboard-toggled intervention lets teleop temporarily take over execution. """ -import logging import json +import logging import time from pathlib import Path from typing import Any from lerobot.configs import parser from lerobot.scripts.lerobot_record import RecordConfig, record -from lerobot.utils.import_utils import register_third_party_plugins from lerobot.utils.constants import HF_LEROBOT_HOME +from lerobot.utils.import_utils import register_third_party_plugins from lerobot.utils.recording_annotations import EPISODE_FAILURE, infer_collector_policy_version @@ -64,7 +64,9 @@ def _load_failure_reset_pose(pose_path: Path) -> dict[str, float]: payload = json.load(f) joint_pos_raw = payload["joint_pos"] if isinstance(payload, dict) and "joint_pos" in payload else payload if not isinstance(joint_pos_raw, dict): - raise ValueError(f"Invalid failure reset pose payload in {pose_path}: expected dict, got {type(joint_pos_raw)}") + raise ValueError( + f"Invalid failure reset pose payload in {pose_path}: expected dict, got {type(joint_pos_raw)}" + ) joint_pos = {str(key): float(value) for key, value in joint_pos_raw.items() if str(key).endswith(".pos")} if not joint_pos: raise ValueError(f"Invalid failure reset pose payload in {pose_path}: no '.pos' joints found.") diff --git a/src/lerobot/scripts/lerobot_record.py b/src/lerobot/scripts/lerobot_record.py index f81cbe0..be9a55d 100644 --- a/src/lerobot/scripts/lerobot_record.py +++ b/src/lerobot/scripts/lerobot_record.py @@ -97,6 +97,13 @@ from lerobot.robots import ( # noqa: F401 so_follower, unitree_g1 as unitree_g1_robot, ) +from lerobot.scripts.recording_hil import ( + ACPInferenceConfig, + PolicySyncDualArmExecutor, + _capture_policy_runtime_state, # noqa: F401 + _predict_policy_action_with_acp_inference, # noqa: F401 +) +from lerobot.scripts.recording_loop import record_loop from lerobot.teleoperators import ( # noqa: F401 TeleoperatorConfig, bi_openarm_leader, @@ -110,13 +117,6 @@ from lerobot.teleoperators import ( # noqa: F401 so_leader, unitree_g1, ) -from lerobot.scripts.recording_hil import ( - ACPInferenceConfig, - PolicySyncDualArmExecutor, - _capture_policy_runtime_state, # noqa: F401 - _predict_policy_action_with_acp_inference, # noqa: F401 -) -from lerobot.scripts.recording_loop import record_loop from lerobot.utils.constants import ACTION from lerobot.utils.control_utils import ( init_keyboard_listener, @@ -288,6 +288,7 @@ class RecordConfig: """This enables the parser to load config from the policy using `--policy.path=local/dir`""" return ["policy"] + @parser.wrap() def record(cfg: RecordConfig) -> LeRobotDataset: init_logging() @@ -325,10 +326,7 @@ def record(cfg: RecordConfig) -> LeRobotDataset: ) if cfg.intervention_state_machine_enabled and cfg.policy is not None and cfg.teleop is not None: action_names = dataset_features[ACTION]["names"] - if action_names is None: - action_names = list(robot.action_features) - else: - action_names = list(action_names) + action_names = list(robot.action_features) if action_names is None else list(action_names) dataset_features["complementary_info.policy_action"] = { "dtype": "float32", "shape": (len(action_names),), @@ -524,9 +522,7 @@ def record(cfg: RecordConfig) -> LeRobotDataset: continue extra_episode_metadata = ( - {"episode_success": episode_success} - if cfg.enable_episode_outcome_labeling - else None + {"episode_success": episode_success} if cfg.enable_episode_outcome_labeling else None ) dataset.save_episode(extra_episode_metadata=extra_episode_metadata) recorded_episodes += 1 diff --git a/src/lerobot/scripts/lerobot_value_infer.py b/src/lerobot/scripts/lerobot_value_infer.py index 0f1992a..40606c8 100644 --- a/src/lerobot/scripts/lerobot_value_infer.py +++ b/src/lerobot/scripts/lerobot_value_infer.py @@ -35,6 +35,9 @@ from lerobot.configs.value import ValueInferencePipelineConfig from lerobot.datasets.lerobot_dataset import LeRobotDataset from lerobot.datasets.utils import load_info, write_info from lerobot.policies.factory import make_policy, make_pre_post_processors +from lerobot.scripts.value_infer_viz import ( + _export_overlay_videos, +) from lerobot.utils.constants import ( CHECKPOINTS_DIR, LAST_CHECKPOINT_LINK, @@ -44,11 +47,6 @@ from lerobot.utils.import_utils import register_third_party_plugins from lerobot.utils.random_utils import set_seed from lerobot.utils.recording_annotations import EPISODE_SUCCESS, resolve_episode_success_label from lerobot.utils.utils import init_logging, inside_slurm -from lerobot.scripts.value_infer_viz import ( - _export_overlay_videos, - _get_episode_value_bounds, - _get_video_encode_options, -) from lerobot.values.pistar06.configuration_pistar06 import Pistar06Config from lerobot.values.pistar06.modeling_pistar06 import ( EpisodeTargetInfo, @@ -310,8 +308,8 @@ def _binarize_advantages( for i in range(advantages.shape[0]): task_idx = int(task_indices[i]) - thr = thresholds[task_idx] - indicators[i] = 1 if float(advantages[i]) >= thr else 0 + threshold = thresholds[task_idx] + indicators[i] = 1 if float(advantages[i]) >= threshold else 0 if force_intervention_positive: intervention_mask = interventions.astype(np.float32) > 0.5 diff --git a/src/lerobot/scripts/lerobot_value_train.py b/src/lerobot/scripts/lerobot_value_train.py index b99535d..c994d26 100644 --- a/src/lerobot/scripts/lerobot_value_train.py +++ b/src/lerobot/scripts/lerobot_value_train.py @@ -52,7 +52,9 @@ def update_policy( if grad_clip_norm > 0: grad_norm = accelerator.clip_grad_norm_(policy.parameters(), grad_clip_norm) else: - grad_norm = torch.nn.utils.clip_grad_norm_(policy.parameters(), float("inf"), error_if_nonfinite=False) + grad_norm = torch.nn.utils.clip_grad_norm_( + policy.parameters(), float("inf"), error_if_nonfinite=False + ) with lock if lock is not None else nullcontext(): optimizer.step() diff --git a/src/lerobot/scripts/recording_hil.py b/src/lerobot/scripts/recording_hil.py index 69d8c2f..8ec1f5b 100644 --- a/src/lerobot/scripts/recording_hil.py +++ b/src/lerobot/scripts/recording_hil.py @@ -14,7 +14,6 @@ """Human-in-loop recording helpers used by `lerobot_record.py`.""" -import logging from collections import deque from concurrent.futures import ThreadPoolExecutor from copy import deepcopy @@ -53,7 +52,9 @@ def _get_torch_rng_state(device: torch.device) -> tuple[torch.Tensor, torch.Tens return cpu_state, cuda_state -def _set_torch_rng_state(device: torch.device, cpu_state: torch.Tensor, cuda_state: torch.Tensor | None) -> None: +def _set_torch_rng_state( + device: torch.device, cpu_state: torch.Tensor, cuda_state: torch.Tensor | None +) -> None: torch.set_rng_state(cpu_state) if device.type == "cuda" and cuda_state is not None: torch.cuda.set_rng_state(cuda_state, device) diff --git a/src/lerobot/scripts/recording_loop.py b/src/lerobot/scripts/recording_loop.py index 8644c62..ef4182c 100644 --- a/src/lerobot/scripts/recording_loop.py +++ b/src/lerobot/scripts/recording_loop.py @@ -35,10 +35,10 @@ from lerobot.processor import ( ) from lerobot.robots import Robot from lerobot.scripts.recording_hil import ( - ACPInferenceConfig, INTERVENTION_STATE_ACTIVE, INTERVENTION_STATE_POLICY, INTERVENTION_STATE_RELEASE, + ACPInferenceConfig, PolicySyncDualArmExecutor, _capture_policy_runtime_state, _predict_policy_action_with_acp_inference, @@ -155,8 +155,8 @@ def record_loop( action_feature_names = list(robot.action_features.keys()) else: action_feature_names = list(robot.action_features) - zero_policy_action = {name: 0.0 for name in action_feature_names} - has_teleop = isinstance(teleop, Teleoperator) or isinstance(teleop, list) + zero_policy_action = dict.fromkeys(action_feature_names, 0.0) + has_teleop = isinstance(teleop, (Teleoperator, list)) intervention_enabled = intervention_state_machine_enabled and policy is not None and has_teleop intervention_state = INTERVENTION_STATE_POLICY last_teleop_action: RobotAction | None = None @@ -282,22 +282,26 @@ def record_loop( # Get action from policy and/or teleop act_processed_policy: RobotAction | None = None act_processed_teleop: RobotAction | None = None - if policy is not None and preprocessor is not None and postprocessor is not None: - if not (intervention_enabled and intervention_state == INTERVENTION_STATE_ACTIVE): - policy_action = _predict_policy_action_with_acp_inference( - observation_frame=observation_frame, - policy=policy, - device=get_safe_torch_device(policy.config.device), - preprocessor=preprocessor, - postprocessor=postprocessor, - use_amp=policy.config.use_amp, - task=single_task, - robot_type=robot.robot_type, - acp_inference=acp_inference, - cond_runtime_state=cond_policy_runtime_state, - uncond_runtime_state=uncond_policy_runtime_state, - ) - act_processed_policy = make_robot_action(policy_action, dataset.features) + if ( + policy is not None + and preprocessor is not None + and postprocessor is not None + and not (intervention_enabled and intervention_state == INTERVENTION_STATE_ACTIVE) + ): + policy_action = _predict_policy_action_with_acp_inference( + observation_frame=observation_frame, + policy=policy, + device=get_safe_torch_device(policy.config.device), + preprocessor=preprocessor, + postprocessor=postprocessor, + use_amp=policy.config.use_amp, + task=single_task, + robot_type=robot.robot_type, + acp_inference=acp_inference, + cond_runtime_state=cond_policy_runtime_state, + uncond_runtime_state=uncond_policy_runtime_state, + ) + act_processed_policy = make_robot_action(policy_action, dataset.features) if isinstance(teleop, Teleoperator): act = run_with_connection_retry("teleop.get_action", teleop.get_action) @@ -325,7 +329,9 @@ def record_loop( last_teleop_action = act_processed_teleop teleop_fallback_warned = False - policy_action_for_storage = act_processed_policy if act_processed_policy is not None else zero_policy_action + policy_action_for_storage = ( + act_processed_policy if act_processed_policy is not None else zero_policy_action + ) is_intervention = 0.0 if intervention_enabled and intervention_state == INTERVENTION_STATE_ACTIVE: @@ -367,12 +373,14 @@ def record_loop( if policy_sync_executor is not None and selected_from_policy: _sent_action = run_with_connection_retry( "policy_sync_executor.send_action", - lambda: policy_sync_executor.send_action(robot_action_to_send), + lambda robot_action_to_send=robot_action_to_send: policy_sync_executor.send_action( + robot_action_to_send + ), ) else: _sent_action = run_with_connection_retry( "robot.send_action", - lambda: robot.send_action(robot_action_to_send), + lambda robot_action_to_send=robot_action_to_send: robot.send_action(robot_action_to_send), ) # Write to dataset diff --git a/src/lerobot/scripts/value_infer_viz.py b/src/lerobot/scripts/value_infer_viz.py index e72a8f7..977aa1b 100644 --- a/src/lerobot/scripts/value_infer_viz.py +++ b/src/lerobot/scripts/value_infer_viz.py @@ -1,14 +1,14 @@ #!/usr/bin/env python -from pathlib import Path import tempfile +from pathlib import Path import numpy as np from PIL import Image, ImageDraw, ImageFont from tqdm.auto import tqdm -from lerobot.datasets.video_utils import decode_video_frames, encode_video_frames from lerobot.datasets.lerobot_dataset import LeRobotDataset +from lerobot.datasets.video_utils import decode_video_frames, encode_video_frames def _select_video_key(camera_keys: list[str], requested_video_key: str | None) -> str: @@ -16,7 +16,9 @@ def _select_video_key(camera_keys: list[str], requested_video_key: str | None) - raise ValueError("No camera key found in dataset.") if requested_video_key is not None: if requested_video_key not in camera_keys: - raise ValueError(f"Unknown video_key '{requested_video_key}'. Available camera keys: {camera_keys}") + raise ValueError( + f"Unknown video_key '{requested_video_key}'. Available camera keys: {camera_keys}" + ) return requested_video_key for key in camera_keys: @@ -277,7 +279,7 @@ def _encode_pil_to_video( fps: int, vcodec: str, ) -> None: - """Encode PIL frames directly to video without writing intermediate PNGs.""" + """Encode PIL frames directly to video without writing intermediate PNG files.""" import av as _av video_options, pix_fmt = _get_video_encode_options(vcodec) @@ -456,7 +458,9 @@ def _export_overlay_videos( ) written_paths: list[Path] = [] - for src, dst, vals, advs, inds, ts in tqdm(tasks, total=len(tasks), desc="Export value overlay videos", leave=False): + for src, dst, vals, advs, inds, ts in tqdm( + tasks, total=len(tasks), desc="Export value overlay videos", leave=False + ): written_paths.append( _export_single_episode( src, diff --git a/src/lerobot/teleoperators/bi_so_leader/bi_so_leader.py b/src/lerobot/teleoperators/bi_so_leader/bi_so_leader.py index d943f8c..b355b0d 100644 --- a/src/lerobot/teleoperators/bi_so_leader/bi_so_leader.py +++ b/src/lerobot/teleoperators/bi_so_leader/bi_so_leader.py @@ -116,15 +116,11 @@ class BiSOLeader(Teleoperator): def send_feedback(self, feedback: dict[str, float]) -> None: # Remove "left_" prefix left_feedback = { - key.removeprefix("left_"): value - for key, value in feedback.items() - if key.startswith("left_") + key.removeprefix("left_"): value for key, value in feedback.items() if key.startswith("left_") } # Remove "right_" prefix right_feedback = { - key.removeprefix("right_"): value - for key, value in feedback.items() - if key.startswith("right_") + key.removeprefix("right_"): value for key, value in feedback.items() if key.startswith("right_") } self.left_arm.send_feedback(left_feedback) diff --git a/src/lerobot/teleoperators/so_leader/so_leader.py b/src/lerobot/teleoperators/so_leader/so_leader.py index 960d6ed..2ba4329 100644 --- a/src/lerobot/teleoperators/so_leader/so_leader.py +++ b/src/lerobot/teleoperators/so_leader/so_leader.py @@ -165,9 +165,7 @@ class SOLeader(Teleoperator): # For phase-A dual-arm execution, reuse leader as a commanded arm by # mapping `{joint}.pos` to motor goal positions. self.set_manual_control(False) - goal_pos = { - key.removesuffix(".pos"): val for key, val in feedback.items() if key.endswith(".pos") - } + goal_pos = {key.removesuffix(".pos"): val for key, val in feedback.items() if key.endswith(".pos")} if not goal_pos: return self.bus.sync_write("Goal_Position", goal_pos) diff --git a/src/lerobot/utils/recording_annotations.py b/src/lerobot/utils/recording_annotations.py index dc57151..688c047 100644 --- a/src/lerobot/utils/recording_annotations.py +++ b/src/lerobot/utils/recording_annotations.py @@ -103,4 +103,3 @@ def resolve_collector_policy_id( if intervention_enabled: return human_id if is_intervention else policy_id return policy_id if selected_from_policy else human_id - diff --git a/src/lerobot/values/__init__.py b/src/lerobot/values/__init__.py index fef66b5..4265cc3 100644 --- a/src/lerobot/values/__init__.py +++ b/src/lerobot/values/__init__.py @@ -1 +1 @@ -#!/usr/bin/env python \ No newline at end of file +#!/usr/bin/env python diff --git a/src/lerobot/values/pistar06/modeling_pistar06.py b/src/lerobot/values/pistar06/modeling_pistar06.py index a8fb4a4..91d4c9f 100644 --- a/src/lerobot/values/pistar06/modeling_pistar06.py +++ b/src/lerobot/values/pistar06/modeling_pistar06.py @@ -12,7 +12,7 @@ from typing import TYPE_CHECKING, Any import numpy as np import torch -import torch.nn.functional as F +import torch.nn.functional as functional from huggingface_hub import hf_hub_download from huggingface_hub.constants import SAFETENSORS_SINGLE_FILE from huggingface_hub.errors import HfHubHTTPError @@ -78,7 +78,7 @@ def project_values_to_bins(values: torch.Tensor, bin_centers: torch.Tensor) -> t def expected_value_from_logits(logits: torch.Tensor, bin_centers: torch.Tensor) -> torch.Tensor: - probs = F.softmax(logits, dim=-1) + probs = functional.softmax(logits, dim=-1) return (probs * bin_centers).sum(dim=-1) @@ -198,13 +198,13 @@ def _resolve_image_size(image_processor: Any) -> tuple[int, int]: return 384, 384 -def _resolve_norm_stats(image_processor: Any) -> tuple[tuple[float, float, float], tuple[float, float, float]]: +def _resolve_norm_stats( + image_processor: Any, +) -> tuple[tuple[float, float, float], tuple[float, float, float]]: mean_raw = getattr(image_processor, "image_mean", [0.5, 0.5, 0.5]) std_raw = getattr(image_processor, "image_std", [0.5, 0.5, 0.5]) if len(mean_raw) != 3 or len(std_raw) != 3: - raise ValueError( - f"Expected RGB normalization stats of len=3, got mean={mean_raw} std={std_raw}." - ) + raise ValueError(f"Expected RGB normalization stats of len=3, got mean={mean_raw} std={std_raw}.") mean = (float(mean_raw[0]), float(mean_raw[1]), float(mean_raw[2])) std = (float(std_raw[0]), float(std_raw[1]), float(std_raw[2])) if any(v <= 0 for v in std): @@ -367,7 +367,7 @@ class Pistar06Model(nn.Module): flat_images = images.view(bsize * num_cameras, *images.shape[2:]) if flat_images.shape[-2:] != self.image_resolution: - flat_images = F.interpolate( + flat_images = functional.interpolate( flat_images, size=self.image_resolution, mode="bilinear", @@ -435,7 +435,9 @@ class Pistar06Model(nn.Module): language_context = torch.no_grad() if self.cfg.freeze_language_model else nullcontext() with language_context: - language_features = self._encode_language(input_ids=input_ids, attention_mask=language_mask.long()) + language_features = self._encode_language( + input_ids=input_ids, attention_mask=language_mask.long() + ) feature_dtype = torch.float32 image_features = image_features.to(dtype=feature_dtype) @@ -445,7 +447,9 @@ class Pistar06Model(nn.Module): camera_token_mask = image_attention_mask.unsqueeze(-1).to(dtype=image_tokens.dtype) image_tokens = image_tokens * camera_token_mask - camera_denominator = image_attention_mask.sum(dim=1, keepdim=True).to(dtype=image_tokens.dtype).clamp_min(1.0) + camera_denominator = ( + image_attention_mask.sum(dim=1, keepdim=True).to(dtype=image_tokens.dtype).clamp_min(1.0) + ) image_pooled = image_tokens.sum(dim=1) / camera_denominator language_token = self.language_projector(language_features) @@ -743,7 +747,7 @@ class Pistar06Policy(PreTrainedPolicy): bin_centers = self.bin_centers.to(device=device) soft_target = project_values_to_bins(value_target, bin_centers) - log_probs = F.log_softmax(logits, dim=-1) + log_probs = functional.log_softmax(logits, dim=-1) per_sample_loss = -(soft_target * log_probs).sum(dim=-1) sample_weight = None @@ -769,13 +773,12 @@ class Pistar06Policy(PreTrainedPolicy): pred_value = expected_value_from_logits(logits, bin_centers) value_mae = (pred_value - value_target).abs().mean() - if reduction == "none": - loss = per_sample_loss - else: - loss = per_sample_loss.mean() + loss = per_sample_loss if reduction == "none" else per_sample_loss.mean() loss_dict = { - "loss": float(loss.mean().detach().item()) if reduction == "none" else float(loss.detach().item()), + "loss": float(loss.mean().detach().item()) + if reduction == "none" + else float(loss.detach().item()), "value_mae": float(value_mae.detach().item()), } if sample_weight is not None: diff --git a/src/lerobot/values/pistar06/processor_pistar06.py b/src/lerobot/values/pistar06/processor_pistar06.py index 7489001..40d4088 100644 --- a/src/lerobot/values/pistar06/processor_pistar06.py +++ b/src/lerobot/values/pistar06/processor_pistar06.py @@ -8,7 +8,7 @@ from typing import Any import numpy as np import torch -import torch.nn.functional as F +import torch.nn.functional as functional from torch import Tensor from lerobot.configs.types import PipelineFeatureType, PolicyFeature @@ -39,7 +39,7 @@ PISTAR06_IMAGE_MASK_KEY = "observation.pistar06.image_attention_mask" def _pad_last_dim(vector: Tensor, new_dim: int) -> Tensor: if vector.shape[-1] >= new_dim: return vector - return F.pad(vector, (0, new_dim - vector.shape[-1])) + return functional.pad(vector, (0, new_dim - vector.shape[-1])) @ProcessorStepRegistry.register(name="pistar06_prepare_task_prompt") @@ -216,7 +216,7 @@ def make_pistar06_pre_post_processors( ]: camera_features = list(config.camera_features) if not camera_features: - camera_features = [k for k in (config.input_features or {}).keys() if k.startswith(OBS_IMAGES)] + camera_features = [k for k in (config.input_features or {}) if k.startswith(OBS_IMAGES)] input_steps: list[ProcessorStep] = [ RenameObservationsProcessorStep(rename_map={}), diff --git a/tests/datasets/test_dataset_tools.py b/tests/datasets/test_dataset_tools.py index 2b43c54..0187897 100644 --- a/tests/datasets/test_dataset_tools.py +++ b/tests/datasets/test_dataset_tools.py @@ -729,7 +729,9 @@ def test_delete_episodes_preserves_extra_episode_metadata(tmp_path, empty_lerobo ) assert "episode_success" in new_dataset.meta.episodes.column_names - assert [new_dataset.meta.episodes[i]["episode_success"] for i in range(new_dataset.meta.total_episodes)] == [ + assert [ + new_dataset.meta.episodes[i]["episode_success"] for i in range(new_dataset.meta.total_episodes) + ] == [ "success", "success", ] diff --git a/tests/test_control_robot.py b/tests/test_control_robot.py index a065c92..e77dab3 100644 --- a/tests/test_control_robot.py +++ b/tests/test_control_robot.py @@ -15,9 +15,8 @@ # limitations under the License. import json -from unittest.mock import MagicMock, patch - from collections import deque +from unittest.mock import MagicMock, patch import numpy as np import pytest @@ -25,12 +24,12 @@ import torch from lerobot.datasets.lerobot_dataset import LeRobotDataset from lerobot.scripts.lerobot_calibrate import CalibrateConfig, calibrate -from lerobot.scripts.lerobot_human_inloop_record import human_inloop_record from lerobot.scripts.lerobot_human_inloop_record import ( _HumanInloopFailureResetController, _load_failure_reset_pose, _save_failure_reset_pose, _slow_reset_all_arms_to_pose, + human_inloop_record, ) from lerobot.scripts.lerobot_record import ( ACPInferenceConfig, @@ -39,8 +38,8 @@ from lerobot.scripts.lerobot_record import ( RecordConfig, _capture_policy_runtime_state, _predict_policy_action_with_acp_inference, - record_loop, record, + record_loop, ) from lerobot.scripts.lerobot_replay import DatasetReplayConfig, ReplayConfig, replay from lerobot.scripts.lerobot_teleoperate import TeleoperateConfig, teleoperate @@ -211,9 +210,7 @@ def test_record_loop_sets_leader_manual_control_during_reset(): def test_save_and_load_failure_reset_pose(tmp_path): - robot = MockRobot( - MockRobotConfig(n_motors=2, random_values=False, static_values=[12.5, -3.0]) - ) + robot = MockRobot(MockRobotConfig(n_motors=2, random_values=False, static_values=[12.5, -3.0])) robot.connect() pose_path = tmp_path / "failure_reset_pose.json" diff --git a/tests/value/test_lerobot_value_infer_sampler.py b/tests/value/test_lerobot_value_infer_sampler.py index e37ee92..8d7dfee 100644 --- a/tests/value/test_lerobot_value_infer_sampler.py +++ b/tests/value/test_lerobot_value_infer_sampler.py @@ -9,7 +9,9 @@ def test_contiguous_distributed_eval_sampler_has_contiguous_shards_with_tail_pad shards: list[list[int]] = [] for rank in range(world_size): - sampler = ContiguousDistributedEvalSampler(dataset_size=dataset_size, num_replicas=world_size, rank=rank) + sampler = ContiguousDistributedEvalSampler( + dataset_size=dataset_size, num_replicas=world_size, rank=rank + ) shard = list(iter(sampler)) shards.append(shard) assert len(shard) == len(sampler) == 4 @@ -30,7 +32,9 @@ def test_contiguous_distributed_eval_sampler_handles_more_ranks_than_samples(): shards: list[list[int]] = [] for rank in range(world_size): - sampler = ContiguousDistributedEvalSampler(dataset_size=dataset_size, num_replicas=world_size, rank=rank) + sampler = ContiguousDistributedEvalSampler( + dataset_size=dataset_size, num_replicas=world_size, rank=rank + ) shard = list(iter(sampler)) shards.append(shard) assert len(shard) == len(sampler) == 1 diff --git a/tests/value/test_lerobot_value_infer_viz.py b/tests/value/test_lerobot_value_infer_viz.py index 25802b4..6bf3bdc 100644 --- a/tests/value/test_lerobot_value_infer_viz.py +++ b/tests/value/test_lerobot_value_infer_viz.py @@ -13,8 +13,7 @@ from lerobot.configs.value import ( ValueInferencePipelineConfig, ValueInferenceVizConfig, ) -from lerobot.scripts import lerobot_value_infer as value_infer -from lerobot.scripts import value_infer_viz +from lerobot.scripts import lerobot_value_infer as value_infer, value_infer_viz class _FakeHFDataset: diff --git a/tests/value/test_pistar06_value_stack.py b/tests/value/test_pistar06_value_stack.py index 8e63915..5fd3f90 100644 --- a/tests/value/test_pistar06_value_stack.py +++ b/tests/value/test_pistar06_value_stack.py @@ -10,8 +10,6 @@ from torch import nn import lerobot.processor.tokenizer_processor as tokenizer_processor import lerobot.values.pistar06.modeling_pistar06 as pistar06_modeling -import lerobot.values.pistar06.processor_pistar06 as pistar06_processor -from lerobot.configs.value_train import ValueTargetsConfig from lerobot.utils.constants import OBS_LANGUAGE_ATTENTION_MASK, OBS_LANGUAGE_TOKENS, OBS_STATE from lerobot.values.pistar06.configuration_pistar06 import Pistar06Config from lerobot.values.pistar06.modeling_pistar06 import ( @@ -95,10 +93,7 @@ class _DummyAutoModel: @classmethod def from_pretrained(cls, repo_id, **kwargs): output_loading_info = bool(kwargs.pop("output_loading_info", False)) - if "siglip" in repo_id.lower(): - model = _DummyVisionModel() - else: - model = _DummyLanguageModel() + model = _DummyVisionModel() if "siglip" in repo_id.lower() else _DummyLanguageModel() if output_loading_info: return model, {"missing_keys": [], "unexpected_keys": [], "mismatched_keys": []} return model From 0be27edc4291023d4f147c4d0159bebe403b66f7 Mon Sep 17 00:00:00 2001 From: Elvin <223015152@link.cuhk.edu.cn> Date: Wed, 4 Mar 2026 12:40:04 +0800 Subject: [PATCH 02/26] test(ci): fix fast tests for ACP config and sarm import guards - update ACP training tests to use current ACPConfig fields and shared ACP tag constants - skip sarm subtask annotation tests when Qwen3VLMoe symbol is unavailable in transformers - keep changes scoped to fast-tests paths and preserve runtime behavior --- .../policies/test_sarm_subtask_annotations.py | 7 ++++++- .../training/test_acp_pi05_prompt_pipeline.py | 7 +++---- tests/training/test_acp_prompt_hook.py | 21 +++++-------------- 3 files changed, 14 insertions(+), 21 deletions(-) diff --git a/tests/policies/test_sarm_subtask_annotations.py b/tests/policies/test_sarm_subtask_annotations.py index 0dc0872..8848281 100644 --- a/tests/policies/test_sarm_subtask_annotations.py +++ b/tests/policies/test_sarm_subtask_annotations.py @@ -16,7 +16,12 @@ import pytest -pytest.importorskip("transformers") +transformers = pytest.importorskip("transformers") +if not hasattr(transformers, "Qwen3VLMoeForConditionalGeneration"): + pytest.skip( + "requires transformers with Qwen3VLMoeForConditionalGeneration", + allow_module_level=True, + ) from lerobot.data_processing.sarm_annotations.subtask_annotation import ( Subtask, diff --git a/tests/training/test_acp_pi05_prompt_pipeline.py b/tests/training/test_acp_pi05_prompt_pipeline.py index c55632d..64ebb01 100644 --- a/tests/training/test_acp_pi05_prompt_pipeline.py +++ b/tests/training/test_acp_pi05_prompt_pipeline.py @@ -9,6 +9,7 @@ from lerobot.configs.types import FeatureType, PolicyFeature from lerobot.policies.pi05.configuration_pi05 import PI05Config from lerobot.policies.pi05.processor_pi05 import make_pi05_pre_post_processors from lerobot.rl.acp_hook import build_acp_raw_batch_hook +from lerobot.rl.acp_tags import ACP_NEGATIVE_TAG, ACP_POSITIVE_TAG from lerobot.utils.constants import OBS_LANGUAGE_ATTENTION_MASK, OBS_LANGUAGE_TOKENS from tests.utils import require_package @@ -78,8 +79,6 @@ def test_acp_prompt_reaches_pi05_tokenizer(mock_auto_tokenizer): ACPConfig( enable=True, indicator_field="complementary_info.acp_indicator", - positive_tag="Advantage: positive", - negative_tag="Advantage: negative", indicator_dropout_prob=0.0, ), seed=123, @@ -101,8 +100,8 @@ def test_acp_prompt_reaches_pi05_tokenizer(mock_auto_tokenizer): prompts = tracking_tokenizer.calls[-1] assert len(prompts) == 2 - assert "Advantage: positive" in prompts[0] - assert "Advantage: negative" in prompts[1] + assert ACP_POSITIVE_TAG in prompts[0] + assert ACP_NEGATIVE_TAG in prompts[1] assert prompts[0].startswith("Task: Pick bottle") assert "State:" in prompts[0] assert OBS_LANGUAGE_TOKENS in out diff --git a/tests/training/test_acp_prompt_hook.py b/tests/training/test_acp_prompt_hook.py index 380c490..bb0c941 100644 --- a/tests/training/test_acp_prompt_hook.py +++ b/tests/training/test_acp_prompt_hook.py @@ -7,6 +7,7 @@ from torch.utils.data import DataLoader from lerobot.configs.train import ACPConfig from lerobot.datasets.lerobot_dataset import LeRobotDataset from lerobot.rl.acp_hook import build_acp_raw_batch_hook +from lerobot.rl.acp_tags import ACP_NEGATIVE_TAG, ACP_POSITIVE_TAG def test_acp_hook_accepts_integer_indicators(): @@ -14,8 +15,6 @@ def test_acp_hook_accepts_integer_indicators(): ACPConfig( enable=True, indicator_field="acp_indicator", - positive_tag="POS", - negative_tag="NEG", indicator_dropout_prob=0.0, ), seed=42, @@ -27,8 +26,8 @@ def test_acp_hook_accepts_integer_indicators(): out = hook(batch, 0) assert out["task"] == [ - "pick bottle\nPOS", - "place bottle\nNEG", + f"pick bottle\n{ACP_POSITIVE_TAG}", + f"place bottle\n{ACP_NEGATIVE_TAG}", ] @@ -37,8 +36,6 @@ def test_acp_hook_injects_tags(): ACPConfig( enable=True, indicator_field="complementary_info.acp_indicator", - positive_tag="POS", - negative_tag="NEG", indicator_dropout_prob=0.0, ), seed=42, @@ -49,7 +46,7 @@ def test_acp_hook_injects_tags(): } out = hook(batch, 0) - assert out["task"] == ["pick bottle\nPOS", "place bottle\nNEG"] + assert out["task"] == [f"pick bottle\n{ACP_POSITIVE_TAG}", f"place bottle\n{ACP_NEGATIVE_TAG}"] def test_acp_hook_dropout_keeps_original_task(): @@ -57,8 +54,6 @@ def test_acp_hook_dropout_keeps_original_task(): ACPConfig( enable=True, indicator_field="complementary_info.acp_indicator", - positive_tag="POS", - negative_tag="NEG", indicator_dropout_prob=1.0, ), seed=42, @@ -87,8 +82,6 @@ def test_acp_hook_non_integer_type_raises(): ACPConfig( enable=True, indicator_field="acp_indicator", - positive_tag="POS", - negative_tag="NEG", indicator_dropout_prob=0.0, ), seed=42, @@ -106,8 +99,6 @@ def test_acp_hook_non_binary_value_raises(): ACPConfig( enable=True, indicator_field="acp_indicator", - positive_tag="POS", - negative_tag="NEG", indicator_dropout_prob=0.0, ), seed=42, @@ -143,8 +134,6 @@ def test_acp_hook_with_real_local_dataset_batch(batch_size: int): ACPConfig( enable=True, indicator_field="complementary_info.acp_indicator", - positive_tag="POS", - negative_tag="NEG", indicator_dropout_prob=0.0, ), seed=42, @@ -153,5 +142,5 @@ def test_acp_hook_with_real_local_dataset_batch(batch_size: int): assert len(out["task"]) == batch_size for i in range(batch_size): - expected_tag = "POS" if int(indicators[i].item()) == 1 else "NEG" + expected_tag = ACP_POSITIVE_TAG if int(indicators[i].item()) == 1 else ACP_NEGATIVE_TAG assert out["task"][i].endswith(expected_tag) From 78dae04a31073cf226dd5a1789f13cad1543c86a Mon Sep 17 00:00:00 2001 From: Elvin <223015152@link.cuhk.edu.cn> Date: Wed, 4 Mar 2026 20:33:12 +0800 Subject: [PATCH 03/26] feat(teleop): add piper leader/follower adapter for LeRobot - add piper_follower robot and piper_leader teleoperator implementations based on Piper SDK CAN APIs - wire piper types into teleoperate/record/replay/calibrate command imports and device factories - add Piper SDK import helper with auto-discovery and unit conversion utilities - add piper teleop tests and camera-compatible follower observation support for record/hil flows --- src/lerobot/robots/piper_follower/__init__.py | 18 ++ .../piper_follower/config_piper_follower.py | 77 ++++++ .../robots/piper_follower/piper_follower.py | 191 +++++++++++++++ src/lerobot/robots/utils.py | 4 + src/lerobot/scripts/lerobot_calibrate.py | 2 + src/lerobot/scripts/lerobot_record.py | 2 + src/lerobot/scripts/lerobot_replay.py | 1 + src/lerobot/scripts/lerobot_teleoperate.py | 2 + .../teleoperators/piper_leader/__init__.py | 18 ++ .../piper_leader/config_piper_leader.py | 75 ++++++ .../piper_leader/piper_leader.py | 220 ++++++++++++++++++ src/lerobot/teleoperators/utils.py | 4 + src/lerobot/utils/piper_sdk.py | 121 ++++++++++ tests/test_piper_teleop.py | 175 ++++++++++++++ 14 files changed, 910 insertions(+) create mode 100644 src/lerobot/robots/piper_follower/__init__.py create mode 100644 src/lerobot/robots/piper_follower/config_piper_follower.py create mode 100644 src/lerobot/robots/piper_follower/piper_follower.py create mode 100644 src/lerobot/teleoperators/piper_leader/__init__.py create mode 100644 src/lerobot/teleoperators/piper_leader/config_piper_leader.py create mode 100644 src/lerobot/teleoperators/piper_leader/piper_leader.py create mode 100644 src/lerobot/utils/piper_sdk.py create mode 100644 tests/test_piper_teleop.py diff --git a/src/lerobot/robots/piper_follower/__init__.py b/src/lerobot/robots/piper_follower/__init__.py new file mode 100644 index 0000000..ec450ff --- /dev/null +++ b/src/lerobot/robots/piper_follower/__init__.py @@ -0,0 +1,18 @@ +#!/usr/bin/env python + +# Copyright 2026 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 .config_piper_follower import PiperFollowerConfig, PiperFollowerConfigBase +from .piper_follower import PiperFollower diff --git a/src/lerobot/robots/piper_follower/config_piper_follower.py b/src/lerobot/robots/piper_follower/config_piper_follower.py new file mode 100644 index 0000000..da9e241 --- /dev/null +++ b/src/lerobot/robots/piper_follower/config_piper_follower.py @@ -0,0 +1,77 @@ +#!/usr/bin/env python + +# Copyright 2026 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, field + +from lerobot.cameras import CameraConfig + +from ..config import RobotConfig + + +@dataclass +class PiperFollowerConfigBase: + """Configuration for a Piper follower arm controlled over CAN.""" + + # CAN interface name (e.g. "can0") + port: str + + # Piper SDK connection options + judge_flag: bool = False + can_auto_init: bool = True + log_level: str = "WARNING" + startup_sleep_s: float = 0.1 + + # Optional role command on startup (0xFC = motion output arm) + set_slave_mode_on_connect: bool = False + + # Motion mode for follower arm + speed_ratio: int = 100 + high_follow: bool = True + mode_refresh_interval_s: float = 1.0 + + # Arm enable behavior + enable_on_connect: bool = True + enable_timeout_s: float = 3.0 + + # Gripper forwarding behavior + sync_gripper: bool = True + gripper_effort_default: int = 1000 + gripper_status_code: int = 0x01 + + # Optional cameras + cameras: dict[str, CameraConfig] = field(default_factory=dict) + + # Safety behavior on disconnect + disable_on_disconnect: bool = False + + +@RobotConfig.register_subclass("piper_follower") +@dataclass +class PiperFollowerConfig(RobotConfig, PiperFollowerConfigBase): + def __post_init__(self): + super().__post_init__() + if not (0 <= self.speed_ratio <= 100): + raise ValueError("`speed_ratio` must be between 0 and 100.") + if self.mode_refresh_interval_s < 0: + raise ValueError("`mode_refresh_interval_s` must be >= 0.") + if self.enable_timeout_s < 0: + raise ValueError("`enable_timeout_s` must be >= 0.") + if self.startup_sleep_s < 0: + raise ValueError("`startup_sleep_s` must be >= 0.") + if not (0 <= self.gripper_effort_default <= 5000): + raise ValueError("`gripper_effort_default` must be between 0 and 5000.") + if self.gripper_status_code not in {0x00, 0x01, 0x02, 0x03}: + raise ValueError("`gripper_status_code` must be one of 0x00, 0x01, 0x02, 0x03.") diff --git a/src/lerobot/robots/piper_follower/piper_follower.py b/src/lerobot/robots/piper_follower/piper_follower.py new file mode 100644 index 0000000..c097c35 --- /dev/null +++ b/src/lerobot/robots/piper_follower/piper_follower.py @@ -0,0 +1,191 @@ +#!/usr/bin/env python + +# Copyright 2026 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 functools import cached_property +from typing import Any + +from lerobot.cameras.utils import make_cameras_from_configs +from lerobot.processor import RobotAction, RobotObservation +from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected +from lerobot.utils.piper_sdk import ( + PIPER_ACTION_KEYS, + PIPER_JOINT_NAMES, + get_piper_sdk, + milli_to_unit, + parse_piper_log_level, + unit_to_milli, +) + +from ..robot import Robot +from .config_piper_follower import PiperFollowerConfig + +logger = logging.getLogger(__name__) + + +class PiperFollower(Robot): + """Piper follower arm controlled through the Piper SDK (CAN).""" + + config_class = PiperFollowerConfig + name = "piper_follower" + + def __init__(self, config: PiperFollowerConfig): + super().__init__(config) + self.config = config + self._is_connected = False + self._last_mode_refresh_t = 0.0 + + interface_cls, _ = get_piper_sdk() + self.arm = interface_cls( + can_name=self.config.port, + judge_flag=self.config.judge_flag, + can_auto_init=self.config.can_auto_init, + logger_level=parse_piper_log_level(self.config.log_level), + ) + self.cameras = make_cameras_from_configs(config.cameras) + + @property + def _cameras_ft(self) -> dict[str, tuple]: + return { + cam: (self.config.cameras[cam].height, self.config.cameras[cam].width, 3) for cam in self.cameras + } + + @cached_property + def observation_features(self) -> dict[str, type | tuple]: + return {**{key: float for key in PIPER_ACTION_KEYS}, **self._cameras_ft} + + @cached_property + def action_features(self) -> dict[str, type]: + return {key: float for key in PIPER_ACTION_KEYS} + + @property + def is_connected(self) -> bool: + return self._is_connected and all(cam.is_connected for cam in self.cameras.values()) + + @check_if_already_connected + def connect(self, calibrate: bool = True) -> None: + del calibrate + self.arm.ConnectPort() + if self.config.startup_sleep_s > 0: + time.sleep(self.config.startup_sleep_s) + + self._is_connected = True + try: + if self.config.set_slave_mode_on_connect: + self.arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00) + time.sleep(0.05) + + self.configure() + if self.config.enable_on_connect and not self._wait_enable(self.config.enable_timeout_s): + logger.warning("Piper follower did not report enabled state before timeout.") + + for cam in self.cameras.values(): + cam.connect() + except Exception: + self.arm.DisconnectPort() + self._is_connected = False + raise + + logger.info("%s connected.", self) + + @property + def is_calibrated(self) -> bool: + return True + + def calibrate(self) -> None: + logger.info("Piper follower does not require lerobot joint-range calibration. Skipping.") + + def configure(self) -> None: + self._send_motion_mode() + + def _send_motion_mode(self) -> None: + mit_mode = 0xAD if self.config.high_follow else 0x00 + self.arm.MotionCtrl_2(0x01, 0x01, self.config.speed_ratio, mit_mode) + self._last_mode_refresh_t = time.monotonic() + + def _refresh_motion_mode_if_needed(self) -> None: + interval_s = self.config.mode_refresh_interval_s + if interval_s <= 0: + return + now = time.monotonic() + if now - self._last_mode_refresh_t >= interval_s: + self._send_motion_mode() + + def _wait_enable(self, timeout_s: float) -> bool: + deadline = time.monotonic() + timeout_s + while time.monotonic() < deadline: + if bool(self.arm.EnablePiper()): + return True + time.sleep(0.02) + return False + + @check_if_not_connected + def get_observation(self) -> RobotObservation: + joint_msg = self.arm.GetArmJointMsgs() + joint_state = getattr(joint_msg, "joint_state", None) + + obs: dict[str, float] = {} + for joint_name in PIPER_JOINT_NAMES: + raw_value = getattr(joint_state, joint_name, 0) + obs[f"{joint_name}.pos"] = milli_to_unit(raw_value) + + gripper_msg = self.arm.GetArmGripperMsgs() + gripper_state = getattr(gripper_msg, "gripper_state", None) + obs["gripper.pos"] = abs(milli_to_unit(getattr(gripper_state, "grippers_angle", 0))) + + for cam_key, cam in self.cameras.items(): + obs[cam_key] = cam.async_read() + return obs + + @check_if_not_connected + def send_action(self, action: RobotAction) -> RobotAction: + self._refresh_motion_mode_if_needed() + + sent_action: dict[str, float] = {} + + joint_keys = [f"{joint_name}.pos" for joint_name in PIPER_JOINT_NAMES] + has_all_joints = all(key in action for key in joint_keys) + if has_all_joints: + joint_commands = [unit_to_milli(action[key]) for key in joint_keys] + self.arm.JointCtrl(*joint_commands) + sent_action.update({key: milli_to_unit(raw) for key, raw in zip(joint_keys, joint_commands, strict=True)}) + elif any(key in action for key in joint_keys): + logger.debug("Ignoring partial Piper joint action. Need all six joint keys to send command.") + + if self.config.sync_gripper and "gripper.pos" in action: + gripper_pos_raw = unit_to_milli(abs(action["gripper.pos"])) + self.arm.GripperCtrl( + gripper_pos_raw, + self.config.gripper_effort_default, + self.config.gripper_status_code, + 0x00, + ) + sent_action["gripper.pos"] = milli_to_unit(gripper_pos_raw) + + return sent_action + + @check_if_not_connected + def disconnect(self) -> None: + try: + if self.config.disable_on_disconnect: + self.arm.DisableArm(7) + finally: + self.arm.DisconnectPort() + for cam in self.cameras.values(): + cam.disconnect() + self._is_connected = False + logger.info("%s disconnected.", self) diff --git a/src/lerobot/robots/utils.py b/src/lerobot/robots/utils.py index 92da597..81288d1 100644 --- a/src/lerobot/robots/utils.py +++ b/src/lerobot/robots/utils.py @@ -64,6 +64,10 @@ def make_robot_from_config(config: RobotConfig) -> Robot: from .openarm_follower import OpenArmFollower return OpenArmFollower(config) + elif config.type == "piper_follower": + from .piper_follower import PiperFollower + + return PiperFollower(config) elif config.type == "bi_openarm_follower": from .bi_openarm_follower import BiOpenArmFollower diff --git a/src/lerobot/scripts/lerobot_calibrate.py b/src/lerobot/scripts/lerobot_calibrate.py index 1b30021..c1386d2 100644 --- a/src/lerobot/scripts/lerobot_calibrate.py +++ b/src/lerobot/scripts/lerobot_calibrate.py @@ -44,6 +44,7 @@ from lerobot.robots import ( # noqa: F401 make_robot_from_config, omx_follower, openarm_follower, + piper_follower, so_follower, ) from lerobot.teleoperators import ( # noqa: F401 @@ -56,6 +57,7 @@ from lerobot.teleoperators import ( # noqa: F401 make_teleoperator_from_config, omx_leader, openarm_leader, + piper_leader, so_leader, unitree_g1, ) diff --git a/src/lerobot/scripts/lerobot_record.py b/src/lerobot/scripts/lerobot_record.py index be9a55d..a83e5d8 100644 --- a/src/lerobot/scripts/lerobot_record.py +++ b/src/lerobot/scripts/lerobot_record.py @@ -93,6 +93,7 @@ from lerobot.robots import ( # noqa: F401 make_robot_from_config, omx_follower, openarm_follower, + piper_follower, reachy2, so_follower, unitree_g1 as unitree_g1_robot, @@ -113,6 +114,7 @@ from lerobot.teleoperators import ( # noqa: F401 make_teleoperator_from_config, omx_leader, openarm_leader, + piper_leader, reachy2_teleoperator, so_leader, unitree_g1, diff --git a/src/lerobot/scripts/lerobot_replay.py b/src/lerobot/scripts/lerobot_replay.py index c9a559d..0a76588 100644 --- a/src/lerobot/scripts/lerobot_replay.py +++ b/src/lerobot/scripts/lerobot_replay.py @@ -61,6 +61,7 @@ from lerobot.robots import ( # noqa: F401 make_robot_from_config, omx_follower, openarm_follower, + piper_follower, reachy2, so_follower, unitree_g1, diff --git a/src/lerobot/scripts/lerobot_teleoperate.py b/src/lerobot/scripts/lerobot_teleoperate.py index b6aa4a7..b9275c0 100644 --- a/src/lerobot/scripts/lerobot_teleoperate.py +++ b/src/lerobot/scripts/lerobot_teleoperate.py @@ -78,6 +78,7 @@ from lerobot.robots import ( # noqa: F401 make_robot_from_config, omx_follower, openarm_follower, + piper_follower, reachy2, so_follower, unitree_g1 as unitree_g1_robot, @@ -94,6 +95,7 @@ from lerobot.teleoperators import ( # noqa: F401 make_teleoperator_from_config, omx_leader, openarm_leader, + piper_leader, reachy2_teleoperator, so_leader, unitree_g1, diff --git a/src/lerobot/teleoperators/piper_leader/__init__.py b/src/lerobot/teleoperators/piper_leader/__init__.py new file mode 100644 index 0000000..1e4110d --- /dev/null +++ b/src/lerobot/teleoperators/piper_leader/__init__.py @@ -0,0 +1,18 @@ +#!/usr/bin/env python + +# Copyright 2026 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 .config_piper_leader import PiperLeaderConfig, PiperLeaderConfigBase +from .piper_leader import PiperLeader diff --git a/src/lerobot/teleoperators/piper_leader/config_piper_leader.py b/src/lerobot/teleoperators/piper_leader/config_piper_leader.py new file mode 100644 index 0000000..768114a --- /dev/null +++ b/src/lerobot/teleoperators/piper_leader/config_piper_leader.py @@ -0,0 +1,75 @@ +#!/usr/bin/env python + +# Copyright 2026 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 + +from ..config import TeleoperatorConfig + + +@dataclass +class PiperLeaderConfigBase: + """Configuration for a Piper leader arm used as teleoperator.""" + + # CAN interface name (e.g. "can1") + port: str + + # Piper SDK connection options + judge_flag: bool = False + can_auto_init: bool = True + log_level: str = "WARNING" + startup_sleep_s: float = 0.1 + + # Optional role command on startup (0xFA = teaching input arm) + set_leader_mode_on_connect: bool = False + + # Manual backdrivable mode for human teleop + manual_control: bool = True + + # Read control messages from leader first, fallback to feedback state if missing + prefer_ctrl_messages: bool = True + fallback_to_feedback: bool = True + + # Gripper handling + sync_gripper: bool = True + gripper_effort_default: int = 1000 + gripper_status_code: int = 0x01 + + # Command mode for send_feedback + command_speed_ratio: int = 100 + command_high_follow: bool = True + mode_refresh_interval_s: float = 1.0 + enable_timeout_s: float = 3.0 + + # Safety behavior on disconnect + disable_on_disconnect: bool = False + + +@TeleoperatorConfig.register_subclass("piper_leader") +@dataclass +class PiperLeaderConfig(TeleoperatorConfig, PiperLeaderConfigBase): + def __post_init__(self): + if not (0 <= self.command_speed_ratio <= 100): + raise ValueError("`command_speed_ratio` must be between 0 and 100.") + if self.mode_refresh_interval_s < 0: + raise ValueError("`mode_refresh_interval_s` must be >= 0.") + if self.enable_timeout_s < 0: + raise ValueError("`enable_timeout_s` must be >= 0.") + if self.startup_sleep_s < 0: + raise ValueError("`startup_sleep_s` must be >= 0.") + if not (0 <= self.gripper_effort_default <= 5000): + raise ValueError("`gripper_effort_default` must be between 0 and 5000.") + if self.gripper_status_code not in {0x00, 0x01, 0x02, 0x03}: + raise ValueError("`gripper_status_code` must be one of 0x00, 0x01, 0x02, 0x03.") diff --git a/src/lerobot/teleoperators/piper_leader/piper_leader.py b/src/lerobot/teleoperators/piper_leader/piper_leader.py new file mode 100644 index 0000000..540a26f --- /dev/null +++ b/src/lerobot/teleoperators/piper_leader/piper_leader.py @@ -0,0 +1,220 @@ +#!/usr/bin/env python + +# Copyright 2026 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 functools import cached_property +from typing import Any + +from lerobot.processor import RobotAction +from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected +from lerobot.utils.piper_sdk import ( + PIPER_ACTION_KEYS, + PIPER_JOINT_NAMES, + get_piper_sdk, + milli_to_unit, + parse_piper_log_level, + unit_to_milli, +) + +from ..teleoperator import Teleoperator +from .config_piper_leader import PiperLeaderConfig + +logger = logging.getLogger(__name__) + + +class PiperLeader(Teleoperator): + """Piper leader arm used as a teleoperator through Piper SDK CAN messages.""" + + config_class = PiperLeaderConfig + name = "piper_leader" + + def __init__(self, config: PiperLeaderConfig): + super().__init__(config) + self.config = config + self._is_connected = False + self._manual_control_enabled: bool | None = None + self._last_mode_refresh_t = 0.0 + + interface_cls, _ = get_piper_sdk() + self.arm = interface_cls( + can_name=self.config.port, + judge_flag=self.config.judge_flag, + can_auto_init=self.config.can_auto_init, + logger_level=parse_piper_log_level(self.config.log_level), + ) + + @cached_property + def action_features(self) -> dict[str, type]: + return {key: float for key in PIPER_ACTION_KEYS} + + @cached_property + def feedback_features(self) -> dict[str, type]: + return {key: float for key in PIPER_ACTION_KEYS} + + @property + def is_connected(self) -> bool: + return self._is_connected + + @check_if_already_connected + def connect(self, calibrate: bool = True) -> None: + del calibrate + self.arm.ConnectPort() + if self.config.startup_sleep_s > 0: + time.sleep(self.config.startup_sleep_s) + + self._is_connected = True + try: + if self.config.set_leader_mode_on_connect: + self.arm.MasterSlaveConfig(0xFA, 0x00, 0x00, 0x00) + time.sleep(0.05) + self.configure() + except Exception: + self.arm.DisconnectPort() + self._is_connected = False + raise + + logger.info("%s connected.", self) + + @property + def is_calibrated(self) -> bool: + return True + + def calibrate(self) -> None: + logger.info("Piper leader does not require lerobot joint-range calibration. Skipping.") + + def _send_command_mode(self) -> None: + mit_mode = 0xAD if self.config.command_high_follow else 0x00 + self.arm.MotionCtrl_2(0x01, 0x01, self.config.command_speed_ratio, mit_mode) + self._last_mode_refresh_t = time.monotonic() + + def _refresh_command_mode_if_needed(self) -> None: + interval_s = self.config.mode_refresh_interval_s + if interval_s <= 0: + return + now = time.monotonic() + if now - self._last_mode_refresh_t >= interval_s: + self._send_command_mode() + + def _wait_enable(self, timeout_s: float) -> bool: + deadline = time.monotonic() + timeout_s + while time.monotonic() < deadline: + if bool(self.arm.EnablePiper()): + return True + time.sleep(0.02) + return False + + def set_manual_control(self, enabled: bool) -> None: + if not self._is_connected: + return + if enabled and self._manual_control_enabled is not True: + self.arm.DisableArm(7) + self._manual_control_enabled = True + return + if not enabled and self._manual_control_enabled is not False: + self._send_command_mode() + if not self._wait_enable(self.config.enable_timeout_s): + logger.warning("Piper leader did not report enabled state before timeout.") + self._manual_control_enabled = False + + def configure(self) -> None: + self.set_manual_control(self.config.manual_control) + + def _read_joint_from_ctrl(self) -> dict[str, float] | None: + joint_ctrl_msg = self.arm.GetArmJointCtrl() + if getattr(joint_ctrl_msg, "time_stamp", 0.0) <= 0: + return None + joint_ctrl = getattr(joint_ctrl_msg, "joint_ctrl", None) + return { + f"{joint_name}.pos": milli_to_unit(getattr(joint_ctrl, joint_name, 0)) + for joint_name in PIPER_JOINT_NAMES + } + + def _read_joint_from_feedback(self) -> dict[str, float] | None: + joint_msg = self.arm.GetArmJointMsgs() + joint_state = getattr(joint_msg, "joint_state", None) + if joint_state is None: + return None + return { + f"{joint_name}.pos": milli_to_unit(getattr(joint_state, joint_name, 0)) + for joint_name in PIPER_JOINT_NAMES + } + + def _read_gripper_from_ctrl(self) -> float | None: + gripper_ctrl_msg = self.arm.GetArmGripperCtrl() + if getattr(gripper_ctrl_msg, "time_stamp", 0.0) <= 0: + return None + gripper_ctrl = getattr(gripper_ctrl_msg, "gripper_ctrl", None) + return abs(milli_to_unit(getattr(gripper_ctrl, "grippers_angle", 0))) + + def _read_gripper_from_feedback(self) -> float | None: + gripper_msg = self.arm.GetArmGripperMsgs() + gripper_state = getattr(gripper_msg, "gripper_state", None) + if gripper_state is None: + return None + return abs(milli_to_unit(getattr(gripper_state, "grippers_angle", 0))) + + @check_if_not_connected + def get_action(self) -> RobotAction: + action: dict[str, float] | None = None + if self.config.prefer_ctrl_messages: + action = self._read_joint_from_ctrl() + + if action is None and self.config.fallback_to_feedback: + action = self._read_joint_from_feedback() + + if action is None: + action = {f"{joint_name}.pos": 0.0 for joint_name in PIPER_JOINT_NAMES} + + if self.config.sync_gripper: + gripper_pos = self._read_gripper_from_ctrl() if self.config.prefer_ctrl_messages else None + if gripper_pos is None and self.config.fallback_to_feedback: + gripper_pos = self._read_gripper_from_feedback() + action["gripper.pos"] = 0.0 if gripper_pos is None else gripper_pos + else: + action["gripper.pos"] = 0.0 + + return action + + @check_if_not_connected + def send_feedback(self, feedback: dict[str, Any]) -> None: + self.set_manual_control(False) + self._refresh_command_mode_if_needed() + + joint_keys = [f"{joint_name}.pos" for joint_name in PIPER_JOINT_NAMES] + has_all_joints = all(key in feedback for key in joint_keys) + if has_all_joints: + joint_commands = [unit_to_milli(feedback[key]) for key in joint_keys] + self.arm.JointCtrl(*joint_commands) + + if self.config.sync_gripper and "gripper.pos" in feedback: + gripper_pos_raw = unit_to_milli(abs(feedback["gripper.pos"])) + self.arm.GripperCtrl( + gripper_pos_raw, + self.config.gripper_effort_default, + self.config.gripper_status_code, + 0x00, + ) + + @check_if_not_connected + def disconnect(self) -> None: + try: + if self.config.disable_on_disconnect: + self.arm.DisableArm(7) + finally: + self.arm.DisconnectPort() + self._is_connected = False + logger.info("%s disconnected.", self) diff --git a/src/lerobot/teleoperators/utils.py b/src/lerobot/teleoperators/utils.py index 16454d5..e51a86b 100644 --- a/src/lerobot/teleoperators/utils.py +++ b/src/lerobot/teleoperators/utils.py @@ -91,6 +91,10 @@ def make_teleoperator_from_config(config: TeleoperatorConfig) -> "Teleoperator": from .openarm_leader import OpenArmLeader return OpenArmLeader(config) + elif config.type == "piper_leader": + from .piper_leader import PiperLeader + + return PiperLeader(config) elif config.type == "bi_openarm_leader": from .bi_openarm_leader import BiOpenArmLeader diff --git a/src/lerobot/utils/piper_sdk.py b/src/lerobot/utils/piper_sdk.py new file mode 100644 index 0000000..c61ae4b --- /dev/null +++ b/src/lerobot/utils/piper_sdk.py @@ -0,0 +1,121 @@ +#!/usr/bin/env python + +# Copyright 2026 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 __future__ import annotations + +import os +import sys +from functools import lru_cache +from pathlib import Path +from typing import Any + +PIPER_JOINT_NAMES = ( + "joint_1", + "joint_2", + "joint_3", + "joint_4", + "joint_5", + "joint_6", +) +PIPER_ACTION_KEYS = tuple(f"{joint}.pos" for joint in PIPER_JOINT_NAMES) + ("gripper.pos",) + + +def milli_to_unit(value: float | int) -> float: + return float(value) * 1e-3 + + +def unit_to_milli(value: float | int) -> int: + return int(round(float(value) * 1e3)) + + +def _iter_candidate_paths() -> list[Path]: + repo_root = Path(__file__).resolve().parents[3] + env_path = os.environ.get("PIPER_SDK_PATH") + candidates: list[Path] = [] + if env_path: + candidates.append(Path(env_path).expanduser()) + + candidates.extend( + [ + Path.cwd(), + repo_root, + repo_root.parent, + repo_root / "piper_sdk", + repo_root.parent / "piper_sdk", + ] + ) + return candidates + + +def _add_path_if_matches(candidate: Path) -> bool: + candidate = candidate.resolve() + + package_dir = candidate / "piper_sdk" + if package_dir.is_dir() and (package_dir / "__init__.py").is_file(): + parent = str(candidate) + if parent not in sys.path: + sys.path.insert(0, parent) + return True + + if candidate.name == "piper_sdk" and (candidate / "__init__.py").is_file(): + parent = str(candidate.parent) + if parent not in sys.path: + sys.path.insert(0, parent) + return True + + return False + + +@lru_cache(maxsize=1) +def get_piper_sdk() -> tuple[type[Any], Any]: + try: + from piper_sdk import C_PiperInterface_V2, LogLevel + + return C_PiperInterface_V2, LogLevel + except ModuleNotFoundError: + pass + + searched_paths: list[str] = [] + for candidate in _iter_candidate_paths(): + searched_paths.append(str(candidate)) + if not candidate.exists(): + continue + _add_path_if_matches(candidate) + try: + from piper_sdk import C_PiperInterface_V2, LogLevel + + return C_PiperInterface_V2, LogLevel + except ModuleNotFoundError: + continue + + searched = "\n- ".join(searched_paths) + raise ModuleNotFoundError( + "Could not import `piper_sdk`. Install it (for example: `pip install -e /path/to/piper_sdk`) " + "or set PIPER_SDK_PATH. Searched paths:\n- " + f"{searched}" + ) + + +def parse_piper_log_level(level_name: str) -> Any: + _, log_level_enum = get_piper_sdk() + normalized = level_name.upper() + try: + return getattr(log_level_enum, normalized) + except AttributeError as exc: + raise ValueError( + f"Invalid Piper log level '{level_name}'. " + "Expected one of: DEBUG, INFO, WARNING, ERROR, CRITICAL, SILENT." + ) from exc diff --git a/tests/test_piper_teleop.py b/tests/test_piper_teleop.py new file mode 100644 index 0000000..dd2608f --- /dev/null +++ b/tests/test_piper_teleop.py @@ -0,0 +1,175 @@ +from types import SimpleNamespace + +import lerobot.utils.piper_sdk as piper_sdk_utils +from lerobot.robots.piper_follower import PiperFollower, PiperFollowerConfig +from lerobot.robots.utils import make_robot_from_config +from lerobot.teleoperators.piper_leader import PiperLeader, PiperLeaderConfig +from lerobot.teleoperators.utils import make_teleoperator_from_config + + +class FakeLogLevel: + DEBUG = "DEBUG" + INFO = "INFO" + WARNING = "WARNING" + ERROR = "ERROR" + CRITICAL = "CRITICAL" + SILENT = "SILENT" + + +class FakePiperInterface: + def __init__(self, can_name, judge_flag=False, can_auto_init=True, logger_level=None): + self.can_name = can_name + self.judge_flag = judge_flag + self.can_auto_init = can_auto_init + self.logger_level = logger_level + self.connected = False + self.mode_commands = [] + self.role_commands = [] + self.last_joint = None + self.last_gripper = None + self.enable_calls = 0 + self.disable_calls = 0 + + self._joint_ctrl = SimpleNamespace( + time_stamp=1.0, + Hz=120.0, + joint_ctrl=SimpleNamespace( + joint_1=10000, + joint_2=20000, + joint_3=30000, + joint_4=40000, + joint_5=50000, + joint_6=60000, + ), + ) + self._joint_state = SimpleNamespace( + time_stamp=1.0, + Hz=120.0, + joint_state=SimpleNamespace( + joint_1=11000, + joint_2=21000, + joint_3=31000, + joint_4=41000, + joint_5=51000, + joint_6=61000, + ), + ) + self._gripper_ctrl = SimpleNamespace( + time_stamp=1.0, + Hz=120.0, + gripper_ctrl=SimpleNamespace(grippers_angle=42000, grippers_effort=1500, status_code=0x01), + ) + self._gripper_state = SimpleNamespace( + time_stamp=1.0, + Hz=120.0, + gripper_state=SimpleNamespace(grippers_angle=43000, grippers_effort=1400, status_code=0x01), + ) + + def ConnectPort(self): + self.connected = True + + def DisconnectPort(self, thread_timeout=0.1): + del thread_timeout + self.connected = False + + def MotionCtrl_2(self, *args): + self.mode_commands.append(args) + + def MasterSlaveConfig(self, *args): + self.role_commands.append(args) + + def EnablePiper(self): + self.enable_calls += 1 + return True + + def DisableArm(self, motor_num): + del motor_num + self.disable_calls += 1 + + def JointCtrl(self, *args): + self.last_joint = args + + def GripperCtrl(self, *args): + self.last_gripper = args + + def GetArmJointCtrl(self): + return self._joint_ctrl + + def GetArmJointMsgs(self): + return self._joint_state + + def GetArmGripperCtrl(self): + return self._gripper_ctrl + + def GetArmGripperMsgs(self): + return self._gripper_state + + +def patch_fake_sdk(monkeypatch): + monkeypatch.setattr(piper_sdk_utils, "get_piper_sdk", lambda: (FakePiperInterface, FakeLogLevel)) + + +def test_piper_leader_follower_teleop_roundtrip(monkeypatch): + patch_fake_sdk(monkeypatch) + + teleop_cfg = PiperLeaderConfig( + port="can1", + set_leader_mode_on_connect=True, + manual_control=False, + sync_gripper=True, + ) + robot_cfg = PiperFollowerConfig( + port="can0", + set_slave_mode_on_connect=True, + sync_gripper=True, + ) + + teleop = make_teleoperator_from_config(teleop_cfg) + robot = make_robot_from_config(robot_cfg) + + assert isinstance(teleop, PiperLeader) + assert isinstance(robot, PiperFollower) + + teleop.connect() + robot.connect() + try: + action = teleop.get_action() + sent = robot.send_action(action) + obs = robot.get_observation() + + assert robot.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000) + assert robot.arm.last_gripper == ( + 42000, + robot_cfg.gripper_effort_default, + robot_cfg.gripper_status_code, + 0x00, + ) + assert sent["joint_1.pos"] == 10.0 + assert sent["gripper.pos"] == 42.0 + assert obs["joint_1.pos"] == 11.0 + assert obs["gripper.pos"] == 43.0 + + teleop.send_feedback(action) + assert teleop.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000) + assert teleop.arm.last_gripper == ( + 42000, + teleop_cfg.gripper_effort_default, + teleop_cfg.gripper_status_code, + 0x00, + ) + finally: + teleop.disconnect() + robot.disconnect() + + +def test_piper_calibration_is_noop(monkeypatch): + patch_fake_sdk(monkeypatch) + + teleop = PiperLeader(PiperLeaderConfig(port="can1")) + robot = PiperFollower(PiperFollowerConfig(port="can0")) + + assert teleop.is_calibrated + assert robot.is_calibrated + + teleop.calibrate() + robot.calibrate() From b4482674ef95b41e88eaa9145acd3925b4768968 Mon Sep 17 00:00:00 2001 From: Elvin <223015152@link.cuhk.edu.cn> Date: Wed, 4 Mar 2026 21:34:42 +0800 Subject: [PATCH 04/26] fix(piper): align follower flags and SDK dependency --- README.md | 114 ++++++++++++++---- pyproject.toml | 1 + .../piper_follower/config_piper_follower.py | 4 +- .../robots/piper_follower/piper_follower.py | 2 +- src/lerobot/utils/piper_sdk.py | 67 +--------- tests/test_piper_teleop.py | 9 +- 6 files changed, 104 insertions(+), 93 deletions(-) diff --git a/README.md b/README.md index 390fd86..b7adcd3 100644 --- a/README.md +++ b/README.md @@ -29,6 +29,15 @@ - **[2026-02-26]** First SO101 real-world RL baseline and reproducible CLI workflow are released. +## 🧭 Table of Contents + +| Getting Started | Training Pipeline | Project Info | +| --- | --- | --- | +| [⚡ Quick Start](#quick-start) | [4) Value Function Training](#value-function-training) | [Model & Dataset](#model--dataset) | +| [1) Installation](#installation) | [5) Value Inference](#value-inference) | [Community Channels](#community-channels) | +| [2) Hardware Setup](#hardware-setup) | [6) Policy Training](#policy-training) | [Affiliations](#affiliations) | +| [3) Data Collection](#data-collection) | [7) Closed-loop Rollout and Next Round](#closed-loop-rollout-and-next-round) | [Citation](#citation) / [License](#license) | +

Value Visual Results

Success Case

@@ -57,10 +66,12 @@ Human-in-the-Loop Result 2

+ ## ⚡ Quick Start **LeRobot-aligned foundation:** we use LeRobot as the base of this codebase because its inference and data-collection logic are highly aligned with real-world RL workflows. + ### 1) Installation ```bash @@ -73,13 +84,13 @@ pip install -e . For setup details and platform-specific dependencies, follow the official [LeRobot configuration guide](https://huggingface.co/docs/lerobot/installation). + ### 2) Hardware Setup -#### SO101 +#### SO Series (SO100/SO101) -Due to hardware limitations, SO100 is **not supported** for real-world reinforcement learning. - -For SO101 setup, please follow the [official SO101 tutorial](https://wiki.seeedstudio.com/cn/lerobot_so100m/) in detail and complete all installation and configuration steps there before continuing. +For SO-series setup, please follow the [official tutorial](https://wiki.seeedstudio.com/cn/lerobot_so100m/) in detail and complete all installation and configuration steps there before continuing. +The examples below use **SO101** as the reference configuration. #### Device path recommendation @@ -186,9 +197,51 @@ For dual-arm camera mapping, it is fine to attach `front` under either the left- If needed, you can also use temporary device paths (for example `/dev/ttyACM*` and `/dev/video*`) during initial debugging. + +#### AgileX PiPER + +For PiPER setup, PiPER uses CAN interfaces instead of serial ports. +So first run `lerobot-setup-can` to confirm CAN interfaces are available: + +```bash +lerobot-setup-can --mode=setup --interfaces=can0,can1 +``` + +PiPER does not use `lerobot-calibrate`, and if you switch master/follower roles with the flags below, you must power-cycle both arms before enabling. + +```bash +--teleop.set_leader_mode_on_connect=true +--robot.set_follower_mode_on_connect=true +``` + +For single-arm users, run the command below to verify the system is ready: + +```bash +lerobot-teleoperate \ + --robot.type=piper_follower \ + --robot.port=can0 \ + --robot.id=my_piper_follower \ + --teleop.type=piper_leader \ + --teleop.port=can1 \ + --teleop.id=my_piper_leader +``` + +Optional PiPER flags: + +```bash +--robot.sync_gripper=true|false +--robot.gripper_effort_default=1000 +--teleop.sync_gripper=true|false +``` + + ### 3) Data Collection -Collect rollout data with `lerobot-human-inloop-record`. Bimanual template: +Collect rollout data with `lerobot-human-inloop-record`. + +#### SO Series (SO100/SO101) + +Bimanual template: ```bash lerobot-human-inloop-record \ @@ -213,6 +266,24 @@ lerobot-human-inloop-record \ Recommendation: use **`fourcc: "MJPG"`** for OpenCV and **`warmup_s`** for RealSense. In this example `front` uses RealSense, but you can switch it to OpenCV with the same structure. +#### AgileX PiPER + +```bash +lerobot-human-inloop-record \ + --robot.type=piper_follower \ + --robot.port=can0 \ + --robot.id=my_piper_follower \ + --teleop.type=piper_leader \ + --teleop.port=can1 \ + --teleop.id=my_piper_leader \ + --dataset.repo_id=/ \ + --dataset.single_task="" \ + --dataset.num_episodes= \ + --dataset.episode_time_s= \ + --dataset.reset_time_s= \ + --dataset.push_to_hub=true +``` + Hotkeys: - `i`: toggle intervention mode (policy <-> teleop takeover) @@ -230,6 +301,7 @@ lerobot-dataset-report --dataset / This prints: dataset meta, totals, episode-length stats/histogram, success/intervention metrics, task list, and full feature schema. + ### 4) Value Function Training Train the value function on the current dataset. Current default: [Pi\*0.6](https://www.pi.website/blog/pistar06) (`--value.type=pistar06`). @@ -268,6 +340,7 @@ To plug in a different value function, minimal path in this repo: - Add `src/lerobot/values//processor_.py` with `make__pre_post_processors(...)`. - Remove/replace the current `pistar06`-only type checks in `src/lerobot/configs/value_train.py` and `src/lerobot/scripts/lerobot_value_infer.py`. + ### 5) Value Inference Infer value signals and write value/advantage/indicator back to the dataset: @@ -307,32 +380,22 @@ CUDA_VISIBLE_DEVICES= accelerate launch \ Parameter notes: -- `--acp.n_step`: n-step advantage horizon. -- `--acp.positive_ratio`: positive label ratio after advantage binarization (e.g., `0.3` = top 30% per task). +```bash +--acp.n_step: n-step advantage horizon. +--acp.positive_ratio: positive label ratio after advantage binarization (e.g., 0.3 = top 30% per task). +``` Expected new columns: -- `complementary_info.value_` -- `complementary_info.advantage_` -- `complementary_info.acp_indicator_` +```bash +complementary_info.value_ +complementary_info.advantage_ +complementary_info.acp_indicator_ +``` These columns are written back to the original dataset specified by `--dataset.repo_id`. -Visualization (optional): - -- Enable with `--viz.enable=true`. -- Output videos are saved to `outputs/value_infer//value/viz/`. -- Each video overlays a value curve and per-frame text (`advantage`, `acp_indicator`) on top of camera frames. -- You can control scope with: - - `--viz.episodes=all` or ranges like `0-9,12` - - `--viz.video_key=` (default picks `front` if available) - - `--viz.overwrite=true|false` - - `--viz.vcodec=` and `--viz.frame_storage_mode=memory|disk` - -Value overlay examples (SO101, smooth=11): - -- GIF assets are stored under `website/assets/gifs/`. - + ### 6) Policy Training Train the policy with advantage-conditioned tags. @@ -377,6 +440,7 @@ CUDA_VISIBLE_DEVICES= accelerate launch \ ``` + ### 7) Closed-loop Rollout and Next Round Deploy the trained policy in human-in-loop mode and collect the next dataset round: diff --git a/pyproject.toml b/pyproject.toml index fbb3296..38b5c88 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -74,6 +74,7 @@ dependencies = [ "packaging>=24.2,<26.0", "pynput>=1.7.7,<1.9.0", "pyserial>=3.5,<4.0", + "piper_sdk>=0.6.1,<0.7.0", "wandb>=0.24.0,<0.25.0", "torch>=2.2.1,<2.8.0", # TODO: Bumb dependency diff --git a/src/lerobot/robots/piper_follower/config_piper_follower.py b/src/lerobot/robots/piper_follower/config_piper_follower.py index da9e241..56bafab 100644 --- a/src/lerobot/robots/piper_follower/config_piper_follower.py +++ b/src/lerobot/robots/piper_follower/config_piper_follower.py @@ -34,8 +34,8 @@ class PiperFollowerConfigBase: log_level: str = "WARNING" startup_sleep_s: float = 0.1 - # Optional role command on startup (0xFC = motion output arm) - set_slave_mode_on_connect: bool = False + # Optional role command on startup (0xFC = motion output/follower arm) + set_follower_mode_on_connect: bool = False # Motion mode for follower arm speed_ratio: int = 100 diff --git a/src/lerobot/robots/piper_follower/piper_follower.py b/src/lerobot/robots/piper_follower/piper_follower.py index c097c35..854fba9 100644 --- a/src/lerobot/robots/piper_follower/piper_follower.py +++ b/src/lerobot/robots/piper_follower/piper_follower.py @@ -85,7 +85,7 @@ class PiperFollower(Robot): self._is_connected = True try: - if self.config.set_slave_mode_on_connect: + if self.config.set_follower_mode_on_connect: self.arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00) time.sleep(0.05) diff --git a/src/lerobot/utils/piper_sdk.py b/src/lerobot/utils/piper_sdk.py index c61ae4b..2a71839 100644 --- a/src/lerobot/utils/piper_sdk.py +++ b/src/lerobot/utils/piper_sdk.py @@ -16,10 +16,7 @@ from __future__ import annotations -import os -import sys from functools import lru_cache -from pathlib import Path from typing import Any PIPER_JOINT_NAMES = ( @@ -41,72 +38,16 @@ def unit_to_milli(value: float | int) -> int: return int(round(float(value) * 1e3)) -def _iter_candidate_paths() -> list[Path]: - repo_root = Path(__file__).resolve().parents[3] - env_path = os.environ.get("PIPER_SDK_PATH") - candidates: list[Path] = [] - if env_path: - candidates.append(Path(env_path).expanduser()) - - candidates.extend( - [ - Path.cwd(), - repo_root, - repo_root.parent, - repo_root / "piper_sdk", - repo_root.parent / "piper_sdk", - ] - ) - return candidates - - -def _add_path_if_matches(candidate: Path) -> bool: - candidate = candidate.resolve() - - package_dir = candidate / "piper_sdk" - if package_dir.is_dir() and (package_dir / "__init__.py").is_file(): - parent = str(candidate) - if parent not in sys.path: - sys.path.insert(0, parent) - return True - - if candidate.name == "piper_sdk" and (candidate / "__init__.py").is_file(): - parent = str(candidate.parent) - if parent not in sys.path: - sys.path.insert(0, parent) - return True - - return False - - @lru_cache(maxsize=1) def get_piper_sdk() -> tuple[type[Any], Any]: try: from piper_sdk import C_PiperInterface_V2, LogLevel return C_PiperInterface_V2, LogLevel - except ModuleNotFoundError: - pass - - searched_paths: list[str] = [] - for candidate in _iter_candidate_paths(): - searched_paths.append(str(candidate)) - if not candidate.exists(): - continue - _add_path_if_matches(candidate) - try: - from piper_sdk import C_PiperInterface_V2, LogLevel - - return C_PiperInterface_V2, LogLevel - except ModuleNotFoundError: - continue - - searched = "\n- ".join(searched_paths) - raise ModuleNotFoundError( - "Could not import `piper_sdk`. Install it (for example: `pip install -e /path/to/piper_sdk`) " - "or set PIPER_SDK_PATH. Searched paths:\n- " - f"{searched}" - ) + except ModuleNotFoundError as exc: + raise ModuleNotFoundError( + "Could not import `piper_sdk`. Install Evo-RL dependencies first (for example: `pip install -e .`)." + ) from exc def parse_piper_log_level(level_name: str) -> Any: diff --git a/tests/test_piper_teleop.py b/tests/test_piper_teleop.py index dd2608f..289d2c7 100644 --- a/tests/test_piper_teleop.py +++ b/tests/test_piper_teleop.py @@ -1,6 +1,8 @@ from types import SimpleNamespace import lerobot.utils.piper_sdk as piper_sdk_utils +import lerobot.robots.piper_follower.piper_follower as piper_follower_module +import lerobot.teleoperators.piper_leader.piper_leader as piper_leader_module from lerobot.robots.piper_follower import PiperFollower, PiperFollowerConfig from lerobot.robots.utils import make_robot_from_config from lerobot.teleoperators.piper_leader import PiperLeader, PiperLeaderConfig @@ -106,7 +108,10 @@ class FakePiperInterface: def patch_fake_sdk(monkeypatch): - monkeypatch.setattr(piper_sdk_utils, "get_piper_sdk", lambda: (FakePiperInterface, FakeLogLevel)) + fake_loader = lambda: (FakePiperInterface, FakeLogLevel) + monkeypatch.setattr(piper_sdk_utils, "get_piper_sdk", fake_loader) + monkeypatch.setattr(piper_follower_module, "get_piper_sdk", fake_loader) + monkeypatch.setattr(piper_leader_module, "get_piper_sdk", fake_loader) def test_piper_leader_follower_teleop_roundtrip(monkeypatch): @@ -120,7 +125,7 @@ def test_piper_leader_follower_teleop_roundtrip(monkeypatch): ) robot_cfg = PiperFollowerConfig( port="can0", - set_slave_mode_on_connect=True, + set_follower_mode_on_connect=True, sync_gripper=True, ) From df08db9005c309e553c01ad5e73b0085b1f8e195 Mon Sep 17 00:00:00 2001 From: Elvin <223015152@link.cuhk.edu.cn> Date: Thu, 5 Mar 2026 17:12:27 +0800 Subject: [PATCH 05/26] docs(piper): clarify leader/follower mode guidance for HIL flows - document MasterSlaveConfig linkage_config semantics (0xFA/0xFC) in README - update piper leader startup role command from 0xFA to 0xFC - add inline note on commandability and power-cycle requirement after role switch - align piper leader config comment with 0xFC startup role --- README.md | 8 ++++++++ .../teleoperators/piper_leader/config_piper_leader.py | 2 +- src/lerobot/teleoperators/piper_leader/piper_leader.py | 7 ++++++- 3 files changed, 15 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index b7adcd3..66c137f 100644 --- a/README.md +++ b/README.md @@ -268,6 +268,14 @@ Recommendation: use **`fourcc: "MJPG"`** for OpenCV and **`warmup_s`** for RealS #### AgileX PiPER +PiPER mode note (policy-sync / human-in-the-loop): + +- Leader/follower linkage is configured via `MasterSlaveConfig(linkage_config, feedback_offset, ctrl_offset, linkage_offset)`. +- `linkage_config=0xFA`: teaching-input arm. +- `linkage_config=0xFC`: motion-output arm. +- `0xFA` is not suitable when the arm must accept external control commands. For policy execution + intervention switching, keep both teleop and robot arms in `0xFC`. +- After switching role mode (`0xFA` <-> `0xFC`), perform a full power-cycle before validation. + ```bash lerobot-human-inloop-record \ --robot.type=piper_follower \ diff --git a/src/lerobot/teleoperators/piper_leader/config_piper_leader.py b/src/lerobot/teleoperators/piper_leader/config_piper_leader.py index 768114a..9799b20 100644 --- a/src/lerobot/teleoperators/piper_leader/config_piper_leader.py +++ b/src/lerobot/teleoperators/piper_leader/config_piper_leader.py @@ -32,7 +32,7 @@ class PiperLeaderConfigBase: log_level: str = "WARNING" startup_sleep_s: float = 0.1 - # Optional role command on startup (0xFA = teaching input arm) + # Optional role command on startup (0xFC = motion output arm) set_leader_mode_on_connect: bool = False # Manual backdrivable mode for human teleop diff --git a/src/lerobot/teleoperators/piper_leader/piper_leader.py b/src/lerobot/teleoperators/piper_leader/piper_leader.py index 540a26f..69a58dd 100644 --- a/src/lerobot/teleoperators/piper_leader/piper_leader.py +++ b/src/lerobot/teleoperators/piper_leader/piper_leader.py @@ -79,7 +79,12 @@ class PiperLeader(Teleoperator): self._is_connected = True try: if self.config.set_leader_mode_on_connect: - self.arm.MasterSlaveConfig(0xFA, 0x00, 0x00, 0x00) + # NOTE: + # - Piper teaching-input mode (0xFA) does not accept external JointCtrl/GripperCtrl commands. + # - For policy-sync and human-in-the-loop switching, the leader must remain commandable, + # so we configure it as motion-output mode (0xFC), same as the follower side. + # - Mode-role changes (e.g., 0xFA <-> 0xFC) may require a full power-cycle to take effect. + self.arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00) time.sleep(0.05) self.configure() except Exception: From 422f5c50e76c3b07c75ba4da33c96c5e55c3d1cc Mon Sep 17 00:00:00 2001 From: Elvin <223015152@link.cuhk.edu.cn> Date: Thu, 5 Mar 2026 21:09:46 +0800 Subject: [PATCH 06/26] feat(piper): add calibration-driven leader/follower mapping - add calibration_scale config and validation for piper leader/follower - implement interactive calibration capture and calibrated offset conversion - require calibration before action/feedback paths and update teleop tests --- .../piper_follower/config_piper_follower.py | 6 + .../robots/piper_follower/piper_follower.py | 112 +++++++++++++-- .../piper_leader/config_piper_leader.py | 6 + .../piper_leader/piper_leader.py | 131 ++++++++++++++++-- tests/test_piper_teleop.py | 46 +++++- 5 files changed, 273 insertions(+), 28 deletions(-) diff --git a/src/lerobot/robots/piper_follower/config_piper_follower.py b/src/lerobot/robots/piper_follower/config_piper_follower.py index 56bafab..c2fdb38 100644 --- a/src/lerobot/robots/piper_follower/config_piper_follower.py +++ b/src/lerobot/robots/piper_follower/config_piper_follower.py @@ -46,6 +46,10 @@ class PiperFollowerConfigBase: enable_on_connect: bool = True enable_timeout_s: float = 3.0 + # Calibration precision: + # homing_offset/range_min/range_max are stored as "degree * calibration_scale". + calibration_scale: int = 1000 + # Gripper forwarding behavior sync_gripper: bool = True gripper_effort_default: int = 1000 @@ -69,6 +73,8 @@ class PiperFollowerConfig(RobotConfig, PiperFollowerConfigBase): raise ValueError("`mode_refresh_interval_s` must be >= 0.") if self.enable_timeout_s < 0: raise ValueError("`enable_timeout_s` must be >= 0.") + if self.calibration_scale <= 0: + raise ValueError("`calibration_scale` must be > 0.") if self.startup_sleep_s < 0: raise ValueError("`startup_sleep_s` must be >= 0.") if not (0 <= self.gripper_effort_default <= 5000): diff --git a/src/lerobot/robots/piper_follower/piper_follower.py b/src/lerobot/robots/piper_follower/piper_follower.py index 854fba9..c80e19c 100644 --- a/src/lerobot/robots/piper_follower/piper_follower.py +++ b/src/lerobot/robots/piper_follower/piper_follower.py @@ -20,6 +20,7 @@ from functools import cached_property from typing import Any from lerobot.cameras.utils import make_cameras_from_configs +from lerobot.motors import MotorCalibration from lerobot.processor import RobotAction, RobotObservation from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected from lerobot.utils.piper_sdk import ( @@ -30,11 +31,14 @@ from lerobot.utils.piper_sdk import ( parse_piper_log_level, unit_to_milli, ) +from lerobot.utils.utils import enter_pressed, move_cursor_up from ..robot import Robot from .config_piper_follower import PiperFollowerConfig logger = logging.getLogger(__name__) +PIPER_CALIB_KEYS = list(PIPER_ACTION_KEYS) +PIPER_CALIB_IDS = {key: idx for idx, key in enumerate(PIPER_CALIB_KEYS)} class PiperFollower(Robot): @@ -78,7 +82,6 @@ class PiperFollower(Robot): @check_if_already_connected def connect(self, calibrate: bool = True) -> None: - del calibrate self.arm.ConnectPort() if self.config.startup_sleep_s > 0: time.sleep(self.config.startup_sleep_s) @@ -90,8 +93,14 @@ class PiperFollower(Robot): time.sleep(0.05) self.configure() - if self.config.enable_on_connect and not self._wait_enable(self.config.enable_timeout_s): + # lerobot-calibrate calls connect(calibrate=False) before running calibrate(). + # Keep the arm in drag mode for calibration by skipping auto-enable in this path. + should_enable = self.config.enable_on_connect and calibrate + if should_enable and not self._wait_enable(self.config.enable_timeout_s): logger.warning("Piper follower did not report enabled state before timeout.") + if not self.is_calibrated and calibrate: + logger.info("No piper-follower calibration file found for '%s'. Running lerobot-calibrate flow.", self.id) + self.calibrate() for cam in self.cameras.values(): cam.connect() @@ -104,10 +113,50 @@ class PiperFollower(Robot): @property def is_calibrated(self) -> bool: + if not all(key in self.calibration for key in PIPER_CALIB_KEYS): + return False + for key in PIPER_CALIB_KEYS: + cal = self.calibration[key] + if cal.range_max <= cal.range_min: + return False return True def calibrate(self) -> None: - logger.info("Piper follower does not require lerobot joint-range calibration. Skipping.") + if self.calibration and self.is_calibrated: + user_input = input( + f"Press ENTER to use existing calibration file for id '{self.id}', " + "or type 'c' and press ENTER to run a new calibration: " + ) + if user_input.strip().lower() != "c": + return + + logger.info("Running calibration for %s", self) + # Calibration for Piper follower should be sampled in pure backdrivable mode. + self.arm.DisableArm(7) + time.sleep(0.1) + input("Move piper-follower to your desired neutral/center pose, then press ENTER...") + neutral = self._read_raw_observation() + print("Move all piper-follower joints through full range. Press ENTER to stop recording...") + range_mins, range_maxes = self._record_ranges_of_motion() + + self.calibration = {} + for key in PIPER_CALIB_KEYS: + min_deg = range_mins[key] + max_deg = range_maxes[key] + if max_deg <= min_deg: + raise ValueError(f"Invalid range for {key}: min={min_deg:.3f}, max={max_deg:.3f}") + + neutral_deg = min(max_deg, max(min_deg, neutral[key])) + self.calibration[key] = MotorCalibration( + id=PIPER_CALIB_IDS[key], + drive_mode=0, + homing_offset=self._to_calibration_units(neutral_deg), + range_min=self._to_calibration_units(min_deg), + range_max=self._to_calibration_units(max_deg), + ) + + self._save_calibration() + print(f"Calibration saved to {self.calibration_fpath}") def configure(self) -> None: self._send_motion_mode() @@ -133,12 +182,11 @@ class PiperFollower(Robot): time.sleep(0.02) return False - @check_if_not_connected - def get_observation(self) -> RobotObservation: + def _read_raw_observation(self) -> RobotObservation: joint_msg = self.arm.GetArmJointMsgs() joint_state = getattr(joint_msg, "joint_state", None) - obs: dict[str, float] = {} + obs: RobotObservation = {} for joint_name in PIPER_JOINT_NAMES: raw_value = getattr(joint_state, joint_name, 0) obs[f"{joint_name}.pos"] = milli_to_unit(raw_value) @@ -146,6 +194,47 @@ class PiperFollower(Robot): gripper_msg = self.arm.GetArmGripperMsgs() gripper_state = getattr(gripper_msg, "gripper_state", None) obs["gripper.pos"] = abs(milli_to_unit(getattr(gripper_state, "grippers_angle", 0))) + return obs + + def _to_calibration_units(self, angle_deg: float) -> int: + return int(round(angle_deg * self.config.calibration_scale)) + + def _from_calibration_units(self, value: int) -> float: + return float(value) / float(self.config.calibration_scale) + + def _offset_to_target(self, key: str, offset_deg: float) -> float: + cal = self.calibration[key] + min_deg = self._from_calibration_units(cal.range_min) + max_deg = self._from_calibration_units(cal.range_max) + home_deg = self._from_calibration_units(cal.homing_offset) + centered = -offset_deg if cal.drive_mode else offset_deg + target = home_deg + centered + return min(max_deg, max(min_deg, target)) + + def _record_ranges_of_motion(self) -> tuple[dict[str, float], dict[str, float]]: + current = self._read_raw_observation() + mins = current.copy() + maxes = current.copy() + + while True: + current = self._read_raw_observation() + mins = {key: min(mins[key], current[key]) for key in PIPER_CALIB_KEYS} + maxes = {key: max(maxes[key], current[key]) for key in PIPER_CALIB_KEYS} + + print("\n-----------------------------") + print("JOINT | MIN | POS | MAX") + for key in PIPER_CALIB_KEYS: + print(f"{key:<11} | {mins[key]:>6.2f} | {current[key]:>6.2f} | {maxes[key]:>6.2f}") + + if enter_pressed(): + break + move_cursor_up(len(PIPER_CALIB_KEYS) + 3) + + return mins, maxes + + @check_if_not_connected + def get_observation(self) -> RobotObservation: + obs: dict[str, float] = self._read_raw_observation() for cam_key, cam in self.cameras.items(): obs[cam_key] = cam.async_read() @@ -153,6 +242,11 @@ class PiperFollower(Robot): @check_if_not_connected def send_action(self, action: RobotAction) -> RobotAction: + if not self.is_calibrated: + raise RuntimeError( + f"{self} is not calibrated. Run `lerobot-calibrate --robot.type=piper_follower --robot.id={self.id}` first." + ) + self._refresh_motion_mode_if_needed() sent_action: dict[str, float] = {} @@ -160,14 +254,16 @@ class PiperFollower(Robot): joint_keys = [f"{joint_name}.pos" for joint_name in PIPER_JOINT_NAMES] has_all_joints = all(key in action for key in joint_keys) if has_all_joints: - joint_commands = [unit_to_milli(action[key]) for key in joint_keys] + joint_targets = [self._offset_to_target(key, action[key]) for key in joint_keys] + joint_commands = [unit_to_milli(value) for value in joint_targets] self.arm.JointCtrl(*joint_commands) sent_action.update({key: milli_to_unit(raw) for key, raw in zip(joint_keys, joint_commands, strict=True)}) elif any(key in action for key in joint_keys): logger.debug("Ignoring partial Piper joint action. Need all six joint keys to send command.") if self.config.sync_gripper and "gripper.pos" in action: - gripper_pos_raw = unit_to_milli(abs(action["gripper.pos"])) + gripper_target = self._offset_to_target("gripper.pos", action["gripper.pos"]) + gripper_pos_raw = unit_to_milli(gripper_target) self.arm.GripperCtrl( gripper_pos_raw, self.config.gripper_effort_default, diff --git a/src/lerobot/teleoperators/piper_leader/config_piper_leader.py b/src/lerobot/teleoperators/piper_leader/config_piper_leader.py index 9799b20..4aa359b 100644 --- a/src/lerobot/teleoperators/piper_leader/config_piper_leader.py +++ b/src/lerobot/teleoperators/piper_leader/config_piper_leader.py @@ -53,6 +53,10 @@ class PiperLeaderConfigBase: mode_refresh_interval_s: float = 1.0 enable_timeout_s: float = 3.0 + # Calibration precision: + # homing_offset/range_min/range_max are stored as "degree * calibration_scale". + calibration_scale: int = 1000 + # Safety behavior on disconnect disable_on_disconnect: bool = False @@ -67,6 +71,8 @@ class PiperLeaderConfig(TeleoperatorConfig, PiperLeaderConfigBase): raise ValueError("`mode_refresh_interval_s` must be >= 0.") if self.enable_timeout_s < 0: raise ValueError("`enable_timeout_s` must be >= 0.") + if self.calibration_scale <= 0: + raise ValueError("`calibration_scale` must be > 0.") if self.startup_sleep_s < 0: raise ValueError("`startup_sleep_s` must be >= 0.") if not (0 <= self.gripper_effort_default <= 5000): diff --git a/src/lerobot/teleoperators/piper_leader/piper_leader.py b/src/lerobot/teleoperators/piper_leader/piper_leader.py index 69a58dd..ceba231 100644 --- a/src/lerobot/teleoperators/piper_leader/piper_leader.py +++ b/src/lerobot/teleoperators/piper_leader/piper_leader.py @@ -19,6 +19,7 @@ import time from functools import cached_property from typing import Any +from lerobot.motors import MotorCalibration from lerobot.processor import RobotAction from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected from lerobot.utils.piper_sdk import ( @@ -29,11 +30,14 @@ from lerobot.utils.piper_sdk import ( parse_piper_log_level, unit_to_milli, ) +from lerobot.utils.utils import enter_pressed, move_cursor_up from ..teleoperator import Teleoperator from .config_piper_leader import PiperLeaderConfig logger = logging.getLogger(__name__) +PIPER_CALIB_KEYS = list(PIPER_ACTION_KEYS) +PIPER_CALIB_IDS = {key: idx for idx, key in enumerate(PIPER_CALIB_KEYS)} class PiperLeader(Teleoperator): @@ -71,7 +75,6 @@ class PiperLeader(Teleoperator): @check_if_already_connected def connect(self, calibrate: bool = True) -> None: - del calibrate self.arm.ConnectPort() if self.config.startup_sleep_s > 0: time.sleep(self.config.startup_sleep_s) @@ -87,6 +90,9 @@ class PiperLeader(Teleoperator): self.arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00) time.sleep(0.05) self.configure() + if not self.is_calibrated and calibrate: + logger.info("No piper-leader calibration file found for '%s'. Running lerobot-calibrate flow.", self.id) + self.calibrate() except Exception: self.arm.DisconnectPort() self._is_connected = False @@ -96,10 +102,47 @@ class PiperLeader(Teleoperator): @property def is_calibrated(self) -> bool: + if not all(key in self.calibration for key in PIPER_CALIB_KEYS): + return False + for key in PIPER_CALIB_KEYS: + cal = self.calibration[key] + if cal.range_max <= cal.range_min: + return False return True def calibrate(self) -> None: - logger.info("Piper leader does not require lerobot joint-range calibration. Skipping.") + if self.calibration and self.is_calibrated: + user_input = input( + f"Press ENTER to use existing calibration file for id '{self.id}', " + "or type 'c' and press ENTER to run a new calibration: " + ) + if user_input.strip().lower() != "c": + return + + logger.info("Running calibration for %s", self) + input("Move piper-leader to your desired neutral/center pose, then press ENTER...") + neutral = self._read_raw_action() + print("Move all piper-leader joints through full range. Press ENTER to stop recording...") + range_mins, range_maxes = self._record_ranges_of_motion() + + self.calibration = {} + for key in PIPER_CALIB_KEYS: + min_deg = range_mins[key] + max_deg = range_maxes[key] + if max_deg <= min_deg: + raise ValueError(f"Invalid range for {key}: min={min_deg:.3f}, max={max_deg:.3f}") + + neutral_deg = min(max_deg, max(min_deg, neutral[key])) + self.calibration[key] = MotorCalibration( + id=PIPER_CALIB_IDS[key], + drive_mode=0, + homing_offset=self._to_calibration_units(neutral_deg), + range_min=self._to_calibration_units(min_deg), + range_max=self._to_calibration_units(max_deg), + ) + + self._save_calibration() + print(f"Calibration saved to {self.calibration_fpath}") def _send_command_mode(self) -> None: mit_mode = 0xAD if self.config.command_high_follow else 0x00 @@ -172,8 +215,7 @@ class PiperLeader(Teleoperator): return None return abs(milli_to_unit(getattr(gripper_state, "grippers_angle", 0))) - @check_if_not_connected - def get_action(self) -> RobotAction: + def _read_raw_action(self) -> RobotAction: action: dict[str, float] | None = None if self.config.prefer_ctrl_messages: action = self._read_joint_from_ctrl() @@ -184,29 +226,92 @@ class PiperLeader(Teleoperator): if action is None: action = {f"{joint_name}.pos": 0.0 for joint_name in PIPER_JOINT_NAMES} - if self.config.sync_gripper: - gripper_pos = self._read_gripper_from_ctrl() if self.config.prefer_ctrl_messages else None - if gripper_pos is None and self.config.fallback_to_feedback: - gripper_pos = self._read_gripper_from_feedback() - action["gripper.pos"] = 0.0 if gripper_pos is None else gripper_pos - else: - action["gripper.pos"] = 0.0 + gripper_pos = self._read_gripper_from_ctrl() if self.config.prefer_ctrl_messages else None + if gripper_pos is None and self.config.fallback_to_feedback: + gripper_pos = self._read_gripper_from_feedback() + action["gripper.pos"] = 0.0 if gripper_pos is None else gripper_pos + return action + def _to_calibration_units(self, angle_deg: float) -> int: + return int(round(angle_deg * self.config.calibration_scale)) + + def _from_calibration_units(self, value: int) -> float: + return float(value) / float(self.config.calibration_scale) + + def _calibrated_to_offset(self, key: str, raw_deg: float) -> float: + cal = self.calibration[key] + min_deg = self._from_calibration_units(cal.range_min) + max_deg = self._from_calibration_units(cal.range_max) + home_deg = self._from_calibration_units(cal.homing_offset) + bounded = min(max_deg, max(min_deg, raw_deg)) + centered = bounded - home_deg + return -centered if cal.drive_mode else centered + + def _offset_to_calibrated(self, key: str, offset_deg: float) -> float: + cal = self.calibration[key] + min_deg = self._from_calibration_units(cal.range_min) + max_deg = self._from_calibration_units(cal.range_max) + home_deg = self._from_calibration_units(cal.homing_offset) + centered = -offset_deg if cal.drive_mode else offset_deg + target = home_deg + centered + return min(max_deg, max(min_deg, target)) + + def _record_ranges_of_motion(self) -> tuple[dict[str, float], dict[str, float]]: + current = self._read_raw_action() + mins = current.copy() + maxes = current.copy() + + while True: + current = self._read_raw_action() + mins = {key: min(mins[key], current[key]) for key in PIPER_CALIB_KEYS} + maxes = {key: max(maxes[key], current[key]) for key in PIPER_CALIB_KEYS} + + print("\n-----------------------------") + print("JOINT | MIN | POS | MAX") + for key in PIPER_CALIB_KEYS: + print(f"{key:<11} | {mins[key]:>6.2f} | {current[key]:>6.2f} | {maxes[key]:>6.2f}") + + if enter_pressed(): + break + move_cursor_up(len(PIPER_CALIB_KEYS) + 3) + + return mins, maxes + + @check_if_not_connected + def get_action(self) -> RobotAction: + if not self.is_calibrated: + raise RuntimeError( + f"{self} is not calibrated. Run `lerobot-calibrate --teleop.type=piper_leader --teleop.id={self.id}` first." + ) + + raw_action = self._read_raw_action() + action: RobotAction = { + key: self._calibrated_to_offset(key, raw_action[key]) for key in PIPER_CALIB_KEYS + } + if not self.config.sync_gripper: + action["gripper.pos"] = 0.0 return action @check_if_not_connected def send_feedback(self, feedback: dict[str, Any]) -> None: + if not self.is_calibrated: + raise RuntimeError( + f"{self} is not calibrated. Run `lerobot-calibrate --teleop.type=piper_leader --teleop.id={self.id}` first." + ) + self.set_manual_control(False) self._refresh_command_mode_if_needed() joint_keys = [f"{joint_name}.pos" for joint_name in PIPER_JOINT_NAMES] has_all_joints = all(key in feedback for key in joint_keys) if has_all_joints: - joint_commands = [unit_to_milli(feedback[key]) for key in joint_keys] + joint_targets = [self._offset_to_calibrated(key, feedback[key]) for key in joint_keys] + joint_commands = [unit_to_milli(value) for value in joint_targets] self.arm.JointCtrl(*joint_commands) if self.config.sync_gripper and "gripper.pos" in feedback: - gripper_pos_raw = unit_to_milli(abs(feedback["gripper.pos"])) + gripper_target = self._offset_to_calibrated("gripper.pos", feedback["gripper.pos"]) + gripper_pos_raw = unit_to_milli(gripper_target) self.arm.GripperCtrl( gripper_pos_raw, self.config.gripper_effort_default, diff --git a/tests/test_piper_teleop.py b/tests/test_piper_teleop.py index 289d2c7..9fc8a11 100644 --- a/tests/test_piper_teleop.py +++ b/tests/test_piper_teleop.py @@ -3,6 +3,7 @@ from types import SimpleNamespace import lerobot.utils.piper_sdk as piper_sdk_utils import lerobot.robots.piper_follower.piper_follower as piper_follower_module import lerobot.teleoperators.piper_leader.piper_leader as piper_leader_module +from lerobot.motors import MotorCalibration from lerobot.robots.piper_follower import PiperFollower, PiperFollowerConfig from lerobot.robots.utils import make_robot_from_config from lerobot.teleoperators.piper_leader import PiperLeader, PiperLeaderConfig @@ -114,6 +115,19 @@ def patch_fake_sdk(monkeypatch): monkeypatch.setattr(piper_leader_module, "get_piper_sdk", fake_loader) +def make_identity_calibration(): + return { + key: MotorCalibration( + id=idx, + drive_mode=0, + homing_offset=0, + range_min=-200000, + range_max=200000, + ) + for idx, key in enumerate([f"joint_{i}.pos" for i in range(1, 7)] + ["gripper.pos"]) + } + + def test_piper_leader_follower_teleop_roundtrip(monkeypatch): patch_fake_sdk(monkeypatch) @@ -135,8 +149,11 @@ def test_piper_leader_follower_teleop_roundtrip(monkeypatch): assert isinstance(teleop, PiperLeader) assert isinstance(robot, PiperFollower) - teleop.connect() - robot.connect() + teleop.calibration = make_identity_calibration() + robot.calibration = make_identity_calibration() + + teleop.connect(calibrate=False) + robot.connect(calibrate=False) try: action = teleop.get_action() sent = robot.send_action(action) @@ -167,14 +184,29 @@ def test_piper_leader_follower_teleop_roundtrip(monkeypatch): robot.disconnect() -def test_piper_calibration_is_noop(monkeypatch): +def test_piper_requires_calibration(monkeypatch): patch_fake_sdk(monkeypatch) teleop = PiperLeader(PiperLeaderConfig(port="can1")) robot = PiperFollower(PiperFollowerConfig(port="can0")) - assert teleop.is_calibrated - assert robot.is_calibrated + assert not teleop.is_calibrated + assert not robot.is_calibrated - teleop.calibrate() - robot.calibrate() + teleop.connect(calibrate=False) + robot.connect(calibrate=False) + try: + try: + teleop.get_action() + assert False, "Expected teleop.get_action() to require calibration." + except RuntimeError: + pass + + try: + robot.send_action({"joint_1.pos": 0.0}) + assert False, "Expected robot.send_action() to require calibration." + except RuntimeError: + pass + finally: + teleop.disconnect() + robot.disconnect() From 7a9aeb38505af275b99436e83a4b11148795d320 Mon Sep 17 00:00:00 2001 From: Elvin <223015152@link.cuhk.edu.cn> Date: Thu, 5 Mar 2026 21:28:56 +0800 Subject: [PATCH 07/26] refactor(piper): tighten reconnect and rollback behavior - reset leader manual-control cache on reconnect/disconnect to avoid stale mode state - rollback already-connected follower cameras when connect fails - reuse piper joint action key constant and add regression tests --- .../robots/piper_follower/piper_follower.py | 7 ++- .../piper_leader/piper_leader.py | 11 +++- src/lerobot/utils/piper_sdk.py | 3 +- tests/test_piper_teleop.py | 60 ++++++++++++++++++- 4 files changed, 76 insertions(+), 5 deletions(-) diff --git a/src/lerobot/robots/piper_follower/piper_follower.py b/src/lerobot/robots/piper_follower/piper_follower.py index c80e19c..672d91f 100644 --- a/src/lerobot/robots/piper_follower/piper_follower.py +++ b/src/lerobot/robots/piper_follower/piper_follower.py @@ -25,6 +25,7 @@ from lerobot.processor import RobotAction, RobotObservation from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected from lerobot.utils.piper_sdk import ( PIPER_ACTION_KEYS, + PIPER_JOINT_ACTION_KEYS, PIPER_JOINT_NAMES, get_piper_sdk, milli_to_unit, @@ -87,6 +88,7 @@ class PiperFollower(Robot): time.sleep(self.config.startup_sleep_s) self._is_connected = True + connected_cameras = [] try: if self.config.set_follower_mode_on_connect: self.arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00) @@ -104,8 +106,11 @@ class PiperFollower(Robot): for cam in self.cameras.values(): cam.connect() + connected_cameras.append(cam) except Exception: self.arm.DisconnectPort() + for cam in connected_cameras: + cam.disconnect() self._is_connected = False raise @@ -251,7 +256,7 @@ class PiperFollower(Robot): sent_action: dict[str, float] = {} - joint_keys = [f"{joint_name}.pos" for joint_name in PIPER_JOINT_NAMES] + joint_keys = PIPER_JOINT_ACTION_KEYS has_all_joints = all(key in action for key in joint_keys) if has_all_joints: joint_targets = [self._offset_to_target(key, action[key]) for key in joint_keys] diff --git a/src/lerobot/teleoperators/piper_leader/piper_leader.py b/src/lerobot/teleoperators/piper_leader/piper_leader.py index ceba231..c659028 100644 --- a/src/lerobot/teleoperators/piper_leader/piper_leader.py +++ b/src/lerobot/teleoperators/piper_leader/piper_leader.py @@ -24,6 +24,7 @@ from lerobot.processor import RobotAction from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected from lerobot.utils.piper_sdk import ( PIPER_ACTION_KEYS, + PIPER_JOINT_ACTION_KEYS, PIPER_JOINT_NAMES, get_piper_sdk, milli_to_unit, @@ -80,6 +81,8 @@ class PiperLeader(Teleoperator): time.sleep(self.config.startup_sleep_s) self._is_connected = True + # Recompute control mode on every fresh connection. + self._manual_control_enabled = None try: if self.config.set_leader_mode_on_connect: # NOTE: @@ -216,17 +219,20 @@ class PiperLeader(Teleoperator): return abs(milli_to_unit(getattr(gripper_state, "grippers_angle", 0))) def _read_raw_action(self) -> RobotAction: + used_feedback_for_joints = False action: dict[str, float] | None = None if self.config.prefer_ctrl_messages: action = self._read_joint_from_ctrl() if action is None and self.config.fallback_to_feedback: action = self._read_joint_from_feedback() + used_feedback_for_joints = action is not None if action is None: action = {f"{joint_name}.pos": 0.0 for joint_name in PIPER_JOINT_NAMES} - gripper_pos = self._read_gripper_from_ctrl() if self.config.prefer_ctrl_messages else None + use_ctrl_for_gripper = self.config.prefer_ctrl_messages and not used_feedback_for_joints + gripper_pos = self._read_gripper_from_ctrl() if use_ctrl_for_gripper else None if gripper_pos is None and self.config.fallback_to_feedback: gripper_pos = self._read_gripper_from_feedback() action["gripper.pos"] = 0.0 if gripper_pos is None else gripper_pos @@ -302,7 +308,7 @@ class PiperLeader(Teleoperator): self.set_manual_control(False) self._refresh_command_mode_if_needed() - joint_keys = [f"{joint_name}.pos" for joint_name in PIPER_JOINT_NAMES] + joint_keys = PIPER_JOINT_ACTION_KEYS has_all_joints = all(key in feedback for key in joint_keys) if has_all_joints: joint_targets = [self._offset_to_calibrated(key, feedback[key]) for key in joint_keys] @@ -327,4 +333,5 @@ class PiperLeader(Teleoperator): finally: self.arm.DisconnectPort() self._is_connected = False + self._manual_control_enabled = None logger.info("%s disconnected.", self) diff --git a/src/lerobot/utils/piper_sdk.py b/src/lerobot/utils/piper_sdk.py index 2a71839..e2f88b2 100644 --- a/src/lerobot/utils/piper_sdk.py +++ b/src/lerobot/utils/piper_sdk.py @@ -27,7 +27,8 @@ PIPER_JOINT_NAMES = ( "joint_5", "joint_6", ) -PIPER_ACTION_KEYS = tuple(f"{joint}.pos" for joint in PIPER_JOINT_NAMES) + ("gripper.pos",) +PIPER_JOINT_ACTION_KEYS = tuple(f"{joint}.pos" for joint in PIPER_JOINT_NAMES) +PIPER_ACTION_KEYS = PIPER_JOINT_ACTION_KEYS + ("gripper.pos",) def milli_to_unit(value: float | int) -> float: diff --git a/tests/test_piper_teleop.py b/tests/test_piper_teleop.py index 9fc8a11..ebd03df 100644 --- a/tests/test_piper_teleop.py +++ b/tests/test_piper_teleop.py @@ -1,5 +1,7 @@ from types import SimpleNamespace +import pytest + import lerobot.utils.piper_sdk as piper_sdk_utils import lerobot.robots.piper_follower.piper_follower as piper_follower_module import lerobot.teleoperators.piper_leader.piper_leader as piper_leader_module @@ -8,6 +10,7 @@ from lerobot.robots.piper_follower import PiperFollower, PiperFollowerConfig from lerobot.robots.utils import make_robot_from_config from lerobot.teleoperators.piper_leader import PiperLeader, PiperLeaderConfig from lerobot.teleoperators.utils import make_teleoperator_from_config +from lerobot.utils.piper_sdk import PIPER_ACTION_KEYS class FakeLogLevel: @@ -124,7 +127,7 @@ def make_identity_calibration(): range_min=-200000, range_max=200000, ) - for idx, key in enumerate([f"joint_{i}.pos" for i in range(1, 7)] + ["gripper.pos"]) + for idx, key in enumerate(PIPER_ACTION_KEYS) } @@ -210,3 +213,58 @@ def test_piper_requires_calibration(monkeypatch): finally: teleop.disconnect() robot.disconnect() + + +def test_piper_leader_reconnect_reapplies_mode(monkeypatch): + patch_fake_sdk(monkeypatch) + + teleop = PiperLeader(PiperLeaderConfig(port="can1", manual_control=False)) + teleop.calibration = make_identity_calibration() + + teleop.connect(calibrate=False) + teleop.disconnect() + teleop.connect(calibrate=False) + try: + assert teleop.arm.enable_calls == 2 + finally: + teleop.disconnect() + + +def test_piper_follower_connect_rolls_back_connected_cameras(monkeypatch): + patch_fake_sdk(monkeypatch) + + class FakeCamera: + def __init__(self, should_fail: bool): + self.should_fail = should_fail + self.is_connected = False + self.disconnect_calls = 0 + + def connect(self): + if self.should_fail: + raise RuntimeError("camera connect failed") + self.is_connected = True + + def disconnect(self): + self.disconnect_calls += 1 + self.is_connected = False + + def async_read(self): + return None + + cam_ok = FakeCamera(should_fail=False) + cam_fail = FakeCamera(should_fail=True) + monkeypatch.setattr( + piper_follower_module, + "make_cameras_from_configs", + lambda _: {"cam_ok": cam_ok, "cam_fail": cam_fail}, + ) + + robot = PiperFollower(PiperFollowerConfig(port="can0")) + robot.calibration = make_identity_calibration() + + with pytest.raises(RuntimeError, match="camera connect failed"): + robot.connect(calibrate=False) + + assert cam_ok.disconnect_calls == 1 + assert not cam_ok.is_connected + assert not robot.is_connected From 294a3706573a7d0769319fba3655d9f4360b1ce9 Mon Sep 17 00:00:00 2001 From: Elvin <223015152@link.cuhk.edu.cn> Date: Thu, 5 Mar 2026 21:49:39 +0800 Subject: [PATCH 08/26] feat(piper): require calibration by default with opt-out mode - set piper calibration_mode default to required and keep off as explicit opt-out - remove auto pairing policy and simplify runtime calibration gating - update README and add test for uncalibrated control when calibration_mode=off --- README.md | 6 ++++- .../piper_follower/config_piper_follower.py | 7 +++++ .../robots/piper_follower/piper_follower.py | 18 ++++++++++--- .../piper_leader/config_piper_leader.py | 8 ++++++ .../piper_leader/piper_leader.py | 26 +++++++++++++++---- src/lerobot/utils/piper_sdk.py | 12 +++++++++ tests/test_piper_teleop.py | 18 +++++++++++++ 7 files changed, 85 insertions(+), 10 deletions(-) diff --git a/README.md b/README.md index 66c137f..8007ddf 100644 --- a/README.md +++ b/README.md @@ -207,7 +207,11 @@ So first run `lerobot-setup-can` to confirm CAN interfaces are available: lerobot-setup-can --mode=setup --interfaces=can0,can1 ``` -PiPER does not use `lerobot-calibrate`, and if you switch master/follower roles with the flags below, you must power-cycle both arms before enabling. +PiPER calibration is required by default. +If needed, you can disable it with +`--robot.calibration_mode=off` and `--teleop.calibration_mode=off`. + +If you switch master/follower roles with the flags below, you must power-cycle both arms before enabling. ```bash --teleop.set_leader_mode_on_connect=true diff --git a/src/lerobot/robots/piper_follower/config_piper_follower.py b/src/lerobot/robots/piper_follower/config_piper_follower.py index c2fdb38..56015ab 100644 --- a/src/lerobot/robots/piper_follower/config_piper_follower.py +++ b/src/lerobot/robots/piper_follower/config_piper_follower.py @@ -17,6 +17,7 @@ from dataclasses import dataclass, field from lerobot.cameras import CameraConfig +from lerobot.utils.piper_sdk import PIPER_CALIBRATION_MODES from ..config import RobotConfig @@ -49,6 +50,10 @@ class PiperFollowerConfigBase: # Calibration precision: # homing_offset/range_min/range_max are stored as "degree * calibration_scale". calibration_scale: int = 1000 + # Calibration policy: + # - required: always require calibration + # - off: never require calibration + calibration_mode: str = "required" # Gripper forwarding behavior sync_gripper: bool = True @@ -75,6 +80,8 @@ class PiperFollowerConfig(RobotConfig, PiperFollowerConfigBase): raise ValueError("`enable_timeout_s` must be >= 0.") if self.calibration_scale <= 0: raise ValueError("`calibration_scale` must be > 0.") + if self.calibration_mode not in PIPER_CALIBRATION_MODES: + raise ValueError("`calibration_mode` must be one of: required, off.") if self.startup_sleep_s < 0: raise ValueError("`startup_sleep_s` must be >= 0.") if not (0 <= self.gripper_effort_default <= 5000): diff --git a/src/lerobot/robots/piper_follower/piper_follower.py b/src/lerobot/robots/piper_follower/piper_follower.py index 672d91f..3daee31 100644 --- a/src/lerobot/robots/piper_follower/piper_follower.py +++ b/src/lerobot/robots/piper_follower/piper_follower.py @@ -30,6 +30,7 @@ from lerobot.utils.piper_sdk import ( get_piper_sdk, milli_to_unit, parse_piper_log_level, + should_require_piper_calibration, unit_to_milli, ) from lerobot.utils.utils import enter_pressed, move_cursor_up @@ -100,7 +101,7 @@ class PiperFollower(Robot): should_enable = self.config.enable_on_connect and calibrate if should_enable and not self._wait_enable(self.config.enable_timeout_s): logger.warning("Piper follower did not report enabled state before timeout.") - if not self.is_calibrated and calibrate: + if not self.is_calibrated and calibrate and should_require_piper_calibration(self.config.calibration_mode): logger.info("No piper-follower calibration file found for '%s'. Running lerobot-calibrate flow.", self.id) self.calibrate() @@ -116,6 +117,9 @@ class PiperFollower(Robot): logger.info("%s connected.", self) + def _use_uncalibrated_passthrough(self) -> bool: + return not self.is_calibrated and not should_require_piper_calibration(self.config.calibration_mode) + @property def is_calibrated(self) -> bool: if not all(key in self.calibration for key in PIPER_CALIB_KEYS): @@ -247,7 +251,7 @@ class PiperFollower(Robot): @check_if_not_connected def send_action(self, action: RobotAction) -> RobotAction: - if not self.is_calibrated: + if not self.is_calibrated and not self._use_uncalibrated_passthrough(): raise RuntimeError( f"{self} is not calibrated. Run `lerobot-calibrate --robot.type=piper_follower --robot.id={self.id}` first." ) @@ -259,7 +263,10 @@ class PiperFollower(Robot): joint_keys = PIPER_JOINT_ACTION_KEYS has_all_joints = all(key in action for key in joint_keys) if has_all_joints: - joint_targets = [self._offset_to_target(key, action[key]) for key in joint_keys] + if self._use_uncalibrated_passthrough(): + joint_targets = [action[key] for key in joint_keys] + else: + joint_targets = [self._offset_to_target(key, action[key]) for key in joint_keys] joint_commands = [unit_to_milli(value) for value in joint_targets] self.arm.JointCtrl(*joint_commands) sent_action.update({key: milli_to_unit(raw) for key, raw in zip(joint_keys, joint_commands, strict=True)}) @@ -267,7 +274,10 @@ class PiperFollower(Robot): logger.debug("Ignoring partial Piper joint action. Need all six joint keys to send command.") if self.config.sync_gripper and "gripper.pos" in action: - gripper_target = self._offset_to_target("gripper.pos", action["gripper.pos"]) + if self._use_uncalibrated_passthrough(): + gripper_target = action["gripper.pos"] + else: + gripper_target = self._offset_to_target("gripper.pos", action["gripper.pos"]) gripper_pos_raw = unit_to_milli(gripper_target) self.arm.GripperCtrl( gripper_pos_raw, diff --git a/src/lerobot/teleoperators/piper_leader/config_piper_leader.py b/src/lerobot/teleoperators/piper_leader/config_piper_leader.py index 4aa359b..c3af356 100644 --- a/src/lerobot/teleoperators/piper_leader/config_piper_leader.py +++ b/src/lerobot/teleoperators/piper_leader/config_piper_leader.py @@ -16,6 +16,8 @@ from dataclasses import dataclass +from lerobot.utils.piper_sdk import PIPER_CALIBRATION_MODES + from ..config import TeleoperatorConfig @@ -56,6 +58,10 @@ class PiperLeaderConfigBase: # Calibration precision: # homing_offset/range_min/range_max are stored as "degree * calibration_scale". calibration_scale: int = 1000 + # Calibration policy: + # - required: always require calibration + # - off: never require calibration + calibration_mode: str = "required" # Safety behavior on disconnect disable_on_disconnect: bool = False @@ -73,6 +79,8 @@ class PiperLeaderConfig(TeleoperatorConfig, PiperLeaderConfigBase): raise ValueError("`enable_timeout_s` must be >= 0.") if self.calibration_scale <= 0: raise ValueError("`calibration_scale` must be > 0.") + if self.calibration_mode not in PIPER_CALIBRATION_MODES: + raise ValueError("`calibration_mode` must be one of: required, off.") if self.startup_sleep_s < 0: raise ValueError("`startup_sleep_s` must be >= 0.") if not (0 <= self.gripper_effort_default <= 5000): diff --git a/src/lerobot/teleoperators/piper_leader/piper_leader.py b/src/lerobot/teleoperators/piper_leader/piper_leader.py index c659028..2b74344 100644 --- a/src/lerobot/teleoperators/piper_leader/piper_leader.py +++ b/src/lerobot/teleoperators/piper_leader/piper_leader.py @@ -29,6 +29,7 @@ from lerobot.utils.piper_sdk import ( get_piper_sdk, milli_to_unit, parse_piper_log_level, + should_require_piper_calibration, unit_to_milli, ) from lerobot.utils.utils import enter_pressed, move_cursor_up @@ -93,7 +94,7 @@ class PiperLeader(Teleoperator): self.arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00) time.sleep(0.05) self.configure() - if not self.is_calibrated and calibrate: + if not self.is_calibrated and calibrate and should_require_piper_calibration(self.config.calibration_mode): logger.info("No piper-leader calibration file found for '%s'. Running lerobot-calibrate flow.", self.id) self.calibrate() except Exception: @@ -103,6 +104,9 @@ class PiperLeader(Teleoperator): logger.info("%s connected.", self) + def _use_uncalibrated_passthrough(self) -> bool: + return not self.is_calibrated and not should_require_piper_calibration(self.config.calibration_mode) + @property def is_calibrated(self) -> bool: if not all(key in self.calibration for key in PIPER_CALIB_KEYS): @@ -285,12 +289,18 @@ class PiperLeader(Teleoperator): @check_if_not_connected def get_action(self) -> RobotAction: - if not self.is_calibrated: + if not self.is_calibrated and not self._use_uncalibrated_passthrough(): raise RuntimeError( f"{self} is not calibrated. Run `lerobot-calibrate --teleop.type=piper_leader --teleop.id={self.id}` first." ) raw_action = self._read_raw_action() + if self._use_uncalibrated_passthrough(): + action: RobotAction = dict(raw_action) + if not self.config.sync_gripper: + action["gripper.pos"] = 0.0 + return action + action: RobotAction = { key: self._calibrated_to_offset(key, raw_action[key]) for key in PIPER_CALIB_KEYS } @@ -300,7 +310,7 @@ class PiperLeader(Teleoperator): @check_if_not_connected def send_feedback(self, feedback: dict[str, Any]) -> None: - if not self.is_calibrated: + if not self.is_calibrated and not self._use_uncalibrated_passthrough(): raise RuntimeError( f"{self} is not calibrated. Run `lerobot-calibrate --teleop.type=piper_leader --teleop.id={self.id}` first." ) @@ -311,12 +321,18 @@ class PiperLeader(Teleoperator): joint_keys = PIPER_JOINT_ACTION_KEYS has_all_joints = all(key in feedback for key in joint_keys) if has_all_joints: - joint_targets = [self._offset_to_calibrated(key, feedback[key]) for key in joint_keys] + if self._use_uncalibrated_passthrough(): + joint_targets = [feedback[key] for key in joint_keys] + else: + joint_targets = [self._offset_to_calibrated(key, feedback[key]) for key in joint_keys] joint_commands = [unit_to_milli(value) for value in joint_targets] self.arm.JointCtrl(*joint_commands) if self.config.sync_gripper and "gripper.pos" in feedback: - gripper_target = self._offset_to_calibrated("gripper.pos", feedback["gripper.pos"]) + if self._use_uncalibrated_passthrough(): + gripper_target = feedback["gripper.pos"] + else: + gripper_target = self._offset_to_calibrated("gripper.pos", feedback["gripper.pos"]) gripper_pos_raw = unit_to_milli(gripper_target) self.arm.GripperCtrl( gripper_pos_raw, diff --git a/src/lerobot/utils/piper_sdk.py b/src/lerobot/utils/piper_sdk.py index e2f88b2..0699d84 100644 --- a/src/lerobot/utils/piper_sdk.py +++ b/src/lerobot/utils/piper_sdk.py @@ -29,6 +29,7 @@ PIPER_JOINT_NAMES = ( ) PIPER_JOINT_ACTION_KEYS = tuple(f"{joint}.pos" for joint in PIPER_JOINT_NAMES) PIPER_ACTION_KEYS = PIPER_JOINT_ACTION_KEYS + ("gripper.pos",) +PIPER_CALIBRATION_MODES = {"required", "off"} def milli_to_unit(value: float | int) -> float: @@ -61,3 +62,14 @@ def parse_piper_log_level(level_name: str) -> Any: f"Invalid Piper log level '{level_name}'. " "Expected one of: DEBUG, INFO, WARNING, ERROR, CRITICAL, SILENT." ) from exc + + +def should_require_piper_calibration(calibration_mode: str) -> bool: + if calibration_mode == "required": + return True + if calibration_mode == "off": + return False + raise ValueError( + f"Invalid Piper calibration mode '{calibration_mode}'. " + "Expected one of: required, off." + ) diff --git a/tests/test_piper_teleop.py b/tests/test_piper_teleop.py index ebd03df..e8e1906 100644 --- a/tests/test_piper_teleop.py +++ b/tests/test_piper_teleop.py @@ -268,3 +268,21 @@ def test_piper_follower_connect_rolls_back_connected_cameras(monkeypatch): assert cam_ok.disconnect_calls == 1 assert not cam_ok.is_connected assert not robot.is_connected + + +def test_piper_calibration_mode_off_allows_uncalibrated_control(monkeypatch): + patch_fake_sdk(monkeypatch) + + teleop = PiperLeader(PiperLeaderConfig(port="can1", calibration_mode="off")) + robot = PiperFollower(PiperFollowerConfig(port="can0", calibration_mode="off")) + + teleop.connect(calibrate=False) + robot.connect(calibrate=False) + try: + action = teleop.get_action() + sent = robot.send_action(action) + assert sent["joint_1.pos"] == 10.0 + assert sent["gripper.pos"] == 42.0 + finally: + teleop.disconnect() + robot.disconnect() From 271518d52feb8b701843c1299e60dd096e97b12f Mon Sep 17 00:00:00 2001 From: Elvin <223015152@link.cuhk.edu.cn> Date: Fri, 6 Mar 2026 00:52:37 +0800 Subject: [PATCH 09/26] fix(piper): enable follower after connect-time calibration --- .../robots/piper_follower/piper_follower.py | 11 ++++--- tests/test_piper_teleop.py | 32 +++++++++++++++++++ 2 files changed, 39 insertions(+), 4 deletions(-) diff --git a/src/lerobot/robots/piper_follower/piper_follower.py b/src/lerobot/robots/piper_follower/piper_follower.py index 3daee31..c6fc14d 100644 --- a/src/lerobot/robots/piper_follower/piper_follower.py +++ b/src/lerobot/robots/piper_follower/piper_follower.py @@ -96,14 +96,17 @@ class PiperFollower(Robot): time.sleep(0.05) self.configure() + should_auto_calibrate = ( + not self.is_calibrated and calibrate and should_require_piper_calibration(self.config.calibration_mode) + ) + if should_auto_calibrate: + logger.info("No piper-follower calibration file found for '%s'. Running lerobot-calibrate flow.", self.id) + self.calibrate() # lerobot-calibrate calls connect(calibrate=False) before running calibrate(). - # Keep the arm in drag mode for calibration by skipping auto-enable in this path. + # Keep the arm in drag mode for that path, but re-enable after connect-time calibration. should_enable = self.config.enable_on_connect and calibrate if should_enable and not self._wait_enable(self.config.enable_timeout_s): logger.warning("Piper follower did not report enabled state before timeout.") - if not self.is_calibrated and calibrate and should_require_piper_calibration(self.config.calibration_mode): - logger.info("No piper-follower calibration file found for '%s'. Running lerobot-calibrate flow.", self.id) - self.calibrate() for cam in self.cameras.values(): cam.connect() diff --git a/tests/test_piper_teleop.py b/tests/test_piper_teleop.py index e8e1906..9f0e646 100644 --- a/tests/test_piper_teleop.py +++ b/tests/test_piper_teleop.py @@ -35,6 +35,7 @@ class FakePiperInterface: self.last_gripper = None self.enable_calls = 0 self.disable_calls = 0 + self.is_enabled = False self._joint_ctrl = SimpleNamespace( time_stamp=1.0, @@ -86,11 +87,13 @@ class FakePiperInterface: def EnablePiper(self): self.enable_calls += 1 + self.is_enabled = True return True def DisableArm(self, motor_num): del motor_num self.disable_calls += 1 + self.is_enabled = False def JointCtrl(self, *args): self.last_joint = args @@ -286,3 +289,32 @@ def test_piper_calibration_mode_off_allows_uncalibrated_control(monkeypatch): finally: teleop.disconnect() robot.disconnect() + + +def test_piper_follower_connect_calibrates_then_reenables(monkeypatch, tmp_path): + patch_fake_sdk(monkeypatch) + + robot = PiperFollower( + PiperFollowerConfig( + port="can0", + id="connect_reenable_after_calibration", + calibration_dir=tmp_path, + calibration_mode="required", + enable_on_connect=True, + ) + ) + + def fake_calibrate(): + # Mirror real calibration's drag-mode behavior. + robot.arm.DisableArm(7) + robot.calibration = make_identity_calibration() + + monkeypatch.setattr(robot, "calibrate", fake_calibrate) + + robot.connect(calibrate=True) + try: + assert robot.arm.disable_calls == 1 + assert robot.arm.enable_calls == 1 + assert robot.arm.is_enabled + finally: + robot.disconnect() From ae8efef693d8898e5a0f2a465d962c6a1b090319 Mon Sep 17 00:00:00 2001 From: Elvin <223015152@link.cuhk.edu.cn> Date: Fri, 6 Mar 2026 01:35:05 +0800 Subject: [PATCH 10/26] fix(piper): decouple connect enable from calibrate flag --- .../robots/piper_follower/piper_follower.py | 6 ++--- tests/test_piper_teleop.py | 22 +++++++++++++++++++ 2 files changed, 25 insertions(+), 3 deletions(-) diff --git a/src/lerobot/robots/piper_follower/piper_follower.py b/src/lerobot/robots/piper_follower/piper_follower.py index c6fc14d..1e96d66 100644 --- a/src/lerobot/robots/piper_follower/piper_follower.py +++ b/src/lerobot/robots/piper_follower/piper_follower.py @@ -102,9 +102,9 @@ class PiperFollower(Robot): if should_auto_calibrate: logger.info("No piper-follower calibration file found for '%s'. Running lerobot-calibrate flow.", self.id) self.calibrate() - # lerobot-calibrate calls connect(calibrate=False) before running calibrate(). - # Keep the arm in drag mode for that path, but re-enable after connect-time calibration. - should_enable = self.config.enable_on_connect and calibrate + # Enable behavior should be controlled by enable_on_connect, independent from calibrate flag. + # This keeps connect(calibrate=False) commandable for callers that only want to skip interactive calibration. + should_enable = self.config.enable_on_connect if should_enable and not self._wait_enable(self.config.enable_timeout_s): logger.warning("Piper follower did not report enabled state before timeout.") diff --git a/tests/test_piper_teleop.py b/tests/test_piper_teleop.py index 9f0e646..f1a3268 100644 --- a/tests/test_piper_teleop.py +++ b/tests/test_piper_teleop.py @@ -318,3 +318,25 @@ def test_piper_follower_connect_calibrates_then_reenables(monkeypatch, tmp_path) assert robot.arm.is_enabled finally: robot.disconnect() + + +def test_piper_follower_connect_without_calibration_still_enables(monkeypatch, tmp_path): + patch_fake_sdk(monkeypatch) + + robot = PiperFollower( + PiperFollowerConfig( + port="can0", + id="connect_enable_without_calibration", + calibration_dir=tmp_path, + calibration_mode="required", + enable_on_connect=True, + ) + ) + + robot.connect(calibrate=False) + try: + assert robot.arm.enable_calls == 1 + assert robot.arm.is_enabled + assert robot.arm.disable_calls == 0 + finally: + robot.disconnect() From fa435f8312f106e3b0db5cc867869bb1121f82dd Mon Sep 17 00:00:00 2001 From: Elvin <223015152@link.cuhk.edu.cn> Date: Fri, 6 Mar 2026 09:48:31 +0800 Subject: [PATCH 11/26] fix(ci): resolve quality workflow failures --- .github/workflows/deploy-website-pages.yml | 2 +- README.md | 19 ++++++++++++++----- .../robots/piper_follower/piper_follower.py | 18 ++++++++++++------ .../piper_leader/piper_leader.py | 15 +++++++++++---- src/lerobot/utils/piper_sdk.py | 5 +---- tests/test_piper_teleop.py | 18 +++++++----------- 6 files changed, 46 insertions(+), 31 deletions(-) diff --git a/.github/workflows/deploy-website-pages.yml b/.github/workflows/deploy-website-pages.yml index fcc5e79..07922c4 100644 --- a/.github/workflows/deploy-website-pages.yml +++ b/.github/workflows/deploy-website-pages.yml @@ -15,7 +15,7 @@ concurrency: jobs: deploy: # Temporarily disabled to keep the website non-public. - if: ${{ false }} + if: false runs-on: ubuntu-latest environment: name: github-pages diff --git a/README.md b/README.md index ecff772..a5714f5 100644 --- a/README.md +++ b/README.md @@ -31,11 +31,11 @@ ## 🧭 Table of Contents -| Getting Started | Training Pipeline | Project Info | -| --- | --- | --- | -| [⚡ Quick Start](#quick-start) | [4) Value Function Training](#value-function-training) | [Model & Dataset](#model--dataset) | -| [1) Installation](#installation) | [5) Value Inference](#value-inference) | [Community Channels](#community-channels) | -| [2) Hardware Setup](#hardware-setup) | [6) Policy Training](#policy-training) | [Affiliations](#affiliations) | +| Getting Started | Training Pipeline | Project Info | +| -------------------------------------- | ---------------------------------------------------------------------------- | ------------------------------------------- | +| [⚡ Quick Start](#quick-start) | [4) Value Function Training](#value-function-training) | [Model & Dataset](#model--dataset) | +| [1) Installation](#installation) | [5) Value Inference](#value-inference) | [Community Channels](#community-channels) | +| [2) Hardware Setup](#hardware-setup) | [6) Policy Training](#policy-training) | [Affiliations](#affiliations) | | [3) Data Collection](#data-collection) | [7) Closed-loop Rollout and Next Round](#closed-loop-rollout-and-next-round) | [Citation](#citation) / [License](#license) |

Value Visual Results

@@ -67,11 +67,13 @@

+ ## ⚡ Quick Start **LeRobot-aligned foundation:** we use LeRobot as the base of this codebase because its inference and data-collection logic are highly aligned with real-world RL workflows. + ### 1) Installation ```bash @@ -85,6 +87,7 @@ pip install -e . For setup details and platform-specific dependencies, follow the official [LeRobot configuration guide](https://huggingface.co/docs/lerobot/installation). + ### 2) Hardware Setup #### SO Series (SO100/SO101) @@ -198,6 +201,7 @@ For dual-arm camera mapping, it is fine to attach `front` under either the left- If needed, you can also use temporary device paths (for example `/dev/ttyACM*` and `/dev/video*`) during initial debugging. + #### AgileX PiPER For PiPER setup, PiPER uses CAN interfaces instead of serial ports. @@ -239,6 +243,7 @@ Optional PiPER flags: ``` + ### 3) Data Collection Collect rollout data with `lerobot-human-inloop-record`. @@ -314,6 +319,7 @@ lerobot-dataset-report --dataset / This prints: dataset meta, totals, episode-length stats/histogram, success/intervention metrics, task list, and full feature schema. + ### 4) Value Function Training Train the value function on the current dataset. Current default: [Pi\*0.6](https://www.pi.website/blog/pistar06) (`--value.type=pistar06`). @@ -353,6 +359,7 @@ To plug in a different value function, minimal path in this repo: - Remove/replace the current `pistar06`-only type checks in `src/lerobot/configs/value_train.py` and `src/lerobot/scripts/lerobot_value_infer.py`. + ### 5) Value Inference Infer value signals and write value/advantage/indicator back to the dataset: @@ -408,6 +415,7 @@ complementary_info.acp_indicator_ These columns are written back to the original dataset specified by `--dataset.repo_id`. + ### 6) Policy Training Train the policy with advantage-conditioned tags. @@ -453,6 +461,7 @@ CUDA_VISIBLE_DEVICES= accelerate launch \ ``` + ### 7) Closed-loop Rollout and Next Round Deploy the trained policy in human-in-loop mode and collect the next dataset round: diff --git a/src/lerobot/robots/piper_follower/piper_follower.py b/src/lerobot/robots/piper_follower/piper_follower.py index 1e96d66..f202742 100644 --- a/src/lerobot/robots/piper_follower/piper_follower.py +++ b/src/lerobot/robots/piper_follower/piper_follower.py @@ -17,7 +17,6 @@ import logging import time from functools import cached_property -from typing import Any from lerobot.cameras.utils import make_cameras_from_configs from lerobot.motors import MotorCalibration @@ -72,11 +71,11 @@ class PiperFollower(Robot): @cached_property def observation_features(self) -> dict[str, type | tuple]: - return {**{key: float for key in PIPER_ACTION_KEYS}, **self._cameras_ft} + return {**dict.fromkeys(PIPER_ACTION_KEYS, float), **self._cameras_ft} @cached_property def action_features(self) -> dict[str, type]: - return {key: float for key in PIPER_ACTION_KEYS} + return dict.fromkeys(PIPER_ACTION_KEYS, float) @property def is_connected(self) -> bool: @@ -97,10 +96,15 @@ class PiperFollower(Robot): self.configure() should_auto_calibrate = ( - not self.is_calibrated and calibrate and should_require_piper_calibration(self.config.calibration_mode) + not self.is_calibrated + and calibrate + and should_require_piper_calibration(self.config.calibration_mode) ) if should_auto_calibrate: - logger.info("No piper-follower calibration file found for '%s'. Running lerobot-calibrate flow.", self.id) + logger.info( + "No piper-follower calibration file found for '%s'. Running lerobot-calibrate flow.", + self.id, + ) self.calibrate() # Enable behavior should be controlled by enable_on_connect, independent from calibrate flag. # This keeps connect(calibrate=False) commandable for callers that only want to skip interactive calibration. @@ -272,7 +276,9 @@ class PiperFollower(Robot): joint_targets = [self._offset_to_target(key, action[key]) for key in joint_keys] joint_commands = [unit_to_milli(value) for value in joint_targets] self.arm.JointCtrl(*joint_commands) - sent_action.update({key: milli_to_unit(raw) for key, raw in zip(joint_keys, joint_commands, strict=True)}) + sent_action.update( + {key: milli_to_unit(raw) for key, raw in zip(joint_keys, joint_commands, strict=True)} + ) elif any(key in action for key in joint_keys): logger.debug("Ignoring partial Piper joint action. Need all six joint keys to send command.") diff --git a/src/lerobot/teleoperators/piper_leader/piper_leader.py b/src/lerobot/teleoperators/piper_leader/piper_leader.py index 2b74344..82135f7 100644 --- a/src/lerobot/teleoperators/piper_leader/piper_leader.py +++ b/src/lerobot/teleoperators/piper_leader/piper_leader.py @@ -65,11 +65,11 @@ class PiperLeader(Teleoperator): @cached_property def action_features(self) -> dict[str, type]: - return {key: float for key in PIPER_ACTION_KEYS} + return dict.fromkeys(PIPER_ACTION_KEYS, float) @cached_property def feedback_features(self) -> dict[str, type]: - return {key: float for key in PIPER_ACTION_KEYS} + return dict.fromkeys(PIPER_ACTION_KEYS, float) @property def is_connected(self) -> bool: @@ -94,8 +94,15 @@ class PiperLeader(Teleoperator): self.arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00) time.sleep(0.05) self.configure() - if not self.is_calibrated and calibrate and should_require_piper_calibration(self.config.calibration_mode): - logger.info("No piper-leader calibration file found for '%s'. Running lerobot-calibrate flow.", self.id) + if ( + not self.is_calibrated + and calibrate + and should_require_piper_calibration(self.config.calibration_mode) + ): + logger.info( + "No piper-leader calibration file found for '%s'. Running lerobot-calibrate flow.", + self.id, + ) self.calibrate() except Exception: self.arm.DisconnectPort() diff --git a/src/lerobot/utils/piper_sdk.py b/src/lerobot/utils/piper_sdk.py index 0699d84..967c018 100644 --- a/src/lerobot/utils/piper_sdk.py +++ b/src/lerobot/utils/piper_sdk.py @@ -69,7 +69,4 @@ def should_require_piper_calibration(calibration_mode: str) -> bool: return True if calibration_mode == "off": return False - raise ValueError( - f"Invalid Piper calibration mode '{calibration_mode}'. " - "Expected one of: required, off." - ) + raise ValueError(f"Invalid Piper calibration mode '{calibration_mode}'. Expected one of: required, off.") diff --git a/tests/test_piper_teleop.py b/tests/test_piper_teleop.py index f1a3268..0c1d95f 100644 --- a/tests/test_piper_teleop.py +++ b/tests/test_piper_teleop.py @@ -1,10 +1,11 @@ +# ruff: noqa: N802 from types import SimpleNamespace import pytest -import lerobot.utils.piper_sdk as piper_sdk_utils import lerobot.robots.piper_follower.piper_follower as piper_follower_module import lerobot.teleoperators.piper_leader.piper_leader as piper_leader_module +import lerobot.utils.piper_sdk as piper_sdk_utils from lerobot.motors import MotorCalibration from lerobot.robots.piper_follower import PiperFollower, PiperFollowerConfig from lerobot.robots.utils import make_robot_from_config @@ -115,7 +116,9 @@ class FakePiperInterface: def patch_fake_sdk(monkeypatch): - fake_loader = lambda: (FakePiperInterface, FakeLogLevel) + def fake_loader(): + return (FakePiperInterface, FakeLogLevel) + monkeypatch.setattr(piper_sdk_utils, "get_piper_sdk", fake_loader) monkeypatch.setattr(piper_follower_module, "get_piper_sdk", fake_loader) monkeypatch.setattr(piper_leader_module, "get_piper_sdk", fake_loader) @@ -202,17 +205,10 @@ def test_piper_requires_calibration(monkeypatch): teleop.connect(calibrate=False) robot.connect(calibrate=False) try: - try: + with pytest.raises(RuntimeError, match="require calibration"): teleop.get_action() - assert False, "Expected teleop.get_action() to require calibration." - except RuntimeError: - pass - - try: + with pytest.raises(RuntimeError, match="require calibration"): robot.send_action({"joint_1.pos": 0.0}) - assert False, "Expected robot.send_action() to require calibration." - except RuntimeError: - pass finally: teleop.disconnect() robot.disconnect() From 5975798aef07d140becf96947f16919e58f6ddd6 Mon Sep 17 00:00:00 2001 From: Elvin <223015152@link.cuhk.edu.cn> Date: Fri, 6 Mar 2026 16:31:02 +0800 Subject: [PATCH 12/26] test(fast): align fast-test behavior with current runtime constraints - gate PI0/PI0Fast tests on HF token availability for gated models - update value infer viz tests to current API ownership and arguments - align piper calibration assertion with runtime error message - set HF_HUB_DISABLE_XET in fast_tests workflow --- .github/workflows/fast_tests.yml | 2 ++ .../pi0_fast/test_pi0_fast_original_vs_lerobot.py | 6 ++++-- tests/policies/pi0_pi05/test_pi0_rtc.py | 6 ++++-- tests/test_piper_teleop.py | 4 ++-- tests/value/test_lerobot_value_infer_viz.py | 14 ++++++++++---- 5 files changed, 22 insertions(+), 10 deletions(-) diff --git a/.github/workflows/fast_tests.yml b/.github/workflows/fast_tests.yml index 10ec911..e6eb9f5 100644 --- a/.github/workflows/fast_tests.yml +++ b/.github/workflows/fast_tests.yml @@ -61,6 +61,8 @@ jobs: MUJOCO_GL: egl HF_HOME: /mnt/cache/.cache/huggingface HF_LEROBOT_HOME: /mnt/cache/.cache/huggingface/lerobot + # Keep dataset/model downloads on the regular Hub path in CI to avoid hf_xet flakiness. + HF_HUB_DISABLE_XET: "1" steps: - uses: actions/checkout@v6 with: diff --git a/tests/policies/pi0_fast/test_pi0_fast_original_vs_lerobot.py b/tests/policies/pi0_fast/test_pi0_fast_original_vs_lerobot.py index 9ebc4ba..c485e2b 100644 --- a/tests/policies/pi0_fast/test_pi0_fast_original_vs_lerobot.py +++ b/tests/policies/pi0_fast/test_pi0_fast_original_vs_lerobot.py @@ -29,8 +29,10 @@ import torch pytest.importorskip("transformers") pytest.importorskip("scipy") pytestmark = pytest.mark.skipif( - os.environ.get("CI") == "true" or os.environ.get("GITHUB_ACTIONS") == "true", - reason="This test requires accepting the model license", + os.environ.get("CI") == "true" + or os.environ.get("GITHUB_ACTIONS") == "true" + or not (os.environ.get("HF_TOKEN") or os.environ.get("HUGGING_FACE_HUB_TOKEN")), + reason="This test requires accepted model license and authenticated HF access token", ) from lerobot.policies.pi0_fast.configuration_pi0_fast import PI0FastConfig diff --git a/tests/policies/pi0_pi05/test_pi0_rtc.py b/tests/policies/pi0_pi05/test_pi0_rtc.py index 68e94dd..9f2ee16 100644 --- a/tests/policies/pi0_pi05/test_pi0_rtc.py +++ b/tests/policies/pi0_pi05/test_pi0_rtc.py @@ -23,8 +23,10 @@ import torch # Skip this entire module in CI pytestmark = pytest.mark.skipif( - os.environ.get("CI") == "true" or os.environ.get("GITHUB_ACTIONS") == "true", - reason="This test requires local OpenPI installation and is not meant for CI", + os.environ.get("CI") == "true" + or os.environ.get("GITHUB_ACTIONS") == "true" + or not (os.environ.get("HF_TOKEN") or os.environ.get("HUGGING_FACE_HUB_TOKEN")), + reason="This test requires local OpenPI setup and authenticated HF access token", ) from lerobot.configs.types import FeatureType, PolicyFeature, RTCAttentionSchedule # noqa: E402 diff --git a/tests/test_piper_teleop.py b/tests/test_piper_teleop.py index 0c1d95f..a288570 100644 --- a/tests/test_piper_teleop.py +++ b/tests/test_piper_teleop.py @@ -205,9 +205,9 @@ def test_piper_requires_calibration(monkeypatch): teleop.connect(calibrate=False) robot.connect(calibrate=False) try: - with pytest.raises(RuntimeError, match="require calibration"): + with pytest.raises(RuntimeError, match="is not calibrated"): teleop.get_action() - with pytest.raises(RuntimeError, match="require calibration"): + with pytest.raises(RuntimeError, match="is not calibrated"): robot.send_action({"joint_1.pos": 0.0}) finally: teleop.disconnect() diff --git a/tests/value/test_lerobot_value_infer_viz.py b/tests/value/test_lerobot_value_infer_viz.py index 6bf3bdc..bb4bb46 100644 --- a/tests/value/test_lerobot_value_infer_viz.py +++ b/tests/value/test_lerobot_value_infer_viz.py @@ -96,6 +96,7 @@ def test_export_overlay_videos_uses_episode_index_for_subset_alignment(monkeypat video_backend, frame_storage_mode, temp_dir_root, + smooth_window=1, ): assert isinstance(src, Path) assert isinstance(dst, Path) @@ -108,17 +109,19 @@ def test_export_overlay_videos_uses_episode_index_for_subset_alignment(monkeypat assert isinstance(ep_values, np.ndarray) assert isinstance(ep_advantages, np.ndarray) assert isinstance(ep_indicators, np.ndarray) + assert smooth_window == 1 return dst monkeypatch.setattr(value_infer_viz, "_export_single_episode", _fake_export_single_episode) - written = value_infer._export_overlay_videos( + written = value_infer_viz._export_overlay_videos( dataset=dataset, value_field="complementary_info.value", advantage_field="complementary_info.advantage", indicator_field="complementary_info.acp_indicator", viz_episodes="all", video_key=None, + video_keys=None, output_dir=tmp_path, overwrite=True, vcodec="libsvtav1", @@ -146,22 +149,25 @@ def test_export_overlay_videos_passes_disk_storage_mode(monkeypatch, tmp_path: P video_backend, frame_storage_mode, temp_dir_root, + smooth_window=1, ): assert isinstance(episode_timestamps_s, np.ndarray) assert tolerance_s > 0 assert frame_storage_mode == "disk" assert temp_dir_root == tmp_path + assert smooth_window == 1 return dst monkeypatch.setattr(value_infer_viz, "_export_single_episode", _fake_export_single_episode) - written = value_infer._export_overlay_videos( + written = value_infer_viz._export_overlay_videos( dataset=dataset, value_field="complementary_info.value", advantage_field="complementary_info.advantage", indicator_field="complementary_info.acp_indicator", viz_episodes="all", video_key=None, + video_keys=None, output_dir=tmp_path, overwrite=True, vcodec="h264", @@ -172,7 +178,7 @@ def test_export_overlay_videos_passes_disk_storage_mode(monkeypatch, tmp_path: P def test_get_video_encode_options_for_h264_nvenc(): - options, pix_fmt = value_infer._get_video_encode_options("h264_nvenc") + options, pix_fmt = value_infer_viz._get_video_encode_options("h264_nvenc") assert pix_fmt == "yuv420p" assert options["preset"] == "p4" assert options["rc"] == "vbr" @@ -183,7 +189,7 @@ def test_get_video_encode_options_for_h264_nvenc(): def test_get_episode_value_bounds_returns_episode_min_max(): - y_min, y_max = value_infer._get_episode_value_bounds(np.array([-0.7, -0.3, -0.5], dtype=np.float32)) + y_min, y_max = value_infer_viz._get_episode_value_bounds(np.array([-0.7, -0.3, -0.5], dtype=np.float32)) assert np.isclose(y_min, -0.7) assert np.isclose(y_max, -0.3) From 300f5a386d1352742ca5f52d80a8f30043d94215 Mon Sep 17 00:00:00 2001 From: Elvin <223015152@link.cuhk.edu.cn> Date: Fri, 6 Mar 2026 16:56:25 +0800 Subject: [PATCH 13/26] fix(piper): bool calibration flag and robust can setup/test fallback --- README.md | 2 +- .../piper_follower/config_piper_follower.py | 12 +- .../robots/piper_follower/piper_follower.py | 5 +- src/lerobot/scripts/lerobot_setup_can.py | 116 ++++++++++++++---- .../piper_leader/config_piper_leader.py | 12 +- .../piper_leader/piper_leader.py | 5 +- src/lerobot/utils/piper_sdk.py | 8 +- 7 files changed, 107 insertions(+), 53 deletions(-) diff --git a/README.md b/README.md index a5714f5..17685e4 100644 --- a/README.md +++ b/README.md @@ -213,7 +213,7 @@ lerobot-setup-can --mode=setup --interfaces=can0,can1 PiPER calibration is required by default. If needed, you can disable it with -`--robot.calibration_mode=off` and `--teleop.calibration_mode=off`. +`--robot.require_calibration=false` and `--teleop.require_calibration=false`. If you switch master/follower roles with the flags below, you must power-cycle both arms before enabling. diff --git a/src/lerobot/robots/piper_follower/config_piper_follower.py b/src/lerobot/robots/piper_follower/config_piper_follower.py index 56015ab..cf56f2c 100644 --- a/src/lerobot/robots/piper_follower/config_piper_follower.py +++ b/src/lerobot/robots/piper_follower/config_piper_follower.py @@ -17,7 +17,7 @@ from dataclasses import dataclass, field from lerobot.cameras import CameraConfig -from lerobot.utils.piper_sdk import PIPER_CALIBRATION_MODES + from ..config import RobotConfig @@ -50,10 +50,8 @@ class PiperFollowerConfigBase: # Calibration precision: # homing_offset/range_min/range_max are stored as "degree * calibration_scale". calibration_scale: int = 1000 - # Calibration policy: - # - required: always require calibration - # - off: never require calibration - calibration_mode: str = "required" + # Whether calibration is required on connect when no calibration file exists + require_calibration: bool = True # Gripper forwarding behavior sync_gripper: bool = True @@ -80,8 +78,8 @@ class PiperFollowerConfig(RobotConfig, PiperFollowerConfigBase): raise ValueError("`enable_timeout_s` must be >= 0.") if self.calibration_scale <= 0: raise ValueError("`calibration_scale` must be > 0.") - if self.calibration_mode not in PIPER_CALIBRATION_MODES: - raise ValueError("`calibration_mode` must be one of: required, off.") + if not isinstance(self.require_calibration, bool): + raise ValueError("require_calibration must be true or false.") if self.startup_sleep_s < 0: raise ValueError("`startup_sleep_s` must be >= 0.") if not (0 <= self.gripper_effort_default <= 5000): diff --git a/src/lerobot/robots/piper_follower/piper_follower.py b/src/lerobot/robots/piper_follower/piper_follower.py index f202742..2994797 100644 --- a/src/lerobot/robots/piper_follower/piper_follower.py +++ b/src/lerobot/robots/piper_follower/piper_follower.py @@ -29,7 +29,6 @@ from lerobot.utils.piper_sdk import ( get_piper_sdk, milli_to_unit, parse_piper_log_level, - should_require_piper_calibration, unit_to_milli, ) from lerobot.utils.utils import enter_pressed, move_cursor_up @@ -98,7 +97,7 @@ class PiperFollower(Robot): should_auto_calibrate = ( not self.is_calibrated and calibrate - and should_require_piper_calibration(self.config.calibration_mode) + and self.config.require_calibration ) if should_auto_calibrate: logger.info( @@ -125,7 +124,7 @@ class PiperFollower(Robot): logger.info("%s connected.", self) def _use_uncalibrated_passthrough(self) -> bool: - return not self.is_calibrated and not should_require_piper_calibration(self.config.calibration_mode) + return not self.is_calibrated and not self.config.require_calibration @property def is_calibrated(self) -> bool: diff --git a/src/lerobot/scripts/lerobot_setup_can.py b/src/lerobot/scripts/lerobot_setup_can.py index a31727e..53e62ae 100644 --- a/src/lerobot/scripts/lerobot_setup_can.py +++ b/src/lerobot/scripts/lerobot_setup_can.py @@ -93,8 +93,8 @@ def check_interface_status(interface: str) -> tuple[bool, str, bool]: return False, "ip command not found", False -def setup_interface(interface: str, bitrate: int, data_bitrate: int, use_fd: bool) -> bool: - """Configure a CAN interface.""" +def setup_interface(interface: str, bitrate: int, data_bitrate: int, use_fd: bool) -> tuple[bool, str]: + """Configure a CAN interface and return (success, error_message).""" try: subprocess.run(["sudo", "ip", "link", "set", interface, "down"], check=False, capture_output=True) # nosec B607 @@ -104,20 +104,42 @@ def setup_interface(interface: str, bitrate: int, data_bitrate: int, use_fd: boo result = subprocess.run(cmd, capture_output=True, text=True) # nosec B607 if result.returncode != 0: - print(f" ✗ Failed to configure: {result.stderr}") - return False + err = (result.stderr or result.stdout or "unknown error").strip() + return False, f"configure failed: {err}" result = subprocess.run( # nosec B607 ["sudo", "ip", "link", "set", interface, "up"], capture_output=True, text=True ) if result.returncode != 0: - print(f" ✗ Failed to bring up: {result.stderr}") - return False + err = (result.stderr or result.stdout or "unknown error").strip() + return False, f"bring-up failed: {err}" - return True + return True, "" except Exception as e: - print(f" ✗ Error: {e}") - return False + return False, f"exception: {e}" + + +def setup_interface_with_fallback(interface: str, cfg: CANSetupConfig) -> tuple[bool, bool | None, str]: + """Try primary mode first, then fallback mode. Returns (success, used_fd, summary).""" + attempts = [cfg.use_fd, not cfg.use_fd] + labels = {True: "CAN FD", False: "CAN 2.0"} + attempt_msgs: list[str] = [] + + for idx, use_fd in enumerate(attempts, start=1): + print(f" Attempt {idx}/{len(attempts)}: {labels[use_fd]}") + ok, err = setup_interface(interface, cfg.bitrate, cfg.data_bitrate, use_fd) + if ok: + is_up, status, is_fd_iface = check_interface_status(interface) + if is_up: + print(f" ✓ {interface}: {status} (using {labels[use_fd]})") + return True, use_fd, labels[use_fd] + err = f"interface not up after setup ({status})" + + print(f" ✗ {err}") + attempt_msgs.append(f"{labels[use_fd]} -> {err}") + + return False, None, " | ".join(attempt_msgs) + def test_motor(bus, motor_id: int, timeout: float, use_fd: bool): @@ -162,7 +184,7 @@ def test_interface(cfg: CANSetupConfig, interface: str): """Test all motors on a CAN interface.""" import can - is_up, status, _ = check_interface_status(interface) + is_up, status, is_fd_iface = check_interface_status(interface) print(f"\n{interface}: {status}") if not is_up: @@ -170,8 +192,13 @@ def test_interface(cfg: CANSetupConfig, interface: str): return {} try: + # Avoid EINVAL on non-FD interfaces. + effective_use_fd = cfg.use_fd and is_fd_iface + if cfg.use_fd and not is_fd_iface: + print(" ⚠ Interface is CAN 2.0; auto-fallback to CAN 2.0 test frames.") + kwargs = {"channel": interface, "interface": "socketcan", "bitrate": cfg.bitrate} - if cfg.use_fd: + if effective_use_fd: kwargs.update({"data_bitrate": cfg.data_bitrate, "fd": True}) bus = can.interface.Bus(**kwargs) except Exception as e: @@ -180,12 +207,15 @@ def test_interface(cfg: CANSetupConfig, interface: str): results = {} try: - while bus.recv(timeout=0.01): - pass + # Drain stale frames briefly, but never block indefinitely on a busy bus. + drain_deadline = time.time() + 0.2 + while time.time() < drain_deadline: + if bus.recv(timeout=0.01) is None: + break for motor_id in cfg.motor_ids: motor_name = MOTOR_NAMES.get(motor_id, f"motor_0x{motor_id:02X}") - responses, error = test_motor(bus, motor_id, cfg.timeout, cfg.use_fd) + responses, error = test_motor(bus, motor_id, cfg.timeout, effective_use_fd) if error: print(f" Motor 0x{motor_id:02X} ({motor_name}): ✗ {error}") @@ -213,7 +243,7 @@ def speed_test(cfg: CANSetupConfig, interface: str): """Test communication speed with motors.""" import can - is_up, status, _ = check_interface_status(interface) + is_up, status, is_fd_iface = check_interface_status(interface) if not is_up: print(f"{interface}: {status} - skipping") return @@ -221,8 +251,13 @@ def speed_test(cfg: CANSetupConfig, interface: str): print(f"\n{interface}: Running speed test ({cfg.speed_iterations} iterations)...") try: + # Avoid EINVAL on non-FD interfaces. + effective_use_fd = cfg.use_fd and is_fd_iface + if cfg.use_fd and not is_fd_iface: + print(" ⚠ Interface is CAN 2.0; auto-fallback to CAN 2.0 test frames.") + kwargs = {"channel": interface, "interface": "socketcan", "bitrate": cfg.bitrate} - if cfg.use_fd: + if effective_use_fd: kwargs.update({"data_bitrate": cfg.data_bitrate, "fd": True}) bus = can.interface.Bus(**kwargs) except Exception as e: @@ -269,29 +304,58 @@ def speed_test(cfg: CANSetupConfig, interface: str): print(" ✗ No successful responses") -def run_setup(cfg: CANSetupConfig): +def run_setup(cfg: CANSetupConfig) -> bool: """Setup CAN interfaces.""" print("=" * 50) print("CAN Interface Setup") print("=" * 50) - print(f"Mode: {'CAN FD' if cfg.use_fd else 'CAN 2.0'}") + print(f"Primary mode: {'CAN FD' if cfg.use_fd else 'CAN 2.0'}") + print(f"Fallback mode: {'CAN 2.0' if cfg.use_fd else 'CAN FD'}") print(f"Bitrate: {cfg.bitrate / 1_000_000:.1f} Mbps") if cfg.use_fd: print(f"Data bitrate: {cfg.data_bitrate / 1_000_000:.1f} Mbps") print() interfaces = cfg.get_interfaces() + success_count = 0 + failed: dict[str, str] = {} + mode_map: dict[str, bool] = {} + for interface in interfaces: print(f"Configuring {interface}...") - if setup_interface(interface, cfg.bitrate, cfg.data_bitrate, cfg.use_fd): - is_up, status, _ = check_interface_status(interface) - print(f" ✓ {interface}: {status}") + ok, used_fd, summary = setup_interface_with_fallback(interface, cfg) + if ok: + success_count += 1 + mode_map[interface] = bool(used_fd) else: - print(f" ✗ {interface}: Failed") + failed[interface] = summary + print(f" ✗ {interface}: setup failed ({summary})") - print("\nSetup complete!") - print("\nNext: Test motors with:") + print() + print("=" * 50) + print("Setup summary") + print("=" * 50) + print(f"Interfaces configured: {success_count}/{len(interfaces)}") + if mode_map: + print("Active mode:") + for interface in interfaces: + if interface in mode_map: + print(f" - {interface}: {'CAN FD' if mode_map[interface] else 'CAN 2.0'}") + if failed: + print("Failed interfaces:") + for interface in interfaces: + if interface in failed: + print(f" - {interface}: {failed[interface]}") + print() + print("✗ Setup failed") + return False + + print() + print("✓ Setup succeeded") + print() + print("Next: Test motors with:") print(f" lerobot-setup-can --mode=test --interfaces {','.join(interfaces)}") + return True def run_test(cfg: CANSetupConfig): @@ -341,7 +405,9 @@ def setup_can(cfg: CANSetupConfig): sys.exit(1) if cfg.mode == "setup": - run_setup(cfg) + ok = run_setup(cfg) + if not ok: + sys.exit(2) elif cfg.mode == "test": run_test(cfg) elif cfg.mode == "speed": diff --git a/src/lerobot/teleoperators/piper_leader/config_piper_leader.py b/src/lerobot/teleoperators/piper_leader/config_piper_leader.py index c3af356..4b19dd7 100644 --- a/src/lerobot/teleoperators/piper_leader/config_piper_leader.py +++ b/src/lerobot/teleoperators/piper_leader/config_piper_leader.py @@ -16,7 +16,7 @@ from dataclasses import dataclass -from lerobot.utils.piper_sdk import PIPER_CALIBRATION_MODES + from ..config import TeleoperatorConfig @@ -58,10 +58,8 @@ class PiperLeaderConfigBase: # Calibration precision: # homing_offset/range_min/range_max are stored as "degree * calibration_scale". calibration_scale: int = 1000 - # Calibration policy: - # - required: always require calibration - # - off: never require calibration - calibration_mode: str = "required" + # Whether calibration is required on connect when no calibration file exists + require_calibration: bool = True # Safety behavior on disconnect disable_on_disconnect: bool = False @@ -79,8 +77,8 @@ class PiperLeaderConfig(TeleoperatorConfig, PiperLeaderConfigBase): raise ValueError("`enable_timeout_s` must be >= 0.") if self.calibration_scale <= 0: raise ValueError("`calibration_scale` must be > 0.") - if self.calibration_mode not in PIPER_CALIBRATION_MODES: - raise ValueError("`calibration_mode` must be one of: required, off.") + if not isinstance(self.require_calibration, bool): + raise ValueError("require_calibration must be true or false.") if self.startup_sleep_s < 0: raise ValueError("`startup_sleep_s` must be >= 0.") if not (0 <= self.gripper_effort_default <= 5000): diff --git a/src/lerobot/teleoperators/piper_leader/piper_leader.py b/src/lerobot/teleoperators/piper_leader/piper_leader.py index 82135f7..9c58c0f 100644 --- a/src/lerobot/teleoperators/piper_leader/piper_leader.py +++ b/src/lerobot/teleoperators/piper_leader/piper_leader.py @@ -29,7 +29,6 @@ from lerobot.utils.piper_sdk import ( get_piper_sdk, milli_to_unit, parse_piper_log_level, - should_require_piper_calibration, unit_to_milli, ) from lerobot.utils.utils import enter_pressed, move_cursor_up @@ -97,7 +96,7 @@ class PiperLeader(Teleoperator): if ( not self.is_calibrated and calibrate - and should_require_piper_calibration(self.config.calibration_mode) + and self.config.require_calibration ): logger.info( "No piper-leader calibration file found for '%s'. Running lerobot-calibrate flow.", @@ -112,7 +111,7 @@ class PiperLeader(Teleoperator): logger.info("%s connected.", self) def _use_uncalibrated_passthrough(self) -> bool: - return not self.is_calibrated and not should_require_piper_calibration(self.config.calibration_mode) + return not self.is_calibrated and not self.config.require_calibration @property def is_calibrated(self) -> bool: diff --git a/src/lerobot/utils/piper_sdk.py b/src/lerobot/utils/piper_sdk.py index 967c018..f393840 100644 --- a/src/lerobot/utils/piper_sdk.py +++ b/src/lerobot/utils/piper_sdk.py @@ -29,7 +29,7 @@ PIPER_JOINT_NAMES = ( ) PIPER_JOINT_ACTION_KEYS = tuple(f"{joint}.pos" for joint in PIPER_JOINT_NAMES) PIPER_ACTION_KEYS = PIPER_JOINT_ACTION_KEYS + ("gripper.pos",) -PIPER_CALIBRATION_MODES = {"required", "off"} + def milli_to_unit(value: float | int) -> float: @@ -64,9 +64,3 @@ def parse_piper_log_level(level_name: str) -> Any: ) from exc -def should_require_piper_calibration(calibration_mode: str) -> bool: - if calibration_mode == "required": - return True - if calibration_mode == "off": - return False - raise ValueError(f"Invalid Piper calibration mode '{calibration_mode}'. Expected one of: required, off.") From fe9869c2bd7a6d494509b12d5c2cbe7429233ed1 Mon Sep 17 00:00:00 2001 From: Elvin <223015152@link.cuhk.edu.cn> Date: Fri, 6 Mar 2026 17:24:35 +0800 Subject: [PATCH 14/26] test(value): skip viz tests when scipy is unavailable - add pytest.importorskip('scipy') at module import time to avoid collection failures in minimal test environments --- tests/value/test_lerobot_value_infer_viz.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/tests/value/test_lerobot_value_infer_viz.py b/tests/value/test_lerobot_value_infer_viz.py index bb4bb46..cae2de3 100644 --- a/tests/value/test_lerobot_value_infer_viz.py +++ b/tests/value/test_lerobot_value_infer_viz.py @@ -3,9 +3,12 @@ from pathlib import Path import numpy as np +import pytest import torch from PIL import Image +pytest.importorskip("scipy") + from lerobot.configs.value import ( ValueInferenceACPConfig, ValueInferenceCheckpointConfig, From 92d9d829b335a0d6d38cbe37d8b77ae5c70fdc8a Mon Sep 17 00:00:00 2001 From: Elvin <223015152@link.cuhk.edu.cn> Date: Sat, 7 Mar 2026 00:48:17 +0800 Subject: [PATCH 15/26] feat(piper): add dedicated piperx types and auto URDF selection - add piperx_leader/piperx_follower config and runtime classes reusing piper logic - remove manual gravity_comp_urdf_path and select bundled URDF by teleop type - add bundled piper/piperx description assets tracked via git lfs - update README piper setup and refresh piper teleop tests --- .gitattributes | 2 + MANIFEST.in | 2 + README.md | 38 ++-- pyproject.toml | 7 + .../piper_description/meshes/base_link.STL | 3 + .../assets/piper_description/meshes/link1.STL | 3 + .../assets/piper_description/meshes/link2.STL | 3 + .../assets/piper_description/meshes/link3.STL | 3 + .../assets/piper_description/meshes/link4.STL | 3 + .../assets/piper_description/meshes/link5.STL | 3 + .../assets/piper_description/meshes/link6.STL | 3 + .../urdf/piper_no_gripper_description.urdf | 3 + .../piper_x_description/meshes/Link6.STL | 3 + .../piper_x_description/meshes/dae/J1.dae | 3 + .../piper_x_description/meshes/dae/J2.dae | 3 + .../piper_x_description/meshes/dae/J3.dae | 3 + .../piper_x_description/meshes/dae/J4.dae | 3 + .../piper_x_description/meshes/dae/J5.dae | 3 + .../meshes/dae/base_link.dae | 3 + .../urdf/piper_x_description_no_gripper.urdf | 3 + src/lerobot/robots/piper_follower/__init__.py | 4 +- .../piper_follower/config_piper_follower.py | 47 +++-- .../robots/piper_follower/piper_follower.py | 25 ++- src/lerobot/robots/utils.py | 4 + .../teleoperators/piper_leader/__init__.py | 4 +- .../piper_leader/config_piper_leader.py | 64 ++++-- .../piper_leader/gravity_compensation.py | 192 ++++++++++++++++++ .../piper_leader/piper_leader.py | 101 ++++++--- src/lerobot/teleoperators/utils.py | 4 + src/lerobot/utils/piper_sdk.py | 65 +++++- tests/test_piper_teleop.py | 184 ++++++++++++++++- 31 files changed, 676 insertions(+), 115 deletions(-) create mode 100755 src/lerobot/assets/piper_description/meshes/base_link.STL create mode 100755 src/lerobot/assets/piper_description/meshes/link1.STL create mode 100755 src/lerobot/assets/piper_description/meshes/link2.STL create mode 100755 src/lerobot/assets/piper_description/meshes/link3.STL create mode 100755 src/lerobot/assets/piper_description/meshes/link4.STL create mode 100755 src/lerobot/assets/piper_description/meshes/link5.STL create mode 100755 src/lerobot/assets/piper_description/meshes/link6.STL create mode 100755 src/lerobot/assets/piper_description/urdf/piper_no_gripper_description.urdf create mode 100755 src/lerobot/assets/piper_x_description/meshes/Link6.STL create mode 100755 src/lerobot/assets/piper_x_description/meshes/dae/J1.dae create mode 100755 src/lerobot/assets/piper_x_description/meshes/dae/J2.dae create mode 100755 src/lerobot/assets/piper_x_description/meshes/dae/J3.dae create mode 100755 src/lerobot/assets/piper_x_description/meshes/dae/J4.dae create mode 100755 src/lerobot/assets/piper_x_description/meshes/dae/J5.dae create mode 100644 src/lerobot/assets/piper_x_description/meshes/dae/base_link.dae create mode 100755 src/lerobot/assets/piper_x_description/urdf/piper_x_description_no_gripper.urdf create mode 100644 src/lerobot/teleoperators/piper_leader/gravity_compensation.py diff --git a/.gitattributes b/.gitattributes index 302ff0a..dc964c8 100644 --- a/.gitattributes +++ b/.gitattributes @@ -20,3 +20,5 @@ tests/artifacts/cameras/*.png filter=lfs diff=lfs merge=lfs -text *.bag filter=lfs diff=lfs merge=lfs -text *.gif filter=lfs diff=lfs merge=lfs -text +src/lerobot/assets/piper_description/** filter=lfs diff=lfs merge=lfs -text +src/lerobot/assets/piper_x_description/** filter=lfs diff=lfs merge=lfs -text diff --git a/MANIFEST.in b/MANIFEST.in index c1fb2ea..7e40031 100644 --- a/MANIFEST.in +++ b/MANIFEST.in @@ -1,2 +1,4 @@ include src/lerobot/templates/lerobot_modelcard_template.md include src/lerobot/datasets/card_template.md +recursive-include src/lerobot/assets/piper_x_description * +recursive-include src/lerobot/assets/piper_description * diff --git a/README.md b/README.md index 17685e4..930a2e8 100644 --- a/README.md +++ b/README.md @@ -204,6 +204,13 @@ If needed, you can also use temporary device paths (for example `/dev/ttyACM*` a #### AgileX PiPER +For PiPER-series robots, make sure Git LFS assets are pulled before running teleoperation: + +```bash +git lfs pull --include="src/lerobot/assets/piper_description/**,src/lerobot/assets/piper_x_description/**" --exclude="*" +git lfs checkout src/lerobot/assets/piper_description src/lerobot/assets/piper_x_description +``` + For PiPER setup, PiPER uses CAN interfaces instead of serial ports. So first run `lerobot-setup-can` to confirm CAN interfaces are available: @@ -211,35 +218,18 @@ So first run `lerobot-setup-can` to confirm CAN interfaces are available: lerobot-setup-can --mode=setup --interfaces=can0,can1 ``` -PiPER calibration is required by default. -If needed, you can disable it with -`--robot.require_calibration=false` and `--teleop.require_calibration=false`. - -If you switch master/follower roles with the flags below, you must power-cycle both arms before enabling. - -```bash ---teleop.set_leader_mode_on_connect=true ---robot.set_follower_mode_on_connect=true -``` - For single-arm users, run the command below to verify the system is ready: ```bash lerobot-teleoperate \ - --robot.type=piper_follower \ + --robot.type=piperx_follower \ --robot.port=can0 \ - --robot.id=my_piper_follower \ - --teleop.type=piper_leader \ + --robot.id=my_piperx_follower \ + --robot.require_calibration=false \ + --teleop.type=piperx_leader \ --teleop.port=can1 \ - --teleop.id=my_piper_leader -``` - -Optional PiPER flags: - -```bash ---robot.sync_gripper=true|false ---robot.gripper_effort_default=1000 ---teleop.sync_gripper=true|false + --teleop.id=my_piperx_leader \ + --teleop.require_calibration=false ``` @@ -290,9 +280,11 @@ lerobot-human-inloop-record \ --robot.type=piper_follower \ --robot.port=can0 \ --robot.id=my_piper_follower \ + --robot.require_calibration=false \ --teleop.type=piper_leader \ --teleop.port=can1 \ --teleop.id=my_piper_leader \ + --teleop.require_calibration=false \ --dataset.repo_id=/ \ --dataset.single_task="" \ --dataset.num_episodes= \ diff --git a/pyproject.toml b/pyproject.toml index 38b5c88..ed11012 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -75,6 +75,7 @@ dependencies = [ "pynput>=1.7.7,<1.9.0", "pyserial>=3.5,<4.0", "piper_sdk>=0.6.1,<0.7.0", + "pin>=3.0.0,<4.0.0", "wandb>=0.24.0,<0.25.0", "torch>=2.2.1,<2.8.0", # TODO: Bumb dependency @@ -220,6 +221,12 @@ lerobot-dataset-report="lerobot.scripts.lerobot_dataset_report:main" [tool.setuptools.packages.find] where = ["src"] +[tool.setuptools.package-data] +lerobot = [ + "assets/piper_description/**/*", + "assets/piper_x_description/**/*", +] + [tool.ruff] target-version = "py310" line-length = 110 diff --git a/src/lerobot/assets/piper_description/meshes/base_link.STL b/src/lerobot/assets/piper_description/meshes/base_link.STL new file mode 100755 index 0000000..e724054 --- /dev/null +++ b/src/lerobot/assets/piper_description/meshes/base_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:fd4a3d6d0266205fe3f725f048645be66592c23564db22e08b0807d5569b2c64 +size 607384 diff --git a/src/lerobot/assets/piper_description/meshes/link1.STL b/src/lerobot/assets/piper_description/meshes/link1.STL new file mode 100755 index 0000000..310339a --- /dev/null +++ b/src/lerobot/assets/piper_description/meshes/link1.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9ee6e1d67c0241f42a0dbaf5c676d19230375ed078ee1943bb8c4c37ab303d14 +size 447784 diff --git a/src/lerobot/assets/piper_description/meshes/link2.STL b/src/lerobot/assets/piper_description/meshes/link2.STL new file mode 100755 index 0000000..cc15d93 --- /dev/null +++ b/src/lerobot/assets/piper_description/meshes/link2.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0d37675b699b643f2e15348de99aed535fac10dc5f09a79b8e4b0acfaf4e0378 +size 3658384 diff --git a/src/lerobot/assets/piper_description/meshes/link3.STL b/src/lerobot/assets/piper_description/meshes/link3.STL new file mode 100755 index 0000000..68b71f1 --- /dev/null +++ b/src/lerobot/assets/piper_description/meshes/link3.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e8ae51f17c0c7f6d67b753922e2cc269f5e8372a65734f859231639a64fe8fad +size 1845784 diff --git a/src/lerobot/assets/piper_description/meshes/link4.STL b/src/lerobot/assets/piper_description/meshes/link4.STL new file mode 100755 index 0000000..51b3f77 --- /dev/null +++ b/src/lerobot/assets/piper_description/meshes/link4.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:efa33022976218f246f736a6133494b5d0e5486e8b50b52900c42f285ae85558 +size 877984 diff --git a/src/lerobot/assets/piper_description/meshes/link5.STL b/src/lerobot/assets/piper_description/meshes/link5.STL new file mode 100755 index 0000000..81a4bf9 --- /dev/null +++ b/src/lerobot/assets/piper_description/meshes/link5.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a1c69d1c7f6f8c60e2c82331c0f444725200ed4053d3e5ec23aa82cb5db5b29f +size 833684 diff --git a/src/lerobot/assets/piper_description/meshes/link6.STL b/src/lerobot/assets/piper_description/meshes/link6.STL new file mode 100755 index 0000000..2550b39 --- /dev/null +++ b/src/lerobot/assets/piper_description/meshes/link6.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:88374973bc3bd9e7389b5853cd6e555af9731c30b658b1ee6f85be34a126cb92 +size 58884 diff --git a/src/lerobot/assets/piper_description/urdf/piper_no_gripper_description.urdf b/src/lerobot/assets/piper_description/urdf/piper_no_gripper_description.urdf new file mode 100755 index 0000000..99cbf30 --- /dev/null +++ b/src/lerobot/assets/piper_description/urdf/piper_no_gripper_description.urdf @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a93c1e3fc245d9d336a427fcc506cb1fc15615268053392b249816f90c011e50 +size 8273 diff --git a/src/lerobot/assets/piper_x_description/meshes/Link6.STL b/src/lerobot/assets/piper_x_description/meshes/Link6.STL new file mode 100755 index 0000000..7d5dccd --- /dev/null +++ b/src/lerobot/assets/piper_x_description/meshes/Link6.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0861e122e406f891a57a550551ad1ce0ef4ca5babf58f476cf8914395e144319 +size 162084 diff --git a/src/lerobot/assets/piper_x_description/meshes/dae/J1.dae b/src/lerobot/assets/piper_x_description/meshes/dae/J1.dae new file mode 100755 index 0000000..2834be8 --- /dev/null +++ b/src/lerobot/assets/piper_x_description/meshes/dae/J1.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e78dee99bc84857c09183463f3fb3fdea292ea7570f1bfb0001a79674bb69d45 +size 2323489 diff --git a/src/lerobot/assets/piper_x_description/meshes/dae/J2.dae b/src/lerobot/assets/piper_x_description/meshes/dae/J2.dae new file mode 100755 index 0000000..dd9a994 --- /dev/null +++ b/src/lerobot/assets/piper_x_description/meshes/dae/J2.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:97e9079580af6e423317c33b2a0f4fe0f6dbc67c3e1d9e6f4836c87c6c8f34a2 +size 14480060 diff --git a/src/lerobot/assets/piper_x_description/meshes/dae/J3.dae b/src/lerobot/assets/piper_x_description/meshes/dae/J3.dae new file mode 100755 index 0000000..79de32f --- /dev/null +++ b/src/lerobot/assets/piper_x_description/meshes/dae/J3.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:db5b39eef745655a67e57870d8ee56f26a03651d2161635ca2ec3c58c9269907 +size 5098355 diff --git a/src/lerobot/assets/piper_x_description/meshes/dae/J4.dae b/src/lerobot/assets/piper_x_description/meshes/dae/J4.dae new file mode 100755 index 0000000..364f5df --- /dev/null +++ b/src/lerobot/assets/piper_x_description/meshes/dae/J4.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ccc08d3ef1cb8bdfb8ab5395ff65893d13386ad526d7bc954d468b7dc885d557 +size 2037373 diff --git a/src/lerobot/assets/piper_x_description/meshes/dae/J5.dae b/src/lerobot/assets/piper_x_description/meshes/dae/J5.dae new file mode 100755 index 0000000..f648b03 --- /dev/null +++ b/src/lerobot/assets/piper_x_description/meshes/dae/J5.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:78718679ffdf9d81130047c4267ed66a081315eff36f216833bc3781c6241417 +size 2155609 diff --git a/src/lerobot/assets/piper_x_description/meshes/dae/base_link.dae b/src/lerobot/assets/piper_x_description/meshes/dae/base_link.dae new file mode 100644 index 0000000..97f74b6 --- /dev/null +++ b/src/lerobot/assets/piper_x_description/meshes/dae/base_link.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:486dddcd07f072b2e3978087dc2f9b641a91722bccb1bcb3421c4c7ce64a39ed +size 1001764 diff --git a/src/lerobot/assets/piper_x_description/urdf/piper_x_description_no_gripper.urdf b/src/lerobot/assets/piper_x_description/urdf/piper_x_description_no_gripper.urdf new file mode 100755 index 0000000..9885a63 --- /dev/null +++ b/src/lerobot/assets/piper_x_description/urdf/piper_x_description_no_gripper.urdf @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d6dc6d31b0fa56341b2323fee5e8a375c2500ae6fb02e31fb52e8e917b608439 +size 8169 diff --git a/src/lerobot/robots/piper_follower/__init__.py b/src/lerobot/robots/piper_follower/__init__.py index ec450ff..a3c5a01 100644 --- a/src/lerobot/robots/piper_follower/__init__.py +++ b/src/lerobot/robots/piper_follower/__init__.py @@ -14,5 +14,5 @@ # See the License for the specific language governing permissions and # limitations under the License. -from .config_piper_follower import PiperFollowerConfig, PiperFollowerConfigBase -from .piper_follower import PiperFollower +from .config_piper_follower import PiperFollowerConfig, PiperFollowerConfigBase, PiperXFollowerConfig +from .piper_follower import PiperFollower, PiperXFollower diff --git a/src/lerobot/robots/piper_follower/config_piper_follower.py b/src/lerobot/robots/piper_follower/config_piper_follower.py index cf56f2c..bac213d 100644 --- a/src/lerobot/robots/piper_follower/config_piper_follower.py +++ b/src/lerobot/robots/piper_follower/config_piper_follower.py @@ -35,9 +35,6 @@ class PiperFollowerConfigBase: log_level: str = "WARNING" startup_sleep_s: float = 0.1 - # Optional role command on startup (0xFC = motion output/follower arm) - set_follower_mode_on_connect: bool = False - # Motion mode for follower arm speed_ratio: int = 100 high_follow: bool = True @@ -65,24 +62,36 @@ class PiperFollowerConfigBase: disable_on_disconnect: bool = False +def _validate_piper_follower_config(config: PiperFollowerConfigBase) -> None: + if not (0 <= config.speed_ratio <= 100): + raise ValueError("`speed_ratio` must be between 0 and 100.") + if config.mode_refresh_interval_s < 0: + raise ValueError("`mode_refresh_interval_s` must be >= 0.") + if config.enable_timeout_s < 0: + raise ValueError("`enable_timeout_s` must be >= 0.") + if config.calibration_scale <= 0: + raise ValueError("`calibration_scale` must be > 0.") + if not isinstance(config.require_calibration, bool): + raise ValueError("require_calibration must be true or false.") + if config.startup_sleep_s < 0: + raise ValueError("`startup_sleep_s` must be >= 0.") + if not (0 <= config.gripper_effort_default <= 5000): + raise ValueError("`gripper_effort_default` must be between 0 and 5000.") + if config.gripper_status_code not in {0x00, 0x01, 0x02, 0x03}: + raise ValueError("`gripper_status_code` must be one of 0x00, 0x01, 0x02, 0x03.") + + @RobotConfig.register_subclass("piper_follower") @dataclass class PiperFollowerConfig(RobotConfig, PiperFollowerConfigBase): def __post_init__(self): super().__post_init__() - if not (0 <= self.speed_ratio <= 100): - raise ValueError("`speed_ratio` must be between 0 and 100.") - if self.mode_refresh_interval_s < 0: - raise ValueError("`mode_refresh_interval_s` must be >= 0.") - if self.enable_timeout_s < 0: - raise ValueError("`enable_timeout_s` must be >= 0.") - if self.calibration_scale <= 0: - raise ValueError("`calibration_scale` must be > 0.") - if not isinstance(self.require_calibration, bool): - raise ValueError("require_calibration must be true or false.") - if self.startup_sleep_s < 0: - raise ValueError("`startup_sleep_s` must be >= 0.") - if not (0 <= self.gripper_effort_default <= 5000): - raise ValueError("`gripper_effort_default` must be between 0 and 5000.") - if self.gripper_status_code not in {0x00, 0x01, 0x02, 0x03}: - raise ValueError("`gripper_status_code` must be one of 0x00, 0x01, 0x02, 0x03.") + _validate_piper_follower_config(self) + + +@RobotConfig.register_subclass("piperx_follower") +@dataclass +class PiperXFollowerConfig(RobotConfig, PiperFollowerConfigBase): + def __post_init__(self): + super().__post_init__() + _validate_piper_follower_config(self) diff --git a/src/lerobot/robots/piper_follower/piper_follower.py b/src/lerobot/robots/piper_follower/piper_follower.py index 2994797..4b2716e 100644 --- a/src/lerobot/robots/piper_follower/piper_follower.py +++ b/src/lerobot/robots/piper_follower/piper_follower.py @@ -24,17 +24,19 @@ from lerobot.processor import RobotAction, RobotObservation from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected from lerobot.utils.piper_sdk import ( PIPER_ACTION_KEYS, + guard_piper_ctrl_mode_on_connect, PIPER_JOINT_ACTION_KEYS, PIPER_JOINT_NAMES, get_piper_sdk, milli_to_unit, parse_piper_log_level, unit_to_milli, + wait_enable_piper, ) from lerobot.utils.utils import enter_pressed, move_cursor_up from ..robot import Robot -from .config_piper_follower import PiperFollowerConfig +from .config_piper_follower import PiperFollowerConfig, PiperXFollowerConfig logger = logging.getLogger(__name__) PIPER_CALIB_KEYS = list(PIPER_ACTION_KEYS) @@ -47,7 +49,7 @@ class PiperFollower(Robot): config_class = PiperFollowerConfig name = "piper_follower" - def __init__(self, config: PiperFollowerConfig): + def __init__(self, config: PiperFollowerConfig | PiperXFollowerConfig): super().__init__(config) self.config = config self._is_connected = False @@ -85,14 +87,11 @@ class PiperFollower(Robot): self.arm.ConnectPort() if self.config.startup_sleep_s > 0: time.sleep(self.config.startup_sleep_s) + guard_piper_ctrl_mode_on_connect(arm=self.arm, interface_name=self.config.port) self._is_connected = True connected_cameras = [] try: - if self.config.set_follower_mode_on_connect: - self.arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00) - time.sleep(0.05) - self.configure() should_auto_calibrate = ( not self.is_calibrated @@ -190,12 +189,7 @@ class PiperFollower(Robot): self._send_motion_mode() def _wait_enable(self, timeout_s: float) -> bool: - deadline = time.monotonic() + timeout_s - while time.monotonic() < deadline: - if bool(self.arm.EnablePiper()): - return True - time.sleep(0.02) - return False + return wait_enable_piper(self.arm, timeout_s) def _read_raw_observation(self) -> RobotObservation: joint_msg = self.arm.GetArmJointMsgs() @@ -259,7 +253,7 @@ class PiperFollower(Robot): def send_action(self, action: RobotAction) -> RobotAction: if not self.is_calibrated and not self._use_uncalibrated_passthrough(): raise RuntimeError( - f"{self} is not calibrated. Run `lerobot-calibrate --robot.type=piper_follower --robot.id={self.id}` first." + f"{self} is not calibrated. Run `lerobot-calibrate --robot.type={self.config.type} --robot.id={self.id}` first." ) self._refresh_motion_mode_if_needed() @@ -308,3 +302,8 @@ class PiperFollower(Robot): cam.disconnect() self._is_connected = False logger.info("%s disconnected.", self) + + +class PiperXFollower(PiperFollower): + config_class = PiperXFollowerConfig + name = "piperx_follower" diff --git a/src/lerobot/robots/utils.py b/src/lerobot/robots/utils.py index 81288d1..3c6e7c5 100644 --- a/src/lerobot/robots/utils.py +++ b/src/lerobot/robots/utils.py @@ -68,6 +68,10 @@ def make_robot_from_config(config: RobotConfig) -> Robot: from .piper_follower import PiperFollower return PiperFollower(config) + elif config.type == "piperx_follower": + from .piper_follower import PiperXFollower + + return PiperXFollower(config) elif config.type == "bi_openarm_follower": from .bi_openarm_follower import BiOpenArmFollower diff --git a/src/lerobot/teleoperators/piper_leader/__init__.py b/src/lerobot/teleoperators/piper_leader/__init__.py index 1e4110d..d063ff6 100644 --- a/src/lerobot/teleoperators/piper_leader/__init__.py +++ b/src/lerobot/teleoperators/piper_leader/__init__.py @@ -14,5 +14,5 @@ # See the License for the specific language governing permissions and # limitations under the License. -from .config_piper_leader import PiperLeaderConfig, PiperLeaderConfigBase -from .piper_leader import PiperLeader +from .config_piper_leader import PiperLeaderConfig, PiperLeaderConfigBase, PiperXLeaderConfig +from .piper_leader import PiperLeader, PiperXLeader diff --git a/src/lerobot/teleoperators/piper_leader/config_piper_leader.py b/src/lerobot/teleoperators/piper_leader/config_piper_leader.py index 4b19dd7..7376850 100644 --- a/src/lerobot/teleoperators/piper_leader/config_piper_leader.py +++ b/src/lerobot/teleoperators/piper_leader/config_piper_leader.py @@ -16,8 +16,6 @@ from dataclasses import dataclass - - from ..config import TeleoperatorConfig @@ -34,9 +32,6 @@ class PiperLeaderConfigBase: log_level: str = "WARNING" startup_sleep_s: float = 0.1 - # Optional role command on startup (0xFC = motion output arm) - set_leader_mode_on_connect: bool = False - # Manual backdrivable mode for human teleop manual_control: bool = True @@ -55,6 +50,14 @@ class PiperLeaderConfigBase: mode_refresh_interval_s: float = 1.0 enable_timeout_s: float = 3.0 + # Gravity compensation settings (used when manual_control=true) + gravity_comp_control_hz: float = 200.0 + gravity_comp_tx_ratio: tuple[float, float, float, float, float, float] = (1.0, 1.0, 1.0, 1.0, 1.0, 1.0) + gravity_comp_torque_limit: float = 8.0 + gravity_comp_mit_kp: float = 0.0 + gravity_comp_mit_kd: float = 0.0 + gravity_comp_base_rpy_deg: tuple[float, float, float] = (0.0, 0.0, 0.0) + # Calibration precision: # homing_offset/range_min/range_max are stored as "degree * calibration_scale". calibration_scale: int = 1000 @@ -65,23 +68,42 @@ class PiperLeaderConfigBase: disable_on_disconnect: bool = False +def _validate_piper_leader_config(config: PiperLeaderConfigBase) -> None: + if not (0 <= config.command_speed_ratio <= 100): + raise ValueError("`command_speed_ratio` must be between 0 and 100.") + if config.mode_refresh_interval_s < 0: + raise ValueError("`mode_refresh_interval_s` must be >= 0.") + if config.enable_timeout_s < 0: + raise ValueError("`enable_timeout_s` must be >= 0.") + if config.gravity_comp_control_hz <= 0: + raise ValueError("`gravity_comp_control_hz` must be > 0.") + if len(config.gravity_comp_tx_ratio) != 6: + raise ValueError("`gravity_comp_tx_ratio` must contain exactly 6 values.") + if config.gravity_comp_torque_limit <= 0: + raise ValueError("`gravity_comp_torque_limit` must be > 0.") + if len(config.gravity_comp_base_rpy_deg) != 3: + raise ValueError("`gravity_comp_base_rpy_deg` must contain exactly 3 values.") + if config.calibration_scale <= 0: + raise ValueError("`calibration_scale` must be > 0.") + if not isinstance(config.require_calibration, bool): + raise ValueError("require_calibration must be true or false.") + if config.startup_sleep_s < 0: + raise ValueError("`startup_sleep_s` must be >= 0.") + if not (0 <= config.gripper_effort_default <= 5000): + raise ValueError("`gripper_effort_default` must be between 0 and 5000.") + if config.gripper_status_code not in {0x00, 0x01, 0x02, 0x03}: + raise ValueError("`gripper_status_code` must be one of 0x00, 0x01, 0x02, 0x03.") + + @TeleoperatorConfig.register_subclass("piper_leader") @dataclass class PiperLeaderConfig(TeleoperatorConfig, PiperLeaderConfigBase): def __post_init__(self): - if not (0 <= self.command_speed_ratio <= 100): - raise ValueError("`command_speed_ratio` must be between 0 and 100.") - if self.mode_refresh_interval_s < 0: - raise ValueError("`mode_refresh_interval_s` must be >= 0.") - if self.enable_timeout_s < 0: - raise ValueError("`enable_timeout_s` must be >= 0.") - if self.calibration_scale <= 0: - raise ValueError("`calibration_scale` must be > 0.") - if not isinstance(self.require_calibration, bool): - raise ValueError("require_calibration must be true or false.") - if self.startup_sleep_s < 0: - raise ValueError("`startup_sleep_s` must be >= 0.") - if not (0 <= self.gripper_effort_default <= 5000): - raise ValueError("`gripper_effort_default` must be between 0 and 5000.") - if self.gripper_status_code not in {0x00, 0x01, 0x02, 0x03}: - raise ValueError("`gripper_status_code` must be one of 0x00, 0x01, 0x02, 0x03.") + _validate_piper_leader_config(self) + + +@TeleoperatorConfig.register_subclass("piperx_leader") +@dataclass +class PiperXLeaderConfig(TeleoperatorConfig, PiperLeaderConfigBase): + def __post_init__(self): + _validate_piper_leader_config(self) diff --git a/src/lerobot/teleoperators/piper_leader/gravity_compensation.py b/src/lerobot/teleoperators/piper_leader/gravity_compensation.py new file mode 100644 index 0000000..fcdaf1e --- /dev/null +++ b/src/lerobot/teleoperators/piper_leader/gravity_compensation.py @@ -0,0 +1,192 @@ +#!/usr/bin/env python + +# Copyright 2026 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 __future__ import annotations + +import logging +import math +import threading +import time +from pathlib import Path +from typing import Any + +import numpy as np + +logger = logging.getLogger(__name__) + + +def _rpy_deg_to_rot(roll_deg: float, pitch_deg: float, yaw_deg: float) -> np.ndarray: + roll = math.radians(roll_deg) + pitch = math.radians(pitch_deg) + yaw = math.radians(yaw_deg) + + cx, sx = math.cos(roll), math.sin(roll) + cy, sy = math.cos(pitch), math.sin(pitch) + cz, sz = math.cos(yaw), math.sin(yaw) + + rot_x = np.array([[1.0, 0.0, 0.0], [0.0, cx, -sx], [0.0, sx, cx]], dtype=np.float64) + rot_y = np.array([[cy, 0.0, sy], [0.0, 1.0, 0.0], [-sy, 0.0, cy]], dtype=np.float64) + rot_z = np.array([[cz, -sz, 0.0], [sz, cz, 0.0], [0.0, 0.0, 1.0]], dtype=np.float64) + return rot_z @ rot_y @ rot_x + + +class PiperGravityCompensationLoop: + """Background MIT torque loop for gravity compensation on Piper leader arm.""" + + def __init__( + self, + *, + arm: Any, + urdf_path: str, + control_hz: float, + tx_ratio: tuple[float, float, float, float, float, float], + torque_limit: float, + mit_kp: float, + mit_kd: float, + base_rpy_deg: tuple[float, float, float], + mode_refresh_interval_s: float, + move_speed_ratio: int, + ) -> None: + import pinocchio as pin + + self._pin = pin + self._arm = arm + self._control_hz = control_hz + self._tx_ratio = np.asarray(tx_ratio, dtype=np.float64) + self._torque_limit = float(torque_limit) + self._mit_kp = float(mit_kp) + self._mit_kd = float(mit_kd) + self._mode_refresh_interval_s = max(0.0, float(mode_refresh_interval_s)) + self._move_speed_ratio = int(move_speed_ratio) + + urdf = Path(urdf_path).expanduser().resolve() + if not urdf.is_file(): + raise FileNotFoundError(f"gravity compensation URDF does not exist: {urdf}") + + package_dirs = [str(urdf.parent.parent)] + self._robot = pin.RobotWrapper.BuildFromURDF(str(urdf), package_dirs) + self._robot.data = self._robot.model.createData() + self._nq = 6 + + rot_world_base = _rpy_deg_to_rot(*base_rpy_deg) + gravity_world = np.array([0.0, 0.0, -9.81], dtype=np.float64) + gravity_base = rot_world_base.T @ gravity_world + self._robot.model.gravity.linear = gravity_base + + self._thread: threading.Thread | None = None + self._stop_event = threading.Event() + self._last_mode_refresh_t = 0.0 + + def start(self) -> None: + if self._thread is not None and self._thread.is_alive(): + return + self._stop_event.clear() + self._thread = threading.Thread( + target=self._run, + name="piper-gravity-comp-loop", + daemon=True, + ) + self._thread.start() + + def stop(self) -> None: + self._stop_event.set() + if self._thread is None: + return + self._thread.join(timeout=1.0) + if self._thread.is_alive(): + logger.warning("Piper gravity compensation thread did not stop within timeout.") + self._thread = None + + def _send_mit_mode(self) -> None: + # Use MOVE M + MIT to ensure torque pass-through mode is active. + self._arm.MotionCtrl_2(0x01, 0x04, self._move_speed_ratio, 0xAD) + self._last_mode_refresh_t = time.monotonic() + + def _refresh_mit_mode_if_needed(self) -> None: + if self._mode_refresh_interval_s <= 0.0: + return + now = time.monotonic() + if now - self._last_mode_refresh_t >= self._mode_refresh_interval_s: + self._send_mit_mode() + + def _read_q_v(self) -> tuple[np.ndarray, np.ndarray]: + joint_msg = self._arm.GetArmJointMsgs() + joint_state = joint_msg.joint_state + q_deg = np.array( + [ + float(joint_state.joint_1) * 1e-3, + float(joint_state.joint_2) * 1e-3, + float(joint_state.joint_3) * 1e-3, + float(joint_state.joint_4) * 1e-3, + float(joint_state.joint_5) * 1e-3, + float(joint_state.joint_6) * 1e-3, + ], + dtype=np.float64, + ) + q_rad = np.deg2rad(q_deg) + + hs = self._arm.GetArmHighSpdInfoMsgs() + v_rad = np.array( + [ + float(hs.motor_1.motor_speed) * 1e-3, + float(hs.motor_2.motor_speed) * 1e-3, + float(hs.motor_3.motor_speed) * 1e-3, + float(hs.motor_4.motor_speed) * 1e-3, + float(hs.motor_5.motor_speed) * 1e-3, + float(hs.motor_6.motor_speed) * 1e-3, + ], + dtype=np.float64, + ) + return q_rad, v_rad + + def _compute_gravity_torque(self, q_rad: np.ndarray, v_rad: np.ndarray) -> np.ndarray: + q_full = np.zeros(self._robot.model.nq, dtype=np.float64) + v_full = np.zeros(self._robot.model.nv, dtype=np.float64) + q_full[: self._nq] = q_rad + v_full[: self._nq] = v_rad + tau_full = self._pin.rnea( + self._robot.model, + self._robot.data, + q_full, + v_full, + np.zeros(self._robot.model.nv, dtype=np.float64), + ) + return np.asarray(tau_full[: self._nq], dtype=np.float64) + + def _run(self) -> None: + self._send_mit_mode() + dt = 1.0 / self._control_hz + while not self._stop_event.is_set(): + start_t = time.perf_counter() + self._refresh_mit_mode_if_needed() + + q_rad, v_rad = self._read_q_v() + tau = self._compute_gravity_torque(q_rad, v_rad) + tau = np.clip(self._tx_ratio * tau, -self._torque_limit, self._torque_limit) + + for idx in range(6): + self._arm.JointMitCtrl( + idx + 1, + 0.0, + 0.0, + self._mit_kp, + self._mit_kd, + float(tau[idx]), + ) + + remain = dt - (time.perf_counter() - start_t) + if remain > 0: + time.sleep(remain) diff --git a/src/lerobot/teleoperators/piper_leader/piper_leader.py b/src/lerobot/teleoperators/piper_leader/piper_leader.py index 9c58c0f..15cbe60 100644 --- a/src/lerobot/teleoperators/piper_leader/piper_leader.py +++ b/src/lerobot/teleoperators/piper_leader/piper_leader.py @@ -17,6 +17,7 @@ import logging import time from functools import cached_property +from importlib import resources from typing import Any from lerobot.motors import MotorCalibration @@ -24,21 +25,26 @@ from lerobot.processor import RobotAction from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected from lerobot.utils.piper_sdk import ( PIPER_ACTION_KEYS, + guard_piper_ctrl_mode_on_connect, PIPER_JOINT_ACTION_KEYS, PIPER_JOINT_NAMES, get_piper_sdk, milli_to_unit, parse_piper_log_level, unit_to_milli, + wait_enable_piper, ) from lerobot.utils.utils import enter_pressed, move_cursor_up from ..teleoperator import Teleoperator -from .config_piper_leader import PiperLeaderConfig +from .config_piper_leader import PiperLeaderConfig, PiperXLeaderConfig +from .gravity_compensation import PiperGravityCompensationLoop logger = logging.getLogger(__name__) PIPER_CALIB_KEYS = list(PIPER_ACTION_KEYS) PIPER_CALIB_IDS = {key: idx for idx, key in enumerate(PIPER_CALIB_KEYS)} +DEFAULT_PIPER_GRAVITY_URDF = "assets/piper_description/urdf/piper_no_gripper_description.urdf" +DEFAULT_PIPERX_GRAVITY_URDF = "assets/piper_x_description/urdf/piper_x_description_no_gripper.urdf" class PiperLeader(Teleoperator): @@ -46,13 +52,15 @@ class PiperLeader(Teleoperator): config_class = PiperLeaderConfig name = "piper_leader" + gravity_comp_urdf_relpath = DEFAULT_PIPER_GRAVITY_URDF - def __init__(self, config: PiperLeaderConfig): + def __init__(self, config: PiperLeaderConfig | PiperXLeaderConfig): super().__init__(config) self.config = config self._is_connected = False self._manual_control_enabled: bool | None = None self._last_mode_refresh_t = 0.0 + self._gravity_comp_loop: PiperGravityCompensationLoop | None = None interface_cls, _ = get_piper_sdk() self.arm = interface_cls( @@ -79,19 +87,12 @@ class PiperLeader(Teleoperator): self.arm.ConnectPort() if self.config.startup_sleep_s > 0: time.sleep(self.config.startup_sleep_s) + guard_piper_ctrl_mode_on_connect(arm=self.arm, interface_name=self.config.port) self._is_connected = True # Recompute control mode on every fresh connection. self._manual_control_enabled = None try: - if self.config.set_leader_mode_on_connect: - # NOTE: - # - Piper teaching-input mode (0xFA) does not accept external JointCtrl/GripperCtrl commands. - # - For policy-sync and human-in-the-loop switching, the leader must remain commandable, - # so we configure it as motion-output mode (0xFC), same as the follower side. - # - Mode-role changes (e.g., 0xFA <-> 0xFC) may require a full power-cycle to take effect. - self.arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00) - time.sleep(0.05) self.configure() if ( not self.is_calibrated @@ -171,26 +172,74 @@ class PiperLeader(Teleoperator): self._send_command_mode() def _wait_enable(self, timeout_s: float) -> bool: - deadline = time.monotonic() + timeout_s - while time.monotonic() < deadline: - if bool(self.arm.EnablePiper()): - return True - time.sleep(0.02) - return False + return wait_enable_piper(self.arm, timeout_s) + + def _send_gripper_ctrl(self, gripper_pos_raw: int, enabled: bool) -> None: + self.arm.GripperCtrl( + gripper_pos_raw, + self.config.gripper_effort_default if enabled else 0, + self.config.gripper_status_code if enabled else 0x00, + 0x00, + ) + + def _set_gripper_enabled(self, enabled: bool) -> None: + gripper_pos_raw = 0 + try: + gripper_msg = self.arm.GetArmGripperMsgs() + gripper_state = getattr(gripper_msg, "gripper_state", None) + if gripper_state is not None: + gripper_pos_raw = abs(int(getattr(gripper_state, "grippers_angle", 0))) + except Exception: + logger.debug("Could not read current gripper angle before setting enable=%s.", enabled) + self._send_gripper_ctrl(gripper_pos_raw, enabled) def set_manual_control(self, enabled: bool) -> None: if not self._is_connected: return if enabled and self._manual_control_enabled is not True: - self.arm.DisableArm(7) + if not self._wait_enable(self.config.enable_timeout_s): + logger.warning("Piper leader did not report enabled state before entering gravity compensation.") + if self.config.sync_gripper: + self._set_gripper_enabled(False) + self._ensure_gravity_comp_loop().start() self._manual_control_enabled = True return if not enabled and self._manual_control_enabled is not False: + self._stop_gravity_comp_loop_if_needed() self._send_command_mode() if not self._wait_enable(self.config.enable_timeout_s): logger.warning("Piper leader did not report enabled state before timeout.") + if self.config.sync_gripper: + self._set_gripper_enabled(True) self._manual_control_enabled = False + def _ensure_gravity_comp_loop(self) -> PiperGravityCompensationLoop: + default_urdf = resources.files("lerobot").joinpath(self.gravity_comp_urdf_relpath) + if not default_urdf.is_file(): + raise FileNotFoundError( + "Bundled gravity compensation URDF is missing: " + f"{self.gravity_comp_urdf_relpath}. Reinstall the package." + ) + urdf_path = str(default_urdf) + if self._gravity_comp_loop is None: + self._gravity_comp_loop = PiperGravityCompensationLoop( + arm=self.arm, + urdf_path=urdf_path, + control_hz=self.config.gravity_comp_control_hz, + tx_ratio=self.config.gravity_comp_tx_ratio, + torque_limit=self.config.gravity_comp_torque_limit, + mit_kp=self.config.gravity_comp_mit_kp, + mit_kd=self.config.gravity_comp_mit_kd, + base_rpy_deg=self.config.gravity_comp_base_rpy_deg, + mode_refresh_interval_s=self.config.mode_refresh_interval_s, + move_speed_ratio=self.config.command_speed_ratio, + ) + return self._gravity_comp_loop + + def _stop_gravity_comp_loop_if_needed(self) -> None: + if self._gravity_comp_loop is not None: + self._gravity_comp_loop.stop() + def configure(self) -> None: self.set_manual_control(self.config.manual_control) @@ -297,7 +346,7 @@ class PiperLeader(Teleoperator): def get_action(self) -> RobotAction: if not self.is_calibrated and not self._use_uncalibrated_passthrough(): raise RuntimeError( - f"{self} is not calibrated. Run `lerobot-calibrate --teleop.type=piper_leader --teleop.id={self.id}` first." + f"{self} is not calibrated. Run `lerobot-calibrate --teleop.type={self.config.type} --teleop.id={self.id}` first." ) raw_action = self._read_raw_action() @@ -318,7 +367,7 @@ class PiperLeader(Teleoperator): def send_feedback(self, feedback: dict[str, Any]) -> None: if not self.is_calibrated and not self._use_uncalibrated_passthrough(): raise RuntimeError( - f"{self} is not calibrated. Run `lerobot-calibrate --teleop.type=piper_leader --teleop.id={self.id}` first." + f"{self} is not calibrated. Run `lerobot-calibrate --teleop.type={self.config.type} --teleop.id={self.id}` first." ) self.set_manual_control(False) @@ -340,16 +389,12 @@ class PiperLeader(Teleoperator): else: gripper_target = self._offset_to_calibrated("gripper.pos", feedback["gripper.pos"]) gripper_pos_raw = unit_to_milli(gripper_target) - self.arm.GripperCtrl( - gripper_pos_raw, - self.config.gripper_effort_default, - self.config.gripper_status_code, - 0x00, - ) + self._send_gripper_ctrl(gripper_pos_raw, enabled=True) @check_if_not_connected def disconnect(self) -> None: try: + self._stop_gravity_comp_loop_if_needed() if self.config.disable_on_disconnect: self.arm.DisableArm(7) finally: @@ -357,3 +402,9 @@ class PiperLeader(Teleoperator): self._is_connected = False self._manual_control_enabled = None logger.info("%s disconnected.", self) + + +class PiperXLeader(PiperLeader): + config_class = PiperXLeaderConfig + name = "piperx_leader" + gravity_comp_urdf_relpath = DEFAULT_PIPERX_GRAVITY_URDF diff --git a/src/lerobot/teleoperators/utils.py b/src/lerobot/teleoperators/utils.py index e51a86b..4797199 100644 --- a/src/lerobot/teleoperators/utils.py +++ b/src/lerobot/teleoperators/utils.py @@ -95,6 +95,10 @@ def make_teleoperator_from_config(config: TeleoperatorConfig) -> "Teleoperator": from .piper_leader import PiperLeader return PiperLeader(config) + elif config.type == "piperx_leader": + from .piper_leader import PiperXLeader + + return PiperXLeader(config) elif config.type == "bi_openarm_leader": from .bi_openarm_leader import BiOpenArmLeader diff --git a/src/lerobot/utils/piper_sdk.py b/src/lerobot/utils/piper_sdk.py index f393840..1ea1426 100644 --- a/src/lerobot/utils/piper_sdk.py +++ b/src/lerobot/utils/piper_sdk.py @@ -16,6 +16,7 @@ from __future__ import annotations +import time from functools import lru_cache from typing import Any @@ -29,7 +30,8 @@ PIPER_JOINT_NAMES = ( ) PIPER_JOINT_ACTION_KEYS = tuple(f"{joint}.pos" for joint in PIPER_JOINT_NAMES) PIPER_ACTION_KEYS = PIPER_JOINT_ACTION_KEYS + ("gripper.pos",) - +PIPER_CTRL_MODE_TEACH = 0x02 +PIPER_CTRL_MODE_LINKAGE_TEACH_INPUT = 0x06 def milli_to_unit(value: float | int) -> float: @@ -64,3 +66,64 @@ def parse_piper_log_level(level_name: str) -> Any: ) from exc +def wait_enable_piper(arm: Any, timeout_s: float, retry_interval_s: float = 0.2) -> bool: + deadline = time.monotonic() + max(0.0, timeout_s) + interval_s = max(0.01, retry_interval_s) + while time.monotonic() < deadline: + if bool(arm.EnablePiper()): + return True + remaining_s = deadline - time.monotonic() + if remaining_s <= 0: + break + time.sleep(min(interval_s, remaining_s)) + return False + + +def _piper_mode_to_int(value: Any) -> int | None: + if isinstance(value, bool): + return int(value) + if isinstance(value, int): + return value + if hasattr(value, "value") and isinstance(value.value, int): + return int(value.value) + if hasattr(value, "__int__"): + return int(value) + return None + + +def read_piper_ctrl_mode(arm: Any, timeout_s: float = 1.0, poll_s: float = 0.02) -> int | None: + deadline = time.monotonic() + max(0.0, timeout_s) + while time.monotonic() < deadline: + status_msg = arm.GetArmStatus() + if getattr(status_msg, "time_stamp", 0.0) > 0.0: + arm_status = getattr(status_msg, "arm_status", None) + if arm_status is not None: + mode = _piper_mode_to_int(getattr(arm_status, "ctrl_mode", None)) + if mode is not None: + return mode + time.sleep(max(0.005, poll_s)) + return None + + +def guard_piper_ctrl_mode_on_connect( + arm: Any, + *, + interface_name: str, + timeout_s: float = 0.5, + poll_s: float = 0.02, + settle_s: float = 0.05, +) -> None: + mode = read_piper_ctrl_mode(arm, timeout_s=timeout_s, poll_s=poll_s) + if mode is None: + raise RuntimeError( + f"[{interface_name}] could not read arm ctrl_mode within {timeout_s:.2f}s. " + "Check CAN wiring/power and rerun." + ) + if mode in {PIPER_CTRL_MODE_TEACH, PIPER_CTRL_MODE_LINKAGE_TEACH_INPUT}: + arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00) + if settle_s > 0: + time.sleep(settle_s) + raise RuntimeError( + f"[{interface_name}] arm is in master/teaching role (ctrl_mode=0x{mode:02X}). " + "Follower role command (0xFC) has been sent. Power-cycle this arm, then retry." + ) diff --git a/tests/test_piper_teleop.py b/tests/test_piper_teleop.py index a288570..eaa450c 100644 --- a/tests/test_piper_teleop.py +++ b/tests/test_piper_teleop.py @@ -1,4 +1,6 @@ # ruff: noqa: N802 +import sys +import time from types import SimpleNamespace import pytest @@ -7,9 +9,9 @@ import lerobot.robots.piper_follower.piper_follower as piper_follower_module import lerobot.teleoperators.piper_leader.piper_leader as piper_leader_module import lerobot.utils.piper_sdk as piper_sdk_utils from lerobot.motors import MotorCalibration -from lerobot.robots.piper_follower import PiperFollower, PiperFollowerConfig +from lerobot.robots.piper_follower import PiperFollower, PiperFollowerConfig, PiperXFollower, PiperXFollowerConfig from lerobot.robots.utils import make_robot_from_config -from lerobot.teleoperators.piper_leader import PiperLeader, PiperLeaderConfig +from lerobot.teleoperators.piper_leader import PiperLeader, PiperLeaderConfig, PiperXLeader, PiperXLeaderConfig from lerobot.teleoperators.utils import make_teleoperator_from_config from lerobot.utils.piper_sdk import PIPER_ACTION_KEYS @@ -34,6 +36,8 @@ class FakePiperInterface: self.role_commands = [] self.last_joint = None self.last_gripper = None + self.gripper_calls = [] + self.joint_mit_calls = [] self.enable_calls = 0 self.disable_calls = 0 self.is_enabled = False @@ -72,6 +76,21 @@ class FakePiperInterface: Hz=120.0, gripper_state=SimpleNamespace(grippers_angle=43000, grippers_effort=1400, status_code=0x01), ) + self._high_spd = SimpleNamespace( + time_stamp=1.0, + Hz=120.0, + motor_1=SimpleNamespace(motor_speed=0), + motor_2=SimpleNamespace(motor_speed=0), + motor_3=SimpleNamespace(motor_speed=0), + motor_4=SimpleNamespace(motor_speed=0), + motor_5=SimpleNamespace(motor_speed=0), + motor_6=SimpleNamespace(motor_speed=0), + ) + self._arm_status = SimpleNamespace( + time_stamp=1.0, + Hz=120.0, + arm_status=SimpleNamespace(ctrl_mode=0x01), + ) def ConnectPort(self): self.connected = True @@ -85,6 +104,8 @@ class FakePiperInterface: def MasterSlaveConfig(self, *args): self.role_commands.append(args) + if args and args[0] == 0xFC: + self._arm_status.arm_status.ctrl_mode = 0x01 def EnablePiper(self): self.enable_calls += 1 @@ -99,8 +120,12 @@ class FakePiperInterface: def JointCtrl(self, *args): self.last_joint = args + def JointMitCtrl(self, *args): + self.joint_mit_calls.append(args) + def GripperCtrl(self, *args): self.last_gripper = args + self.gripper_calls.append(args) def GetArmJointCtrl(self): return self._joint_ctrl @@ -114,6 +139,12 @@ class FakePiperInterface: def GetArmGripperMsgs(self): return self._gripper_state + def GetArmHighSpdInfoMsgs(self): + return self._high_spd + + def GetArmStatus(self): + return self._arm_status + def patch_fake_sdk(monkeypatch): def fake_loader(): @@ -137,18 +168,25 @@ def make_identity_calibration(): } +def wait_until(predicate, timeout_s: float = 0.2, poll_s: float = 0.005) -> bool: + deadline = time.monotonic() + timeout_s + while time.monotonic() < deadline: + if predicate(): + return True + time.sleep(poll_s) + return False + + def test_piper_leader_follower_teleop_roundtrip(monkeypatch): patch_fake_sdk(monkeypatch) teleop_cfg = PiperLeaderConfig( port="can1", - set_leader_mode_on_connect=True, manual_control=False, sync_gripper=True, ) robot_cfg = PiperFollowerConfig( port="can0", - set_follower_mode_on_connect=True, sync_gripper=True, ) @@ -193,6 +231,60 @@ def test_piper_leader_follower_teleop_roundtrip(monkeypatch): robot.disconnect() +def test_piperx_leader_follower_teleop_roundtrip(monkeypatch): + patch_fake_sdk(monkeypatch) + + teleop_cfg = PiperXLeaderConfig( + port="can1", + manual_control=False, + sync_gripper=True, + ) + robot_cfg = PiperXFollowerConfig( + port="can0", + sync_gripper=True, + ) + + teleop = make_teleoperator_from_config(teleop_cfg) + robot = make_robot_from_config(robot_cfg) + + assert isinstance(teleop, PiperXLeader) + assert isinstance(robot, PiperXFollower) + + teleop.calibration = make_identity_calibration() + robot.calibration = make_identity_calibration() + + teleop.connect(calibrate=False) + robot.connect(calibrate=False) + try: + action = teleop.get_action() + sent = robot.send_action(action) + obs = robot.get_observation() + + assert robot.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000) + assert robot.arm.last_gripper == ( + 42000, + robot_cfg.gripper_effort_default, + robot_cfg.gripper_status_code, + 0x00, + ) + assert sent["joint_1.pos"] == 10.0 + assert sent["gripper.pos"] == 42.0 + assert obs["joint_1.pos"] == 11.0 + assert obs["gripper.pos"] == 43.0 + + teleop.send_feedback(action) + assert teleop.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000) + assert teleop.arm.last_gripper == ( + 42000, + teleop_cfg.gripper_effort_default, + teleop_cfg.gripper_status_code, + 0x00, + ) + finally: + teleop.disconnect() + robot.disconnect() + + def test_piper_requires_calibration(monkeypatch): patch_fake_sdk(monkeypatch) @@ -269,11 +361,11 @@ def test_piper_follower_connect_rolls_back_connected_cameras(monkeypatch): assert not robot.is_connected -def test_piper_calibration_mode_off_allows_uncalibrated_control(monkeypatch): +def test_piper_require_calibration_false_allows_uncalibrated_control(monkeypatch): patch_fake_sdk(monkeypatch) - teleop = PiperLeader(PiperLeaderConfig(port="can1", calibration_mode="off")) - robot = PiperFollower(PiperFollowerConfig(port="can0", calibration_mode="off")) + teleop = PiperLeader(PiperLeaderConfig(port="can1", require_calibration=False)) + robot = PiperFollower(PiperFollowerConfig(port="can0", require_calibration=False)) teleop.connect(calibrate=False) robot.connect(calibrate=False) @@ -295,7 +387,7 @@ def test_piper_follower_connect_calibrates_then_reenables(monkeypatch, tmp_path) port="can0", id="connect_reenable_after_calibration", calibration_dir=tmp_path, - calibration_mode="required", + require_calibration=True, enable_on_connect=True, ) ) @@ -324,7 +416,7 @@ def test_piper_follower_connect_without_calibration_still_enables(monkeypatch, t port="can0", id="connect_enable_without_calibration", calibration_dir=tmp_path, - calibration_mode="required", + require_calibration=True, enable_on_connect=True, ) ) @@ -336,3 +428,77 @@ def test_piper_follower_connect_without_calibration_still_enables(monkeypatch, t assert robot.arm.disable_calls == 0 finally: robot.disconnect() + + +def test_piper_leader_gravity_comp_manual_control_uses_mit(monkeypatch): + patch_fake_sdk(monkeypatch) + + class FakeModel: + def __init__(self): + self.nq = 6 + self.nv = 6 + self.gravity = SimpleNamespace(linear=None) + + def createData(self): + return object() + + class FakeRobotWrapper: + @staticmethod + def BuildFromURDF(urdf_path, package_dirs): + del urdf_path, package_dirs + return SimpleNamespace(model=FakeModel(), data=object()) + + class FakePinocchio: + RobotWrapper = FakeRobotWrapper + + @staticmethod + def rnea(model, data, q, v, a): + del model, data, q, v, a + return [0.1] * 6 + + monkeypatch.setitem(sys.modules, "pinocchio", FakePinocchio) + + teleop = PiperLeader( + PiperLeaderConfig( + port="can1", + manual_control=True, + gravity_comp_control_hz=200.0, + mode_refresh_interval_s=0.01, + ) + ) + teleop.calibration = make_identity_calibration() + + teleop.connect(calibrate=False) + try: + assert wait_until(lambda: len(teleop.arm.joint_mit_calls) > 0) + assert teleop.arm.disable_calls == 0 + assert len(teleop.arm.joint_mit_calls) > 0 + assert len(teleop.arm.gripper_calls) > 0 + assert teleop.arm.gripper_calls[0][2] == 0x00 + + teleop.set_manual_control(False) + calls_after_stop = len(teleop.arm.joint_mit_calls) + time.sleep(0.03) + assert len(teleop.arm.joint_mit_calls) == calls_after_stop + assert teleop.arm.gripper_calls[-1][2] == 0x01 + finally: + teleop.disconnect() + + +@pytest.mark.parametrize( + ("device_kind", "port", "robot_id"), + [("leader", "can1", None), ("follower", "can0", "piper_role_guard")], +) +def test_piper_connect_fails_and_writes_follower_role_when_in_teach_mode(monkeypatch, device_kind, port, robot_id): + patch_fake_sdk(monkeypatch) + + if device_kind == "leader": + device = PiperLeader(PiperLeaderConfig(port=port)) + else: + device = PiperFollower(PiperFollowerConfig(port=port, id=robot_id)) + device.arm._arm_status.arm_status.ctrl_mode = 0x06 + + with pytest.raises(RuntimeError, match="Follower role command .* sent.*Power-cycle"): + device.connect(calibrate=False) + + assert device.arm.role_commands[-1] == (0xFC, 0x00, 0x00, 0x00) From 8934fdbabf2293c1d0cb2777517f8ac9cf4406bd Mon Sep 17 00:00:00 2001 From: Elvin <223015152@link.cuhk.edu.cn> Date: Sat, 7 Mar 2026 01:34:17 +0800 Subject: [PATCH 16/26] fix(piper): guard LFS pointer URDF and normalize asset file modes - detect Git LFS pointer URDF early and raise actionable runtime error - add regression test for LFS-pointer URDF guard - normalize PiPER/PiPER-X asset modes from 100755 to 100644 --- .../piper_description/meshes/base_link.STL | 0 .../assets/piper_description/meshes/link1.STL | 0 .../assets/piper_description/meshes/link2.STL | 0 .../assets/piper_description/meshes/link3.STL | 0 .../assets/piper_description/meshes/link4.STL | 0 .../assets/piper_description/meshes/link5.STL | 0 .../assets/piper_description/meshes/link6.STL | 0 .../urdf/piper_no_gripper_description.urdf | 0 .../assets/piper_x_description/meshes/Link6.STL | 0 .../assets/piper_x_description/meshes/dae/J1.dae | 0 .../assets/piper_x_description/meshes/dae/J2.dae | 0 .../assets/piper_x_description/meshes/dae/J3.dae | 0 .../assets/piper_x_description/meshes/dae/J4.dae | 0 .../assets/piper_x_description/meshes/dae/J5.dae | 0 .../urdf/piper_x_description_no_gripper.urdf | 0 .../teleoperators/piper_leader/piper_leader.py | 16 ++++++++++++++++ tests/test_piper_teleop.py | 12 ++++++++++++ 17 files changed, 28 insertions(+) mode change 100755 => 100644 src/lerobot/assets/piper_description/meshes/base_link.STL mode change 100755 => 100644 src/lerobot/assets/piper_description/meshes/link1.STL mode change 100755 => 100644 src/lerobot/assets/piper_description/meshes/link2.STL mode change 100755 => 100644 src/lerobot/assets/piper_description/meshes/link3.STL mode change 100755 => 100644 src/lerobot/assets/piper_description/meshes/link4.STL mode change 100755 => 100644 src/lerobot/assets/piper_description/meshes/link5.STL mode change 100755 => 100644 src/lerobot/assets/piper_description/meshes/link6.STL mode change 100755 => 100644 src/lerobot/assets/piper_description/urdf/piper_no_gripper_description.urdf mode change 100755 => 100644 src/lerobot/assets/piper_x_description/meshes/Link6.STL mode change 100755 => 100644 src/lerobot/assets/piper_x_description/meshes/dae/J1.dae mode change 100755 => 100644 src/lerobot/assets/piper_x_description/meshes/dae/J2.dae mode change 100755 => 100644 src/lerobot/assets/piper_x_description/meshes/dae/J3.dae mode change 100755 => 100644 src/lerobot/assets/piper_x_description/meshes/dae/J4.dae mode change 100755 => 100644 src/lerobot/assets/piper_x_description/meshes/dae/J5.dae mode change 100755 => 100644 src/lerobot/assets/piper_x_description/urdf/piper_x_description_no_gripper.urdf diff --git a/src/lerobot/assets/piper_description/meshes/base_link.STL b/src/lerobot/assets/piper_description/meshes/base_link.STL old mode 100755 new mode 100644 diff --git a/src/lerobot/assets/piper_description/meshes/link1.STL b/src/lerobot/assets/piper_description/meshes/link1.STL old mode 100755 new mode 100644 diff --git a/src/lerobot/assets/piper_description/meshes/link2.STL b/src/lerobot/assets/piper_description/meshes/link2.STL old mode 100755 new mode 100644 diff --git a/src/lerobot/assets/piper_description/meshes/link3.STL b/src/lerobot/assets/piper_description/meshes/link3.STL old mode 100755 new mode 100644 diff --git a/src/lerobot/assets/piper_description/meshes/link4.STL b/src/lerobot/assets/piper_description/meshes/link4.STL old mode 100755 new mode 100644 diff --git a/src/lerobot/assets/piper_description/meshes/link5.STL b/src/lerobot/assets/piper_description/meshes/link5.STL old mode 100755 new mode 100644 diff --git a/src/lerobot/assets/piper_description/meshes/link6.STL b/src/lerobot/assets/piper_description/meshes/link6.STL old mode 100755 new mode 100644 diff --git a/src/lerobot/assets/piper_description/urdf/piper_no_gripper_description.urdf b/src/lerobot/assets/piper_description/urdf/piper_no_gripper_description.urdf old mode 100755 new mode 100644 diff --git a/src/lerobot/assets/piper_x_description/meshes/Link6.STL b/src/lerobot/assets/piper_x_description/meshes/Link6.STL old mode 100755 new mode 100644 diff --git a/src/lerobot/assets/piper_x_description/meshes/dae/J1.dae b/src/lerobot/assets/piper_x_description/meshes/dae/J1.dae old mode 100755 new mode 100644 diff --git a/src/lerobot/assets/piper_x_description/meshes/dae/J2.dae b/src/lerobot/assets/piper_x_description/meshes/dae/J2.dae old mode 100755 new mode 100644 diff --git a/src/lerobot/assets/piper_x_description/meshes/dae/J3.dae b/src/lerobot/assets/piper_x_description/meshes/dae/J3.dae old mode 100755 new mode 100644 diff --git a/src/lerobot/assets/piper_x_description/meshes/dae/J4.dae b/src/lerobot/assets/piper_x_description/meshes/dae/J4.dae old mode 100755 new mode 100644 diff --git a/src/lerobot/assets/piper_x_description/meshes/dae/J5.dae b/src/lerobot/assets/piper_x_description/meshes/dae/J5.dae old mode 100755 new mode 100644 diff --git a/src/lerobot/assets/piper_x_description/urdf/piper_x_description_no_gripper.urdf b/src/lerobot/assets/piper_x_description/urdf/piper_x_description_no_gripper.urdf old mode 100755 new mode 100644 diff --git a/src/lerobot/teleoperators/piper_leader/piper_leader.py b/src/lerobot/teleoperators/piper_leader/piper_leader.py index 15cbe60..2520e29 100644 --- a/src/lerobot/teleoperators/piper_leader/piper_leader.py +++ b/src/lerobot/teleoperators/piper_leader/piper_leader.py @@ -45,6 +45,21 @@ PIPER_CALIB_KEYS = list(PIPER_ACTION_KEYS) PIPER_CALIB_IDS = {key: idx for idx, key in enumerate(PIPER_CALIB_KEYS)} DEFAULT_PIPER_GRAVITY_URDF = "assets/piper_description/urdf/piper_no_gripper_description.urdf" DEFAULT_PIPERX_GRAVITY_URDF = "assets/piper_x_description/urdf/piper_x_description_no_gripper.urdf" +GIT_LFS_POINTER_PREFIX = b"version https://git-lfs.github.com/spec/v1" + + +def _ensure_not_lfs_pointer(path_obj: Any, relpath: str) -> None: + with path_obj.open("rb") as f: + head = f.read(128) + if head.startswith(GIT_LFS_POINTER_PREFIX): + raise RuntimeError( + "Bundled PiPER assets are still Git LFS pointer files. " + "Please run `git lfs pull --include=" + "\"src/lerobot/assets/piper_description/**,src/lerobot/assets/piper_x_description/**\" " + "--exclude=\"*\"` and `git lfs checkout src/lerobot/assets/piper_description " + "src/lerobot/assets/piper_x_description`, then retry. " + f"(file: {relpath})" + ) class PiperLeader(Teleoperator): @@ -220,6 +235,7 @@ class PiperLeader(Teleoperator): "Bundled gravity compensation URDF is missing: " f"{self.gravity_comp_urdf_relpath}. Reinstall the package." ) + _ensure_not_lfs_pointer(default_urdf, self.gravity_comp_urdf_relpath) urdf_path = str(default_urdf) if self._gravity_comp_loop is None: self._gravity_comp_loop = PiperGravityCompensationLoop( diff --git a/tests/test_piper_teleop.py b/tests/test_piper_teleop.py index eaa450c..d71a3d7 100644 --- a/tests/test_piper_teleop.py +++ b/tests/test_piper_teleop.py @@ -502,3 +502,15 @@ def test_piper_connect_fails_and_writes_follower_role_when_in_teach_mode(monkeyp device.connect(calibrate=False) assert device.arm.role_commands[-1] == (0xFC, 0x00, 0x00, 0x00) + + +def test_piper_lfs_pointer_urdf_raises_actionable_error(tmp_path): + pointer_file = tmp_path / "lfs_pointer.urdf" + pointer_file.write_text( + "version https://git-lfs.github.com/spec/v1\n" + "oid sha256:deadbeef\n" + "size 123\n" + ) + + with pytest.raises(RuntimeError, match="Git LFS pointer files"): + piper_leader_module._ensure_not_lfs_pointer(pointer_file, "assets/piper_description/urdf/pointer.urdf") From f68f8eb543b8e49220c6ee12e971fc48d9bdbfc9 Mon Sep 17 00:00:00 2001 From: Elvin <223015152@link.cuhk.edu.cn> Date: Sat, 7 Mar 2026 01:50:26 +0800 Subject: [PATCH 17/26] refactor(piper): simplify teleop tests and gravity-comp init - parametrize piper/piperx roundtrip teleop test to remove duplication - stop gravity-comp loop before disconnect on connect rollback - avoid repeating URDF/LFS checks after loop initialization --- .../piper_leader/piper_leader.py | 17 ++-- tests/test_piper_teleop.py | 87 +++++-------------- 2 files changed, 29 insertions(+), 75 deletions(-) diff --git a/src/lerobot/teleoperators/piper_leader/piper_leader.py b/src/lerobot/teleoperators/piper_leader/piper_leader.py index 2520e29..d6d5740 100644 --- a/src/lerobot/teleoperators/piper_leader/piper_leader.py +++ b/src/lerobot/teleoperators/piper_leader/piper_leader.py @@ -120,6 +120,7 @@ class PiperLeader(Teleoperator): ) self.calibrate() except Exception: + self._stop_gravity_comp_loop_if_needed() self.arm.DisconnectPort() self._is_connected = False raise @@ -229,15 +230,15 @@ class PiperLeader(Teleoperator): self._manual_control_enabled = False def _ensure_gravity_comp_loop(self) -> PiperGravityCompensationLoop: - default_urdf = resources.files("lerobot").joinpath(self.gravity_comp_urdf_relpath) - if not default_urdf.is_file(): - raise FileNotFoundError( - "Bundled gravity compensation URDF is missing: " - f"{self.gravity_comp_urdf_relpath}. Reinstall the package." - ) - _ensure_not_lfs_pointer(default_urdf, self.gravity_comp_urdf_relpath) - urdf_path = str(default_urdf) if self._gravity_comp_loop is None: + default_urdf = resources.files("lerobot").joinpath(self.gravity_comp_urdf_relpath) + if not default_urdf.is_file(): + raise FileNotFoundError( + "Bundled gravity compensation URDF is missing: " + f"{self.gravity_comp_urdf_relpath}. Reinstall the package." + ) + _ensure_not_lfs_pointer(default_urdf, self.gravity_comp_urdf_relpath) + urdf_path = str(default_urdf) self._gravity_comp_loop = PiperGravityCompensationLoop( arm=self.arm, urdf_path=urdf_path, diff --git a/tests/test_piper_teleop.py b/tests/test_piper_teleop.py index d71a3d7..a8734e0 100644 --- a/tests/test_piper_teleop.py +++ b/tests/test_piper_teleop.py @@ -177,78 +177,31 @@ def wait_until(predicate, timeout_s: float = 0.2, poll_s: float = 0.005) -> bool return False -def test_piper_leader_follower_teleop_roundtrip(monkeypatch): +@pytest.mark.parametrize( + ("teleop_cfg", "robot_cfg", "teleop_cls", "robot_cls"), + [ + ( + PiperLeaderConfig(port="can1", manual_control=False, sync_gripper=True), + PiperFollowerConfig(port="can0", sync_gripper=True), + PiperLeader, + PiperFollower, + ), + ( + PiperXLeaderConfig(port="can1", manual_control=False, sync_gripper=True), + PiperXFollowerConfig(port="can0", sync_gripper=True), + PiperXLeader, + PiperXFollower, + ), + ], +) +def test_piper_leader_follower_teleop_roundtrip(monkeypatch, teleop_cfg, robot_cfg, teleop_cls, robot_cls): patch_fake_sdk(monkeypatch) - teleop_cfg = PiperLeaderConfig( - port="can1", - manual_control=False, - sync_gripper=True, - ) - robot_cfg = PiperFollowerConfig( - port="can0", - sync_gripper=True, - ) - teleop = make_teleoperator_from_config(teleop_cfg) robot = make_robot_from_config(robot_cfg) - assert isinstance(teleop, PiperLeader) - assert isinstance(robot, PiperFollower) - - teleop.calibration = make_identity_calibration() - robot.calibration = make_identity_calibration() - - teleop.connect(calibrate=False) - robot.connect(calibrate=False) - try: - action = teleop.get_action() - sent = robot.send_action(action) - obs = robot.get_observation() - - assert robot.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000) - assert robot.arm.last_gripper == ( - 42000, - robot_cfg.gripper_effort_default, - robot_cfg.gripper_status_code, - 0x00, - ) - assert sent["joint_1.pos"] == 10.0 - assert sent["gripper.pos"] == 42.0 - assert obs["joint_1.pos"] == 11.0 - assert obs["gripper.pos"] == 43.0 - - teleop.send_feedback(action) - assert teleop.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000) - assert teleop.arm.last_gripper == ( - 42000, - teleop_cfg.gripper_effort_default, - teleop_cfg.gripper_status_code, - 0x00, - ) - finally: - teleop.disconnect() - robot.disconnect() - - -def test_piperx_leader_follower_teleop_roundtrip(monkeypatch): - patch_fake_sdk(monkeypatch) - - teleop_cfg = PiperXLeaderConfig( - port="can1", - manual_control=False, - sync_gripper=True, - ) - robot_cfg = PiperXFollowerConfig( - port="can0", - sync_gripper=True, - ) - - teleop = make_teleoperator_from_config(teleop_cfg) - robot = make_robot_from_config(robot_cfg) - - assert isinstance(teleop, PiperXLeader) - assert isinstance(robot, PiperXFollower) + assert isinstance(teleop, teleop_cls) + assert isinstance(robot, robot_cls) teleop.calibration = make_identity_calibration() robot.calibration = make_identity_calibration() From 20ce8ca6515e747d8bbb449967fc73e99b330d5e Mon Sep 17 00:00:00 2001 From: Elvin <223015152@link.cuhk.edu.cn> Date: Sat, 7 Mar 2026 01:56:21 +0800 Subject: [PATCH 18/26] Revert "test(value): skip viz tests when scipy is unavailable" This reverts commit fe9869c2bd7a6d494509b12d5c2cbe7429233ed1. --- tests/value/test_lerobot_value_infer_viz.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/tests/value/test_lerobot_value_infer_viz.py b/tests/value/test_lerobot_value_infer_viz.py index cae2de3..bb4bb46 100644 --- a/tests/value/test_lerobot_value_infer_viz.py +++ b/tests/value/test_lerobot_value_infer_viz.py @@ -3,12 +3,9 @@ from pathlib import Path import numpy as np -import pytest import torch from PIL import Image -pytest.importorskip("scipy") - from lerobot.configs.value import ( ValueInferenceACPConfig, ValueInferenceCheckpointConfig, From b69ffe3c02f5b40620b5de5580d4ef9dc29e6157 Mon Sep 17 00:00:00 2001 From: Elvin <223015152@link.cuhk.edu.cn> Date: Sat, 7 Mar 2026 02:09:58 +0800 Subject: [PATCH 19/26] fix(can): honor effective fd mode in speed_test --- src/lerobot/scripts/lerobot_setup_can.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lerobot/scripts/lerobot_setup_can.py b/src/lerobot/scripts/lerobot_setup_can.py index 53e62ae..78f63ba 100644 --- a/src/lerobot/scripts/lerobot_setup_can.py +++ b/src/lerobot/scripts/lerobot_setup_can.py @@ -266,7 +266,7 @@ def speed_test(cfg: CANSetupConfig, interface: str): responding_motor = None for motor_id in cfg.motor_ids: - responses, _ = test_motor(bus, motor_id, 0.5, cfg.use_fd) + responses, _ = test_motor(bus, motor_id, 0.5, effective_use_fd) if responses: responding_motor = motor_id break @@ -285,7 +285,7 @@ def speed_test(cfg: CANSetupConfig, interface: str): arbitration_id=responding_motor, data=[0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC], is_extended_id=False, - is_fd=cfg.use_fd, + is_fd=effective_use_fd, ) bus.send(msg) resp = bus.recv(timeout=0.1) From 1be38954e6afb4048319109bdfbb145aeed64b81 Mon Sep 17 00:00:00 2001 From: Elvin <223015152@link.cuhk.edu.cn> Date: Sat, 7 Mar 2026 02:21:09 +0800 Subject: [PATCH 20/26] fix(ci): make pre-commit quality checks pass --- .pre-commit-config.yaml | 2 ++ .../piper_follower/config_piper_follower.py | 1 - .../robots/piper_follower/piper_follower.py | 8 ++---- src/lerobot/scripts/lerobot_setup_can.py | 1 - .../piper_leader/piper_leader.py | 16 +++++------ tests/test_piper_teleop.py | 28 +++++++++++++------ 6 files changed, 30 insertions(+), 26 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index bfa3340..642c50d 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -36,7 +36,9 @@ repos: - id: check-yaml - id: check-toml - id: end-of-file-fixer + exclude: ^src/lerobot/assets/(piper_description|piper_x_description)/ - id: trailing-whitespace + exclude: ^src/lerobot/assets/(piper_description|piper_x_description)/ - repo: https://github.com/astral-sh/ruff-pre-commit rev: v0.14.1 diff --git a/src/lerobot/robots/piper_follower/config_piper_follower.py b/src/lerobot/robots/piper_follower/config_piper_follower.py index bac213d..7f97432 100644 --- a/src/lerobot/robots/piper_follower/config_piper_follower.py +++ b/src/lerobot/robots/piper_follower/config_piper_follower.py @@ -18,7 +18,6 @@ from dataclasses import dataclass, field from lerobot.cameras import CameraConfig - from ..config import RobotConfig diff --git a/src/lerobot/robots/piper_follower/piper_follower.py b/src/lerobot/robots/piper_follower/piper_follower.py index 4b2716e..35874bd 100644 --- a/src/lerobot/robots/piper_follower/piper_follower.py +++ b/src/lerobot/robots/piper_follower/piper_follower.py @@ -24,10 +24,10 @@ from lerobot.processor import RobotAction, RobotObservation from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected from lerobot.utils.piper_sdk import ( PIPER_ACTION_KEYS, - guard_piper_ctrl_mode_on_connect, PIPER_JOINT_ACTION_KEYS, PIPER_JOINT_NAMES, get_piper_sdk, + guard_piper_ctrl_mode_on_connect, milli_to_unit, parse_piper_log_level, unit_to_milli, @@ -93,11 +93,7 @@ class PiperFollower(Robot): connected_cameras = [] try: self.configure() - should_auto_calibrate = ( - not self.is_calibrated - and calibrate - and self.config.require_calibration - ) + should_auto_calibrate = not self.is_calibrated and calibrate and self.config.require_calibration if should_auto_calibrate: logger.info( "No piper-follower calibration file found for '%s'. Running lerobot-calibrate flow.", diff --git a/src/lerobot/scripts/lerobot_setup_can.py b/src/lerobot/scripts/lerobot_setup_can.py index 78f63ba..9637d35 100644 --- a/src/lerobot/scripts/lerobot_setup_can.py +++ b/src/lerobot/scripts/lerobot_setup_can.py @@ -141,7 +141,6 @@ def setup_interface_with_fallback(interface: str, cfg: CANSetupConfig) -> tuple[ return False, None, " | ".join(attempt_msgs) - def test_motor(bus, motor_id: int, timeout: float, use_fd: bool): """Test a single motor and return responses.""" import can diff --git a/src/lerobot/teleoperators/piper_leader/piper_leader.py b/src/lerobot/teleoperators/piper_leader/piper_leader.py index d6d5740..ffca58b 100644 --- a/src/lerobot/teleoperators/piper_leader/piper_leader.py +++ b/src/lerobot/teleoperators/piper_leader/piper_leader.py @@ -25,10 +25,10 @@ from lerobot.processor import RobotAction from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected from lerobot.utils.piper_sdk import ( PIPER_ACTION_KEYS, - guard_piper_ctrl_mode_on_connect, PIPER_JOINT_ACTION_KEYS, PIPER_JOINT_NAMES, get_piper_sdk, + guard_piper_ctrl_mode_on_connect, milli_to_unit, parse_piper_log_level, unit_to_milli, @@ -55,8 +55,8 @@ def _ensure_not_lfs_pointer(path_obj: Any, relpath: str) -> None: raise RuntimeError( "Bundled PiPER assets are still Git LFS pointer files. " "Please run `git lfs pull --include=" - "\"src/lerobot/assets/piper_description/**,src/lerobot/assets/piper_x_description/**\" " - "--exclude=\"*\"` and `git lfs checkout src/lerobot/assets/piper_description " + '"src/lerobot/assets/piper_description/**,src/lerobot/assets/piper_x_description/**" ' + '--exclude="*"` and `git lfs checkout src/lerobot/assets/piper_description ' "src/lerobot/assets/piper_x_description`, then retry. " f"(file: {relpath})" ) @@ -109,11 +109,7 @@ class PiperLeader(Teleoperator): self._manual_control_enabled = None try: self.configure() - if ( - not self.is_calibrated - and calibrate - and self.config.require_calibration - ): + if not self.is_calibrated and calibrate and self.config.require_calibration: logger.info( "No piper-leader calibration file found for '%s'. Running lerobot-calibrate flow.", self.id, @@ -214,7 +210,9 @@ class PiperLeader(Teleoperator): return if enabled and self._manual_control_enabled is not True: if not self._wait_enable(self.config.enable_timeout_s): - logger.warning("Piper leader did not report enabled state before entering gravity compensation.") + logger.warning( + "Piper leader did not report enabled state before entering gravity compensation." + ) if self.config.sync_gripper: self._set_gripper_enabled(False) self._ensure_gravity_comp_loop().start() diff --git a/tests/test_piper_teleop.py b/tests/test_piper_teleop.py index a8734e0..852313b 100644 --- a/tests/test_piper_teleop.py +++ b/tests/test_piper_teleop.py @@ -9,9 +9,19 @@ import lerobot.robots.piper_follower.piper_follower as piper_follower_module import lerobot.teleoperators.piper_leader.piper_leader as piper_leader_module import lerobot.utils.piper_sdk as piper_sdk_utils from lerobot.motors import MotorCalibration -from lerobot.robots.piper_follower import PiperFollower, PiperFollowerConfig, PiperXFollower, PiperXFollowerConfig +from lerobot.robots.piper_follower import ( + PiperFollower, + PiperFollowerConfig, + PiperXFollower, + PiperXFollowerConfig, +) from lerobot.robots.utils import make_robot_from_config -from lerobot.teleoperators.piper_leader import PiperLeader, PiperLeaderConfig, PiperXLeader, PiperXLeaderConfig +from lerobot.teleoperators.piper_leader import ( + PiperLeader, + PiperLeaderConfig, + PiperXLeader, + PiperXLeaderConfig, +) from lerobot.teleoperators.utils import make_teleoperator_from_config from lerobot.utils.piper_sdk import PIPER_ACTION_KEYS @@ -442,7 +452,9 @@ def test_piper_leader_gravity_comp_manual_control_uses_mit(monkeypatch): ("device_kind", "port", "robot_id"), [("leader", "can1", None), ("follower", "can0", "piper_role_guard")], ) -def test_piper_connect_fails_and_writes_follower_role_when_in_teach_mode(monkeypatch, device_kind, port, robot_id): +def test_piper_connect_fails_and_writes_follower_role_when_in_teach_mode( + monkeypatch, device_kind, port, robot_id +): patch_fake_sdk(monkeypatch) if device_kind == "leader": @@ -459,11 +471,9 @@ def test_piper_connect_fails_and_writes_follower_role_when_in_teach_mode(monkeyp def test_piper_lfs_pointer_urdf_raises_actionable_error(tmp_path): pointer_file = tmp_path / "lfs_pointer.urdf" - pointer_file.write_text( - "version https://git-lfs.github.com/spec/v1\n" - "oid sha256:deadbeef\n" - "size 123\n" - ) + pointer_file.write_text("version https://git-lfs.github.com/spec/v1\noid sha256:deadbeef\nsize 123\n") with pytest.raises(RuntimeError, match="Git LFS pointer files"): - piper_leader_module._ensure_not_lfs_pointer(pointer_file, "assets/piper_description/urdf/pointer.urdf") + piper_leader_module._ensure_not_lfs_pointer( + pointer_file, "assets/piper_description/urdf/pointer.urdf" + ) From 2190f5c2371efa556156aa6ba595637b5d363212 Mon Sep 17 00:00:00 2001 From: Elvin <223015152@link.cuhk.edu.cn> Date: Sat, 7 Mar 2026 11:40:42 +0800 Subject: [PATCH 21/26] fix(piper): robust ctrl_mode parsing for enum/string values --- src/lerobot/utils/piper_sdk.py | 70 +++++++++++++++++++++++++++------- 1 file changed, 57 insertions(+), 13 deletions(-) diff --git a/src/lerobot/utils/piper_sdk.py b/src/lerobot/utils/piper_sdk.py index 1ea1426..a6ae81b 100644 --- a/src/lerobot/utils/piper_sdk.py +++ b/src/lerobot/utils/piper_sdk.py @@ -16,7 +16,10 @@ from __future__ import annotations +import re import time +from dataclasses import dataclass +from enum import Enum from functools import lru_cache from typing import Any @@ -32,6 +35,13 @@ PIPER_JOINT_ACTION_KEYS = tuple(f"{joint}.pos" for joint in PIPER_JOINT_NAMES) PIPER_ACTION_KEYS = PIPER_JOINT_ACTION_KEYS + ("gripper.pos",) PIPER_CTRL_MODE_TEACH = 0x02 PIPER_CTRL_MODE_LINKAGE_TEACH_INPUT = 0x06 +_CTRL_MODE_HEX_RE = re.compile(r"0x([0-9a-fA-F]+)") + + +@dataclass(frozen=True) +class PiperCtrlMode: + code: int | None + text: str def milli_to_unit(value: float | int) -> float: @@ -79,27 +89,54 @@ def wait_enable_piper(arm: Any, timeout_s: float, retry_interval_s: float = 0.2) return False -def _piper_mode_to_int(value: Any) -> int | None: +def parse_piper_ctrl_mode(value: Any) -> PiperCtrlMode: + if value is None: + return PiperCtrlMode(code=None, text="None") + if isinstance(value, bool): - return int(value) + code = int(value) + return PiperCtrlMode(code=code, text=f"0x{code:02X}") + if isinstance(value, int): - return value - if hasattr(value, "value") and isinstance(value.value, int): - return int(value.value) - if hasattr(value, "__int__"): - return int(value) - return None + return PiperCtrlMode(code=value, text=f"0x{value:02X}") + + if isinstance(value, Enum): + enum_value = value.value + if isinstance(enum_value, int): + return PiperCtrlMode(code=enum_value, text=str(value)) + if isinstance(enum_value, str): + m = _CTRL_MODE_HEX_RE.search(enum_value) + if m: + return PiperCtrlMode(code=int(m.group(1), 16), text=str(value)) + if enum_value.isdigit(): + return PiperCtrlMode(code=int(enum_value), text=str(value)) + return PiperCtrlMode(code=None, text=str(value)) + + raw_value = getattr(value, "value", None) + if isinstance(raw_value, int): + return PiperCtrlMode(code=raw_value, text=str(value)) + + if isinstance(value, str): + m = _CTRL_MODE_HEX_RE.search(value) + if m: + return PiperCtrlMode(code=int(m.group(1), 16), text=value) + if value.isdigit(): + code = int(value) + return PiperCtrlMode(code=code, text=f"0x{code:02X}") + return PiperCtrlMode(code=None, text=value) + + return PiperCtrlMode(code=None, text=repr(value)) -def read_piper_ctrl_mode(arm: Any, timeout_s: float = 1.0, poll_s: float = 0.02) -> int | None: +def read_piper_ctrl_mode(arm: Any, timeout_s: float = 1.0, poll_s: float = 0.02) -> PiperCtrlMode | None: deadline = time.monotonic() + max(0.0, timeout_s) while time.monotonic() < deadline: status_msg = arm.GetArmStatus() if getattr(status_msg, "time_stamp", 0.0) > 0.0: arm_status = getattr(status_msg, "arm_status", None) if arm_status is not None: - mode = _piper_mode_to_int(getattr(arm_status, "ctrl_mode", None)) - if mode is not None: + mode = parse_piper_ctrl_mode(getattr(arm_status, "ctrl_mode", None)) + if mode.code is not None: return mode time.sleep(max(0.005, poll_s)) return None @@ -119,11 +156,18 @@ def guard_piper_ctrl_mode_on_connect( f"[{interface_name}] could not read arm ctrl_mode within {timeout_s:.2f}s. " "Check CAN wiring/power and rerun." ) - if mode in {PIPER_CTRL_MODE_TEACH, PIPER_CTRL_MODE_LINKAGE_TEACH_INPUT}: + + if mode.code is None: + raise RuntimeError( + f"[{interface_name}] got unreadable arm ctrl_mode value: {mode.text}. " + "Check SDK version/firmware and rerun." + ) + + if mode.code in {PIPER_CTRL_MODE_TEACH, PIPER_CTRL_MODE_LINKAGE_TEACH_INPUT}: arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00) if settle_s > 0: time.sleep(settle_s) raise RuntimeError( - f"[{interface_name}] arm is in master/teaching role (ctrl_mode=0x{mode:02X}). " + f"[{interface_name}] arm is in master/teaching role (ctrl_mode={mode.text}). " "Follower role command (0xFC) has been sent. Power-cycle this arm, then retry." ) From 5e1d1cc99978bc0490bbbf83b17ad212a890cf7f Mon Sep 17 00:00:00 2001 From: Elvin <223015152@link.cuhk.edu.cn> Date: Sat, 7 Mar 2026 11:44:14 +0800 Subject: [PATCH 22/26] Revert "fix(piper): robust ctrl_mode parsing for enum/string values" This reverts commit 2190f5c2371efa556156aa6ba595637b5d363212. --- src/lerobot/utils/piper_sdk.py | 70 +++++++--------------------------- 1 file changed, 13 insertions(+), 57 deletions(-) diff --git a/src/lerobot/utils/piper_sdk.py b/src/lerobot/utils/piper_sdk.py index a6ae81b..1ea1426 100644 --- a/src/lerobot/utils/piper_sdk.py +++ b/src/lerobot/utils/piper_sdk.py @@ -16,10 +16,7 @@ from __future__ import annotations -import re import time -from dataclasses import dataclass -from enum import Enum from functools import lru_cache from typing import Any @@ -35,13 +32,6 @@ PIPER_JOINT_ACTION_KEYS = tuple(f"{joint}.pos" for joint in PIPER_JOINT_NAMES) PIPER_ACTION_KEYS = PIPER_JOINT_ACTION_KEYS + ("gripper.pos",) PIPER_CTRL_MODE_TEACH = 0x02 PIPER_CTRL_MODE_LINKAGE_TEACH_INPUT = 0x06 -_CTRL_MODE_HEX_RE = re.compile(r"0x([0-9a-fA-F]+)") - - -@dataclass(frozen=True) -class PiperCtrlMode: - code: int | None - text: str def milli_to_unit(value: float | int) -> float: @@ -89,54 +79,27 @@ def wait_enable_piper(arm: Any, timeout_s: float, retry_interval_s: float = 0.2) return False -def parse_piper_ctrl_mode(value: Any) -> PiperCtrlMode: - if value is None: - return PiperCtrlMode(code=None, text="None") - +def _piper_mode_to_int(value: Any) -> int | None: if isinstance(value, bool): - code = int(value) - return PiperCtrlMode(code=code, text=f"0x{code:02X}") - + return int(value) if isinstance(value, int): - return PiperCtrlMode(code=value, text=f"0x{value:02X}") - - if isinstance(value, Enum): - enum_value = value.value - if isinstance(enum_value, int): - return PiperCtrlMode(code=enum_value, text=str(value)) - if isinstance(enum_value, str): - m = _CTRL_MODE_HEX_RE.search(enum_value) - if m: - return PiperCtrlMode(code=int(m.group(1), 16), text=str(value)) - if enum_value.isdigit(): - return PiperCtrlMode(code=int(enum_value), text=str(value)) - return PiperCtrlMode(code=None, text=str(value)) - - raw_value = getattr(value, "value", None) - if isinstance(raw_value, int): - return PiperCtrlMode(code=raw_value, text=str(value)) - - if isinstance(value, str): - m = _CTRL_MODE_HEX_RE.search(value) - if m: - return PiperCtrlMode(code=int(m.group(1), 16), text=value) - if value.isdigit(): - code = int(value) - return PiperCtrlMode(code=code, text=f"0x{code:02X}") - return PiperCtrlMode(code=None, text=value) - - return PiperCtrlMode(code=None, text=repr(value)) + return value + if hasattr(value, "value") and isinstance(value.value, int): + return int(value.value) + if hasattr(value, "__int__"): + return int(value) + return None -def read_piper_ctrl_mode(arm: Any, timeout_s: float = 1.0, poll_s: float = 0.02) -> PiperCtrlMode | None: +def read_piper_ctrl_mode(arm: Any, timeout_s: float = 1.0, poll_s: float = 0.02) -> int | None: deadline = time.monotonic() + max(0.0, timeout_s) while time.monotonic() < deadline: status_msg = arm.GetArmStatus() if getattr(status_msg, "time_stamp", 0.0) > 0.0: arm_status = getattr(status_msg, "arm_status", None) if arm_status is not None: - mode = parse_piper_ctrl_mode(getattr(arm_status, "ctrl_mode", None)) - if mode.code is not None: + mode = _piper_mode_to_int(getattr(arm_status, "ctrl_mode", None)) + if mode is not None: return mode time.sleep(max(0.005, poll_s)) return None @@ -156,18 +119,11 @@ def guard_piper_ctrl_mode_on_connect( f"[{interface_name}] could not read arm ctrl_mode within {timeout_s:.2f}s. " "Check CAN wiring/power and rerun." ) - - if mode.code is None: - raise RuntimeError( - f"[{interface_name}] got unreadable arm ctrl_mode value: {mode.text}. " - "Check SDK version/firmware and rerun." - ) - - if mode.code in {PIPER_CTRL_MODE_TEACH, PIPER_CTRL_MODE_LINKAGE_TEACH_INPUT}: + if mode in {PIPER_CTRL_MODE_TEACH, PIPER_CTRL_MODE_LINKAGE_TEACH_INPUT}: arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00) if settle_s > 0: time.sleep(settle_s) raise RuntimeError( - f"[{interface_name}] arm is in master/teaching role (ctrl_mode={mode.text}). " + f"[{interface_name}] arm is in master/teaching role (ctrl_mode=0x{mode:02X}). " "Follower role command (0xFC) has been sent. Power-cycle this arm, then retry." ) From 70c4b2d665c8695c1ac46adbd56cb8a7dd3b1f9b Mon Sep 17 00:00:00 2001 From: Elvin <223015152@link.cuhk.edu.cn> Date: Sat, 7 Mar 2026 12:07:54 +0800 Subject: [PATCH 23/26] fix(piper): normalize int-like ctrl_mode enum to int --- src/lerobot/utils/piper_sdk.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lerobot/utils/piper_sdk.py b/src/lerobot/utils/piper_sdk.py index 1ea1426..be1634d 100644 --- a/src/lerobot/utils/piper_sdk.py +++ b/src/lerobot/utils/piper_sdk.py @@ -83,7 +83,7 @@ def _piper_mode_to_int(value: Any) -> int | None: if isinstance(value, bool): return int(value) if isinstance(value, int): - return value + return int(value) if hasattr(value, "value") and isinstance(value.value, int): return int(value.value) if hasattr(value, "__int__"): From 0714316bab3e7f3e2712c2d66cec5ba89114d774 Mon Sep 17 00:00:00 2001 From: Elvin <223015152@link.cuhk.edu.cn> Date: Sat, 7 Mar 2026 12:45:43 +0800 Subject: [PATCH 24/26] feat(policy): add minimal swing policy for hil record smoke runs --- src/lerobot/policies/__init__.py | 2 + src/lerobot/policies/swing/__init__.py | 6 ++ .../policies/swing/configuration_swing.py | 48 +++++++++++++++ src/lerobot/policies/swing/modeling_swing.py | 60 +++++++++++++++++++ src/lerobot/policies/swing/processor_swing.py | 43 +++++++++++++ 5 files changed, 159 insertions(+) create mode 100644 src/lerobot/policies/swing/__init__.py create mode 100644 src/lerobot/policies/swing/configuration_swing.py create mode 100644 src/lerobot/policies/swing/modeling_swing.py create mode 100644 src/lerobot/policies/swing/processor_swing.py diff --git a/src/lerobot/policies/__init__.py b/src/lerobot/policies/__init__.py index c7951f0..0ae6fc7 100644 --- a/src/lerobot/policies/__init__.py +++ b/src/lerobot/policies/__init__.py @@ -20,6 +20,7 @@ from .pi0_fast.configuration_pi0_fast import PI0FastConfig as PI0FastConfig from .pi05.configuration_pi05 import PI05Config as PI05Config from .smolvla.configuration_smolvla import SmolVLAConfig as SmolVLAConfig from .smolvla.processor_smolvla import SmolVLANewLineProcessor +from .swing.configuration_swing import SwingConfig as SwingConfig from .tdmpc.configuration_tdmpc import TDMPCConfig as TDMPCConfig from .vqbet.configuration_vqbet import VQBeTConfig as VQBeTConfig from .wall_x.configuration_wall_x import WallXConfig as WallXConfig @@ -32,6 +33,7 @@ __all__ = [ "PI05Config", "PI0FastConfig", "SmolVLAConfig", + "SwingConfig", "SARMConfig", "TDMPCConfig", "VQBeTConfig", diff --git a/src/lerobot/policies/swing/__init__.py b/src/lerobot/policies/swing/__init__.py new file mode 100644 index 0000000..1737f5f --- /dev/null +++ b/src/lerobot/policies/swing/__init__.py @@ -0,0 +1,6 @@ +#!/usr/bin/env python + +from .configuration_swing import SwingConfig as SwingConfig +from .modeling_swing import SwingPolicy as SwingPolicy + +__all__ = ["SwingConfig", "SwingPolicy"] diff --git a/src/lerobot/policies/swing/configuration_swing.py b/src/lerobot/policies/swing/configuration_swing.py new file mode 100644 index 0000000..6440558 --- /dev/null +++ b/src/lerobot/policies/swing/configuration_swing.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python + +from dataclasses import dataclass + +from lerobot.configs.policies import PreTrainedConfig +from lerobot.optim.optimizers import AdamConfig + + +@PreTrainedConfig.register_subclass("swing") +@dataclass +class SwingConfig(PreTrainedConfig): + n_obs_steps: int = 1 + swing_joint_index: int = 0 + swing_amplitude: float = 0.08 + swing_period_steps: int = 60 + center_from_observation: bool = True + optimizer_lr: float = 1e-4 + + def __post_init__(self) -> None: + super().__post_init__() + if self.swing_period_steps <= 0: + raise ValueError("`swing_period_steps` must be > 0.") + if self.swing_amplitude < 0: + raise ValueError("`swing_amplitude` must be >= 0.") + + def get_optimizer_preset(self) -> AdamConfig: + return AdamConfig(lr=self.optimizer_lr) + + def get_scheduler_preset(self) -> None: + return None + + def validate_features(self) -> None: + if self.action_feature is None: + raise ValueError("Swing policy requires an action feature.") + if not self.action_feature.shape: + raise ValueError("Swing policy action feature shape must be non-empty.") + + @property + def observation_delta_indices(self) -> None: + return None + + @property + def action_delta_indices(self) -> None: + return None + + @property + def reward_delta_indices(self) -> None: + return None diff --git a/src/lerobot/policies/swing/modeling_swing.py b/src/lerobot/policies/swing/modeling_swing.py new file mode 100644 index 0000000..0f6e936 --- /dev/null +++ b/src/lerobot/policies/swing/modeling_swing.py @@ -0,0 +1,60 @@ +#!/usr/bin/env python + +import math +from typing import Any + +import torch +from torch import Tensor, nn + +from lerobot.policies.pretrained import ActionSelectKwargs, PreTrainedPolicy +from lerobot.utils.constants import OBS_STATE + +from .configuration_swing import SwingConfig + + +class SwingPolicy(PreTrainedPolicy): + config_class = SwingConfig + name = "swing" + + def __init__(self, config: SwingConfig, *inputs, **kwargs): + super().__init__(config, *inputs, **kwargs) + self._step = 0 + self._anchor = nn.Parameter(torch.zeros(1), requires_grad=True) + + def get_optim_params(self) -> dict: + return {"params": [self._anchor]} + + def reset(self): + self._step = 0 + + def forward(self, batch: dict[str, Tensor]) -> tuple[Tensor, dict | None]: + loss = self._anchor.sum() * 0.0 + return loss, None + + def _action_dim(self) -> int: + action_feature = self.config.action_feature + if action_feature is None or not action_feature.shape: + raise ValueError("Swing policy requires a valid action feature shape.") + return int(action_feature.shape[0]) + + def predict_action_chunk(self, batch: dict[str, Tensor], **kwargs: ActionSelectKwargs) -> Tensor: + return self.select_action(batch, **kwargs) + + def select_action(self, batch: dict[str, Tensor], **kwargs: ActionSelectKwargs) -> Tensor: + action_dim = self._action_dim() + action = torch.zeros((1, action_dim), device=self._anchor.device, dtype=torch.float32) + + if self.config.center_from_observation and OBS_STATE in batch: + obs_state = batch[OBS_STATE] + if obs_state.dim() == 1: + obs_state = obs_state.unsqueeze(0) + if obs_state.shape[-1] >= action_dim: + action = obs_state[:, :action_dim].to(dtype=torch.float32).clone() + + period = max(self.config.swing_period_steps, 1) + phase = 2.0 * math.pi * ((self._step % period) / period) + offset = self.config.swing_amplitude if math.sin(phase) >= 0 else -self.config.swing_amplitude + joint_idx = max(0, min(self.config.swing_joint_index, action_dim - 1)) + action[:, joint_idx] = action[:, joint_idx] + offset + self._step += 1 + return action diff --git a/src/lerobot/policies/swing/processor_swing.py b/src/lerobot/policies/swing/processor_swing.py new file mode 100644 index 0000000..e009a45 --- /dev/null +++ b/src/lerobot/policies/swing/processor_swing.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python + +from typing import Any + +from lerobot.processor import ( + AddBatchDimensionProcessorStep, + DeviceProcessorStep, + PolicyAction, + PolicyProcessorPipeline, + RenameObservationsProcessorStep, +) +from lerobot.processor.converters import policy_action_to_transition, transition_to_policy_action +from lerobot.utils.constants import POLICY_POSTPROCESSOR_DEFAULT_NAME, POLICY_PREPROCESSOR_DEFAULT_NAME + +from .configuration_swing import SwingConfig + + +def make_swing_pre_post_processors( + config: SwingConfig, + dataset_stats: dict[str, dict[str, Any]] | None = None, +) -> tuple[ + PolicyProcessorPipeline[dict[str, Any], dict[str, Any]], + PolicyProcessorPipeline[PolicyAction, PolicyAction], +]: + del dataset_stats + pre_steps = [ + RenameObservationsProcessorStep(rename_map={}), + AddBatchDimensionProcessorStep(), + DeviceProcessorStep(device=config.device), + ] + post_steps = [DeviceProcessorStep(device="cpu")] + return ( + PolicyProcessorPipeline[dict[str, Any], dict[str, Any]]( + steps=pre_steps, + name=POLICY_PREPROCESSOR_DEFAULT_NAME, + ), + PolicyProcessorPipeline[PolicyAction, PolicyAction]( + steps=post_steps, + name=POLICY_POSTPROCESSOR_DEFAULT_NAME, + to_transition=policy_action_to_transition, + to_output=transition_to_policy_action, + ), + ) From 0d2ed91280d9e5f7fd0e54fdd96fe565a05982b9 Mon Sep 17 00:00:00 2001 From: Elvin <223015152@link.cuhk.edu.cn> Date: Sat, 7 Mar 2026 16:36:06 +0800 Subject: [PATCH 25/26] Revert "feat(policy): add minimal swing policy for hil record smoke runs" This reverts commit 0714316bab3e7f3e2712c2d66cec5ba89114d774. --- src/lerobot/policies/__init__.py | 2 - src/lerobot/policies/swing/__init__.py | 6 -- .../policies/swing/configuration_swing.py | 48 --------------- src/lerobot/policies/swing/modeling_swing.py | 60 ------------------- src/lerobot/policies/swing/processor_swing.py | 43 ------------- 5 files changed, 159 deletions(-) delete mode 100644 src/lerobot/policies/swing/__init__.py delete mode 100644 src/lerobot/policies/swing/configuration_swing.py delete mode 100644 src/lerobot/policies/swing/modeling_swing.py delete mode 100644 src/lerobot/policies/swing/processor_swing.py diff --git a/src/lerobot/policies/__init__.py b/src/lerobot/policies/__init__.py index 0ae6fc7..c7951f0 100644 --- a/src/lerobot/policies/__init__.py +++ b/src/lerobot/policies/__init__.py @@ -20,7 +20,6 @@ from .pi0_fast.configuration_pi0_fast import PI0FastConfig as PI0FastConfig from .pi05.configuration_pi05 import PI05Config as PI05Config from .smolvla.configuration_smolvla import SmolVLAConfig as SmolVLAConfig from .smolvla.processor_smolvla import SmolVLANewLineProcessor -from .swing.configuration_swing import SwingConfig as SwingConfig from .tdmpc.configuration_tdmpc import TDMPCConfig as TDMPCConfig from .vqbet.configuration_vqbet import VQBeTConfig as VQBeTConfig from .wall_x.configuration_wall_x import WallXConfig as WallXConfig @@ -33,7 +32,6 @@ __all__ = [ "PI05Config", "PI0FastConfig", "SmolVLAConfig", - "SwingConfig", "SARMConfig", "TDMPCConfig", "VQBeTConfig", diff --git a/src/lerobot/policies/swing/__init__.py b/src/lerobot/policies/swing/__init__.py deleted file mode 100644 index 1737f5f..0000000 --- a/src/lerobot/policies/swing/__init__.py +++ /dev/null @@ -1,6 +0,0 @@ -#!/usr/bin/env python - -from .configuration_swing import SwingConfig as SwingConfig -from .modeling_swing import SwingPolicy as SwingPolicy - -__all__ = ["SwingConfig", "SwingPolicy"] diff --git a/src/lerobot/policies/swing/configuration_swing.py b/src/lerobot/policies/swing/configuration_swing.py deleted file mode 100644 index 6440558..0000000 --- a/src/lerobot/policies/swing/configuration_swing.py +++ /dev/null @@ -1,48 +0,0 @@ -#!/usr/bin/env python - -from dataclasses import dataclass - -from lerobot.configs.policies import PreTrainedConfig -from lerobot.optim.optimizers import AdamConfig - - -@PreTrainedConfig.register_subclass("swing") -@dataclass -class SwingConfig(PreTrainedConfig): - n_obs_steps: int = 1 - swing_joint_index: int = 0 - swing_amplitude: float = 0.08 - swing_period_steps: int = 60 - center_from_observation: bool = True - optimizer_lr: float = 1e-4 - - def __post_init__(self) -> None: - super().__post_init__() - if self.swing_period_steps <= 0: - raise ValueError("`swing_period_steps` must be > 0.") - if self.swing_amplitude < 0: - raise ValueError("`swing_amplitude` must be >= 0.") - - def get_optimizer_preset(self) -> AdamConfig: - return AdamConfig(lr=self.optimizer_lr) - - def get_scheduler_preset(self) -> None: - return None - - def validate_features(self) -> None: - if self.action_feature is None: - raise ValueError("Swing policy requires an action feature.") - if not self.action_feature.shape: - raise ValueError("Swing policy action feature shape must be non-empty.") - - @property - def observation_delta_indices(self) -> None: - return None - - @property - def action_delta_indices(self) -> None: - return None - - @property - def reward_delta_indices(self) -> None: - return None diff --git a/src/lerobot/policies/swing/modeling_swing.py b/src/lerobot/policies/swing/modeling_swing.py deleted file mode 100644 index 0f6e936..0000000 --- a/src/lerobot/policies/swing/modeling_swing.py +++ /dev/null @@ -1,60 +0,0 @@ -#!/usr/bin/env python - -import math -from typing import Any - -import torch -from torch import Tensor, nn - -from lerobot.policies.pretrained import ActionSelectKwargs, PreTrainedPolicy -from lerobot.utils.constants import OBS_STATE - -from .configuration_swing import SwingConfig - - -class SwingPolicy(PreTrainedPolicy): - config_class = SwingConfig - name = "swing" - - def __init__(self, config: SwingConfig, *inputs, **kwargs): - super().__init__(config, *inputs, **kwargs) - self._step = 0 - self._anchor = nn.Parameter(torch.zeros(1), requires_grad=True) - - def get_optim_params(self) -> dict: - return {"params": [self._anchor]} - - def reset(self): - self._step = 0 - - def forward(self, batch: dict[str, Tensor]) -> tuple[Tensor, dict | None]: - loss = self._anchor.sum() * 0.0 - return loss, None - - def _action_dim(self) -> int: - action_feature = self.config.action_feature - if action_feature is None or not action_feature.shape: - raise ValueError("Swing policy requires a valid action feature shape.") - return int(action_feature.shape[0]) - - def predict_action_chunk(self, batch: dict[str, Tensor], **kwargs: ActionSelectKwargs) -> Tensor: - return self.select_action(batch, **kwargs) - - def select_action(self, batch: dict[str, Tensor], **kwargs: ActionSelectKwargs) -> Tensor: - action_dim = self._action_dim() - action = torch.zeros((1, action_dim), device=self._anchor.device, dtype=torch.float32) - - if self.config.center_from_observation and OBS_STATE in batch: - obs_state = batch[OBS_STATE] - if obs_state.dim() == 1: - obs_state = obs_state.unsqueeze(0) - if obs_state.shape[-1] >= action_dim: - action = obs_state[:, :action_dim].to(dtype=torch.float32).clone() - - period = max(self.config.swing_period_steps, 1) - phase = 2.0 * math.pi * ((self._step % period) / period) - offset = self.config.swing_amplitude if math.sin(phase) >= 0 else -self.config.swing_amplitude - joint_idx = max(0, min(self.config.swing_joint_index, action_dim - 1)) - action[:, joint_idx] = action[:, joint_idx] + offset - self._step += 1 - return action diff --git a/src/lerobot/policies/swing/processor_swing.py b/src/lerobot/policies/swing/processor_swing.py deleted file mode 100644 index e009a45..0000000 --- a/src/lerobot/policies/swing/processor_swing.py +++ /dev/null @@ -1,43 +0,0 @@ -#!/usr/bin/env python - -from typing import Any - -from lerobot.processor import ( - AddBatchDimensionProcessorStep, - DeviceProcessorStep, - PolicyAction, - PolicyProcessorPipeline, - RenameObservationsProcessorStep, -) -from lerobot.processor.converters import policy_action_to_transition, transition_to_policy_action -from lerobot.utils.constants import POLICY_POSTPROCESSOR_DEFAULT_NAME, POLICY_PREPROCESSOR_DEFAULT_NAME - -from .configuration_swing import SwingConfig - - -def make_swing_pre_post_processors( - config: SwingConfig, - dataset_stats: dict[str, dict[str, Any]] | None = None, -) -> tuple[ - PolicyProcessorPipeline[dict[str, Any], dict[str, Any]], - PolicyProcessorPipeline[PolicyAction, PolicyAction], -]: - del dataset_stats - pre_steps = [ - RenameObservationsProcessorStep(rename_map={}), - AddBatchDimensionProcessorStep(), - DeviceProcessorStep(device=config.device), - ] - post_steps = [DeviceProcessorStep(device="cpu")] - return ( - PolicyProcessorPipeline[dict[str, Any], dict[str, Any]]( - steps=pre_steps, - name=POLICY_PREPROCESSOR_DEFAULT_NAME, - ), - PolicyProcessorPipeline[PolicyAction, PolicyAction]( - steps=post_steps, - name=POLICY_POSTPROCESSOR_DEFAULT_NAME, - to_transition=policy_action_to_transition, - to_output=transition_to_policy_action, - ), - ) From cb29370c78dfce9c61bd1a6c15d91b54e46071ae Mon Sep 17 00:00:00 2001 From: Elvin <223015152@link.cuhk.edu.cn> Date: Sat, 7 Mar 2026 22:27:09 +0800 Subject: [PATCH 26/26] feat(piper): add bimanual PiPER/PiPER-X support and refresh docs - add bi_piper/bi_piperx follower and leader implementations with configs - wire bimanual Piper types into factories, scripts, and async inference mappings - add pairing validation and bimanual parsing/teleop test coverage - update README news and AgileX teleop/data-collection command templates --- README.md | 66 ++++--- src/lerobot/async_inference/constants.py | 11 +- src/lerobot/async_inference/robot_client.py | 2 + .../robots/bi_piper_follower/__init__.py | 18 ++ .../bi_piper_follower/bi_piper_follower.py | 171 ++++++++++++++++++ .../config_bi_piper_follower.py | 39 ++++ src/lerobot/robots/utils.py | 4 + src/lerobot/scripts/lerobot_calibrate.py | 2 + src/lerobot/scripts/lerobot_record.py | 4 + src/lerobot/scripts/lerobot_replay.py | 1 + src/lerobot/scripts/lerobot_setup_motors.py | 6 + src/lerobot/scripts/lerobot_teleoperate.py | 4 + .../teleoperators/bi_piper_leader/__init__.py | 18 ++ .../bi_piper_leader/bi_piper_leader.py | 163 +++++++++++++++++ .../bi_piper_leader/config_bi_piper_leader.py | 39 ++++ src/lerobot/teleoperators/utils.py | 4 + src/lerobot/utils/control_utils.py | 25 +++ tests/scripts/test_bi_piper_config_parsing.py | 78 ++++++++ tests/test_piper_teleop.py | 122 +++++++++++++ tests/utils/test_control_utils.py | 41 +++++ 20 files changed, 795 insertions(+), 23 deletions(-) create mode 100644 src/lerobot/robots/bi_piper_follower/__init__.py create mode 100644 src/lerobot/robots/bi_piper_follower/bi_piper_follower.py create mode 100644 src/lerobot/robots/bi_piper_follower/config_bi_piper_follower.py create mode 100644 src/lerobot/teleoperators/bi_piper_leader/__init__.py create mode 100644 src/lerobot/teleoperators/bi_piper_leader/bi_piper_leader.py create mode 100644 src/lerobot/teleoperators/bi_piper_leader/config_bi_piper_leader.py create mode 100644 tests/scripts/test_bi_piper_config_parsing.py create mode 100644 tests/utils/test_control_utils.py diff --git a/README.md b/README.md index 930a2e8..8e6fb8d 100644 --- a/README.md +++ b/README.md @@ -21,12 +21,13 @@ ## 🎯 Evo-RL Focus -- **Open real-world RL on two platforms**: we build and release full real-world RL pipelines on SO101 and AgileX PiPER. +- **Open real-world RL on two platforms**: we build and release full real-world RL pipelines on SO101 and AgileX (PiPER/PiPER-X). - **Open code, models, and datasets for reproducibility**: we continuously release runnable offline RL assets so more people can reproduce results and apply them to real-world tasks. - **Open algorithm and community co-evolution**: we reproduce existing real-world RL methods, propose new methods, and keep publishing data/benchmarks to grow a collaborative open-source community. ## 🚀 News +- **[2026-03-07]** Added AgileX (PiPER/PiPER-X) support for real-world RL. - **[2026-02-26]** First SO101 real-world RL baseline and reproducible CLI workflow are released. ## 🧭 Table of Contents @@ -202,7 +203,9 @@ If needed, you can also use temporary device paths (for example `/dev/ttyACM*` a -#### AgileX PiPER +#### AgileX (PiPER/PiPER-X) + +PiPER arms in master/teaching mode cannot receive external control commands, so all arms must be configured to follower/motion-output mode (0xFC), and firmware must be version 1.8.5 or above. For PiPER-series robots, make sure Git LFS assets are pulled before running teleoperation: @@ -215,7 +218,7 @@ For PiPER setup, PiPER uses CAN interfaces instead of serial ports. So first run `lerobot-setup-can` to confirm CAN interfaces are available: ```bash -lerobot-setup-can --mode=setup --interfaces=can0,can1 +lerobot-setup-can --mode=setup --interfaces=,,, ``` For single-arm users, run the command below to verify the system is ready: @@ -223,15 +226,35 @@ For single-arm users, run the command below to verify the system is ready: ```bash lerobot-teleoperate \ --robot.type=piperx_follower \ - --robot.port=can0 \ + --robot.port= \ --robot.id=my_piperx_follower \ --robot.require_calibration=false \ --teleop.type=piperx_leader \ - --teleop.port=can1 \ + --teleop.port= \ --teleop.id=my_piperx_leader \ --teleop.require_calibration=false ``` +For bimanual users, run this command to verify dual-arm teleoperation: + +```bash +lerobot-teleoperate \ + --robot.type=bi_piperx_follower \ + --robot.id=my_bi_piperx_follower \ + --robot.left_arm_config.port= \ + --robot.right_arm_config.port= \ + --robot.left_arm_config.require_calibration=false \ + --robot.right_arm_config.require_calibration=false \ + --teleop.type=bi_piperx_leader \ + --teleop.id=my_bi_piperx_leader \ + --teleop.left_arm_config.port= \ + --teleop.right_arm_config.port= \ + --teleop.left_arm_config.require_calibration=false \ + --teleop.right_arm_config.require_calibration=false +``` + +For PiPER (non-X), replace `bi_piperx_follower`/`bi_piperx_leader` with `bi_piper_follower`/`bi_piper_leader`. + ### 3) Data Collection @@ -265,32 +288,31 @@ lerobot-human-inloop-record \ Recommendation: use **`fourcc: "MJPG"`** for OpenCV and **`warmup_s`** for RealSense. In this example `front` uses RealSense, but you can switch it to OpenCV with the same structure. -#### AgileX PiPER +#### AgileX (PiPER/PiPER-X) -PiPER mode note (policy-sync / human-in-the-loop): - -- Leader/follower linkage is configured via `MasterSlaveConfig(linkage_config, feedback_offset, ctrl_offset, linkage_offset)`. -- `linkage_config=0xFA`: teaching-input arm. -- `linkage_config=0xFC`: motion-output arm. -- `0xFA` is not suitable when the arm must accept external control commands. For policy execution + intervention switching, keep both teleop and robot arms in `0xFC`. -- After switching role mode (`0xFA` <-> `0xFC`), perform a full power-cycle before validation. +Bimanual template (left/right, PiPER-X example): ```bash lerobot-human-inloop-record \ - --robot.type=piper_follower \ - --robot.port=can0 \ - --robot.id=my_piper_follower \ - --robot.require_calibration=false \ - --teleop.type=piper_leader \ - --teleop.port=can1 \ - --teleop.id=my_piper_leader \ - --teleop.require_calibration=false \ + --robot.type=bi_piperx_follower \ + --robot.id=my_bi_piperx_follower \ + --robot.left_arm_config.port= \ + --robot.right_arm_config.port= \ + --robot.left_arm_config.require_calibration=false \ + --robot.right_arm_config.require_calibration=false \ + --teleop.type=bi_piperx_leader \ + --teleop.id=my_bi_piperx_leader \ + --teleop.left_arm_config.port= \ + --teleop.right_arm_config.port= \ + --teleop.left_arm_config.require_calibration=false \ + --teleop.right_arm_config.require_calibration=false \ --dataset.repo_id=/ \ --dataset.single_task="" \ --dataset.num_episodes= \ --dataset.episode_time_s= \ --dataset.reset_time_s= \ - --dataset.push_to_hub=true + --dataset.push_to_hub=true \ + --display_data=true ``` Hotkeys: diff --git a/src/lerobot/async_inference/constants.py b/src/lerobot/async_inference/constants.py index 56910e6..7659c3c 100644 --- a/src/lerobot/async_inference/constants.py +++ b/src/lerobot/async_inference/constants.py @@ -26,4 +26,13 @@ DEFAULT_OBS_QUEUE_TIMEOUT = 2 SUPPORTED_POLICIES = ["act", "smolvla", "diffusion", "tdmpc", "vqbet", "pi0", "pi05", "groot"] # TODO: Add all other robots -SUPPORTED_ROBOTS = ["so100_follower", "so101_follower", "bi_so_follower", "omx_follower"] +SUPPORTED_ROBOTS = [ + "so100_follower", + "so101_follower", + "bi_so_follower", + "piper_follower", + "piperx_follower", + "bi_piper_follower", + "bi_piperx_follower", + "omx_follower", +] diff --git a/src/lerobot/async_inference/robot_client.py b/src/lerobot/async_inference/robot_client.py index e4d2165..b5d068d 100644 --- a/src/lerobot/async_inference/robot_client.py +++ b/src/lerobot/async_inference/robot_client.py @@ -52,10 +52,12 @@ from lerobot.cameras.realsense.configuration_realsense import RealSenseCameraCon from lerobot.robots import ( # noqa: F401 Robot, RobotConfig, + bi_piper_follower, bi_so_follower, koch_follower, make_robot_from_config, omx_follower, + piper_follower, so_follower, ) from lerobot.transport import ( diff --git a/src/lerobot/robots/bi_piper_follower/__init__.py b/src/lerobot/robots/bi_piper_follower/__init__.py new file mode 100644 index 0000000..5fd09c4 --- /dev/null +++ b/src/lerobot/robots/bi_piper_follower/__init__.py @@ -0,0 +1,18 @@ +#!/usr/bin/env python + +# Copyright 2026 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 .bi_piper_follower import BiPiperFollower, BiPiperXFollower +from .config_bi_piper_follower import BiPiperFollowerConfig, BiPiperXFollowerConfig diff --git a/src/lerobot/robots/bi_piper_follower/bi_piper_follower.py b/src/lerobot/robots/bi_piper_follower/bi_piper_follower.py new file mode 100644 index 0000000..7031275 --- /dev/null +++ b/src/lerobot/robots/bi_piper_follower/bi_piper_follower.py @@ -0,0 +1,171 @@ +#!/usr/bin/env python + +# Copyright 2026 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 +from functools import cached_property + +from lerobot.processor import RobotAction, RobotObservation +from lerobot.robots.piper_follower import ( + PiperFollower, + PiperFollowerConfig, + PiperXFollower, + PiperXFollowerConfig, +) +from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected + +from ..robot import Robot +from .config_bi_piper_follower import BiPiperFollowerConfig, BiPiperXFollowerConfig + +logger = logging.getLogger(__name__) + + +class BiPiperFollower(Robot): + """Bimanual PiPER/PiPER-X follower arms.""" + + config_class = BiPiperFollowerConfig + name = "bi_piper_follower" + _side_field_names = ( + "port", + "judge_flag", + "can_auto_init", + "log_level", + "startup_sleep_s", + "speed_ratio", + "high_follow", + "mode_refresh_interval_s", + "enable_on_connect", + "enable_timeout_s", + "calibration_scale", + "require_calibration", + "sync_gripper", + "gripper_effort_default", + "gripper_status_code", + "cameras", + "disable_on_disconnect", + ) + + def _build_arm_config(self, arm_config_cls, side_cfg, side: str): + kwargs = {name: getattr(side_cfg, name) for name in self._side_field_names} + kwargs["id"] = f"{self.config.id}_{side}" if self.config.id else None + kwargs["calibration_dir"] = self.config.calibration_dir + return arm_config_cls(**kwargs) + + def __init__(self, config: BiPiperFollowerConfig | BiPiperXFollowerConfig): + super().__init__(config) + self.config = config + + if config.type == "bi_piperx_follower": + arm_config_cls = PiperXFollowerConfig + arm_cls = PiperXFollower + else: + arm_config_cls = PiperFollowerConfig + arm_cls = PiperFollower + + left_arm_config = self._build_arm_config(arm_config_cls, config.left_arm_config, "left") + right_arm_config = self._build_arm_config(arm_config_cls, config.right_arm_config, "right") + + self.left_arm = arm_cls(left_arm_config) + self.right_arm = arm_cls(right_arm_config) + + # Only for compatibility with other parts of the codebase that expect `robot.cameras`. + self.cameras = {**self.left_arm.cameras, **self.right_arm.cameras} + + @property + def _motors_ft(self) -> dict[str, type]: + left_arm_motors_ft = self.left_arm._motors_ft + right_arm_motors_ft = self.right_arm._motors_ft + return { + **{f"left_{k}": v for k, v in left_arm_motors_ft.items()}, + **{f"right_{k}": v for k, v in right_arm_motors_ft.items()}, + } + + @property + def _cameras_ft(self) -> dict[str, tuple]: + left_arm_cameras_ft = self.left_arm._cameras_ft + right_arm_cameras_ft = self.right_arm._cameras_ft + return { + **{f"left_{k}": v for k, v in left_arm_cameras_ft.items()}, + **{f"right_{k}": v for k, v in right_arm_cameras_ft.items()}, + } + + @cached_property + def observation_features(self) -> dict[str, type | tuple]: + return {**self._motors_ft, **self._cameras_ft} + + @cached_property + def action_features(self) -> dict[str, type]: + return self._motors_ft + + @property + def is_connected(self) -> bool: + return self.left_arm.is_connected and self.right_arm.is_connected + + @check_if_already_connected + def connect(self, calibrate: bool = True) -> None: + self.left_arm.connect(calibrate) + self.right_arm.connect(calibrate) + + @property + def is_calibrated(self) -> bool: + return self.left_arm.is_calibrated and self.right_arm.is_calibrated + + def calibrate(self) -> None: + self.left_arm.calibrate() + self.right_arm.calibrate() + + def configure(self) -> None: + self.left_arm.configure() + self.right_arm.configure() + + def setup_motors(self) -> None: + self.left_arm.setup_motors() + self.right_arm.setup_motors() + + @check_if_not_connected + def get_observation(self) -> RobotObservation: + obs_dict: RobotObservation = {} + left_obs = self.left_arm.get_observation() + obs_dict.update({f"left_{key}": value for key, value in left_obs.items()}) + right_obs = self.right_arm.get_observation() + obs_dict.update({f"right_{key}": value for key, value in right_obs.items()}) + return obs_dict + + @check_if_not_connected + def send_action(self, action: RobotAction) -> RobotAction: + left_action: RobotAction = {} + right_action: RobotAction = {} + for key, value in action.items(): + if key.startswith("left_"): + left_action[key.removeprefix("left_")] = value + elif key.startswith("right_"): + right_action[key.removeprefix("right_")] = value + + sent_action_left = self.left_arm.send_action(left_action) + sent_action_right = self.right_arm.send_action(right_action) + + prefixed_sent_action_left = {f"left_{key}": value for key, value in sent_action_left.items()} + prefixed_sent_action_right = {f"right_{key}": value for key, value in sent_action_right.items()} + return {**prefixed_sent_action_left, **prefixed_sent_action_right} + + @check_if_not_connected + def disconnect(self): + self.left_arm.disconnect() + self.right_arm.disconnect() + + +class BiPiperXFollower(BiPiperFollower): + config_class = BiPiperXFollowerConfig + name = "bi_piperx_follower" diff --git a/src/lerobot/robots/bi_piper_follower/config_bi_piper_follower.py b/src/lerobot/robots/bi_piper_follower/config_bi_piper_follower.py new file mode 100644 index 0000000..63f6feb --- /dev/null +++ b/src/lerobot/robots/bi_piper_follower/config_bi_piper_follower.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python + +# Copyright 2026 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 + +from lerobot.robots.piper_follower import PiperFollowerConfigBase + +from ..config import RobotConfig + + +@RobotConfig.register_subclass("bi_piper_follower") +@dataclass +class BiPiperFollowerConfig(RobotConfig): + """Configuration class for bimanual PiPER follower robots.""" + + left_arm_config: PiperFollowerConfigBase + right_arm_config: PiperFollowerConfigBase + + +@RobotConfig.register_subclass("bi_piperx_follower") +@dataclass +class BiPiperXFollowerConfig(RobotConfig): + """Configuration class for bimanual PiPER-X follower robots.""" + + left_arm_config: PiperFollowerConfigBase + right_arm_config: PiperFollowerConfigBase diff --git a/src/lerobot/robots/utils.py b/src/lerobot/robots/utils.py index 3c6e7c5..c71cd71 100644 --- a/src/lerobot/robots/utils.py +++ b/src/lerobot/robots/utils.py @@ -72,6 +72,10 @@ def make_robot_from_config(config: RobotConfig) -> Robot: from .piper_follower import PiperXFollower return PiperXFollower(config) + elif config.type in {"bi_piper_follower", "bi_piperx_follower"}: + from .bi_piper_follower import BiPiperFollower, BiPiperXFollower + + return BiPiperXFollower(config) if config.type == "bi_piperx_follower" else BiPiperFollower(config) elif config.type == "bi_openarm_follower": from .bi_openarm_follower import BiOpenArmFollower diff --git a/src/lerobot/scripts/lerobot_calibrate.py b/src/lerobot/scripts/lerobot_calibrate.py index c1386d2..cd5aba3 100644 --- a/src/lerobot/scripts/lerobot_calibrate.py +++ b/src/lerobot/scripts/lerobot_calibrate.py @@ -37,6 +37,7 @@ from lerobot.robots import ( # noqa: F401 Robot, RobotConfig, bi_openarm_follower, + bi_piper_follower, bi_so_follower, hope_jr, koch_follower, @@ -51,6 +52,7 @@ from lerobot.teleoperators import ( # noqa: F401 Teleoperator, TeleoperatorConfig, bi_openarm_leader, + bi_piper_leader, bi_so_leader, homunculus, koch_leader, diff --git a/src/lerobot/scripts/lerobot_record.py b/src/lerobot/scripts/lerobot_record.py index a83e5d8..53940c1 100644 --- a/src/lerobot/scripts/lerobot_record.py +++ b/src/lerobot/scripts/lerobot_record.py @@ -86,6 +86,7 @@ from lerobot.processor.rename_processor import rename_stats from lerobot.robots import ( # noqa: F401 RobotConfig, bi_openarm_follower, + bi_piper_follower, bi_so_follower, earthrover_mini_plus, hope_jr, @@ -108,6 +109,7 @@ from lerobot.scripts.recording_loop import record_loop from lerobot.teleoperators import ( # noqa: F401 TeleoperatorConfig, bi_openarm_leader, + bi_piper_leader, bi_so_leader, homunculus, koch_leader, @@ -123,6 +125,7 @@ from lerobot.utils.constants import ACTION from lerobot.utils.control_utils import ( init_keyboard_listener, is_headless, + sanity_check_bimanual_piper_pair, sanity_check_dataset_name, sanity_check_dataset_robot_compatibility, ) @@ -249,6 +252,7 @@ class RecordConfig: if self.teleop is None and self.policy is None: raise ValueError("Choose a policy, a teleoperator or both to control the robot") + sanity_check_bimanual_piper_pair(self.robot, self.teleop) if not self.intervention_toggle_key or len(self.intervention_toggle_key) != 1: raise ValueError("`intervention_toggle_key` must be a single character.") diff --git a/src/lerobot/scripts/lerobot_replay.py b/src/lerobot/scripts/lerobot_replay.py index 0a76588..eacc7c2 100644 --- a/src/lerobot/scripts/lerobot_replay.py +++ b/src/lerobot/scripts/lerobot_replay.py @@ -54,6 +54,7 @@ from lerobot.robots import ( # noqa: F401 Robot, RobotConfig, bi_openarm_follower, + bi_piper_follower, bi_so_follower, earthrover_mini_plus, hope_jr, diff --git a/src/lerobot/scripts/lerobot_setup_motors.py b/src/lerobot/scripts/lerobot_setup_motors.py index 01af95b..3d675c6 100644 --- a/src/lerobot/scripts/lerobot_setup_motors.py +++ b/src/lerobot/scripts/lerobot_setup_motors.py @@ -30,6 +30,7 @@ import draccus from lerobot.robots import ( # noqa: F401 RobotConfig, + bi_piper_follower, bi_so_follower, koch_follower, lekiwi, @@ -39,6 +40,7 @@ from lerobot.robots import ( # noqa: F401 ) from lerobot.teleoperators import ( # noqa: F401 TeleoperatorConfig, + bi_piper_leader, bi_so_leader, koch_leader, make_teleoperator_from_config, @@ -55,6 +57,10 @@ COMPATIBLE_DEVICES = [ "so100_leader", "so101_follower", "so101_leader", + "bi_piper_follower", + "bi_piperx_follower", + "bi_piper_leader", + "bi_piperx_leader", "lekiwi", ] diff --git a/src/lerobot/scripts/lerobot_teleoperate.py b/src/lerobot/scripts/lerobot_teleoperate.py index b9275c0..96ff492 100644 --- a/src/lerobot/scripts/lerobot_teleoperate.py +++ b/src/lerobot/scripts/lerobot_teleoperate.py @@ -71,6 +71,7 @@ from lerobot.robots import ( # noqa: F401 Robot, RobotConfig, bi_openarm_follower, + bi_piper_follower, bi_so_follower, earthrover_mini_plus, hope_jr, @@ -87,6 +88,7 @@ from lerobot.teleoperators import ( # noqa: F401 Teleoperator, TeleoperatorConfig, bi_openarm_leader, + bi_piper_leader, bi_so_leader, gamepad, homunculus, @@ -100,6 +102,7 @@ from lerobot.teleoperators import ( # noqa: F401 so_leader, unitree_g1, ) +from lerobot.utils.control_utils import sanity_check_bimanual_piper_pair from lerobot.utils.import_utils import register_third_party_plugins from lerobot.utils.robot_utils import precise_sleep from lerobot.utils.utils import init_logging, move_cursor_up @@ -206,6 +209,7 @@ def teleop_loop( @parser.wrap() def teleoperate(cfg: TeleoperateConfig): init_logging() + sanity_check_bimanual_piper_pair(cfg.robot, cfg.teleop) logging.info(pformat(asdict(cfg))) if cfg.display_data: init_rerun(session_name="teleoperation", ip=cfg.display_ip, port=cfg.display_port) diff --git a/src/lerobot/teleoperators/bi_piper_leader/__init__.py b/src/lerobot/teleoperators/bi_piper_leader/__init__.py new file mode 100644 index 0000000..90633c8 --- /dev/null +++ b/src/lerobot/teleoperators/bi_piper_leader/__init__.py @@ -0,0 +1,18 @@ +#!/usr/bin/env python + +# Copyright 2026 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 .bi_piper_leader import BiPiperLeader, BiPiperXLeader +from .config_bi_piper_leader import BiPiperLeaderConfig, BiPiperXLeaderConfig diff --git a/src/lerobot/teleoperators/bi_piper_leader/bi_piper_leader.py b/src/lerobot/teleoperators/bi_piper_leader/bi_piper_leader.py new file mode 100644 index 0000000..b6c5ae5 --- /dev/null +++ b/src/lerobot/teleoperators/bi_piper_leader/bi_piper_leader.py @@ -0,0 +1,163 @@ +#!/usr/bin/env python + +# Copyright 2026 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 +from functools import cached_property +from typing import Any + +from lerobot.processor import RobotAction +from lerobot.teleoperators.piper_leader import ( + PiperLeader, + PiperLeaderConfig, + PiperXLeader, + PiperXLeaderConfig, +) +from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected + +from ..teleoperator import Teleoperator +from .config_bi_piper_leader import BiPiperLeaderConfig, BiPiperXLeaderConfig + +logger = logging.getLogger(__name__) + + +class BiPiperLeader(Teleoperator): + """Bimanual PiPER/PiPER-X leader arms.""" + + config_class = BiPiperLeaderConfig + name = "bi_piper_leader" + _side_field_names = ( + "port", + "judge_flag", + "can_auto_init", + "log_level", + "startup_sleep_s", + "manual_control", + "prefer_ctrl_messages", + "fallback_to_feedback", + "sync_gripper", + "gripper_effort_default", + "gripper_status_code", + "command_speed_ratio", + "command_high_follow", + "mode_refresh_interval_s", + "enable_timeout_s", + "gravity_comp_control_hz", + "gravity_comp_tx_ratio", + "gravity_comp_torque_limit", + "gravity_comp_mit_kp", + "gravity_comp_mit_kd", + "gravity_comp_base_rpy_deg", + "calibration_scale", + "require_calibration", + "disable_on_disconnect", + ) + + def _build_arm_config(self, arm_config_cls, side_cfg, side: str): + kwargs = {name: getattr(side_cfg, name) for name in self._side_field_names} + kwargs["id"] = f"{self.config.id}_{side}" if self.config.id else None + kwargs["calibration_dir"] = self.config.calibration_dir + return arm_config_cls(**kwargs) + + def __init__(self, config: BiPiperLeaderConfig | BiPiperXLeaderConfig): + super().__init__(config) + self.config = config + + if config.type == "bi_piperx_leader": + arm_config_cls = PiperXLeaderConfig + arm_cls = PiperXLeader + else: + arm_config_cls = PiperLeaderConfig + arm_cls = PiperLeader + + left_arm_config = self._build_arm_config(arm_config_cls, config.left_arm_config, "left") + right_arm_config = self._build_arm_config(arm_config_cls, config.right_arm_config, "right") + + self.left_arm = arm_cls(left_arm_config) + self.right_arm = arm_cls(right_arm_config) + + @cached_property + def action_features(self) -> dict[str, type]: + left_arm_features = self.left_arm.action_features + right_arm_features = self.right_arm.action_features + return { + **{f"left_{k}": v for k, v in left_arm_features.items()}, + **{f"right_{k}": v for k, v in right_arm_features.items()}, + } + + @cached_property + def feedback_features(self) -> dict[str, type]: + return {} + + @property + def is_connected(self) -> bool: + return self.left_arm.is_connected and self.right_arm.is_connected + + @check_if_already_connected + def connect(self, calibrate: bool = True) -> None: + self.left_arm.connect(calibrate) + self.right_arm.connect(calibrate) + + @property + def is_calibrated(self) -> bool: + return self.left_arm.is_calibrated and self.right_arm.is_calibrated + + def calibrate(self) -> None: + self.left_arm.calibrate() + self.right_arm.calibrate() + + def configure(self) -> None: + self.left_arm.configure() + self.right_arm.configure() + + def setup_motors(self) -> None: + self.left_arm.setup_motors() + self.right_arm.setup_motors() + + @check_if_not_connected + def set_manual_control(self, enabled: bool) -> None: + self.left_arm.set_manual_control(enabled) + self.right_arm.set_manual_control(enabled) + + @check_if_not_connected + def get_action(self) -> RobotAction: + action_dict: RobotAction = {} + left_action = self.left_arm.get_action() + action_dict.update({f"left_{key}": value for key, value in left_action.items()}) + right_action = self.right_arm.get_action() + action_dict.update({f"right_{key}": value for key, value in right_action.items()}) + return action_dict + + @check_if_not_connected + def send_feedback(self, feedback: dict[str, Any]) -> None: + left_feedback: dict[str, Any] = {} + right_feedback: dict[str, Any] = {} + for key, value in feedback.items(): + if key.startswith("left_"): + left_feedback[key.removeprefix("left_")] = value + elif key.startswith("right_"): + right_feedback[key.removeprefix("right_")] = value + self.left_arm.send_feedback(left_feedback) + self.right_arm.send_feedback(right_feedback) + + @check_if_not_connected + def disconnect(self) -> None: + self.left_arm.disconnect() + self.right_arm.disconnect() + + +class BiPiperXLeader(BiPiperLeader): + config_class = BiPiperXLeaderConfig + name = "bi_piperx_leader" diff --git a/src/lerobot/teleoperators/bi_piper_leader/config_bi_piper_leader.py b/src/lerobot/teleoperators/bi_piper_leader/config_bi_piper_leader.py new file mode 100644 index 0000000..51e3601 --- /dev/null +++ b/src/lerobot/teleoperators/bi_piper_leader/config_bi_piper_leader.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python + +# Copyright 2026 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 + +from lerobot.teleoperators.piper_leader import PiperLeaderConfigBase + +from ..config import TeleoperatorConfig + + +@TeleoperatorConfig.register_subclass("bi_piper_leader") +@dataclass +class BiPiperLeaderConfig(TeleoperatorConfig): + """Configuration class for bimanual PiPER leader teleoperators.""" + + left_arm_config: PiperLeaderConfigBase + right_arm_config: PiperLeaderConfigBase + + +@TeleoperatorConfig.register_subclass("bi_piperx_leader") +@dataclass +class BiPiperXLeaderConfig(TeleoperatorConfig): + """Configuration class for bimanual PiPER-X leader teleoperators.""" + + left_arm_config: PiperLeaderConfigBase + right_arm_config: PiperLeaderConfigBase diff --git a/src/lerobot/teleoperators/utils.py b/src/lerobot/teleoperators/utils.py index 4797199..2f47181 100644 --- a/src/lerobot/teleoperators/utils.py +++ b/src/lerobot/teleoperators/utils.py @@ -99,6 +99,10 @@ def make_teleoperator_from_config(config: TeleoperatorConfig) -> "Teleoperator": from .piper_leader import PiperXLeader return PiperXLeader(config) + elif config.type in {"bi_piper_leader", "bi_piperx_leader"}: + from .bi_piper_leader import BiPiperLeader, BiPiperXLeader + + return BiPiperXLeader(config) if config.type == "bi_piperx_leader" else BiPiperLeader(config) elif config.type == "bi_openarm_leader": from .bi_openarm_leader import BiOpenArmLeader diff --git a/src/lerobot/utils/control_utils.py b/src/lerobot/utils/control_utils.py index c2be198..8bb7517 100644 --- a/src/lerobot/utils/control_utils.py +++ b/src/lerobot/utils/control_utils.py @@ -262,3 +262,28 @@ def sanity_check_dataset_robot_compatibility( raise ValueError( "Dataset metadata compatibility check failed with mismatches:\n" + "\n".join(mismatches) ) + + +def sanity_check_bimanual_piper_pair(robot_cfg, teleop_cfg) -> None: + """Ensure bimanual PiPER configs are not mixed between PiPER and PiPER-X variants.""" + if teleop_cfg is None: + return + + robot_type = getattr(robot_cfg, "type", None) + teleop_type = getattr(teleop_cfg, "type", None) + expected_teleop_by_robot = { + "bi_piper_follower": "bi_piper_leader", + "bi_piperx_follower": "bi_piperx_leader", + } + expected_robot_by_teleop = {teleop: robot for robot, teleop in expected_teleop_by_robot.items()} + + if robot_type in expected_teleop_by_robot and teleop_type != expected_teleop_by_robot[robot_type]: + expected = expected_teleop_by_robot[robot_type] + raise ValueError( + f"In bimanual PiPER mode, '{robot_type}' must be paired with '{expected}', got '{teleop_type}'." + ) + if teleop_type in expected_robot_by_teleop and robot_type != expected_robot_by_teleop[teleop_type]: + expected = expected_robot_by_teleop[teleop_type] + raise ValueError( + f"In bimanual PiPER mode, '{teleop_type}' must be paired with '{expected}', got '{robot_type}'." + ) diff --git a/tests/scripts/test_bi_piper_config_parsing.py b/tests/scripts/test_bi_piper_config_parsing.py new file mode 100644 index 0000000..ff04269 --- /dev/null +++ b/tests/scripts/test_bi_piper_config_parsing.py @@ -0,0 +1,78 @@ +import draccus +import pytest +from draccus.utils import ParsingError + +from lerobot.scripts.lerobot_record import RecordConfig +from lerobot.scripts.lerobot_teleoperate import TeleoperateConfig + + +def test_teleoperate_parses_bi_piperx_types(): + args = [ + "--robot.type=bi_piperx_follower", + "--robot.id=my_bi_piperx_follower", + "--robot.left_arm_config.port=can0", + "--robot.right_arm_config.port=can2", + "--robot.left_arm_config.require_calibration=false", + "--robot.right_arm_config.require_calibration=false", + "--teleop.type=bi_piperx_leader", + "--teleop.id=my_bi_piperx_leader", + "--teleop.left_arm_config.port=can1", + "--teleop.right_arm_config.port=can3", + "--teleop.left_arm_config.require_calibration=false", + "--teleop.right_arm_config.require_calibration=false", + ] + cfg = draccus.parse(config_class=TeleoperateConfig, config_path=None, args=args) + assert cfg.robot.type == "bi_piperx_follower" + assert cfg.teleop.type == "bi_piperx_leader" + + +def test_record_rejects_mixed_bi_piper_pair(): + args = [ + "--robot.type=bi_piper_follower", + "--robot.id=my_bi_piper_follower", + "--robot.left_arm_config.port=can0", + "--robot.right_arm_config.port=can2", + "--robot.left_arm_config.require_calibration=false", + "--robot.right_arm_config.require_calibration=false", + "--teleop.type=bi_piperx_leader", + "--teleop.id=my_bi_piperx_leader", + "--teleop.left_arm_config.port=can1", + "--teleop.right_arm_config.port=can3", + "--teleop.left_arm_config.require_calibration=false", + "--teleop.right_arm_config.require_calibration=false", + "--dataset.repo_id=dummy/dummy", + "--dataset.single_task=test", + "--dataset.num_episodes=1", + "--dataset.episode_time_s=1", + "--dataset.reset_time_s=1", + "--dataset.push_to_hub=false", + ] + with pytest.raises(ParsingError) as exc_info: + draccus.parse(config_class=RecordConfig, config_path=None, args=args) + assert "Couldn't instantiate class RecordConfig" in str(exc_info.value) + assert exc_info.value.__cause__ is not None + assert "must be paired" in str(exc_info.value.__cause__) + + +def test_record_rejects_bimanual_piper_teleop_with_non_bimanual_robot(): + args = [ + "--robot.type=so101_follower", + "--robot.port=/dev/mock", + "--teleop.type=bi_piperx_leader", + "--teleop.id=my_bi_piperx_leader", + "--teleop.left_arm_config.port=can1", + "--teleop.right_arm_config.port=can3", + "--teleop.left_arm_config.require_calibration=false", + "--teleop.right_arm_config.require_calibration=false", + "--dataset.repo_id=dummy/dummy", + "--dataset.single_task=test", + "--dataset.num_episodes=1", + "--dataset.episode_time_s=1", + "--dataset.reset_time_s=1", + "--dataset.push_to_hub=false", + ] + with pytest.raises(ParsingError) as exc_info: + draccus.parse(config_class=RecordConfig, config_path=None, args=args) + assert "Couldn't instantiate class RecordConfig" in str(exc_info.value) + assert exc_info.value.__cause__ is not None + assert "must be paired" in str(exc_info.value.__cause__) diff --git a/tests/test_piper_teleop.py b/tests/test_piper_teleop.py index 852313b..effb41c 100644 --- a/tests/test_piper_teleop.py +++ b/tests/test_piper_teleop.py @@ -9,16 +9,30 @@ import lerobot.robots.piper_follower.piper_follower as piper_follower_module import lerobot.teleoperators.piper_leader.piper_leader as piper_leader_module import lerobot.utils.piper_sdk as piper_sdk_utils from lerobot.motors import MotorCalibration +from lerobot.robots.bi_piper_follower import ( + BiPiperFollower, + BiPiperFollowerConfig, + BiPiperXFollower, + BiPiperXFollowerConfig, +) from lerobot.robots.piper_follower import ( PiperFollower, PiperFollowerConfig, + PiperFollowerConfigBase, PiperXFollower, PiperXFollowerConfig, ) from lerobot.robots.utils import make_robot_from_config +from lerobot.teleoperators.bi_piper_leader import ( + BiPiperLeader, + BiPiperLeaderConfig, + BiPiperXLeader, + BiPiperXLeaderConfig, +) from lerobot.teleoperators.piper_leader import ( PiperLeader, PiperLeaderConfig, + PiperLeaderConfigBase, PiperXLeader, PiperXLeaderConfig, ) @@ -477,3 +491,111 @@ def test_piper_lfs_pointer_urdf_raises_actionable_error(tmp_path): piper_leader_module._ensure_not_lfs_pointer( pointer_file, "assets/piper_description/urdf/pointer.urdf" ) + + +@pytest.mark.parametrize( + ( + "teleop_cfg", + "robot_cfg", + "bi_teleop_cls", + "bi_robot_cls", + "left_teleop_cls", + "right_teleop_cls", + "left_robot_cls", + "right_robot_cls", + ), + [ + ( + BiPiperLeaderConfig( + left_arm_config=PiperLeaderConfigBase(port="can1", manual_control=False, sync_gripper=True), + right_arm_config=PiperLeaderConfigBase(port="can3", manual_control=False, sync_gripper=True), + ), + BiPiperFollowerConfig( + left_arm_config=PiperFollowerConfigBase(port="can0", sync_gripper=True), + right_arm_config=PiperFollowerConfigBase(port="can2", sync_gripper=True), + ), + BiPiperLeader, + BiPiperFollower, + PiperLeader, + PiperLeader, + PiperFollower, + PiperFollower, + ), + ( + BiPiperXLeaderConfig( + left_arm_config=PiperLeaderConfigBase(port="can1", manual_control=False, sync_gripper=True), + right_arm_config=PiperLeaderConfigBase(port="can3", manual_control=False, sync_gripper=True), + ), + BiPiperXFollowerConfig( + left_arm_config=PiperFollowerConfigBase(port="can0", sync_gripper=True), + right_arm_config=PiperFollowerConfigBase(port="can2", sync_gripper=True), + ), + BiPiperXLeader, + BiPiperXFollower, + PiperXLeader, + PiperXLeader, + PiperXFollower, + PiperXFollower, + ), + ], +) +def test_bimanual_piper_leader_follower_roundtrip( + monkeypatch, + teleop_cfg, + robot_cfg, + bi_teleop_cls, + bi_robot_cls, + left_teleop_cls, + right_teleop_cls, + left_robot_cls, + right_robot_cls, +): + patch_fake_sdk(monkeypatch) + + teleop = make_teleoperator_from_config(teleop_cfg) + robot = make_robot_from_config(robot_cfg) + + assert isinstance(teleop, bi_teleop_cls) + assert isinstance(robot, bi_robot_cls) + assert isinstance(teleop.left_arm, left_teleop_cls) + assert isinstance(teleop.right_arm, right_teleop_cls) + assert isinstance(robot.left_arm, left_robot_cls) + assert isinstance(robot.right_arm, right_robot_cls) + + teleop.left_arm.calibration = make_identity_calibration() + teleop.right_arm.calibration = make_identity_calibration() + robot.left_arm.calibration = make_identity_calibration() + robot.right_arm.calibration = make_identity_calibration() + + teleop.connect(calibrate=False) + robot.connect(calibrate=False) + try: + action = teleop.get_action() + assert "left_joint_1.pos" in action + assert "right_joint_1.pos" in action + + sent = robot.send_action(action) + obs = robot.get_observation() + + assert robot.left_arm.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000) + assert robot.right_arm.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000) + assert robot.left_arm.arm.last_gripper[0] == 42000 + assert robot.right_arm.arm.last_gripper[0] == 42000 + + assert sent["left_joint_1.pos"] == 10.0 + assert sent["right_joint_1.pos"] == 10.0 + assert sent["left_gripper.pos"] == 42.0 + assert sent["right_gripper.pos"] == 42.0 + assert obs["left_joint_1.pos"] == 11.0 + assert obs["right_joint_1.pos"] == 11.0 + assert obs["left_gripper.pos"] == 43.0 + assert obs["right_gripper.pos"] == 43.0 + + teleop.send_feedback(action) + assert teleop.left_arm.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000) + assert teleop.right_arm.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000) + assert teleop.left_arm.arm.last_gripper[0] == 42000 + assert teleop.right_arm.arm.last_gripper[0] == 42000 + finally: + teleop.disconnect() + robot.disconnect() diff --git a/tests/utils/test_control_utils.py b/tests/utils/test_control_utils.py new file mode 100644 index 0000000..bbf3b14 --- /dev/null +++ b/tests/utils/test_control_utils.py @@ -0,0 +1,41 @@ +from types import SimpleNamespace + +import pytest + +from lerobot.utils.control_utils import sanity_check_bimanual_piper_pair + + +@pytest.mark.parametrize( + ("robot_type", "teleop_type"), + [ + ("bi_piper_follower", "bi_piper_leader"), + ("bi_piperx_follower", "bi_piperx_leader"), + ("so101_follower", "so101_leader"), + ], +) +def test_sanity_check_bimanual_piper_pair_accepts_valid_pairs(robot_type, teleop_type): + sanity_check_bimanual_piper_pair( + SimpleNamespace(type=robot_type), + SimpleNamespace(type=teleop_type), + ) + + +def test_sanity_check_bimanual_piper_pair_accepts_missing_teleop(): + sanity_check_bimanual_piper_pair(SimpleNamespace(type="bi_piperx_follower"), None) + + +@pytest.mark.parametrize( + ("robot_type", "teleop_type"), + [ + ("bi_piper_follower", "bi_piperx_leader"), + ("bi_piperx_follower", "bi_piper_leader"), + ("so101_follower", "bi_piperx_leader"), + ("so101_follower", "bi_piper_leader"), + ], +) +def test_sanity_check_bimanual_piper_pair_rejects_mixed_pairs(robot_type, teleop_type): + with pytest.raises(ValueError, match="must be paired"): + sanity_check_bimanual_piper_pair( + SimpleNamespace(type=robot_type), + SimpleNamespace(type=teleop_type), + )