File size: 13,062 Bytes
67c4f10
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
"""Inference client and high-level runtime for Parvulus robot policies."""

from pathlib import Path
from typing import Dict, List, Optional, Sequence, Tuple, Union

import cv2
import numpy as np
import torch
from PIL import Image

from action_utils import DEFAULT_SCALE_PATH, denormalize_action, load_action_scale
from parvulus import ParvulusModel


class ParvulusInference:
    """High-level inference runner for Parvulus robot policies."""

    def __init__(
        self,
        model_path: Union[str, Path] = ".",
        scale_path: Union[str, Path] = DEFAULT_SCALE_PATH,
        device: str = "auto",
        use_kv_cache: bool = True,
        load_auxiliary: bool = False,
        chunk_size: int = 8,
        replan_stride: int = 8,
        num_ode_steps: int = 10,
        resolution: int = 256,
        flip_convention: bool = True,
        gamma: Optional[float] = None,
        auto_gamma: bool = False,
        backend: str = "pytorch",
    ):
        """Initializes Parvulus inference runner.

        Args:
            model_path: Directory containing parvulus.safetensors and config.json.
            scale_path: Path to action_scale.json.
            device: 'cuda', 'cpu', 'xpu', 'hybrid', or 'auto'.
                    'hybrid' executes visual perception on Intel iGPU (xpu) and the ODE loop on CPU.
            use_kv_cache: Precomputes visual keys/values for fast 38ms action chunking.
            load_auxiliary: If True, also loads video, change, and self-reconstruction heads.
            chunk_size: Action chunk horizon (default 8).
            replan_stride: Frequency to replan in step() (default 8).
            num_ode_steps: Euler integration steps (default 10).
            resolution: Camera resolution (256x256).
            flip_convention: Applies standard Robosuite/LIBERO camera orientation flip.
            gamma: Explicit exposure correction gamma.
            auto_gamma: Adaptively brightens underexposed manipulation scenes.
            backend: Runtime execution engine: 'pytorch' (standard), 'cuda_c' (pure C ABI),
                     or 'cuda_graph' (CUDA Graph replay, 7.68ms / chunk).
        """
        self.backend = backend
        self.chunk_size = chunk_size
        self.replan_stride = replan_stride
        self.num_ode_steps = num_ode_steps
        self.resolution = resolution
        self.flip_convention = flip_convention
        self.use_kv_cache = use_kv_cache
        self.gamma = gamma
        self.auto_gamma = auto_gamma

        if backend in ("parvulus", "pure_cuda", "cuda_c", "cuda_graph"):
            from c_engine.infer_c import ParvulusEngine, ParvulusCUDAGraphEngine
            dev_id = 0
            if isinstance(device, str) and "cuda:" in device:
                dev_id = int(device.split(":")[-1])
            elif isinstance(device, torch.device) and device.index is not None:
                dev_id = device.index

            if backend in ("parvulus", "pure_cuda", "cuda_c"):
                self._c_engine = ParvulusEngine(
                    device_id=dev_id,
                    chunk_size=chunk_size,
                    replan_stride=replan_stride,
                    num_ode_steps=num_ode_steps,
                )
            elif backend == "cuda_graph":
                self._c_engine = ParvulusCUDAGraphEngine(
                    device_id=dev_id,
                    chunk_size=chunk_size,
                    replan_stride=replan_stride,
                    num_ode_steps=num_ode_steps,
                )
            return

        self.is_hybrid = False
        if device == "auto":
            if torch.cuda.is_available():
                self.device = torch.device("cuda")
            elif hasattr(torch, "xpu") and torch.xpu.is_available():
                self.device = torch.device("xpu")
            else:
                self.device = torch.device("cpu")
        elif device == "hybrid":
            if not (hasattr(torch, "xpu") and torch.xpu.is_available()):
                print("[Warning] Device 'hybrid' requested but torch.xpu is unavailable. Falling back to CPU.")
                self.device = torch.device("cpu")
            else:
                self.is_hybrid = True
                self.device = torch.device("cpu")
                self.xpu_device = torch.device("xpu")
        else:
            self.device = torch.device(device)

        eff_gamma = self.gamma if (self.gamma is not None and self.gamma != 1.0) else 0.55
        self._gamma_lut = np.array([((i / 255.0) ** eff_gamma) * 255.0 for i in range(256)]).astype(np.uint8)

        self.action_min, self.action_max = load_action_scale(scale_path)
        self.action_min = self.action_min.to(self.device)
        self.action_max = self.action_max.to(self.device)

        self.model = ParvulusModel.from_pretrained(
            pretrained_path=model_path,
            load_auxiliary=load_auxiliary,
            load_text_encoder=True,
            device=self.device,
        )

        if self.is_hybrid:
            self.model.encoder = self.model.encoder.to(self.xpu_device).eval()

        self.current_task_desc: Optional[str] = None
        self.current_text_emb: Optional[torch.Tensor] = None
        self.current_text_emb_xpu: Optional[torch.Tensor] = None
        self.action_queue: List[np.ndarray] = []
        self.stride_counter: int = 0

    @classmethod
    def from_pretrained(cls, pretrained_path: Union[str, Path] = ".", **kwargs) -> "ParvulusInference":
        return cls(model_path=pretrained_path, **kwargs)

    def preprocess_image(self, img: np.ndarray, target_device: Optional[torch.device] = None) -> torch.Tensor:
        """Preprocesses [H, W, 3] uint8 image -> [1, 3, res, res] tensor in [-1, 1]."""
        dev = target_device or self.device
        if self.flip_convention:
            img = np.ascontiguousarray(img[::-1, ::-1])
        if img.shape[0] != self.resolution or img.shape[1] != self.resolution:
            img = np.array(Image.fromarray(img).resize((self.resolution, self.resolution), Image.BILINEAR))

        if self.gamma is not None and self.gamma != 1.0:
            img = cv2.LUT(img, self._gamma_lut)
        elif self.auto_gamma:
            mean_y = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY).mean()
            if mean_y < 112.0:
                img = cv2.LUT(img, self._gamma_lut)

        t = torch.from_numpy(img).float() / 255.0
        t = t.permute(2, 0, 1) * 2.0 - 1.0
        return t.unsqueeze(0).to(dev)

    def reset(self, task_description: Optional[str] = None, text_emb: Optional[torch.Tensor] = None):
        """Resets policy state for a new episode."""
        if self.backend in ("parvulus", "pure_cuda", "cuda_c"):
            if text_emb is not None:
                vec = text_emb.detach().cpu().numpy()
                self._c_engine.reset(task_description=task_description, task_vector=vec)
            elif task_description is not None:
                # If model with text_encoder is loaded, encode on the fly to support perturbations
                if hasattr(self, "model") and hasattr(self.model, "text_encoder") and self.model.text_encoder is not None:
                    with torch.no_grad():
                        vec = self.model.text_encoder([task_description]).detach().cpu().numpy()[0]
                    self._c_engine.reset(task_description=task_description, task_vector=vec)
                else:
                    self._c_engine.reset(task_description=task_description)
            else:
                self._c_engine.reset()
            return
        elif self.backend == "cuda_graph":
            self._c_engine.reset(task_description)
            return

        self.current_task_desc = task_description
        if text_emb is not None:
            self.current_text_emb = text_emb if text_emb.dim() == 2 else text_emb.unsqueeze(0).to(self.device)
        elif task_description is not None:
            with torch.no_grad():
                self.current_text_emb = self.model.text_encoder([task_description]).to(self.device)
        else:
            self.current_text_emb = None

        if self.is_hybrid and self.current_text_emb is not None:
            self.current_text_emb_xpu = self.current_text_emb.to(self.xpu_device)

        self.action_queue.clear()
        self.stride_counter = 0

    @torch.no_grad()
    def predict_chunk(
        self,
        agentview_img: np.ndarray,
        wrist_img: np.ndarray,
        task_description: Optional[str] = None,
        denormalize: bool = True,
    ) -> np.ndarray:
        """Predicts full 8-step continuous action chunk [8, 7]."""
        if self.backend in ("parvulus", "pure_cuda", "cuda_c", "cuda_graph"):
            return self._c_engine.predict_chunk(agentview_img, wrist_img, task_description=task_description)

        if task_description is not None:
            self.reset(task_description)
        elif self.current_text_emb is None:
            raise ValueError("Call reset(task_description=...) before predicting actions!")

        if self.is_hybrid:
            agent_t = self.preprocess_image(agentview_img, target_device=self.xpu_device)
            wrist_t = self.preprocess_image(wrist_img, target_device=self.xpu_device)
            obs_xpu = torch.cat([agent_t, wrist_t], dim=1)

            _, z_vis_xpu = self.model.encoder(obs_xpu, self.current_text_emb_xpu)
            torch.xpu.synchronize()

            z_vis = z_vis_xpu.to(self.device)

            z = torch.randn(1, self.chunk_size, self.model.action_dim, device=self.device)
            kv_layers = self.model._get_kv_layers()
            kv_caches = [layer.precompute_kv(z_vis) for layer in kv_layers]
            dt = 1.0 / self.num_ode_steps

            for step in range(self.num_ode_steps):
                sigma = torch.tensor([step * dt], device=self.device)
                cond = self.current_text_emb + self.model.action_t_mlp(sigma)
                h = self.model.action_decoder.action_in_proj(z) + self.model.action_decoder.pos_emb[:, :self.chunk_size, :]
                for layer, (k_v, v_v) in zip(kv_layers, kv_caches):
                    h = layer.forward_with_kv(h, k_v, v_v, cond)
                v_pred = self.model.action_decoder.out_head(self.model.action_decoder.final_norm(h))
                z = z + v_pred * dt

            chunk_norm = z.clamp(-1.0, 1.0)
        else:
            agent_t = self.preprocess_image(agentview_img)
            wrist_t = self.preprocess_image(wrist_img)
            obs = torch.cat([agent_t, wrist_t], dim=1)

            chunk_norm = self.model.predict_action_chunk(
                obs,
                text_emb=self.current_text_emb,
                num_ode_steps=self.num_ode_steps,
                use_kv_cache=self.use_kv_cache,
            )

        if denormalize:
            chunk = denormalize_action(chunk_norm.squeeze(0), self.action_min, self.action_max)
        else:
            chunk = chunk_norm.squeeze(0)

        return chunk.cpu().numpy()

    def step(self, agentview_img: np.ndarray, wrist_img: np.ndarray) -> np.ndarray:
        """Returns single 7-DoF action, replanning every C=replan_stride steps."""
        if self.backend in ("parvulus", "pure_cuda", "cuda_c", "cuda_graph"):
            return self._c_engine.step(agentview_img, wrist_img)

        if not self.action_queue or self.stride_counter >= self.replan_stride:
            chunk = self.predict_chunk(agentview_img, wrist_img, denormalize=True)
            self.action_queue = list(chunk[: self.replan_stride])
            self.stride_counter = 0

        action = self.action_queue.pop(0)
        self.stride_counter += 1
        return action

    @torch.no_grad()
    def rollout_step(
        self,
        agentview_img: np.ndarray,
        wrist_img: np.ndarray,
        task_description: Optional[str] = None,
    ) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
        """Auxiliary world-model step: predicts action chunk AND next dual-camera observation.

        Returns:
            action_chunk: [8, 7] float32 action chunk.
            next_agent_img: [256, 256, 3] uint8 predicted agentview.
            next_wrist_img: [256, 256, 3] uint8 predicted wristview.
        """
        action_chunk = self.predict_chunk(agentview_img, wrist_img, task_description=task_description)

        agent_t = self.preprocess_image(agentview_img)
        wrist_t = self.preprocess_image(wrist_img)
        obs = torch.cat([agent_t, wrist_t], dim=1)

        pred_obs = self.model.predict_next_frame(obs, text_emb=self.current_text_emb, num_ode_steps=self.num_ode_steps)
        pred_obs = ((pred_obs.squeeze(0) + 1.0) / 2.0 * 255.0).clamp(0, 255).byte().permute(1, 2, 0).cpu().numpy()

        next_agent = pred_obs[:, :, 0:3]
        next_wrist = pred_obs[:, :, 3:6]
        if self.flip_convention:
            next_agent = np.ascontiguousarray(next_agent[::-1, ::-1])
            next_wrist = np.ascontiguousarray(next_wrist[::-1, ::-1])

        return action_chunk, next_agent, next_wrist


ParvulusInfer = ParvulusInference

__all__ = ["ParvulusInference", "ParvulusInfer"]