dhb-xr 0.2.1__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- dhb_xr/__init__.py +61 -0
- dhb_xr/cli.py +206 -0
- dhb_xr/core/__init__.py +28 -0
- dhb_xr/core/geometry.py +167 -0
- dhb_xr/core/geometry_torch.py +77 -0
- dhb_xr/core/types.py +113 -0
- dhb_xr/database/__init__.py +10 -0
- dhb_xr/database/motion_db.py +79 -0
- dhb_xr/database/retrieval.py +6 -0
- dhb_xr/database/similarity.py +71 -0
- dhb_xr/decoder/__init__.py +13 -0
- dhb_xr/decoder/decoder_torch.py +52 -0
- dhb_xr/decoder/dhb_dr.py +261 -0
- dhb_xr/decoder/dhb_qr.py +89 -0
- dhb_xr/encoder/__init__.py +27 -0
- dhb_xr/encoder/dhb_dr.py +418 -0
- dhb_xr/encoder/dhb_qr.py +129 -0
- dhb_xr/encoder/dhb_ti.py +204 -0
- dhb_xr/encoder/encoder_torch.py +54 -0
- dhb_xr/encoder/padding.py +82 -0
- dhb_xr/generative/__init__.py +78 -0
- dhb_xr/generative/flow_matching.py +705 -0
- dhb_xr/generative/latent_encoder.py +536 -0
- dhb_xr/generative/sampling.py +203 -0
- dhb_xr/generative/training.py +475 -0
- dhb_xr/generative/vfm_tokenizer.py +485 -0
- dhb_xr/integration/__init__.py +13 -0
- dhb_xr/integration/vla/__init__.py +11 -0
- dhb_xr/integration/vla/libero.py +132 -0
- dhb_xr/integration/vla/pipeline.py +85 -0
- dhb_xr/integration/vla/robocasa.py +85 -0
- dhb_xr/losses/__init__.py +16 -0
- dhb_xr/losses/geodesic_loss.py +91 -0
- dhb_xr/losses/hybrid_loss.py +36 -0
- dhb_xr/losses/invariant_loss.py +73 -0
- dhb_xr/optimization/__init__.py +72 -0
- dhb_xr/optimization/casadi_solver.py +342 -0
- dhb_xr/optimization/constraints.py +32 -0
- dhb_xr/optimization/cusadi_solver.py +311 -0
- dhb_xr/optimization/export_casadi_decode.py +111 -0
- dhb_xr/optimization/fatrop_solver.py +477 -0
- dhb_xr/optimization/torch_solver.py +85 -0
- dhb_xr/preprocessing/__init__.py +42 -0
- dhb_xr/preprocessing/diagnostics.py +330 -0
- dhb_xr/preprocessing/trajectory_cleaner.py +485 -0
- dhb_xr/tokenization/__init__.py +56 -0
- dhb_xr/tokenization/causal_encoder.py +54 -0
- dhb_xr/tokenization/compression.py +749 -0
- dhb_xr/tokenization/hierarchical.py +359 -0
- dhb_xr/tokenization/rvq.py +178 -0
- dhb_xr/tokenization/vqvae.py +155 -0
- dhb_xr/utils/__init__.py +24 -0
- dhb_xr/utils/io.py +59 -0
- dhb_xr/utils/resampling.py +66 -0
- dhb_xr/utils/xdof_loader.py +89 -0
- dhb_xr/visualization/__init__.py +5 -0
- dhb_xr/visualization/plot.py +242 -0
- dhb_xr-0.2.1.dist-info/METADATA +784 -0
- dhb_xr-0.2.1.dist-info/RECORD +82 -0
- dhb_xr-0.2.1.dist-info/WHEEL +5 -0
- dhb_xr-0.2.1.dist-info/entry_points.txt +2 -0
- dhb_xr-0.2.1.dist-info/top_level.txt +3 -0
- examples/__init__.py +54 -0
- examples/basic_encoding.py +82 -0
- examples/benchmark_backends.py +37 -0
- examples/dhb_qr_comparison.py +79 -0
- examples/dhb_ti_time_invariant.py +72 -0
- examples/gpu_batch_optimization.py +102 -0
- examples/imitation_learning.py +53 -0
- examples/integration/__init__.py +19 -0
- examples/integration/libero_full_demo.py +692 -0
- examples/integration/libero_pro_dhb_demo.py +1063 -0
- examples/integration/libero_simulation_demo.py +286 -0
- examples/integration/libero_swap_demo.py +534 -0
- examples/integration/robocasa_libero_dhb_pipeline.py +56 -0
- examples/integration/test_libero_adapter.py +47 -0
- examples/integration/test_libero_encoding.py +75 -0
- examples/integration/test_libero_retrieval.py +105 -0
- examples/motion_database.py +88 -0
- examples/trajectory_adaptation.py +85 -0
- examples/vla_tokenization.py +107 -0
- notebooks/__init__.py +24 -0
|
@@ -0,0 +1,79 @@
|
|
|
1
|
+
"""Motion database: store trajectories and invariants, similarity search."""
|
|
2
|
+
|
|
3
|
+
import numpy as np
|
|
4
|
+
from typing import List, Dict, Any, Optional
|
|
5
|
+
|
|
6
|
+
from dhb_xr.encoder.dhb_dr import encode_dhb_dr
|
|
7
|
+
from dhb_xr.core.types import DHBMethod, EncodingMethod
|
|
8
|
+
from dhb_xr.database.similarity import invariant_distance, soft_dtw_distance
|
|
9
|
+
|
|
10
|
+
try:
|
|
11
|
+
import faiss
|
|
12
|
+
HAS_FAISS = True
|
|
13
|
+
except ImportError:
|
|
14
|
+
HAS_FAISS = False
|
|
15
|
+
|
|
16
|
+
|
|
17
|
+
class MotionDatabase:
|
|
18
|
+
"""
|
|
19
|
+
Store trajectories as invariants; optional FAISS index for fast retrieval.
|
|
20
|
+
add(trajectory, metadata) -> encode and store
|
|
21
|
+
retrieve(query_trajectory, k) -> top-k similar (by L2 on invariants or DTW).
|
|
22
|
+
"""
|
|
23
|
+
|
|
24
|
+
def __init__(self, dhb_method: str = "double_reflection", use_faiss: bool = False):
|
|
25
|
+
self.dhb_method = DHBMethod.DOUBLE_REFLECTION if dhb_method == "double_reflection" else DHBMethod.ORIGINAL
|
|
26
|
+
self.k_inv = 4 if self.dhb_method == DHBMethod.DOUBLE_REFLECTION else 3
|
|
27
|
+
self.invariants_list: List[np.ndarray] = []
|
|
28
|
+
self.metadata_list: List[Dict[str, Any]] = []
|
|
29
|
+
self.use_faiss = use_faiss and HAS_FAISS
|
|
30
|
+
self.index: Optional[Any] = None
|
|
31
|
+
self.embedding_dim: Optional[int] = None
|
|
32
|
+
|
|
33
|
+
def add(
|
|
34
|
+
self,
|
|
35
|
+
positions: np.ndarray,
|
|
36
|
+
quaternions: np.ndarray,
|
|
37
|
+
metadata: Optional[Dict[str, Any]] = None,
|
|
38
|
+
) -> None:
|
|
39
|
+
out = encode_dhb_dr(
|
|
40
|
+
positions, quaternions,
|
|
41
|
+
method=EncodingMethod.POSITION, use_default_initial_frames=True, dhb_method=self.dhb_method,
|
|
42
|
+
)
|
|
43
|
+
U = np.concatenate([
|
|
44
|
+
out["linear_motion_invariants"],
|
|
45
|
+
out["angular_motion_invariants"],
|
|
46
|
+
], axis=1)
|
|
47
|
+
self.invariants_list.append(U.astype(np.float32))
|
|
48
|
+
self.metadata_list.append(metadata or {})
|
|
49
|
+
if self.use_faiss and U.size > 0:
|
|
50
|
+
flat = U.flatten()
|
|
51
|
+
if self.embedding_dim is None:
|
|
52
|
+
self.embedding_dim = flat.shape[0]
|
|
53
|
+
self.index = faiss.IndexFlatL2(self.embedding_dim)
|
|
54
|
+
emb = flat.reshape(1, -1).astype(np.float32)
|
|
55
|
+
if emb.shape[1] == self.embedding_dim:
|
|
56
|
+
self.index.add(emb)
|
|
57
|
+
|
|
58
|
+
def retrieve(
|
|
59
|
+
self,
|
|
60
|
+
query_positions: np.ndarray,
|
|
61
|
+
query_quaternions: np.ndarray,
|
|
62
|
+
k: int = 10,
|
|
63
|
+
use_dtw: bool = False,
|
|
64
|
+
) -> List[tuple]:
|
|
65
|
+
out = encode_dhb_dr(
|
|
66
|
+
query_positions, query_quaternions,
|
|
67
|
+
method=EncodingMethod.POSITION, use_default_initial_frames=True, dhb_method=self.dhb_method,
|
|
68
|
+
)
|
|
69
|
+
U_q = np.concatenate([
|
|
70
|
+
out["linear_motion_invariants"],
|
|
71
|
+
out["angular_motion_invariants"],
|
|
72
|
+
], axis=1)
|
|
73
|
+
if use_dtw:
|
|
74
|
+
dists = [soft_dtw_distance(U_q, U) for U in self.invariants_list]
|
|
75
|
+
idx = np.argsort(dists)[:k]
|
|
76
|
+
return [(self.invariants_list[i], self.metadata_list[i], dists[i]) for i in idx]
|
|
77
|
+
dists = [invariant_distance(U_q, U) for U in self.invariants_list]
|
|
78
|
+
idx = np.argsort(dists)[:k]
|
|
79
|
+
return [(self.invariants_list[i], self.metadata_list[i], dists[i]) for i in idx]
|
|
@@ -0,0 +1,6 @@
|
|
|
1
|
+
"""ANN retrieval helpers (FAISS). Re-exported via database/__init__.py."""
|
|
2
|
+
|
|
3
|
+
from dhb_xr.database.motion_db import MotionDatabase
|
|
4
|
+
from dhb_xr.database.similarity import invariant_distance, soft_dtw_distance
|
|
5
|
+
|
|
6
|
+
__all__ = ["MotionDatabase", "invariant_distance", "soft_dtw_distance"]
|
|
@@ -0,0 +1,71 @@
|
|
|
1
|
+
"""Similarity and distance measures for invariant sequences."""
|
|
2
|
+
|
|
3
|
+
import numpy as np
|
|
4
|
+
from typing import Optional
|
|
5
|
+
|
|
6
|
+
from dhb_xr.core import geometry as geom
|
|
7
|
+
|
|
8
|
+
|
|
9
|
+
def quaternion_geodesic_distance(q1: np.ndarray, q2: np.ndarray) -> np.ndarray:
|
|
10
|
+
"""Quaternion geodesic distance (rad). q1, q2: (..., 4) wxyz. Returns scalar or array."""
|
|
11
|
+
q1 = np.asarray(q1).reshape(-1, 4)
|
|
12
|
+
q2 = np.asarray(q2).reshape(-1, 4)
|
|
13
|
+
dot = np.abs(np.sum(q1 * q2, axis=-1))
|
|
14
|
+
dot = np.clip(dot, 0, 1)
|
|
15
|
+
return 2 * np.arccos(dot)
|
|
16
|
+
|
|
17
|
+
|
|
18
|
+
def invariant_distance(
|
|
19
|
+
U: np.ndarray,
|
|
20
|
+
V: np.ndarray,
|
|
21
|
+
weights: Optional[np.ndarray] = None,
|
|
22
|
+
use_quat_geodesic: bool = False,
|
|
23
|
+
quat_start_idx: int = 1,
|
|
24
|
+
quat_len: int = 4,
|
|
25
|
+
) -> float:
|
|
26
|
+
"""
|
|
27
|
+
Aligned L2 distance between invariant sequences.
|
|
28
|
+
U, V: (N, 2*k) or (N, k_lin + k_ang). If use_quat_geodesic, angular quaternion part uses geodesic.
|
|
29
|
+
"""
|
|
30
|
+
U = np.asarray(U)
|
|
31
|
+
V = np.asarray(V)
|
|
32
|
+
assert U.shape == V.shape
|
|
33
|
+
if weights is None:
|
|
34
|
+
weights = np.ones(U.shape[1])
|
|
35
|
+
weights = np.asarray(weights).reshape(-1)
|
|
36
|
+
diff = U - V
|
|
37
|
+
if use_quat_geodesic:
|
|
38
|
+
# Assume linear then angular; each component [m, ...]. Angular has quat at [quat_start_idx:quat_start_idx+quat_len]
|
|
39
|
+
half = U.shape[1] // 2
|
|
40
|
+
for i in range(U.shape[0]):
|
|
41
|
+
for j in [0, 1]:
|
|
42
|
+
base = j * half
|
|
43
|
+
if base + quat_start_idx + quat_len <= U.shape[1]:
|
|
44
|
+
q1 = U[i, base + quat_start_idx : base + quat_start_idx + quat_len]
|
|
45
|
+
q2 = V[i, base + quat_start_idx : base + quat_start_idx + quat_len]
|
|
46
|
+
d = quaternion_geodesic_distance(q1, q2)
|
|
47
|
+
diff[i, base + quat_start_idx] = d
|
|
48
|
+
dist = np.sqrt(np.sum(weights * (diff ** 2)))
|
|
49
|
+
else:
|
|
50
|
+
dist = np.sqrt(np.sum(weights * (diff ** 2)))
|
|
51
|
+
return float(dist)
|
|
52
|
+
|
|
53
|
+
|
|
54
|
+
def soft_dtw_distance(
|
|
55
|
+
U: np.ndarray,
|
|
56
|
+
V: np.ndarray,
|
|
57
|
+
gamma: float = 1.0,
|
|
58
|
+
) -> float:
|
|
59
|
+
"""Soft-DTW (differentiable) between two sequences. Requires dtaidistance or simple implementation."""
|
|
60
|
+
try:
|
|
61
|
+
from dtaidistance import dtw
|
|
62
|
+
return float(dtw.distance(U, V, use_c=True))
|
|
63
|
+
except ImportError:
|
|
64
|
+
n, m = len(U), len(V)
|
|
65
|
+
D = np.zeros((n + 1, m + 1)) + np.inf
|
|
66
|
+
D[0, 0] = 0
|
|
67
|
+
for i in range(1, n + 1):
|
|
68
|
+
for j in range(1, m + 1):
|
|
69
|
+
d_ij = np.sum((U[i - 1] - V[j - 1]) ** 2)
|
|
70
|
+
D[i, j] = d_ij + min(D[i - 1, j], D[i, j - 1], D[i - 1, j - 1])
|
|
71
|
+
return float(D[n, m])
|
|
@@ -0,0 +1,13 @@
|
|
|
1
|
+
"""DHB decoders: DHB-DR, DHB-QR, batched PyTorch."""
|
|
2
|
+
|
|
3
|
+
from dhb_xr.decoder.dhb_dr import decode_dhb_dr
|
|
4
|
+
|
|
5
|
+
__all__ = [
|
|
6
|
+
"decode_dhb_dr",
|
|
7
|
+
"decode_dhb_qr",
|
|
8
|
+
]
|
|
9
|
+
|
|
10
|
+
try:
|
|
11
|
+
from dhb_xr.decoder.dhb_qr import decode_dhb_qr
|
|
12
|
+
except ImportError:
|
|
13
|
+
decode_dhb_qr = None
|
|
@@ -0,0 +1,52 @@
|
|
|
1
|
+
"""Batched DHB decoder in PyTorch: wrapper over numpy decode."""
|
|
2
|
+
|
|
3
|
+
try:
|
|
4
|
+
import torch
|
|
5
|
+
import torch.nn as nn
|
|
6
|
+
except ImportError:
|
|
7
|
+
torch = None
|
|
8
|
+
nn = None
|
|
9
|
+
|
|
10
|
+
if torch is not None:
|
|
11
|
+
|
|
12
|
+
import numpy as np
|
|
13
|
+
from dhb_xr.decoder.dhb_dr import decode_dhb_dr
|
|
14
|
+
from dhb_xr.core.types import DHBMethod
|
|
15
|
+
|
|
16
|
+
class DHBDecoderTorch(nn.Module):
|
|
17
|
+
"""
|
|
18
|
+
Batched DHB-DR decoder.
|
|
19
|
+
invariants (B, N-2, 8), initial_poses list of dict -> positions (B, N', 3), quaternions (B, N', 4).
|
|
20
|
+
"""
|
|
21
|
+
|
|
22
|
+
def __init__(self, dhb_method: str = "double_reflection"):
|
|
23
|
+
super().__init__()
|
|
24
|
+
self.dhb_method = DHBMethod.DOUBLE_REFLECTION if dhb_method == "double_reflection" else DHBMethod.ORIGINAL
|
|
25
|
+
|
|
26
|
+
def forward(
|
|
27
|
+
self,
|
|
28
|
+
invariants: torch.Tensor,
|
|
29
|
+
initial_poses: list,
|
|
30
|
+
) -> tuple:
|
|
31
|
+
B = invariants.shape[0]
|
|
32
|
+
k = 4 if self.dhb_method == DHBMethod.DOUBLE_REFLECTION else 3
|
|
33
|
+
assert invariants.shape[2] == 2 * k
|
|
34
|
+
pos_list = []
|
|
35
|
+
quat_list = []
|
|
36
|
+
for b in range(B):
|
|
37
|
+
inv = invariants[b].detach().cpu().numpy()
|
|
38
|
+
lin = inv[:, :k]
|
|
39
|
+
ang = inv[:, k:]
|
|
40
|
+
init = initial_poses[b] if isinstance(initial_poses[b], dict) else {
|
|
41
|
+
"position": initial_poses[b]["position"].cpu().numpy() if hasattr(initial_poses[b]["position"], "cpu") else np.asarray(initial_poses[b]["position"]),
|
|
42
|
+
"quaternion": initial_poses[b]["quaternion"].cpu().numpy() if hasattr(initial_poses[b]["quaternion"], "cpu") else np.asarray(initial_poses[b]["quaternion"]),
|
|
43
|
+
}
|
|
44
|
+
out = decode_dhb_dr(lin, ang, init, method=EncodingMethod.POSITION, dhb_method=self.dhb_method, drop_padded=True)
|
|
45
|
+
pos_list.append(torch.from_numpy(out["positions"]))
|
|
46
|
+
quat_list.append(torch.from_numpy(out["quaternions"]))
|
|
47
|
+
positions = torch.stack(pos_list, dim=0)
|
|
48
|
+
quaternions = torch.stack(quat_list, dim=0)
|
|
49
|
+
return positions, quaternions
|
|
50
|
+
|
|
51
|
+
else:
|
|
52
|
+
DHBDecoderTorch = None
|
dhb_xr/decoder/dhb_dr.py
ADDED
|
@@ -0,0 +1,261 @@
|
|
|
1
|
+
"""
|
|
2
|
+
DHB-DR decoder: reconstruct SE(3) trajectory from invariants.
|
|
3
|
+
Quaternion convention: wxyz (scalar-first).
|
|
4
|
+
|
|
5
|
+
Robustness features:
|
|
6
|
+
- Initial pose validation
|
|
7
|
+
- Divergence detection with optional warnings
|
|
8
|
+
- NaN/Inf handling for degenerate invariants
|
|
9
|
+
"""
|
|
10
|
+
|
|
11
|
+
import numpy as np
|
|
12
|
+
import warnings
|
|
13
|
+
from typing import Dict, Any, Optional
|
|
14
|
+
from dataclasses import dataclass
|
|
15
|
+
|
|
16
|
+
from dhb_xr.core.types import DHBMethod, EncodingMethod
|
|
17
|
+
from dhb_xr.core import geometry as geom
|
|
18
|
+
|
|
19
|
+
EPSILON = 1e-10
|
|
20
|
+
|
|
21
|
+
|
|
22
|
+
@dataclass
|
|
23
|
+
class DecodingDiagnostics:
|
|
24
|
+
"""Diagnostics from decoding."""
|
|
25
|
+
initial_pose_valid: bool = True
|
|
26
|
+
num_nan_positions: int = 0
|
|
27
|
+
num_nan_quaternions: int = 0
|
|
28
|
+
max_step_size: float = 0.0
|
|
29
|
+
trajectory_length: float = 0.0
|
|
30
|
+
|
|
31
|
+
|
|
32
|
+
def _validate_initial_pose(initial_pose: Dict[str, np.ndarray]) -> bool:
|
|
33
|
+
"""Validate that initial pose is well-formed."""
|
|
34
|
+
try:
|
|
35
|
+
pos = np.asarray(initial_pose["position"]).reshape(3)
|
|
36
|
+
quat = np.asarray(initial_pose["quaternion"]).reshape(4)
|
|
37
|
+
|
|
38
|
+
# Check for NaN/Inf
|
|
39
|
+
if not np.all(np.isfinite(pos)):
|
|
40
|
+
return False
|
|
41
|
+
if not np.all(np.isfinite(quat)):
|
|
42
|
+
return False
|
|
43
|
+
|
|
44
|
+
# Check quaternion is normalized
|
|
45
|
+
quat_norm = np.linalg.norm(quat)
|
|
46
|
+
if abs(quat_norm - 1.0) > 0.01:
|
|
47
|
+
return False
|
|
48
|
+
|
|
49
|
+
return True
|
|
50
|
+
except (KeyError, ValueError):
|
|
51
|
+
return False
|
|
52
|
+
|
|
53
|
+
|
|
54
|
+
def _normalize_quaternion(quat: np.ndarray) -> np.ndarray:
|
|
55
|
+
"""Normalize quaternion to unit length."""
|
|
56
|
+
norm = np.linalg.norm(quat)
|
|
57
|
+
if norm > EPSILON:
|
|
58
|
+
return quat / norm
|
|
59
|
+
return np.array([1.0, 0.0, 0.0, 0.0])
|
|
60
|
+
|
|
61
|
+
|
|
62
|
+
def decode_dhb_dr(
|
|
63
|
+
linear_motion_invariants: np.ndarray,
|
|
64
|
+
angular_motion_invariants: np.ndarray,
|
|
65
|
+
initial_pose: Dict[str, np.ndarray],
|
|
66
|
+
method: EncodingMethod = EncodingMethod.POSITION,
|
|
67
|
+
dhb_method: DHBMethod = DHBMethod.DOUBLE_REFLECTION,
|
|
68
|
+
drop_padded: bool = True,
|
|
69
|
+
robust_mode: bool = False,
|
|
70
|
+
validate_initial_pose: bool = False,
|
|
71
|
+
return_diagnostics: bool = False,
|
|
72
|
+
) -> Dict[str, Any]:
|
|
73
|
+
"""
|
|
74
|
+
Reconstruct poses from DHB invariants.
|
|
75
|
+
|
|
76
|
+
Parameters:
|
|
77
|
+
linear_motion_invariants: (N, k) linear invariants
|
|
78
|
+
angular_motion_invariants: (N, k) angular invariants
|
|
79
|
+
initial_pose: {'position': (3,), 'quaternion': (4,) wxyz}
|
|
80
|
+
method: 'pos' or 'vel'
|
|
81
|
+
dhb_method: DHBMethod.ORIGINAL (3 inv) or DHBMethod.DOUBLE_REFLECTION (4 inv)
|
|
82
|
+
drop_padded: if True, return positions/quaternions[2:] to match encoder padding
|
|
83
|
+
robust_mode: Enable robustness features (NaN handling, normalization)
|
|
84
|
+
validate_initial_pose: Check initial pose validity before decoding
|
|
85
|
+
return_diagnostics: Include decoding diagnostics in output
|
|
86
|
+
|
|
87
|
+
Returns:
|
|
88
|
+
dict with 'positions' (N,3), 'quaternions' (N,4) wxyz,
|
|
89
|
+
and optionally 'diagnostics' if return_diagnostics=True.
|
|
90
|
+
"""
|
|
91
|
+
diagnostics = DecodingDiagnostics() if return_diagnostics else None
|
|
92
|
+
|
|
93
|
+
# Validate initial pose if requested
|
|
94
|
+
if validate_initial_pose or robust_mode:
|
|
95
|
+
if not _validate_initial_pose(initial_pose):
|
|
96
|
+
if diagnostics:
|
|
97
|
+
diagnostics.initial_pose_valid = False
|
|
98
|
+
if robust_mode:
|
|
99
|
+
warnings.warn("Invalid initial pose detected. Using identity pose.")
|
|
100
|
+
initial_pose = {
|
|
101
|
+
"position": np.zeros(3),
|
|
102
|
+
"quaternion": np.array([1.0, 0.0, 0.0, 0.0])
|
|
103
|
+
}
|
|
104
|
+
else:
|
|
105
|
+
raise ValueError("Invalid initial pose: position or quaternion is malformed or contains NaN/Inf")
|
|
106
|
+
linear_inv = np.asarray(linear_motion_invariants, dtype=np.float64)
|
|
107
|
+
angular_inv = np.asarray(angular_motion_invariants, dtype=np.float64)
|
|
108
|
+
num_samples = linear_inv.shape[0]
|
|
109
|
+
assert angular_inv.shape[0] == num_samples
|
|
110
|
+
|
|
111
|
+
if dhb_method == DHBMethod.ORIGINAL:
|
|
112
|
+
linear_magnitude = linear_inv[:, 0]
|
|
113
|
+
linear_angle_y = linear_inv[:, 1]
|
|
114
|
+
linear_angle_x = linear_inv[:, 2]
|
|
115
|
+
angular_magnitude = angular_inv[:, 0]
|
|
116
|
+
angular_angle_y = angular_inv[:, 1]
|
|
117
|
+
angular_angle_x = angular_inv[:, 2]
|
|
118
|
+
else:
|
|
119
|
+
linear_magnitude = linear_inv[:, 0]
|
|
120
|
+
linear_angles = linear_inv[:, 1:4]
|
|
121
|
+
angular_magnitude = angular_inv[:, 0]
|
|
122
|
+
angular_angles = angular_inv[:, 1:4]
|
|
123
|
+
|
|
124
|
+
position_mode = method == EncodingMethod.POSITION
|
|
125
|
+
init_pos = np.asarray(initial_pose["position"]).reshape(3)
|
|
126
|
+
init_quat = np.asarray(initial_pose["quaternion"]).reshape(4)
|
|
127
|
+
|
|
128
|
+
trajectory_position = np.zeros((num_samples, 3))
|
|
129
|
+
trajectory_quaternions = np.zeros((num_samples, 4))
|
|
130
|
+
trajectory_quaternions[0] = init_quat
|
|
131
|
+
|
|
132
|
+
linear_frame = np.eye(4)
|
|
133
|
+
linear_frame[:3, 3] = init_pos
|
|
134
|
+
linear_frame[3, 3] = float(position_mode)
|
|
135
|
+
|
|
136
|
+
angular_frame = np.eye(4)
|
|
137
|
+
angular_rot_temp = geom.quat_to_rot(init_quat)
|
|
138
|
+
|
|
139
|
+
prev_pos = init_pos.copy()
|
|
140
|
+
|
|
141
|
+
for i in range(num_samples):
|
|
142
|
+
if position_mode:
|
|
143
|
+
trajectory_position[i] = linear_frame[:3, 3]
|
|
144
|
+
|
|
145
|
+
if dhb_method == DHBMethod.ORIGINAL:
|
|
146
|
+
R_lin = geom.y_rot(linear_angle_y[i]) @ geom.x_rot(linear_angle_x[i])
|
|
147
|
+
else:
|
|
148
|
+
R_lin = geom.euler_to_rot(linear_angles[i])
|
|
149
|
+
|
|
150
|
+
trans = np.array([linear_magnitude[i], 0.0, 0.0])
|
|
151
|
+
T = np.eye(4)
|
|
152
|
+
T[:3, :3] = R_lin
|
|
153
|
+
T[:3, 3] = trans
|
|
154
|
+
T[3, 3] = float(position_mode)
|
|
155
|
+
linear_frame = linear_frame @ T
|
|
156
|
+
|
|
157
|
+
if not position_mode:
|
|
158
|
+
trajectory_position[i] = linear_frame[:3, 3]
|
|
159
|
+
|
|
160
|
+
# Handle NaN in robust mode
|
|
161
|
+
if robust_mode:
|
|
162
|
+
if not np.all(np.isfinite(trajectory_position[i])):
|
|
163
|
+
if diagnostics:
|
|
164
|
+
diagnostics.num_nan_positions += 1
|
|
165
|
+
trajectory_position[i] = prev_pos.copy()
|
|
166
|
+
else:
|
|
167
|
+
# Track step size for diagnostics
|
|
168
|
+
step_size = np.linalg.norm(trajectory_position[i] - prev_pos)
|
|
169
|
+
if diagnostics:
|
|
170
|
+
diagnostics.max_step_size = max(diagnostics.max_step_size, step_size)
|
|
171
|
+
diagnostics.trajectory_length += step_size
|
|
172
|
+
prev_pos = trajectory_position[i].copy()
|
|
173
|
+
|
|
174
|
+
rvec = angular_frame[:3, :3] @ np.array([angular_magnitude[i], 0.0, 0.0])
|
|
175
|
+
|
|
176
|
+
if dhb_method == DHBMethod.ORIGINAL:
|
|
177
|
+
R_ang = geom.y_rot(angular_angle_y[i]) @ geom.x_rot(angular_angle_x[i])
|
|
178
|
+
else:
|
|
179
|
+
R_ang = geom.euler_to_rot(angular_angles[i])
|
|
180
|
+
|
|
181
|
+
angular_rot_temp = angular_rot_temp @ geom.axis_angle_to_rot(rvec).T
|
|
182
|
+
if i < num_samples - 1:
|
|
183
|
+
quat = geom.rot_to_quat(angular_rot_temp)
|
|
184
|
+
# Normalize quaternion in robust mode
|
|
185
|
+
if robust_mode:
|
|
186
|
+
quat = _normalize_quaternion(quat)
|
|
187
|
+
if not np.all(np.isfinite(quat)):
|
|
188
|
+
if diagnostics:
|
|
189
|
+
diagnostics.num_nan_quaternions += 1
|
|
190
|
+
quat = trajectory_quaternions[i].copy() # Use previous
|
|
191
|
+
trajectory_quaternions[i + 1] = quat
|
|
192
|
+
angular_frame[:3, :3] = angular_frame[:3, :3] @ R_ang
|
|
193
|
+
|
|
194
|
+
if drop_padded:
|
|
195
|
+
trajectory_position = trajectory_position[2:]
|
|
196
|
+
trajectory_quaternions = trajectory_quaternions[2:]
|
|
197
|
+
|
|
198
|
+
result = {
|
|
199
|
+
"positions": trajectory_position,
|
|
200
|
+
"quaternions": trajectory_quaternions,
|
|
201
|
+
}
|
|
202
|
+
|
|
203
|
+
if return_diagnostics and diagnostics is not None:
|
|
204
|
+
result["diagnostics"] = diagnostics
|
|
205
|
+
|
|
206
|
+
return result
|
|
207
|
+
|
|
208
|
+
|
|
209
|
+
def reconstruct_trajectory_single_step(
|
|
210
|
+
linear_invariants: np.ndarray,
|
|
211
|
+
angular_invariants: np.ndarray,
|
|
212
|
+
current_linear_frame: np.ndarray,
|
|
213
|
+
current_angular_frame: np.ndarray,
|
|
214
|
+
method: str,
|
|
215
|
+
dhb_method: DHBMethod = DHBMethod.DOUBLE_REFLECTION,
|
|
216
|
+
) -> tuple:
|
|
217
|
+
"""Single-step reconstruction for use in optimization. Returns (new_linear_frame, new_angular_frame, new_position, new_rotation_vec)."""
|
|
218
|
+
if dhb_method == DHBMethod.ORIGINAL:
|
|
219
|
+
linear_magnitude = linear_invariants[0]
|
|
220
|
+
linear_angle_y = linear_invariants[1]
|
|
221
|
+
linear_angle_x = linear_invariants[2]
|
|
222
|
+
angular_magnitude = angular_invariants[0]
|
|
223
|
+
angular_angle_y = angular_invariants[1]
|
|
224
|
+
angular_angle_x = angular_invariants[2]
|
|
225
|
+
else:
|
|
226
|
+
linear_magnitude = float(linear_invariants[0])
|
|
227
|
+
linear_angles = linear_invariants[1:4]
|
|
228
|
+
angular_magnitude = float(angular_invariants[0])
|
|
229
|
+
angular_angles = angular_invariants[1:4]
|
|
230
|
+
|
|
231
|
+
position_mode = method == EncodingMethod.POSITION
|
|
232
|
+
if position_mode:
|
|
233
|
+
new_position = current_linear_frame[:3, 3].copy()
|
|
234
|
+
|
|
235
|
+
if dhb_method == DHBMethod.ORIGINAL:
|
|
236
|
+
R_lin = geom.y_rot(linear_angle_y) @ geom.x_rot(linear_angle_x)
|
|
237
|
+
else:
|
|
238
|
+
R_lin = geom.euler_to_rot(linear_angles)
|
|
239
|
+
|
|
240
|
+
T = np.eye(4)
|
|
241
|
+
T[:3, :3] = R_lin
|
|
242
|
+
T[:3, 3] = np.array([linear_magnitude, 0.0, 0.0])
|
|
243
|
+
T[3, 3] = float(position_mode)
|
|
244
|
+
new_linear_frame = current_linear_frame @ T
|
|
245
|
+
|
|
246
|
+
if not position_mode:
|
|
247
|
+
new_position = new_linear_frame[:3, 3].copy()
|
|
248
|
+
|
|
249
|
+
new_rotation = current_angular_frame[:3, :3] @ np.array(
|
|
250
|
+
[angular_magnitude, 0.0, 0.0]
|
|
251
|
+
)
|
|
252
|
+
|
|
253
|
+
if dhb_method == DHBMethod.ORIGINAL:
|
|
254
|
+
R_ang = geom.y_rot(angular_angle_y) @ geom.x_rot(angular_angle_x)
|
|
255
|
+
else:
|
|
256
|
+
R_ang = geom.euler_to_rot(angular_angles)
|
|
257
|
+
|
|
258
|
+
new_angular_frame = np.eye(4)
|
|
259
|
+
new_angular_frame[:3, :3] = current_angular_frame[:3, :3] @ R_ang
|
|
260
|
+
|
|
261
|
+
return new_linear_frame, new_angular_frame, new_position, new_rotation
|
dhb_xr/decoder/dhb_qr.py
ADDED
|
@@ -0,0 +1,89 @@
|
|
|
1
|
+
"""
|
|
2
|
+
DHB-QR decoder: reconstruct trajectory from quaternion-based invariants.
|
|
3
|
+
Per-step: magnitude + unit quaternion (5 values per component).
|
|
4
|
+
"""
|
|
5
|
+
|
|
6
|
+
import numpy as np
|
|
7
|
+
from typing import Dict, Any
|
|
8
|
+
|
|
9
|
+
from dhb_xr.core.types import EncodingMethod
|
|
10
|
+
from dhb_xr.core import geometry as geom
|
|
11
|
+
|
|
12
|
+
|
|
13
|
+
def decode_dhb_qr(
|
|
14
|
+
linear_motion_invariants: np.ndarray,
|
|
15
|
+
angular_motion_invariants: np.ndarray,
|
|
16
|
+
initial_pose: Dict[str, np.ndarray],
|
|
17
|
+
method: str = "pos",
|
|
18
|
+
drop_padded: bool = True,
|
|
19
|
+
) -> Dict[str, Any]:
|
|
20
|
+
"""
|
|
21
|
+
Reconstruct poses from DHB-QR invariants.
|
|
22
|
+
|
|
23
|
+
initial_pose: {'position': (3,), 'quaternion': (4,) wxyz}
|
|
24
|
+
drop_padded: if True, return positions/quaternions[2:].
|
|
25
|
+
|
|
26
|
+
Returns dict with 'positions' (N,3), 'quaternions' (N,4) wxyz.
|
|
27
|
+
"""
|
|
28
|
+
linear_inv = np.asarray(linear_motion_invariants, dtype=np.float64)
|
|
29
|
+
angular_inv = np.asarray(angular_motion_invariants, dtype=np.float64)
|
|
30
|
+
num_samples = linear_inv.shape[0]
|
|
31
|
+
assert angular_inv.shape[0] == num_samples and linear_inv.shape[1] == 5
|
|
32
|
+
|
|
33
|
+
linear_magnitude = linear_inv[:, 0]
|
|
34
|
+
linear_quat = linear_inv[:, 1:5] # (N, 4) wxyz
|
|
35
|
+
angular_magnitude = angular_inv[:, 0]
|
|
36
|
+
angular_quat = angular_inv[:, 1:5]
|
|
37
|
+
|
|
38
|
+
# Normalize quaternions
|
|
39
|
+
for i in range(num_samples):
|
|
40
|
+
n = np.linalg.norm(linear_quat[i])
|
|
41
|
+
if n > 1e-12:
|
|
42
|
+
linear_quat[i] /= n
|
|
43
|
+
n = np.linalg.norm(angular_quat[i])
|
|
44
|
+
if n > 1e-12:
|
|
45
|
+
angular_quat[i] /= n
|
|
46
|
+
|
|
47
|
+
position_mode = method == EncodingMethod.POSITION
|
|
48
|
+
init_pos = np.asarray(initial_pose["position"]).reshape(3)
|
|
49
|
+
init_quat = np.asarray(initial_pose["quaternion"]).reshape(4)
|
|
50
|
+
|
|
51
|
+
trajectory_position = np.zeros((num_samples, 3))
|
|
52
|
+
trajectory_quaternions = np.zeros((num_samples, 4))
|
|
53
|
+
trajectory_quaternions[0] = init_quat
|
|
54
|
+
|
|
55
|
+
linear_frame = np.eye(4)
|
|
56
|
+
linear_frame[:3, 3] = init_pos
|
|
57
|
+
linear_frame[3, 3] = float(position_mode)
|
|
58
|
+
angular_rot_temp = geom.quat_to_rot(init_quat)
|
|
59
|
+
|
|
60
|
+
for i in range(num_samples):
|
|
61
|
+
if position_mode:
|
|
62
|
+
trajectory_position[i] = linear_frame[:3, 3]
|
|
63
|
+
|
|
64
|
+
R_lin = geom.quat_to_rot(linear_quat[i])
|
|
65
|
+
trans = R_lin @ np.array([linear_magnitude[i], 0.0, 0.0])
|
|
66
|
+
T = np.eye(4)
|
|
67
|
+
T[:3, :3] = R_lin
|
|
68
|
+
T[:3, 3] = trans
|
|
69
|
+
T[3, 3] = float(position_mode)
|
|
70
|
+
linear_frame = linear_frame @ T
|
|
71
|
+
|
|
72
|
+
if not position_mode:
|
|
73
|
+
trajectory_position[i] = linear_frame[:3, 3]
|
|
74
|
+
|
|
75
|
+
rvec = geom.quat_to_rot(angular_quat[i]) @ np.array(
|
|
76
|
+
[angular_magnitude[i], 0.0, 0.0]
|
|
77
|
+
)
|
|
78
|
+
angular_rot_temp = angular_rot_temp @ geom.axis_angle_to_rot(rvec).T
|
|
79
|
+
if i < num_samples - 1:
|
|
80
|
+
trajectory_quaternions[i + 1] = geom.rot_to_quat(angular_rot_temp)
|
|
81
|
+
|
|
82
|
+
if drop_padded:
|
|
83
|
+
trajectory_position = trajectory_position[2:]
|
|
84
|
+
trajectory_quaternions = trajectory_quaternions[2:]
|
|
85
|
+
|
|
86
|
+
return {
|
|
87
|
+
"positions": trajectory_position,
|
|
88
|
+
"quaternions": trajectory_quaternions,
|
|
89
|
+
}
|
|
@@ -0,0 +1,27 @@
|
|
|
1
|
+
"""DHB encoders: DHB-DR (Euler), DHB-QR (quaternion), DHB-TI (time-invariant), batched PyTorch."""
|
|
2
|
+
|
|
3
|
+
from dhb_xr.core.types import DHBMethod
|
|
4
|
+
from dhb_xr.encoder.dhb_dr import encode_dhb_dr
|
|
5
|
+
from dhb_xr.encoder.padding import extrapolate_boundary_poses
|
|
6
|
+
from dhb_xr.encoder.dhb_ti import (
|
|
7
|
+
compute_progress,
|
|
8
|
+
resample_by_progress,
|
|
9
|
+
encode_dhb_dr_ti,
|
|
10
|
+
encode_dhb_qr_ti,
|
|
11
|
+
)
|
|
12
|
+
|
|
13
|
+
__all__ = [
|
|
14
|
+
"encode_dhb_dr",
|
|
15
|
+
"encode_dhb_qr",
|
|
16
|
+
"encode_dhb_dr_ti",
|
|
17
|
+
"encode_dhb_qr_ti",
|
|
18
|
+
"compute_progress",
|
|
19
|
+
"resample_by_progress",
|
|
20
|
+
"DHBMethod",
|
|
21
|
+
"extrapolate_boundary_poses",
|
|
22
|
+
]
|
|
23
|
+
|
|
24
|
+
try:
|
|
25
|
+
from dhb_xr.encoder.dhb_qr import encode_dhb_qr
|
|
26
|
+
except ImportError:
|
|
27
|
+
encode_dhb_qr = None
|