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.
Files changed (82) hide show
  1. dhb_xr/__init__.py +61 -0
  2. dhb_xr/cli.py +206 -0
  3. dhb_xr/core/__init__.py +28 -0
  4. dhb_xr/core/geometry.py +167 -0
  5. dhb_xr/core/geometry_torch.py +77 -0
  6. dhb_xr/core/types.py +113 -0
  7. dhb_xr/database/__init__.py +10 -0
  8. dhb_xr/database/motion_db.py +79 -0
  9. dhb_xr/database/retrieval.py +6 -0
  10. dhb_xr/database/similarity.py +71 -0
  11. dhb_xr/decoder/__init__.py +13 -0
  12. dhb_xr/decoder/decoder_torch.py +52 -0
  13. dhb_xr/decoder/dhb_dr.py +261 -0
  14. dhb_xr/decoder/dhb_qr.py +89 -0
  15. dhb_xr/encoder/__init__.py +27 -0
  16. dhb_xr/encoder/dhb_dr.py +418 -0
  17. dhb_xr/encoder/dhb_qr.py +129 -0
  18. dhb_xr/encoder/dhb_ti.py +204 -0
  19. dhb_xr/encoder/encoder_torch.py +54 -0
  20. dhb_xr/encoder/padding.py +82 -0
  21. dhb_xr/generative/__init__.py +78 -0
  22. dhb_xr/generative/flow_matching.py +705 -0
  23. dhb_xr/generative/latent_encoder.py +536 -0
  24. dhb_xr/generative/sampling.py +203 -0
  25. dhb_xr/generative/training.py +475 -0
  26. dhb_xr/generative/vfm_tokenizer.py +485 -0
  27. dhb_xr/integration/__init__.py +13 -0
  28. dhb_xr/integration/vla/__init__.py +11 -0
  29. dhb_xr/integration/vla/libero.py +132 -0
  30. dhb_xr/integration/vla/pipeline.py +85 -0
  31. dhb_xr/integration/vla/robocasa.py +85 -0
  32. dhb_xr/losses/__init__.py +16 -0
  33. dhb_xr/losses/geodesic_loss.py +91 -0
  34. dhb_xr/losses/hybrid_loss.py +36 -0
  35. dhb_xr/losses/invariant_loss.py +73 -0
  36. dhb_xr/optimization/__init__.py +72 -0
  37. dhb_xr/optimization/casadi_solver.py +342 -0
  38. dhb_xr/optimization/constraints.py +32 -0
  39. dhb_xr/optimization/cusadi_solver.py +311 -0
  40. dhb_xr/optimization/export_casadi_decode.py +111 -0
  41. dhb_xr/optimization/fatrop_solver.py +477 -0
  42. dhb_xr/optimization/torch_solver.py +85 -0
  43. dhb_xr/preprocessing/__init__.py +42 -0
  44. dhb_xr/preprocessing/diagnostics.py +330 -0
  45. dhb_xr/preprocessing/trajectory_cleaner.py +485 -0
  46. dhb_xr/tokenization/__init__.py +56 -0
  47. dhb_xr/tokenization/causal_encoder.py +54 -0
  48. dhb_xr/tokenization/compression.py +749 -0
  49. dhb_xr/tokenization/hierarchical.py +359 -0
  50. dhb_xr/tokenization/rvq.py +178 -0
  51. dhb_xr/tokenization/vqvae.py +155 -0
  52. dhb_xr/utils/__init__.py +24 -0
  53. dhb_xr/utils/io.py +59 -0
  54. dhb_xr/utils/resampling.py +66 -0
  55. dhb_xr/utils/xdof_loader.py +89 -0
  56. dhb_xr/visualization/__init__.py +5 -0
  57. dhb_xr/visualization/plot.py +242 -0
  58. dhb_xr-0.2.1.dist-info/METADATA +784 -0
  59. dhb_xr-0.2.1.dist-info/RECORD +82 -0
  60. dhb_xr-0.2.1.dist-info/WHEEL +5 -0
  61. dhb_xr-0.2.1.dist-info/entry_points.txt +2 -0
  62. dhb_xr-0.2.1.dist-info/top_level.txt +3 -0
  63. examples/__init__.py +54 -0
  64. examples/basic_encoding.py +82 -0
  65. examples/benchmark_backends.py +37 -0
  66. examples/dhb_qr_comparison.py +79 -0
  67. examples/dhb_ti_time_invariant.py +72 -0
  68. examples/gpu_batch_optimization.py +102 -0
  69. examples/imitation_learning.py +53 -0
  70. examples/integration/__init__.py +19 -0
  71. examples/integration/libero_full_demo.py +692 -0
  72. examples/integration/libero_pro_dhb_demo.py +1063 -0
  73. examples/integration/libero_simulation_demo.py +286 -0
  74. examples/integration/libero_swap_demo.py +534 -0
  75. examples/integration/robocasa_libero_dhb_pipeline.py +56 -0
  76. examples/integration/test_libero_adapter.py +47 -0
  77. examples/integration/test_libero_encoding.py +75 -0
  78. examples/integration/test_libero_retrieval.py +105 -0
  79. examples/motion_database.py +88 -0
  80. examples/trajectory_adaptation.py +85 -0
  81. examples/vla_tokenization.py +107 -0
  82. 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
@@ -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
@@ -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