File size: 13,790 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
"""Standalone high-performance policy runtime for Parvulus via native C ABI ctypes."""

import ctypes
import os
from pathlib import Path
from typing import Dict, List, Optional, Sequence, Union

import numpy as np

_MODULE_DIR = Path(__file__).resolve().parent
_C_ENGINE_DIR = _MODULE_DIR / "c_engine"
_LIB_PATH = _C_ENGINE_DIR / "libparvulus.so"

_lib: Optional[ctypes.CDLL] = None


def _load_c_abi() -> ctypes.CDLL:
    global _lib
    if _lib is not None:
        return _lib

    if not _LIB_PATH.exists():
        try:
            import subprocess
            print(f"[ParvulusRuntime] Building {_LIB_PATH.name}...")
            subprocess.run(["make", "libparvulus.so"], cwd=str(_C_ENGINE_DIR), check=True)
        except Exception:
            pass

    if not _LIB_PATH.exists():
        raise FileNotFoundError(
            f"Pure CUDA shared library not found at {_LIB_PATH}. "
            f"Run 'make' or './build.sh' inside {_C_ENGINE_DIR} first."
        )

    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_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

    _lib = lib
    return _lib


class ParvulusRuntime:
    """Standalone high-performance Parvulus policy inference engine (Vector-First)."""

    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_euler_steps: int = 10,
    ):
        self.device_id = device_id
        self.chunk_size = chunk_size
        self.replan_stride = replan_stride
        self.num_euler_steps = num_euler_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"

        self.weights_path = Path(weights_path)
        self.embeddings_path = Path(embeddings_path) if embeddings_path else None
        self.scale_path = Path(scale_path) if scale_path else None

        if not self.weights_path.exists():
            print(f"[ParvulusRuntime] Generating {self.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 = _MODULE_DIR / "parvulus.safetensors"
            if not st_path.exists():
                st_path = _MODULE_DIR / "model.safetensors"
            export_binary_weights(str(st_path), str(self.weights_path))
            if self.scale_path and not self.scale_path.exists():
                export_action_scale(str(_MODULE_DIR / "action_scale.json"), str(self.scale_path))
            if self.embeddings_path and not self.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(self.embeddings_path))

        self._c_lib = _load_c_abi()
        self.handle = self._c_lib.parvulus_create(self.device_id)
        if not self.handle:
            raise RuntimeError(f"Failed to create Parvulus C/CUDA engine on GPU {self.device_id}")

        emb_c = str(self.embeddings_path).encode("utf-8") if (self.embeddings_path and self.embeddings_path.exists()) else None
        scl_c = str(self.scale_path).encode("utf-8") if (self.scale_path and self.scale_path.exists()) else None

        ok = self._c_lib.parvulus_load(
            self.handle,
            str(self.weights_path).encode("utf-8"),
            emb_c,
            scl_c,
        )
        if not ok:
            self._c_lib.parvulus_destroy(self.handle)
            self.handle = None
            raise RuntimeError("Failed to load Parvulus model weights into GPU VRAM!")

        self.current_task_desc: Optional[str] = None
        self.current_task_vector: Optional[np.ndarray] = None
        self.action_queue: List[np.ndarray] = []
        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 self._c_lib is not None:
            self._c_lib.parvulus_destroy(self.handle)
            self.handle = None

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

    def reset(
        self,
        task_description: Optional[str] = None,
        task_vector: Optional[Union[np.ndarray, Sequence[float]]] = None,
    ):
        """Resets action queue and updates task conditioning for a new episode.

        Accepts:
        - task_vector: 256-dim float vector (first-class input).
        - task_description: text instruction string (automatically handles perturbations
          by encoding them on the fly into 256-dim conditioning vectors).
        """
        if task_vector is not None:
            self.set_task_vector(task_vector)
            self.current_task_desc = task_description or "<custom_vector>"
        elif task_description is not None:
            self.current_task_desc = task_description
            task_bytes = task_description.encode("utf-8")
            if not self._c_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 register_task_embedding(self, task_description: str, text_embedding: np.ndarray) -> bool:
        """Registers a 256-dim embedding vector for a task description in engine memory."""
        task_bytes = task_description.encode("utf-8")
        emb_c = np.ascontiguousarray(text_embedding, dtype=np.float32).flatten()[:256]
        emb_ptr = emb_c.ctypes.data_as(ctypes.POINTER(ctypes.c_float))
        return bool(self._c_lib.parvulus_register_task_embedding(self.handle, task_bytes, emb_ptr))

    def encode_text(self, text: str) -> np.ndarray:
        """Encodes an arbitrary natural language prompt into a 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(_MODULE_DIR),
                )
            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:
            import torch
            from text_encoder import ParvulusTextEncoder
            from safetensors.torch import load_file

            if self._torch_text_encoder is None:
                te = ParvulusTextEncoder().cpu().eval()
                st_path = _MODULE_DIR / "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 predict_chunk(
        self,
        agentview_img: np.ndarray,
        wrist_img: np.ndarray,
        task_vector: Optional[Union[np.ndarray, Sequence[float]]] = None,
        task_description: Optional[str] = None,
        custom_z0: Optional[np.ndarray] = None,
    ) -> np.ndarray:
        """Predicts an [8, 7] action chunk using active or newly provided task conditioning."""
        if task_vector is not None or task_description is not None:
            self.reset(task_description=task_description, task_vector=task_vector)

        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 = self._c_lib.parvulus_predict_chunk(
            self.handle,
            agent_ptr,
            wrist_ptr,
            None,
            out_ptr,
            self.num_euler_steps,
            z0_ptr,
        )
        if not ok:
            raise RuntimeError("Pure CUDA predict_chunk failed! Check task conditioning vector.")

        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:
        """Alias for predict_chunk(agentview_img, wrist_img, task_vector=text_embedding)."""
        return self.predict_chunk(
            agentview_img=agentview_img,
            wrist_img=wrist_img,
            task_vector=text_embedding,
            custom_z0=custom_z0,
        )

    def step(self, agentview_img: np.ndarray, wrist_img: np.ndarray) -> np.ndarray:
        """Executes a single 7-DoF step, automatically managing the receding horizon action queue."""
        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