File size: 8,170 Bytes
b1b6f13
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
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
"""Canonical sensor-frame action encoding.

One definition, shared by every consumer. This used to be duplicated in
``cfm/data/encode_episodes.py``, ``jepa/model/anchors.py`` and
``vm_diffusion/data/dataset.py``; all three computed the rotation in the sensor's
local frame but the translation in the *world* frame, and two of them disagreed
about the 6D rotation layout.

The action from frame t to t+1 is expressed entirely in the sensor frame at time t:

    dp   = R_t^T (p_{t+1} - p_t)          (3,)  metres
    dR   = R_t^T R_{t+1}                  (3,3)
    r6d  = Rot6D(dR)                      (6,)  first two columns, [c1; c2]
    a_t  = [dp, r6d]                      (9,)

Being body-frame matters because the tactile image is rigidly attached to the
sensor: a given slide across the gel must map to the same action vector no matter
how the hand is oriented in the world. Equivalently, the encoding is invariant to
any global transform of the whole pose trajectory β€” which is exactly what
``tests`` check, and what the old world-frame translation failed.

The 6D rotation follows Zhou et al. 2019 (the first two columns of the matrix,
concatenated), which is continuous in SO(3) and so friendlier to regression than
quaternions or Euler angles.
"""
from __future__ import annotations

import torch

# Rot6D of the identity rotation, in [c1; c2] layout. Subtracting this makes a
# near-stationary action sit near zero instead of on a large constant offset;
# consumers that standardize their conditioning want it, the stored artifact does not.
ROT6D_IDENTITY: tuple[float, ...] = (1.0, 0.0, 0.0, 0.0, 1.0, 0.0)


