from __future__ import annotations from typing import Any, List, Optional, Union import torch import torch.nn as nn from psi.models.psi0 import HumanFoundationModelOutput, Psi0Model class MotionHiddenEncoder(nn.Module): def __init__( self, action_dim: int, action_horizon: int, hidden_size: int, num_tokens: int = 64, mlp_hidden_dim: int = 2048, dropout: float = 0.0, ) -> None: super().__init__() self.action_dim = int(action_dim) self.action_horizon = int(action_horizon) self.hidden_size = int(hidden_size) self.num_tokens = int(num_tokens) input_dim = self.action_dim * self.action_horizon self.motion_mlp = nn.Sequential( nn.LayerNorm(input_dim), nn.Linear(input_dim, int(mlp_hidden_dim)), nn.SiLU(), nn.Dropout(float(dropout)), nn.Linear(int(mlp_hidden_dim), self.hidden_size), ) self.pos_embed = nn.Embedding(self.num_tokens, self.hidden_size) self.token_mlp = nn.Sequential( nn.LayerNorm(self.hidden_size), nn.Linear(self.hidden_size, self.hidden_size), nn.SiLU(), nn.Dropout(float(dropout)), nn.Linear(self.hidden_size, self.hidden_size), nn.LayerNorm(self.hidden_size), ) def _fit_horizon(self, actions: torch.Tensor) -> torch.Tensor: actions = actions[..., : self.action_dim] if actions.shape[1] == self.action_horizon: return actions if actions.shape[1] > self.action_horizon: return actions[:, : self.action_horizon] pad = torch.zeros( actions.shape[0], self.action_horizon - actions.shape[1], actions.shape[2], device=actions.device, dtype=actions.dtype, ) return torch.cat([actions, pad], dim=1) def forward( self, actions: torch.Tensor, ) -> torch.Tensor: motion = self._fit_horizon(actions).reshape(actions.shape[0], -1) context = self.motion_mlp(motion.float()).to(dtype=actions.dtype) positions = torch.arange(self.num_tokens, device=actions.device) tokens = context[:, None, :] + self.pos_embed(positions)[None, :, :].to(dtype=context.dtype) return self.token_mlp(tokens) class MotionDistillPsi0Model(Psi0Model): def __init__(self, model_cfg, vlm_model): super().__init__(model_cfg=model_cfg, vlm_model=vlm_model) input_horizon = int(getattr(model_cfg, "motion_encoder_input_horizon", 0) or model_cfg.action_chunk_size) self.motion_encoder = MotionHiddenEncoder( action_dim=int(model_cfg.action_dim), action_horizon=input_horizon, hidden_size=int(model_cfg.view_feature_dim), num_tokens=int(getattr(model_cfg, "motion_latent_num_tokens", 64)), mlp_hidden_dim=int(getattr(model_cfg, "motion_encoder_hidden_dim", model_cfg.view_feature_dim)), dropout=float(getattr(model_cfg, "motion_encoder_dropout", 0.0)), ) self.motion_latent_num_tokens = int(getattr(model_cfg, "motion_latent_num_tokens", 64)) self.motion_latent_token = str(getattr(model_cfg, "motion_latent_token", "<|motion_latent|>")) self.motion_latent_token_id: int | None = None @classmethod def from_pretrained(cls, run_dir, ckpt_step, launch_config, device): model = super().from_pretrained(run_dir, ckpt_step, launch_config, device) tokenizer = model.vlm_processor.tokenizer added_tokens = tokenizer.add_special_tokens( {"additional_special_tokens": [model.motion_latent_token]} ) if added_tokens > 0: embedding = model.vlm_model.get_input_embeddings() if len(tokenizer) > int(embedding.num_embeddings): try: model.vlm_model.resize_token_embeddings( len(tokenizer), pad_to_multiple_of=192, mean_resizing=True, ) except TypeError: model.vlm_model.resize_token_embeddings(len(tokenizer)) token_id = tokenizer.convert_tokens_to_ids(model.motion_latent_token) unk_token_id = getattr(tokenizer, "unk_token_id", None) if ( token_id is None or int(token_id) < 0 or (unk_token_id is not None and int(token_id) == int(unk_token_id)) ): raise ValueError( f"Motion latent token {model.motion_latent_token!r} is missing from tokenizer. " "Check that the checkpoint was trained with motion distill config." ) model.motion_latent_token_id = int(token_id) return model def _append_motion_latent_to_instructions(self, instructions: List[str]) -> List[str]: latent_text = " ".join([self.motion_latent_token] * self.motion_latent_num_tokens) output = [] for instruction in instructions: text = str(instruction or "").strip() if self.motion_latent_token in text: output.append(text) elif text: output.append(f"{text}\n{latent_text}") else: output.append(latent_text) return output def predict_action( self, observations, states: torch.Tensor, instructions: List[str], num_inference_steps: int, traj2ds, **kwargs, ) -> torch.Tensor: return super().predict_action( observations=observations, states=states, instructions=self._append_motion_latent_to_instructions(instructions), num_inference_steps=num_inference_steps, traj2ds=traj2ds, **kwargs, ) def predict_action_with_training_rtc_flow( self, observations, states: torch.Tensor, instructions: List[str], num_inference_steps: int, traj2ds, prev_actions: torch.Tensor = None, inference_delay: int = 0, max_delay: int = 0, **kwargs, ) -> torch.Tensor: return super().predict_action_with_training_rtc_flow( observations=observations, states=states, instructions=self._append_motion_latent_to_instructions(instructions), num_inference_steps=num_inference_steps, traj2ds=traj2ds, prev_actions=prev_actions, inference_delay=inference_delay, max_delay=max_delay, **kwargs, ) def _action_condition_hidden_states( self, vlm_hidden_states: torch.Tensor, input_ids: Optional[torch.Tensor] = None, attention_mask: Optional[torch.Tensor] = None, ) -> torch.Tensor: del attention_mask if input_ids is None: raise ValueError("Motion distill inference requires input_ids for latent token selection") hidden_states = self.extract_motion_latent_hidden(vlm_hidden_states, input_ids) return hidden_states.unsqueeze(1) def _vlm_kwargs( self, input_ids, attention_mask, pixel_values=None, image_grid_thw=None, input_features=None, feature_attention_mask=None, audio_feature_lengths=None, pixel_values_videos=None, video_grid_thw=None, video_second_per_grid=None, ) -> dict[str, Any]: vlm_attention_mask = attention_mask if vlm_attention_mask is not None and vlm_attention_mask.dtype == torch.bool: vlm_attention_mask = vlm_attention_mask.to(dtype=torch.long) vlm_kwargs = { "input_ids": input_ids, "attention_mask": vlm_attention_mask, "output_hidden_states": True, "return_dict": True, "use_cache": False, } optional_items = { "pixel_values": pixel_values, "image_grid_thw": image_grid_thw, "input_features": input_features, "feature_attention_mask": feature_attention_mask, "audio_feature_lengths": audio_feature_lengths, "pixel_values_videos": pixel_values_videos, "video_grid_thw": video_grid_thw, "video_second_per_grid": video_second_per_grid, } for key, value in optional_items.items(): if value is not None: vlm_kwargs[key] = value return vlm_kwargs def extract_vlm_hidden_states(self, **kwargs) -> torch.Tensor: return self._extract_vlm_hidden_states(self._vlm_kwargs(**kwargs)) def encode_motion_hidden( self, gt_actions: torch.Tensor, ) -> torch.Tensor: return self.motion_encoder(gt_actions) def extract_motion_latent_hidden( self, vlm_hidden_states: torch.Tensor, input_ids: torch.Tensor, ) -> torch.Tensor: if self.motion_latent_token_id is None: raise ValueError("motion_latent_token_id is not initialized") latent_mask = input_ids.eq(int(self.motion_latent_token_id)) latents = [] for batch_idx in range(input_ids.shape[0]): positions = latent_mask[batch_idx].nonzero(as_tuple=False).flatten() if int(positions.numel()) < self.motion_latent_num_tokens: raise ValueError( f"Expected at least {self.motion_latent_num_tokens} motion latent tokens, " f"got {int(positions.numel())}" ) positions = positions[-self.motion_latent_num_tokens:] latents.append(vlm_hidden_states[batch_idx, positions]) return torch.stack(latents, dim=0) def action_from_hidden( self, hidden_states: torch.Tensor, attention_mask: Optional[torch.Tensor], action_samples: torch.Tensor, states: torch.Tensor, timestep: torch.LongTensor, traj2ds: Optional[torch.Tensor] = None, progress: Optional[torch.Tensor] = None, future_progress: Optional[torch.Tensor] = None, return_dict: bool = True, ): if attention_mask is None or int(attention_mask.shape[-1]) != int(hidden_states.shape[1]): vlm_attn_mask = torch.ones( hidden_states.shape[:2], device=hidden_states.device, dtype=torch.long, ) else: vlm_attn_mask = attention_mask model_output = self.action_header( hidden_states=None, timestep=timestep, joint_attention_kwargs=dict( action_hidden_embeds=action_samples, views=hidden_states.unsqueeze(1), obs=states, traj2ds=traj2ds, progress=progress, future_progress=future_progress, ), vlm_attn_mask=vlm_attn_mask, return_dict=return_dict, ) return model_output def forward( self, input_ids, attention_mask, pixel_values, image_grid_thw, action_samples: torch.Tensor, states: torch.Tensor, timestep: torch.LongTensor, input_features=None, feature_attention_mask=None, audio_feature_lengths=None, pixel_values_videos=None, video_grid_thw=None, video_second_per_grid=None, traj2ds: Optional[torch.Tensor] = None, progress: Optional[torch.Tensor] = None, future_progress: Optional[torch.Tensor] = None, gt_actions: Optional[torch.Tensor] = None, use_motion_teacher: bool = False, return_hidden_states: bool = False, return_action: bool = True, return_dict: bool = True, skip_layers: Optional[List[int]] = None, ) -> Union[torch.Tensor, tuple[HumanFoundationModelOutput, torch.Tensor], HumanFoundationModelOutput]: del skip_layers if use_motion_teacher: if gt_actions is None: raise ValueError("use_motion_teacher=True requires gt_actions") hidden_states = self.encode_motion_hidden(gt_actions) hidden_attention_mask = None else: vlm_hidden_states = self.extract_vlm_hidden_states( input_ids=input_ids, attention_mask=attention_mask, pixel_values=pixel_values, image_grid_thw=image_grid_thw, input_features=input_features, feature_attention_mask=feature_attention_mask, audio_feature_lengths=audio_feature_lengths, pixel_values_videos=pixel_values_videos, video_grid_thw=video_grid_thw, video_second_per_grid=video_second_per_grid, ) hidden_states = self.extract_motion_latent_hidden(vlm_hidden_states, input_ids) hidden_attention_mask = None if not return_action: if not return_hidden_states: raise ValueError("return_action=False requires return_hidden_states=True") return hidden_states model_output = self.action_from_hidden( hidden_states=hidden_states, attention_mask=hidden_attention_mask, action_samples=action_samples, states=states, timestep=timestep, traj2ds=traj2ds, progress=progress, future_progress=future_progress, return_dict=return_dict, ) output = HumanFoundationModelOutput(action=model_output.action) if return_hidden_states: return output, hidden_states return output