File size: 17,811 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
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
"""C++ and CUDA inference backends for Parvulus."""

import ctypes
import os
import time
from pathlib import Path
from typing import Dict, List, Optional, Sequence, Tuple, Union
import numpy as np
import torch

C_ENGINE_DIR = Path(__file__).resolve().parent
LIB_PATH = C_ENGINE_DIR / "libparvulus.so"

_lib = None
if LIB_PATH.exists():
    try:
        _lib = ctypes.CDLL(str(LIB_PATH))
        _lib.parvulus_create.argtypes = [ctypes.c_int]
        _lib.parvulus_create.restype = ctypes.c_void_p

        _lib.parvulus_load.argtypes = [
            ctypes.c_void_p,
            ctypes.c_char_p,
            ctypes.c_char_p,
            ctypes.c_char_p,
        ]
        _lib.parvulus_load.restype = ctypes.c_bool

        _lib.parvulus_set_task_vector.argtypes = [
            ctypes.c_void_p,
            ctypes.POINTER(ctypes.c_float),
        ]
        _lib.parvulus_set_task_vector.restype = ctypes.c_bool

        _lib.parvulus_set_task_string.argtypes = [
            ctypes.c_void_p,
            ctypes.c_char_p,
        ]
        _lib.parvulus_set_task_string.restype = ctypes.c_bool

        _lib.parvulus_predict_chunk.argtypes = [
            ctypes.c_void_p,
            ctypes.POINTER(ctypes.c_uint8),
            ctypes.POINTER(ctypes.c_uint8),
            ctypes.c_char_p,
            ctypes.POINTER(ctypes.c_float),
            ctypes.c_int,
            ctypes.POINTER(ctypes.c_float),
        ]
        _lib.parvulus_predict_chunk.restype = ctypes.c_bool

        _lib.parvulus_predict_chunk_with_vector.argtypes = [
            ctypes.c_void_p,
            ctypes.POINTER(ctypes.c_uint8),
            ctypes.POINTER(ctypes.c_uint8),
            ctypes.POINTER(ctypes.c_float),
            ctypes.POINTER(ctypes.c_float),
            ctypes.c_int,
            ctypes.POINTER(ctypes.c_float),
        ]
        _lib.parvulus_predict_chunk_with_vector.restype = ctypes.c_bool

        _lib.parvulus_predict_chunk_with_emb.argtypes = [
            ctypes.c_void_p,
            ctypes.POINTER(ctypes.c_uint8),
            ctypes.POINTER(ctypes.c_uint8),
            ctypes.POINTER(ctypes.c_float),
            ctypes.POINTER(ctypes.c_float),
            ctypes.c_int,
            ctypes.POINTER(ctypes.c_float),
        ]
        _lib.parvulus_predict_chunk_with_emb.restype = ctypes.c_bool

        _lib.parvulus_register_task_embedding.argtypes = [
            ctypes.c_void_p,
            ctypes.c_char_p,
            ctypes.POINTER(ctypes.c_float),
        ]
        _lib.parvulus_register_task_embedding.restype = ctypes.c_bool

        _lib.parvulus_destroy.argtypes = [ctypes.c_void_p]
        _lib.parvulus_destroy.restype = None
    except Exception as e:
        print(f"[Warning] Could not load libparvulus.so: {e}")
        _lib = None