def identity_action(action_dim: int = 9) -> torch.Tensor:
    """The action of standing still, as an (action_dim,) vector.

    Zero translation plus the identity rotation, repeated per sensor. Useful as the
    default conditioning offset: near-stationary steps then sit near zero instead of
    on the constant 1.0s that Rot6D(I) puts in two of every nine dims.
    """
    if action_dim % 9:
        raise ValueError(f"action_dim must be a multiple of 9, got {action_dim}")
    one = torch.tensor((0.0, 0.0, 0.0) + ROT6D_IDENTITY)
    return one.repeat(action_dim // 9)


def quat_to_rotmat(q: torch.Tensor) -> torch.Tensor:
    """(..., 4) quaternion in scipy xyzw order -> (..., 3, 3) rotation matrix.

    The quaternion is renormalized first: the stored poses are only unit to
    float32 precision, and the drift compounds through the relative products.
    """
    if q.shape[-1] != 4:
        raise ValueError(f"expected (...,4) xyzw quaternion, got {tuple(q.shape)}")

    q = q / q.norm(dim=-1, keepdim=True).clamp_min(1e-8)
    qx, qy, qz, qw = q.unbind(-1)

    r00 = 1 - 2 * (qy * qy + qz * qz)
    r01 = 2 * (qx * qy - qz * qw)
    r02 = 2 * (qx * qz + qy * qw)
    r10 = 2 * (qx * qy + qz * qw)
    r11 = 1 - 2 * (qx * qx + qz * qz)
    r12 = 2 * (qy * qz - qx * qw)
    r20 = 2 * (qx * qz - qy * qw)
    r21 = 2 * (qy * qz + qx * qw)
    r22 = 1 - 2 * (qx * qx + qy * qy)

    return torch.stack([
        torch.stack([r00, r01, r02], dim=-1),
        torch.stack([r10, r11, r12], dim=-1),
        torch.stack([r20, r21, r22], dim=-1),
    ], dim=-2)


def matrix_to_rot6d(R: torch.Tensor) -> torch.Tensor:
    """(..., 3, 3) -> (..., 6), the first two columns as [c1x,c1y,c1z, c2x,c2y,c2z]."""
    cols = R[..., :, :2]                                    # (..., 3, 2)
    return cols.transpose(-1, -2).reshape(*R.shape[:-2], 6)


def rot6d_to_matrix(r6: torch.Tensor) -> torch.Tensor:
    """(..., 6) -> (..., 3, 3), inverting matrix_to_rot6d by Gram-Schmidt.

    The third column is recovered as c1 x c2, so the result is orthonormal even if
    the input drifted (e.g. came out of a network rather than a real rotation).
    """
    if r6.shape[-1] != 6:
        raise ValueError(f"expected (...,6) rot6d, got {tuple(r6.shape)}")

    a1, a2 = r6[..., :3], r6[..., 3:]
    c1 = a1 / a1.norm(dim=-1, keepdim=True).clamp_min(1e-8)
    a2 = a2 - (c1 * a2).sum(-1, keepdim=True) * c1
    c2 = a2 / a2.norm(dim=-1, keepdim=True).clamp_min(1e-8)
    c3 = torch.cross(c1, c2, dim=-1)

    return torch.stack([c1, c2, c3], dim=-1)                # columns


def sensor_action_9d(pose7: torch.Tensor) -> torch.Tensor:
    """(..., T, 7) poses [x,y,z, qx,qy,qz,qw] -> (..., T-1, 9) sensor-frame actions.

    Poses are world <- sensor. Output row t is the motion from frame t to t+1 as
    seen from the sensor frame at t: ``[dp(3), Rot6D(dR)(6)]``.
    """
    if pose7.shape[-1] != 7:
        raise ValueError(f"expected (...,T,7) poses, got {tuple(pose7.shape)}")
    if pose7.shape[-2] < 2:
        raise ValueError(f"need at least 2 frames to form an action, got {pose7.shape[-2]}")

    pos = pose7[..., :3]
    R = quat_to_rotmat(pose7[..., 3:])                      # (..., T, 3, 3)

    R_t = R[..., :-1, :, :]
    R_t1 = R[..., 1:, :, :]

    # Contract the *first* index of R_t, i.e. apply R_t^T to the world-frame delta.
    dp = torch.einsum("...ji,...j->...i", R_t, pos[..., 1:, :] - pos[..., :-1, :])
    dR = R_t.transpose(-1, -2) @ R_t1

    return torch.cat([dp, matrix_to_rot6d(dR)], dim=-1)


def pose_action_delta(left: torch.Tensor, right: torch.Tensor) -> torch.Tensor:
    """Left+right pose streams -> (..., T-1, 18), the two 9D actions concatenated."""
    if left.shape != right.shape:
        raise ValueError(f"pose shape mismatch: left={tuple(left.shape)} right={tuple(right.shape)}")
    return torch.cat([sensor_action_9d(left), sensor_action_9d(right)], dim=-1)


# ── SE(3) composition ─────────────────────────────────────────────────────────
# Actions are stored per consecutive frame pair. Consumers that sample frames at a
# stride (fps-subsampled latents) need the action *across* the stride, which is the
# ordered product of the steps it spans, not the first step of the run.

def action9_to_matrix(a9: torch.Tensor) -> torch.Tensor:
    """(..., 9) -> (..., 4, 4) homogeneous, with T_{t+1} = T_t @ action."""
    if a9.shape[-1] != 9:
        raise ValueError(f"expected (...,9) action, got {tuple(a9.shape)}")

    T = torch.eye(4, dtype=a9.dtype, device=a9.device).expand(*a9.shape[:-1], 4, 4).clone()
    T[..., :3, :3] = rot6d_to_matrix(a9[..., 3:])
    T[..., :3, 3] = a9[..., :3]
    return T


def matrix_to_action9(T: torch.Tensor) -> torch.Tensor:
    """(..., 4, 4) -> (..., 9), inverse of action9_to_matrix."""
    return torch.cat([T[..., :3, 3], matrix_to_rot6d(T[..., :3, :3])], dim=-1)


def compose_action9(a9: torch.Tensor) -> torch.Tensor:
    """(M, 9) consecutive actions -> (9,) their ordered composition.

    An empty input yields the identity action (no motion), which is what a clamped
    boundary window should see.
    """
    if a9.ndim != 2 or a9.shape[-1] != 9:
        raise ValueError(f"expected (M,9) actions, got {tuple(a9.shape)}")

    if a9.shape[0] == 0:
        eye = torch.eye(4, dtype=a9.dtype, device=a9.device)
        return matrix_to_action9(eye)
    if a9.shape[0] == 1:
        return a9[0]

    T = action9_to_matrix(a9)
    out = T[0]
    for i in range(1, T.shape[0]):
        out = out @ T[i]
    return matrix_to_action9(out)


def window_action9(a9: torch.Tensor, frame_idx: torch.Tensor) -> torch.Tensor:
    """(N, 9) episode actions + (W,) frame indices -> (W-1, 9) per-window actions.

    ``frame_idx`` are indices into the *pose/frame* stream (so valid actions are
    ``a9[i]`` for i in ``[frame_idx[k], frame_idx[k+1])``). Repeated indices β€” which
    is how a clamped boundary window ends β€” give the identity action.
    """
    steps = []
    for k in range(len(frame_idx) - 1):
        i0, i1 = int(frame_idx[k]), int(frame_idx[k + 1])
        steps.append(compose_action9(a9[i0:max(i0, i1)]))
    return torch.stack(steps, dim=0)


__all__ = [
    "ROT6D_IDENTITY",
    "quat_to_rotmat",
    "matrix_to_rot6d",
    "rot6d_to_matrix",
    "identity_action",
    "sensor_action_9d",
    "pose_action_delta",
    "action9_to_matrix",
    "matrix_to_action9",
    "compose_action9",
    "window_action9",
]