From 52703ecf954a20b5b53eb38c528d5b711f1bb1b0 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Mon, 20 Apr 2026 19:12:07 +0200 Subject: [PATCH] debug fixes --- src/lerobot/rollout/context.py | 49 ++++++++++++++++++++++++-- src/lerobot/rollout/inference/sync.py | 24 +++++++++++-- src/lerobot/rollout/strategies/core.py | 12 +++++++ 3 files changed, 80 insertions(+), 5 deletions(-) diff --git a/src/lerobot/rollout/context.py b/src/lerobot/rollout/context.py index 273198d56..bf5361945 100644 --- a/src/lerobot/rollout/context.py +++ b/src/lerobot/rollout/context.py @@ -272,11 +272,17 @@ def build_rollout_context( # ) # --- 4. Features + action-key reconciliation --------------------- + # Only `.pos` joint features are used for policy inference — velocity and + # torque channels are observation-only and must be excluded from the state + # and action tensors that the policy sees. This matches the filtering + # applied by the old ``hil_data_collection`` script. all_obs_features = robot.observation_features observation_features_hw = { - k: v for k, v in all_obs_features.items() if v is float or isinstance(v, tuple) + k: v + for k, v in all_obs_features.items() + if isinstance(v, tuple) or (v is float and k.endswith(".pos")) } - action_features_hw = robot.action_features + action_features_hw = {k: v for k, v in robot.action_features.items() if k.endswith(".pos")} # The action side is always needed: sync inference reads action names from # ``dataset_features[ACTION]`` to map policy tensors back to robot actions. @@ -293,13 +299,50 @@ def build_rollout_context( ) dataset_features = combine_feature_dicts(action_dataset_features, observation_dataset_features) hw_features = hw_to_dataset_features(observation_features_hw, "observation") - raw_action_keys = list(robot.action_features.keys()) + raw_action_keys = list(action_features_hw.keys()) policy_action_names = getattr(policy_config, "action_feature_names", None) ordered_action_keys = _resolve_action_key_order( list(policy_action_names) if policy_action_names else None, raw_action_keys, ) + # --- Diagnostic logging --- + _act_ft = dataset_features.get("action", {}) + _obs_ft = dataset_features.get("observation.state", {}) + logger.info( + "Feature reconciliation: action_dim=%d, obs_state_dim=%d, ordered_action_keys=%d", + _act_ft.get("shape", (0,))[0], + _obs_ft.get("shape", (0,))[0], + len(ordered_action_keys), + ) + logger.info(" action names : %s", _act_ft.get("names", [])) + logger.info(" obs state names: %s", _obs_ft.get("names", [])) + logger.info(" ordered keys : %s", ordered_action_keys) + logger.info( + " policy.action_feature_names: %s", + list(policy_action_names) if policy_action_names else "None (using raw_action_keys)", + ) + if full_config.input_features: + logger.info(" policy input_features: %s", list(full_config.input_features.keys())) + else: + logger.warning(" policy input_features is EMPTY — policy may not process images!") + if full_config.output_features: + for k, v in full_config.output_features.items(): + logger.info(" policy output_features[%s]: shape=%s", k, v.shape) + # Validate action dimension consistency + if full_config.output_features: + for ft in full_config.output_features.values(): + policy_action_dim = ft.shape[0] + if len(ordered_action_keys) != policy_action_dim: + logger.error( + "ACTION DIM MISMATCH: policy expects %d dims, hardware produces %d keys. " + "First 5 keys: %s", + policy_action_dim, + len(ordered_action_keys), + ordered_action_keys[:5], + ) + break + # Validate visual features if no rename_map is active rename_map = cfg.dataset.rename_map if cfg.dataset else {} if not rename_map: diff --git a/src/lerobot/rollout/inference/sync.py b/src/lerobot/rollout/inference/sync.py index aaed0b356..cc66b8f76 100644 --- a/src/lerobot/rollout/inference/sync.py +++ b/src/lerobot/rollout/inference/sync.py @@ -97,10 +97,30 @@ class SyncInferenceEngine(InferenceEngine): observation, self._device, self._task, self._robot_type ) observation = self._preprocessor(observation) - action = self._policy.select_action(observation) - action = self._postprocessor(action) + action_raw = self._policy.select_action(observation) + action = self._postprocessor(action_raw) action_tensor = action.squeeze(0).cpu() + if not hasattr(self, "_log_count"): + self._log_count = 0 + if self._log_count < 3: + raw_flat = action_raw.squeeze(0).cpu() + logger.info( + "[Sync tick %d] raw action (first 5): %s | post-processed (first 5): %s", + self._log_count, + raw_flat[:5].tolist(), + action_tensor[:5].tolist(), + ) + obs_state = obs_frame.get("observation.state") + if obs_state is not None: + logger.info( + "[Sync tick %d] obs_frame['observation.state'] (first 5): %s | shape: %s", + self._log_count, + obs_state[:5].tolist() if hasattr(obs_state, "tolist") else str(obs_state)[:80], + obs_state.shape if hasattr(obs_state, "shape") else "?", + ) + self._log_count += 1 + # Reorder to match dataset action ordering so the caller can treat # the returned tensor uniformly across backends. action_dict = make_robot_action(action_tensor, self._dataset_features) diff --git a/src/lerobot/rollout/strategies/core.py b/src/lerobot/rollout/strategies/core.py index f0f146109..384b10a0c 100644 --- a/src/lerobot/rollout/strategies/core.py +++ b/src/lerobot/rollout/strategies/core.py @@ -268,5 +268,17 @@ def send_next_action( action_dict = {k: interp[i].item() for i, k in enumerate(ordered_keys) if i < len(interp)} processed = ctx.processors.robot_action_processor((action_dict, obs_raw)) + + if not hasattr(send_next_action, "_log_count"): + send_next_action._log_count = 0 + if send_next_action._log_count < 3: + sample = {k: round(v, 4) for k, v in list(processed.items())[:5]} + logger.info( + "[send_next_action tick %d] action sent to robot (first 5): %s", + send_next_action._log_count, + sample, + ) + send_next_action._log_count += 1 + ctx.hardware.robot_wrapper.send_action(processed) return action_dict