class ParvulusEngine:
    """Native Parvulus CUDA/cuBLAS runtime engine (Zero LibTorch, Zero PyTorch)."""

    def __init__(
        self,
        device_id: int = 0,
        weights_path: Optional[Union[str, Path]] = None,
        embeddings_path: Optional[Union[str, Path]] = None,
        scale_path: Optional[Union[str, Path]] = None,
        chunk_size: int = 8,
        replan_stride: int = 8,
        num_ode_steps: int = 10,
    ):
        if _lib is None:
            raise RuntimeError(f"libparvulus.so not loaded. Check: {LIB_PATH}")

        self.device_id = device_id
        self.chunk_size = chunk_size
        self.replan_stride = replan_stride
        self.num_ode_steps = num_ode_steps

        if weights_path is None:
            weights_path = C_ENGINE_DIR / "parvulus_model_weights.bin"
        if embeddings_path is None:
            embeddings_path = C_ENGINE_DIR / "task_embeddings.bin"
        if scale_path is None:
            scale_path = C_ENGINE_DIR / "action_scale.bin"

        weights_path = Path(weights_path)
        embeddings_path = Path(embeddings_path)
        scale_path = Path(scale_path)

        if not weights_path.exists():
            print(f"[ParvulusEngine] Generating {weights_path.name} from SafeTensors...")
            import sys
            if str(C_ENGINE_DIR) not in sys.path:
                sys.path.insert(0, str(C_ENGINE_DIR))
            from export_from_safetensors import export_binary_weights, export_action_scale, export_task_embeddings

            st_path = C_ENGINE_DIR.parent / "parvulus.safetensors"
            if not st_path.exists():
                st_path = C_ENGINE_DIR.parent / "model.safetensors"
            export_binary_weights(str(st_path), str(weights_path))
            if not scale_path.exists():
                export_action_scale(str(C_ENGINE_DIR.parent / "action_scale.json"), str(scale_path))
            if not embeddings_path.exists() and (C_ENGINE_DIR / "canonical_tasks.json").exists():
                export_task_embeddings(str(C_ENGINE_DIR / "canonical_tasks.json"), str(st_path), str(embeddings_path))

        self.handle = _lib.parvulus_create(self.device_id)
        if not self.handle:
            raise RuntimeError(f"Failed to initialize Parvulus engine on device {self.device_id}")

        ok = _lib.parvulus_load(
            self.handle,
            str(weights_path).encode("utf-8"),
            str(embeddings_path).encode("utf-8"),
            str(scale_path).encode("utf-8"),
        )
        if not ok:
            _lib.parvulus_destroy(self.handle)
            self.handle = None
            raise RuntimeError("Failed to load Parvulus engine weights!")

        self.current_task_desc: Optional[str] = None
        self.action_queue = []
        self.stride_counter: int = 0
        self._actions_buffer = np.zeros(56, dtype=np.float32)

        self._onnx_session = None
        self._tokenizer = None
        self._torch_text_encoder = None

    def __del__(self):
        if hasattr(self, "handle") and self.handle is not None and _lib is not None:
            _lib.parvulus_destroy(self.handle)
            self.handle = None

    def set_task_vector(self, task_vector: np.ndarray) -> bool:
        """Sets the active 256-dim task conditioning vector on the GPU."""
        vec_c = np.ascontiguousarray(task_vector.flatten()[:256], dtype=np.float32)
        vec_ptr = vec_c.ctypes.data_as(ctypes.POINTER(ctypes.c_float))
        return bool(_lib.parvulus_set_task_vector(self.handle, vec_ptr))

    def encode_text(self, text: str) -> np.ndarray:
        """Encodes arbitrary natural language prompts or perturbations into 256-dim vector."""
        onnx_path = C_ENGINE_DIR / "parvulus_text_encoder.onnx"
        if not onnx_path.exists() and os.environ.get("PARVULUS_HF_REPO"):
            try:
                from huggingface_hub import hf_hub_download
                hf_hub_download(
                    repo_id=os.environ["PARVULUS_HF_REPO"],
                    filename="parvulus_text_encoder.onnx",
                    subfolder="c_engine",
                    local_dir=str(C_ENGINE_DIR.parent),
                )
            except Exception:
                pass

        if onnx_path.exists():
            try:
                import onnxruntime as ort
                from transformers import AutoTokenizer

                if self._onnx_session is None:
                    available = ort.get_available_providers()
                    providers = [p for p in ["CUDAExecutionProvider", "CPUExecutionProvider"] if p in available]
                    self._onnx_session = ort.InferenceSession(str(onnx_path), providers=providers)
                    self._tokenizer = AutoTokenizer.from_pretrained("google-t5/t5-small")

                tokens = self._tokenizer([text], padding=True, truncation=True, return_tensors="np")
                ort_out = self._onnx_session.run(
                    None,
                    {
                        "input_ids": tokens["input_ids"].astype(np.int64),
                        "attention_mask": tokens["attention_mask"].astype(np.int64),
                    },
                )[0]
                return ort_out[0].astype(np.float32)
            except Exception:
                pass

        try:
            from text_encoder import ParvulusTextEncoder
            from safetensors.torch import load_file

            if self._torch_text_encoder is None:
                te = ParvulusTextEncoder().cpu().eval()
                st_path = C_ENGINE_DIR.parent / "parvulus.safetensors"
                if st_path.exists():
                    weights = load_file(str(st_path))
                    proj_dict = {
                        "0.weight": weights["text_encoder.proj.0.weight"],
                        "0.bias": weights["text_encoder.proj.0.bias"],
                        "2.weight": weights["text_encoder.proj.2.weight"],
                        "2.bias": weights["text_encoder.proj.2.bias"],
                    }
                    te.proj.load_state_dict(proj_dict)
                self._torch_text_encoder = te

            with torch.no_grad():
                emb = self._torch_text_encoder([text]).cpu().numpy()[0]
            return emb.astype(np.float32)
        except Exception as e:
            raise RuntimeError(
                f"Failed to dynamically encode text prompt '{text}': {e}. "
                "Ensure onnxruntime or transformers/torch is installed."
            )

    def reset(
        self,
        task_description: Optional[str] = None,
        task_vector: Optional[np.ndarray] = None,
    ):
        if task_vector is not None:
            self.set_task_vector(task_vector)
            self.current_task_desc = task_description or "<vector>"
        elif task_description is not None:
            self.current_task_desc = task_description
            task_bytes = task_description.encode("utf-8")
            if not _lib.parvulus_set_task_string(self.handle, task_bytes):
                encoded_vec = self.encode_text(task_description)
                self.set_task_vector(encoded_vec)

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

    def predict_chunk(
        self,
        agentview_img: np.ndarray,
        wrist_img: np.ndarray,
        task_description: Optional[str] = None,
        task_vector: Optional[np.ndarray] = None,
        custom_z0: Optional[np.ndarray] = None,
    ) -> np.ndarray:
        if task_vector is not None:
            self.set_task_vector(task_vector)
        elif task_description is not None:
            self.reset(task_description=task_description)

        agent_c = np.ascontiguousarray(agentview_img, dtype=np.uint8)
        wrist_c = np.ascontiguousarray(wrist_img, dtype=np.uint8)

        agent_ptr = agent_c.ctypes.data_as(ctypes.POINTER(ctypes.c_uint8))
        wrist_ptr = wrist_c.ctypes.data_as(ctypes.POINTER(ctypes.c_uint8))
        out_ptr = self._actions_buffer.ctypes.data_as(ctypes.POINTER(ctypes.c_float))

        z0_ptr = None
        if custom_z0 is not None:
            z0_c = np.ascontiguousarray(custom_z0, dtype=np.float32)
            z0_ptr = z0_c.ctypes.data_as(ctypes.POINTER(ctypes.c_float))

        ok = _lib.parvulus_predict_chunk(
            self.handle,
            agent_ptr,
            wrist_ptr,
            None,
            out_ptr,
            self.num_ode_steps,
            z0_ptr,
        )
        if not ok:
            raise RuntimeError("Parvulus predict_chunk failed!")

        return self._actions_buffer.reshape(8, 7).copy()

    def predict_chunk_with_emb(
        self,
        agentview_img: np.ndarray,
        wrist_img: np.ndarray,
        text_embedding: np.ndarray,
        custom_z0: Optional[np.ndarray] = None,
    ) -> np.ndarray:
        """Predicts action chunk using a directly provided 256-dim text embedding vector."""
        return self.predict_chunk(
            agentview_img,
            wrist_img,
            task_vector=text_embedding,
            custom_z0=custom_z0,
        )

    def register_task_embedding(self, task_description: str, text_embedding: np.ndarray) -> bool:
        """Dynamically registers or updates a task embedding vector in engine memory."""
        task_bytes = task_description.encode("utf-8")
        emb_c = np.ascontiguousarray(text_embedding.flatten()[:256], dtype=np.float32)
        emb_ptr = emb_c.ctypes.data_as(ctypes.POINTER(ctypes.c_float))
        return bool(_lib.parvulus_register_task_embedding(self.handle, task_bytes, emb_ptr))

    def step(self, agentview_img: np.ndarray, wrist_img: np.ndarray) -> np.ndarray:
        if not self.action_queue or self.stride_counter >= self.replan_stride:
            chunk = self.predict_chunk(agentview_img, wrist_img)
            self.action_queue = list(chunk[: self.replan_stride])
            self.stride_counter = 0

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


