mirror of
https://github.com/huggingface/lerobot.git
synced 2026-05-31 19:01:28 +00:00
Compare commits
11 Commits
feat/custo
...
fix/datase
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
bcc13f1d90 | ||
|
|
76f25f6afd | ||
|
|
ce23681d4b | ||
|
|
e195f8d287 | ||
|
|
bbcffc4999 | ||
|
|
20333abc72 | ||
|
|
00a4e6bfb3 | ||
|
|
a19bd6e84d | ||
|
|
550866a3c5 | ||
|
|
3ec4e4ce37 | ||
|
|
f6b16f6d97 |
@@ -39,6 +39,7 @@ from lerobot.datasets.aggregate import aggregate_datasets
|
||||
from lerobot.datasets.compute_stats import aggregate_stats
|
||||
from lerobot.datasets.lerobot_dataset import LeRobotDataset, LeRobotDatasetMetadata
|
||||
from lerobot.datasets.utils import (
|
||||
DATA_DIR,
|
||||
DEFAULT_CHUNK_SIZE,
|
||||
DEFAULT_DATA_FILE_SIZE_IN_MB,
|
||||
DEFAULT_DATA_PATH,
|
||||
@@ -962,28 +963,23 @@ def _copy_data_with_feature_changes(
|
||||
remove_features: list[str] | None = None,
|
||||
) -> None:
|
||||
"""Copy data while adding or removing features."""
|
||||
if dataset.meta.episodes is None:
|
||||
dataset.meta.episodes = load_episodes(dataset.meta.root)
|
||||
data_dir = dataset.root / DATA_DIR
|
||||
parquet_files = sorted(data_dir.glob("*/*.parquet"))
|
||||
|
||||
# Map file paths to episode indices to extract chunk/file indices
|
||||
file_to_episodes: dict[Path, set[int]] = {}
|
||||
for ep_idx in range(dataset.meta.total_episodes):
|
||||
file_path = dataset.meta.get_data_file_path(ep_idx)
|
||||
if file_path not in file_to_episodes:
|
||||
file_to_episodes[file_path] = set()
|
||||
file_to_episodes[file_path].add(ep_idx)
|
||||
if not parquet_files:
|
||||
raise ValueError(f"No parquet files found in {data_dir}")
|
||||
|
||||
frame_idx = 0
|
||||
|
||||
for src_path in tqdm(sorted(file_to_episodes.keys()), desc="Processing data files"):
|
||||
df = pd.read_parquet(dataset.root / src_path).reset_index(drop=True)
|
||||
for src_path in tqdm(parquet_files, desc="Processing data files"):
|
||||
df = pd.read_parquet(src_path).reset_index(drop=True)
|
||||
|
||||
# Get chunk_idx and file_idx from the source file's first episode
|
||||
episodes_in_file = file_to_episodes[src_path]
|
||||
first_ep_idx = min(episodes_in_file)
|
||||
src_ep = dataset.meta.episodes[first_ep_idx]
|
||||
chunk_idx = src_ep["data/chunk_index"]
|
||||
file_idx = src_ep["data/file_index"]
|
||||
relative_path = src_path.relative_to(dataset.root)
|
||||
chunk_dir = relative_path.parts[1]
|
||||
file_name = relative_path.parts[2]
|
||||
|
||||
chunk_idx = int(chunk_dir.split("-")[1])
|
||||
file_idx = int(file_name.split("-")[1].split(".")[0])
|
||||
|
||||
if remove_features:
|
||||
df = df.drop(columns=remove_features, errors="ignore")
|
||||
@@ -1009,7 +1005,7 @@ def _copy_data_with_feature_changes(
|
||||
df[feature_name] = feature_slice
|
||||
frame_idx = end_idx
|
||||
|
||||
# Write using the preserved chunk_idx and file_idx from source
|
||||
# Write using the same chunk/file structure as source
|
||||
dst_path = new_meta.root / DEFAULT_DATA_PATH.format(chunk_index=chunk_idx, file_index=file_idx)
|
||||
dst_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
|
||||
@@ -19,6 +19,7 @@ import shutil
|
||||
import tempfile
|
||||
from collections.abc import Callable
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import datasets
|
||||
import numpy as np
|
||||
@@ -31,6 +32,8 @@ import torch
|
||||
import torch.utils
|
||||
from huggingface_hub import HfApi, snapshot_download
|
||||
from huggingface_hub.errors import RevisionNotFoundError
|
||||
from PIL import Image as PILImage
|
||||
from torchvision import transforms
|
||||
|
||||
from lerobot.datasets.compute_stats import aggregate_stats, compute_episode_stats
|
||||
from lerobot.datasets.image_writer import AsyncImageWriter, write_image
|
||||
@@ -50,11 +53,9 @@ from lerobot.datasets.utils import (
|
||||
get_file_size_in_mb,
|
||||
get_hf_features_from_features,
|
||||
get_safe_version,
|
||||
hf_transform_to_torch,
|
||||
is_valid_version,
|
||||
load_episodes,
|
||||
load_info,
|
||||
load_nested_dataset,
|
||||
load_stats,
|
||||
load_tasks,
|
||||
update_chunk_file_indices,
|
||||
@@ -79,6 +80,51 @@ from lerobot.utils.constants import HF_LEROBOT_HOME
|
||||
CODEBASE_VERSION = "v3.0"
|
||||
|
||||
|
||||
def hf_transform_to_torch(items_dict: dict[str, list[Any]]) -> dict[str, list[torch.Tensor | str]]:
|
||||
"""
|
||||
Converts a batch from a Hugging Face dataset to torch tensors.
|
||||
"""
|
||||
|
||||
# Create a single ToTensor transform instance to reuse
|
||||
to_tensor = transforms.ToTensor()
|
||||
|
||||
for key in items_dict:
|
||||
items_list = items_dict[key]
|
||||
|
||||
# Check if the list is non-empty
|
||||
if not items_list:
|
||||
continue
|
||||
|
||||
first_item = items_list[0]
|
||||
|
||||
if isinstance(first_item, PILImage.Image):
|
||||
# This is the (slow) CPU-bound part.
|
||||
# We convert every image in the batch list to a tensor.
|
||||
items_dict[key] = [to_tensor(img) for img in items_list]
|
||||
|
||||
elif isinstance(first_item, (str, bytes)):
|
||||
# List of strings (e.g., 'task'), do nothing
|
||||
pass
|
||||
|
||||
elif first_item is None:
|
||||
# List of Nones, do nothing
|
||||
pass
|
||||
|
||||
else:
|
||||
# List of other things (int, float, list, np.ndarray)
|
||||
try:
|
||||
# Convert each item in the list to a tensor
|
||||
items_dict[key] = [torch.tensor(item) for item in items_list]
|
||||
except Exception as e:
|
||||
# This catch is what was missing from the original v3.0 code
|
||||
print(
|
||||
f"Error converting batch['{key}'] to tensor. First item: {first_item}, Type: {type(first_item)}"
|
||||
)
|
||||
raise e
|
||||
|
||||
return items_dict
|
||||
|
||||
|
||||
class LeRobotDatasetMetadata:
|
||||
def __init__(
|
||||
self,
|
||||
@@ -693,6 +739,9 @@ class LeRobotDataset(torch.utils.data.Dataset):
|
||||
self.repo_id, self.root, self.revision, force_cache_sync=force_cache_sync
|
||||
)
|
||||
|
||||
# Pre-load episodes metadata into memory to avoid file I/O in __getitem__
|
||||
self.episodes_metadata_list = [ep for ep in self.meta.episodes]
|
||||
|
||||
# Track dataset state for efficient incremental writing
|
||||
self._lazy_loading = False
|
||||
self._recorded_frames = self.meta.total_frames
|
||||
@@ -829,8 +878,36 @@ class LeRobotDataset(torch.utils.data.Dataset):
|
||||
|
||||
def load_hf_dataset(self) -> datasets.Dataset:
|
||||
"""hf_dataset contains all the observations, states, actions, rewards, etc."""
|
||||
|
||||
features = get_hf_features_from_features(self.features)
|
||||
hf_dataset = load_nested_dataset(self.root / "data", features=features)
|
||||
|
||||
if self.episodes is not None:
|
||||
# Path for episode-specific loading (e.g., visualization)
|
||||
fpaths = set()
|
||||
for ep_idx in self.episodes:
|
||||
ep_meta = self.episodes_metadata_list[ep_idx]
|
||||
chunk_idx = ep_meta["data/chunk_index"]
|
||||
file_idx = ep_meta["data/file_index"]
|
||||
fpath_str = self.meta.data_path.format(chunk_index=chunk_idx, file_index=file_idx)
|
||||
fpaths.add(str(self.root / fpath_str))
|
||||
|
||||
data_files = sorted(list(fpaths))
|
||||
|
||||
hf_dataset = datasets.load_dataset(
|
||||
"parquet", data_files=data_files, features=features, split="train"
|
||||
)
|
||||
|
||||
requested_episodes_set = set(self.episodes)
|
||||
hf_dataset = hf_dataset.filter(
|
||||
lambda x: x["episode_index"] in requested_episodes_set, batched=True, batch_size=1000
|
||||
)
|
||||
|
||||
else:
|
||||
# THIS IS THE FAST PATH FOR TRAINING (self.episodes is None)
|
||||
# Use `data_dir` to trigger the v2.1-style efficient cache.
|
||||
data_dir = str(self.root / "data")
|
||||
hf_dataset = datasets.load_dataset("parquet", data_dir=data_dir, features=features, split="train")
|
||||
|
||||
hf_dataset.set_transform(hf_transform_to_torch)
|
||||
return hf_dataset
|
||||
|
||||
@@ -909,7 +986,7 @@ class LeRobotDataset(torch.utils.data.Dataset):
|
||||
return get_hf_features_from_features(self.features)
|
||||
|
||||
def _get_query_indices(self, idx: int, ep_idx: int) -> tuple[dict[str, list[int | bool]]]:
|
||||
ep = self.meta.episodes[ep_idx]
|
||||
ep = self.episodes_metadata_list[ep_idx]
|
||||
ep_start = ep["dataset_from_index"]
|
||||
ep_end = ep["dataset_to_index"]
|
||||
query_indices = {
|
||||
@@ -952,7 +1029,7 @@ class LeRobotDataset(torch.utils.data.Dataset):
|
||||
Segmentation Fault. This probably happens because a memory reference to the video loader is created in
|
||||
the main process and a subprocess fails to access it.
|
||||
"""
|
||||
ep = self.meta.episodes[ep_idx]
|
||||
ep = self.episodes_metadata_list[ep_idx]
|
||||
item = {}
|
||||
for vid_key, query_ts in query_timestamps.items():
|
||||
# Episodes are stored sequentially on a single mp4 to reduce the number of files.
|
||||
@@ -983,29 +1060,72 @@ class LeRobotDataset(torch.utils.data.Dataset):
|
||||
def __getitem__(self, idx) -> dict:
|
||||
# Ensure dataset is loaded when we actually need to read from it
|
||||
self._ensure_hf_dataset_loaded()
|
||||
item = self.hf_dataset[idx]
|
||||
ep_idx = item["episode_index"].item()
|
||||
|
||||
# 1. Get query indices if deltas are needed
|
||||
query_indices = None
|
||||
padding = {}
|
||||
if self.delta_indices is not None:
|
||||
query_indices, padding = self._get_query_indices(idx, ep_idx)
|
||||
query_result = self._query_hf_dataset(query_indices)
|
||||
item = {**item, **padding}
|
||||
for key, val in query_result.items():
|
||||
item[key] = val
|
||||
# We need the episode index *first* to get boundaries.
|
||||
# This is a small read for just one item.
|
||||
ep_idx_only = self.hf_dataset[idx : idx + 1]["episode_index"][0].item()
|
||||
query_indices, padding = self._get_query_indices(idx, ep_idx_only)
|
||||
|
||||
# 2. Fetch all data (including images)
|
||||
if query_indices is not None:
|
||||
# --- Delta path ---
|
||||
# Fetch all keys (state, action, AND images) for all deltas
|
||||
item_batch = self.hf_dataset[query_indices["index"]]
|
||||
|
||||
# hf_transform_to_torch (item-level) has already run,
|
||||
# so all values are tensors. We stack them.
|
||||
item = {}
|
||||
for key in item_batch:
|
||||
item[key] = torch.stack(item_batch[key])
|
||||
|
||||
item.update(padding)
|
||||
|
||||
# Use the "current" item's index/timestamp/ep_idx
|
||||
# (assuming 'index' is the key for the main query)
|
||||
current_idx_in_batch = query_indices["index"].index(idx)
|
||||
item["index"] = item["index"][current_idx_in_batch]
|
||||
item["timestamp"] = item["timestamp"][current_idx_in_batch]
|
||||
item["episode_index"] = item["episode_index"][current_idx_in_batch]
|
||||
item["task_index"] = item["task_index"][current_idx_in_batch]
|
||||
|
||||
ep_idx = item["episode_index"].item()
|
||||
|
||||
else:
|
||||
# --- Single-frame path ---
|
||||
item = self.hf_dataset[idx]
|
||||
ep_idx = item["episode_index"].item()
|
||||
|
||||
# 3. Handle videos (which are always separate)
|
||||
if len(self.meta.video_keys) > 0:
|
||||
current_ts = item["timestamp"].item()
|
||||
query_timestamps = self._get_query_timestamps(current_ts, query_indices)
|
||||
current_ts = (
|
||||
item["timestamp"].item()
|
||||
if query_indices is None
|
||||
else item["timestamp"][current_idx_in_batch].item()
|
||||
)
|
||||
|
||||
video_query_indices = query_indices
|
||||
if video_query_indices is None:
|
||||
# If no deltas, create a dummy query_indices for _get_query_timestamps
|
||||
video_query_indices = {key: [idx] for key in self.meta.video_keys}
|
||||
|
||||
query_timestamps = self._get_query_timestamps(current_ts, video_query_indices)
|
||||
video_frames = self._query_videos(query_timestamps, ep_idx)
|
||||
|
||||
# video_frames are already stacked tensors (B, C, H, W) or (C, H, W)
|
||||
item = {**video_frames, **item}
|
||||
|
||||
# 4. Apply image transforms
|
||||
if self.image_transforms is not None:
|
||||
image_keys = self.meta.camera_keys
|
||||
for cam in image_keys:
|
||||
item[cam] = self.image_transforms(item[cam])
|
||||
if cam in item: # videos or images
|
||||
item[cam] = self.image_transforms(item[cam])
|
||||
|
||||
# Add task as a string
|
||||
# 5. Add task string
|
||||
task_idx = item["task_index"].item()
|
||||
item["task"] = self.meta.tasks.iloc[task_idx].name
|
||||
return item
|
||||
|
||||
@@ -35,7 +35,6 @@ from datasets.table import embed_table_storage
|
||||
from huggingface_hub import DatasetCard, DatasetCardData, HfApi
|
||||
from huggingface_hub.errors import RevisionNotFoundError
|
||||
from PIL import Image as PILImage
|
||||
from torchvision import transforms
|
||||
|
||||
from lerobot.configs.types import FeatureType, PolicyFeature
|
||||
from lerobot.datasets.backward_compatibility import (
|
||||
@@ -116,10 +115,15 @@ def load_nested_dataset(pq_dir: Path, features: datasets.Features | None = None)
|
||||
if len(paths) == 0:
|
||||
raise FileNotFoundError(f"Provided directory does not contain any parquet file: {pq_dir}")
|
||||
|
||||
# Convert Path objects to a list of strings
|
||||
file_paths = [str(path) for path in paths]
|
||||
|
||||
# Use datasets.load_dataset to force creation of an efficient cache
|
||||
# This pre-decodes the images and avoids the on-the-fly bottleneck.
|
||||
# TODO(rcadene): set num_proc to accelerate conversion to pyarrow
|
||||
with SuppressProgressBars():
|
||||
datasets = Dataset.from_parquet([str(path) for path in paths], features=features)
|
||||
return datasets
|
||||
dataset = datasets.load_dataset("parquet", data_files=file_paths, features=features, split="train")
|
||||
return dataset
|
||||
|
||||
|
||||
def get_parquet_num_frames(parquet_path: str | Path) -> int:
|
||||
@@ -394,33 +398,6 @@ def load_image_as_numpy(
|
||||
return img_array
|
||||
|
||||
|
||||
def hf_transform_to_torch(items_dict: dict[str, list[Any]]) -> dict[str, list[torch.Tensor | str]]:
|
||||
"""Convert a batch from a Hugging Face dataset to torch tensors.
|
||||
|
||||
This transform function converts items from Hugging Face dataset format (pyarrow)
|
||||
to torch tensors. Importantly, images are converted from PIL objects (H, W, C, uint8)
|
||||
to a torch image representation (C, H, W, float32) in the range [0, 1]. Other
|
||||
types are converted to torch.tensor.
|
||||
|
||||
Args:
|
||||
items_dict (dict): A dictionary representing a batch of data from a
|
||||
Hugging Face dataset.
|
||||
|
||||
Returns:
|
||||
dict: The batch with items converted to torch tensors.
|
||||
"""
|
||||
for key in items_dict:
|
||||
first_item = items_dict[key][0]
|
||||
if isinstance(first_item, PILImage.Image):
|
||||
to_tensor = transforms.ToTensor()
|
||||
items_dict[key] = [to_tensor(img) for img in items_dict[key]]
|
||||
elif first_item is None:
|
||||
pass
|
||||
else:
|
||||
items_dict[key] = [x if isinstance(x, str) else torch.tensor(x) for x in items_dict[key]]
|
||||
return items_dict
|
||||
|
||||
|
||||
def is_valid_version(version: str) -> bool:
|
||||
"""Check if a string is a valid PEP 440 version.
|
||||
|
||||
|
||||
@@ -1,18 +0,0 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from .config_custom import CustomConfig
|
||||
from .custom import Custom
|
||||
@@ -1,32 +0,0 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
from ..config import TeleoperatorConfig
|
||||
|
||||
|
||||
@TeleoperatorConfig.register_subclass("custom")
|
||||
@dataclass
|
||||
class CustomConfig(TeleoperatorConfig):
|
||||
"""Custom teleoperator config that dynamically wraps a base teleoperator class.
|
||||
|
||||
The base class and its configuration are loaded from a JSON config file at runtime.
|
||||
Port and baud_rate are taken from the first device in the config file.
|
||||
"""
|
||||
config_path: str | None = None # REQUIRED: Path to custom config JSON file
|
||||
port: str = "/dev/ttyACM0" # Default port
|
||||
baud_rate: int = 115200 # Default baud rate
|
||||
@@ -1,206 +0,0 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
import importlib
|
||||
import json
|
||||
import logging
|
||||
from pathlib import Path
|
||||
|
||||
from lerobot.motors.motors_bus import MotorNormMode
|
||||
|
||||
from ..teleoperator import Teleoperator
|
||||
from .config_custom import CustomConfig
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
class Custom(Teleoperator):
|
||||
"""
|
||||
Custom teleoperator that dynamically wraps a base teleoperator class and applies configurable joint mapping.
|
||||
The base class is specified in custom_config.json, allowing flexible teleoperator configurations.
|
||||
"""
|
||||
|
||||
config_class = CustomConfig
|
||||
name = "custom"
|
||||
|
||||
def __init__(self, config: CustomConfig):
|
||||
# Load custom configuration from JSON file
|
||||
if config.config_path is None:
|
||||
raise ValueError(
|
||||
"config_path must be provided for custom teleoperator. "
|
||||
"Example: --teleop.config_path=/path/to/custom_config.json"
|
||||
)
|
||||
|
||||
config_path = Path(config.config_path)
|
||||
|
||||
with open(config_path) as f:
|
||||
custom_config = json.load(f)
|
||||
|
||||
logger.info(f"Loaded custom config from {config_path}")
|
||||
logger.info(f"Found {len(custom_config)} teleoperator(s): {list(custom_config.keys())}")
|
||||
|
||||
# Initialize the base Teleoperator class
|
||||
super().__init__(config)
|
||||
|
||||
# Store multiple base teleoperators and their action mappings
|
||||
self.base_teleops = {}
|
||||
self.robot_actions_configs = {}
|
||||
|
||||
# Instantiate each base teleoperator from the config
|
||||
for device_name, device_config in custom_config.items():
|
||||
base_class_name = device_config["base_class"]
|
||||
|
||||
# Create a config copy for this teleoperator
|
||||
from dataclasses import replace
|
||||
teleop_config = replace(
|
||||
config,
|
||||
port=device_config.get("port", config.port),
|
||||
id=device_config.get("id", f"{config.id}_{device_name}"),
|
||||
baud_rate=device_config.get("baud_rate", config.baud_rate)
|
||||
)
|
||||
|
||||
logger.info(f" {device_name}: class={base_class_name}, port={teleop_config.port}, id={teleop_config.id}")
|
||||
|
||||
# Dynamically import and instantiate the base teleoperator class
|
||||
module_path, class_name_full = base_class_name.rsplit(".", 1)
|
||||
module = importlib.import_module(module_path)
|
||||
base_class = getattr(module, class_name_full)
|
||||
|
||||
# Store the teleoperator and its action mapping
|
||||
self.base_teleops[device_name] = base_class(teleop_config)
|
||||
self.robot_actions_configs[device_name] = device_config["robot_actions"]
|
||||
|
||||
@property
|
||||
def action_features(self) -> dict:
|
||||
# Aggregate action features from all teleoperators' action mappings
|
||||
all_actions = {}
|
||||
for device_config in self.robot_actions_configs.values():
|
||||
for robot_action in device_config.keys():
|
||||
all_actions[robot_action] = float
|
||||
return all_actions
|
||||
|
||||
@property
|
||||
def feedback_features(self) -> dict:
|
||||
# Aggregate feedback features from all base teleoperators
|
||||
all_feedback = {}
|
||||
for teleop in self.base_teleops.values():
|
||||
all_feedback.update(teleop.feedback_features)
|
||||
return all_feedback
|
||||
|
||||
@property
|
||||
def is_connected(self) -> bool:
|
||||
# All teleoperators must be connected
|
||||
return all(teleop.is_connected for teleop in self.base_teleops.values())
|
||||
|
||||
@property
|
||||
def is_calibrated(self) -> bool:
|
||||
# All teleoperators must be calibrated
|
||||
return all(teleop.is_calibrated for teleop in self.base_teleops.values())
|
||||
|
||||
def connect(self, calibrate: bool = True) -> None:
|
||||
# Connect all base teleoperators
|
||||
for device_name, teleop in self.base_teleops.items():
|
||||
logger.info(f"Connecting {device_name}...")
|
||||
teleop.connect(calibrate=calibrate)
|
||||
|
||||
def calibrate(self) -> None:
|
||||
# Calibrate all base teleoperators
|
||||
for device_name, teleop in self.base_teleops.items():
|
||||
logger.info(f"Calibrating {device_name}...")
|
||||
teleop.calibrate()
|
||||
|
||||
def configure(self) -> None:
|
||||
# Configure all base teleoperators
|
||||
for teleop in self.base_teleops.values():
|
||||
teleop.configure()
|
||||
|
||||
def send_feedback(self, feedback: dict[str, float]) -> None:
|
||||
# Send feedback to all base teleoperators
|
||||
for teleop in self.base_teleops.values():
|
||||
teleop.send_feedback(feedback)
|
||||
|
||||
def disconnect(self) -> None:
|
||||
# Disconnect all base teleoperators
|
||||
for device_name, teleop in self.base_teleops.items():
|
||||
logger.info(f"Disconnecting {device_name}...")
|
||||
teleop.disconnect()
|
||||
|
||||
def _normalize_to_unit_range(self, teleop, joint_name: str, value: float) -> float:
|
||||
"""Convert a joint value from base teleoperator's normalization mode to [0, 1] range.
|
||||
|
||||
Args:
|
||||
teleop: The base teleoperator instance
|
||||
joint_name: Name of the joint (e.g., "shoulder_pitch")
|
||||
value: Value in the base teleoperator's normalization mode
|
||||
|
||||
Returns:
|
||||
Value normalized to [0, 1] range
|
||||
"""
|
||||
norm_mode = teleop.joints[joint_name]
|
||||
|
||||
if norm_mode == MotorNormMode.RANGE_M100_100:
|
||||
# Convert from [-100, 100] to [0, 1]
|
||||
return (value + 100.0) / 200.0
|
||||
elif norm_mode == MotorNormMode.RANGE_0_100:
|
||||
# Convert from [0, 100] to [0, 1]
|
||||
return value / 100.0
|
||||
elif norm_mode == MotorNormMode.DEGREES:
|
||||
# For degrees, we need calibration to know the range
|
||||
# Use calibration min/max to normalize
|
||||
if teleop.calibration and joint_name in teleop.calibration:
|
||||
min_deg = teleop.calibration[joint_name].range_min
|
||||
max_deg = teleop.calibration[joint_name].range_max
|
||||
if max_deg != min_deg:
|
||||
return (value - min_deg) / (max_deg - min_deg)
|
||||
# Fallback: assume common range like [-180, 180]
|
||||
return (value + 180.0) / 360.0
|
||||
else:
|
||||
raise ValueError(f"Unknown normalization mode: {norm_mode}")
|
||||
|
||||
def get_action(self) -> dict[str, float]:
|
||||
# Build action dict by reading from all base teleoperators
|
||||
action = {}
|
||||
|
||||
# Loop through each teleoperator
|
||||
for device_name, teleop in self.base_teleops.items():
|
||||
# Read joint positions from this teleoperator
|
||||
# These are in the teleoperator's normalization mode (e.g., -100 to 100)
|
||||
joint_positions = teleop._read()
|
||||
|
||||
# Get the robot actions config for this teleoperator
|
||||
robot_actions_config = self.robot_actions_configs[device_name]
|
||||
|
||||
# Process each robot action for this teleoperator
|
||||
for robot_action, config in robot_actions_config.items():
|
||||
if config["source"] == "neutral":
|
||||
# Use fixed neutral value (already in [0, 1] range)
|
||||
value = config["value"]
|
||||
elif config["source"] == "teleop":
|
||||
# Get value from teleop joint
|
||||
teleop_joint = config["joint"]
|
||||
value = joint_positions[teleop_joint]
|
||||
|
||||
# Convert from base teleoperator's normalization mode to [0, 1] range
|
||||
value = self._normalize_to_unit_range(teleop, teleop_joint, value)
|
||||
|
||||
# Apply inversion if specified
|
||||
if config.get("invert", False):
|
||||
value = 1.0 - value
|
||||
else:
|
||||
raise ValueError(f"Unknown source '{config['source']}' for robot action '{robot_action}'")
|
||||
|
||||
action[robot_action] = value
|
||||
return action
|
||||
@@ -1,76 +0,0 @@
|
||||
{
|
||||
"right_arm": {
|
||||
"base_class": "lerobot.teleoperators.homunculus.homunculus_arm.HomunculusArm",
|
||||
"port": "/dev/ttyACM0",
|
||||
"id": "unitree_right",
|
||||
"baud_rate": 115200,
|
||||
"robot_actions": {
|
||||
"kRightShoulderPitch.pos": {
|
||||
"source": "neutral",
|
||||
"value": 0.5
|
||||
},
|
||||
"kRightShoulderRoll.pos": {
|
||||
"source": "neutral",
|
||||
"value": 0.5
|
||||
},
|
||||
"kRightShoulderYaw.pos": {
|
||||
"source": "neutral",
|
||||
"value": 0.5
|
||||
},
|
||||
"kRightElbow.pos": {
|
||||
"source": "neutral",
|
||||
"value": 0.5
|
||||
},
|
||||
"kRightWristRoll.pos": {
|
||||
"source": "teleop",
|
||||
"joint": "wrist_roll",
|
||||
"invert": true
|
||||
},
|
||||
"kRightWristPitch.pos": {
|
||||
"source": "neutral",
|
||||
"value": 0.5
|
||||
},
|
||||
"kRightWristYaw.pos": {
|
||||
"source": "neutral",
|
||||
"value": 0.5
|
||||
}
|
||||
}
|
||||
},
|
||||
"left_arm": {
|
||||
"base_class": "lerobot.teleoperators.homunculus.homunculus_arm.HomunculusArm",
|
||||
"port": "/dev/ttyACM1",
|
||||
"id": "unitree_left",
|
||||
"baud_rate": 115200,
|
||||
"robot_actions": {
|
||||
"kLeftShoulderPitch.pos": {
|
||||
"source": "neutral",
|
||||
"value": 0.5
|
||||
},
|
||||
"kLeftShoulderRoll.pos": {
|
||||
"source": "neutral",
|
||||
"value": 0.5
|
||||
},
|
||||
"kLeftShoulderYaw.pos": {
|
||||
"source": "neutral",
|
||||
"value": 0.5
|
||||
},
|
||||
"kLeftElbow.pos": {
|
||||
"source": "neutral",
|
||||
"value": 0.5
|
||||
},
|
||||
"kLeftWristRoll.pos": {
|
||||
"source": "teleop",
|
||||
"joint": "wrist_roll",
|
||||
"invert": true
|
||||
},
|
||||
"kLeftWristPitch.pos": {
|
||||
"source": "neutral",
|
||||
"value": 0.5
|
||||
},
|
||||
"kLeftWristyaw.pos": {
|
||||
"source": "neutral",
|
||||
"value": 0.5
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user