# Aliases for backward compatibility
ParvulusPureCUDAEngine = ParvulusEngine
ParvulusCUDAEngine = ParvulusEngine


class ParvulusCUDAGraphEngine:
    """CUDA Graph replay engine for PyTorch models.

    Pre-allocates static GPU buffers, captures the complete ODE Transformer loop into
    a CUDA execution graph, and replays it with zero host kernel launch overhead.
    """

    def __init__(
        self,
        device_id: int = 0,
        model_path: Optional[Union[str, Path]] = None,
        embeddings_path: Optional[Union[str, Path]] = None,
        scale_path: Optional[Union[str, Path]] = None,
        chunk_size: int = 8,
        replan_stride: int = 8,
        num_ode_steps: int = 10,
    ):
        self.device = torch.device(f"cuda:{device_id}")
        self.chunk_size = chunk_size
        self.replan_stride = replan_stride
        self.num_ode_steps = num_ode_steps

        if model_path is None:
            model_path = C_ENGINE_DIR / "parvulus_traced_model.pt"

        if embeddings_path is None:
            embeddings_path = C_ENGINE_DIR / "task_embeddings.bin"
        if scale_path is None:
            scale_path = C_ENGINE_DIR / "action_scale.bin"

        import struct
        with open(scale_path, "rb") as f:
            min_vals = struct.unpack("<7f", f.read(28))
            max_vals = struct.unpack("<7f", f.read(28))
        self.action_min = torch.tensor(min_vals, device=self.device, dtype=torch.float32).view(1, 1, 7)
        self.action_max = torch.tensor(max_vals, device=self.device, dtype=torch.float32).view(1, 1, 7)

        self.task_embeddings: Dict[str, torch.Tensor] = {}
        with open(embeddings_path, "rb") as f:
            num_tasks = struct.unpack("<I", f.read(4))[0]
            for _ in range(num_tasks):
                s_len = struct.unpack("<H", f.read(2))[0]
                task_str = f.read(s_len).decode("utf-8")
                emb = struct.unpack("<256f", f.read(1024))
                self.task_embeddings[task_str] = torch.tensor(emb, device=self.device, dtype=torch.float32).view(1, 256)

        if Path(model_path).exists():
            self.model = torch.jit.load(str(model_path), map_location=self.device).eval()

            self.static_obs = torch.zeros(1, 6, 256, 256, device=self.device, dtype=torch.float32)
            self.static_text = torch.zeros(1, 256, device=self.device, dtype=torch.float32)
            self.static_z0 = torch.zeros(1, 8, 7, device=self.device, dtype=torch.float32)
            self.static_out = torch.zeros(1, 8, 7, device=self.device, dtype=torch.float32)

            s = torch.cuda.Stream(device=self.device)
            s.wait_stream(torch.cuda.current_stream(device=self.device))
            with torch.cuda.stream(s):
                for _ in range(5):
                    _ = self.model(self.static_obs, self.static_text, self.static_z0)
            torch.cuda.current_stream(device=self.device).wait_stream(s)

            self.graph = torch.cuda.CUDAGraph()
            with torch.cuda.graph(self.graph, stream=s):
                self.static_out = self.model(self.static_obs, self.static_text, self.static_z0)
        else:
            self.model = None
            self.graph = None

        self.current_task_desc: Optional[str] = None
        self.action_queue = []
        self.stride_counter: int = 0

    def reset(self, task_description: Optional[str] = None):
        if task_description is not None:
            self.current_task_desc = task_description
            if self.graph is not None and task_description in self.task_embeddings:
                self.static_text.copy_(self.task_embeddings[task_description])
        self.action_queue.clear()
        self.stride_counter = 0

    def predict_chunk(
        self,
        agentview_img: np.ndarray,
        wrist_img: np.ndarray,
        task_description: Optional[str] = None,
    ) -> np.ndarray:
        if self.graph is None:
            raise RuntimeError("ParvulusCUDAGraphEngine requires a valid TorchScript model.")

        if task_description is not None:
            self.reset(task_description)

        combined_np = np.stack([agentview_img, wrist_img], axis=0)
        combined_t = torch.from_numpy(combined_np).to(self.device, non_blocking=True).float()
        combined_t = combined_t.flip(1, 2)
        combined_t = (combined_t / 127.5) - 1.0
        self.static_obs.copy_(combined_t.permute(0, 3, 1, 2).reshape(1, 6, 256, 256))

        self.static_z0.normal_()
        self.graph.replay()

        actions = (self.static_out + 1.0) * 0.5 * (self.action_max - self.action_min) + self.action_min
        return actions.squeeze(0).detach().cpu().numpy()

    def step(self, agentview_img: np.ndarray, wrist_img: np.ndarray) -> np.ndarray:
        if not self.action_queue or self.stride_counter >= self.replan_stride:
            chunk = self.predict_chunk(agentview_img, wrist_img)
            self.action_queue = list(chunk[: self.replan_stride])
            self.stride_counter = 0

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