from __future__ import annotations import numpy as np # SEED absolute pose keeps the old layout: # joints(29), root xyz(3), root quat wxyz(4). SEED_JOINT_DIM = 29 SEED_ABS_DIM = 36 SEED_TEXTOP_ACTION_DIM = 39 TEXTOP_JOINT_ABS = slice(0, 29) TEXTOP_ROOT_DELTA_LOCAL_XY = slice(29, 31) TEXTOP_ROOT_ROT5_DELTA = slice(31, 36) TEXTOP_ROOT_HEIGHT = slice(36, 37) TEXTOP_FOOT_CONTACT = slice(37, 39) # 兼容旧推理脚本的import名称;语义已变为2D/5D,joint_delta不再存在。 TEXTOP_JOINT_DELTA = slice(0, 0) TEXTOP_ROOT_DELTA_LOCAL = TEXTOP_ROOT_DELTA_LOCAL_XY TEXTOP_ROOT_ROT6D_DELTA = TEXTOP_ROOT_ROT5_DELTA def normalize_quat_wxyz(quat: np.ndarray) -> np.ndarray: quat = np.asarray(quat, dtype=np.float32) norm = np.linalg.norm(quat, axis=-1, keepdims=True) norm = np.where(norm < 1e-8, 1.0, norm) out = quat / norm if out.shape[-1] == 4: out = np.where(out[..., :1] < 0.0, -out, out) return out.astype(np.float32) def quat_mul_wxyz(a: np.ndarray, b: np.ndarray) -> np.ndarray: aw, ax, ay, az = np.moveaxis(a, -1, 0) bw, bx, by, bz = np.moveaxis(b, -1, 0) out = np.stack( [ aw * bw - ax * bx - ay * by - az * bz, aw * bx + ax * bw + ay * bz - az * by, aw * by - ax * bz + ay * bw + az * bx, aw * bz + ax * by - ay * bx + az * bw, ], axis=-1, ) return normalize_quat_wxyz(out) def quat_inv_wxyz(quat: np.ndarray) -> np.ndarray: quat = normalize_quat_wxyz(quat) out = quat.copy() out[..., 1:] *= -1.0 return out.astype(np.float32) def quat_to_yaw_wxyz(quat: np.ndarray) -> np.ndarray: quat = normalize_quat_wxyz(quat) w, x, y, z = np.moveaxis(quat, -1, 0) return np.arctan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)).astype(np.float32) def wrap_angle(angle: np.ndarray) -> np.ndarray: return ((angle + np.pi) % (2.0 * np.pi) - np.pi).astype(np.float32) def quat_to_rpy_wxyz(quat: np.ndarray) -> np.ndarray: quat = normalize_quat_wxyz(quat) w, x, y, z = np.moveaxis(quat, -1, 0) roll = np.arctan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y)) pitch_arg = np.clip(2.0 * (w * y - z * x), -1.0, 1.0) pitch = np.arcsin(pitch_arg) yaw = quat_to_yaw_wxyz(quat) return np.stack([roll, pitch, yaw], axis=-1).astype(np.float32) def rpy_to_quat_wxyz(roll: float, pitch: float, yaw: float) -> np.ndarray: cr = np.cos(roll * 0.5) sr = np.sin(roll * 0.5) cp = np.cos(pitch * 0.5) sp = np.sin(pitch * 0.5) cy = np.cos(yaw * 0.5) sy = np.sin(yaw * 0.5) quat = np.array( [ cr * cp * cy + sr * sp * sy, sr * cp * cy - cr * sp * sy, cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy, ], dtype=np.float32, ) return normalize_quat_wxyz(quat) def quat_delta_to_textop_rot5(prev_quat: np.ndarray, cur_quat: np.ndarray) -> np.ndarray: prev_rpy = quat_to_rpy_wxyz(prev_quat) cur_rpy = quat_to_rpy_wxyz(cur_quat) roll = cur_rpy[..., 0] pitch = cur_rpy[..., 1] yaw_delta = wrap_angle(cur_rpy[..., 2] - prev_rpy[..., 2]) return np.stack( [np.sin(roll), np.cos(roll), np.sin(pitch), np.cos(pitch), yaw_delta], axis=-1, ).astype(np.float32) def local_delta_from_world(prev_pos: np.ndarray, cur_pos: np.ndarray, prev_quat: np.ndarray) -> np.ndarray: delta = np.asarray(cur_pos, dtype=np.float32) - np.asarray(prev_pos, dtype=np.float32) yaw = quat_to_yaw_wxyz(prev_quat) cos_yaw = np.cos(yaw) sin_yaw = np.sin(yaw) out = np.empty_like(delta, dtype=np.float32) out[..., 0] = cos_yaw * delta[..., 0] + sin_yaw * delta[..., 1] out[..., 1] = -sin_yaw * delta[..., 0] + cos_yaw * delta[..., 1] out[..., 2] = delta[..., 2] return out def world_delta_from_local(prev_quat: np.ndarray, local_delta: np.ndarray) -> np.ndarray: local_delta = np.asarray(local_delta, dtype=np.float32) yaw = quat_to_yaw_wxyz(prev_quat) cos_yaw = np.cos(yaw) sin_yaw = np.sin(yaw) out = np.empty_like(local_delta, dtype=np.float32) out[..., 0] = cos_yaw * local_delta[..., 0] - sin_yaw * local_delta[..., 1] out[..., 1] = sin_yaw * local_delta[..., 0] + cos_yaw * local_delta[..., 1] out[..., 2] = local_delta[..., 2] return out def quat_to_matrix_wxyz(quat: np.ndarray) -> np.ndarray: quat = normalize_quat_wxyz(quat) w, x, y, z = np.moveaxis(quat, -1, 0) matrix = np.empty(quat.shape[:-1] + (3, 3), dtype=np.float32) matrix[..., 0, 0] = 1.0 - 2.0 * (y * y + z * z) matrix[..., 0, 1] = 2.0 * (x * y - z * w) matrix[..., 0, 2] = 2.0 * (x * z + y * w) matrix[..., 1, 0] = 2.0 * (x * y + z * w) matrix[..., 1, 1] = 1.0 - 2.0 * (x * x + z * z) matrix[..., 1, 2] = 2.0 * (y * z - x * w) matrix[..., 2, 0] = 2.0 * (x * z - y * w) matrix[..., 2, 1] = 2.0 * (y * z + x * w) matrix[..., 2, 2] = 1.0 - 2.0 * (x * x + y * y) return matrix def matrix_to_quat_wxyz(matrix: np.ndarray) -> np.ndarray: matrix = np.asarray(matrix, dtype=np.float32) flat = matrix.reshape(-1, 3, 3) quats = [] for rot in flat: trace = float(np.trace(rot)) if trace > 0.0: s = np.sqrt(trace + 1.0) * 2.0 quat = np.array( [ 0.25 * s, (rot[2, 1] - rot[1, 2]) / s, (rot[0, 2] - rot[2, 0]) / s, (rot[1, 0] - rot[0, 1]) / s, ], dtype=np.float32, ) else: idx = int(np.argmax(np.diag(rot))) if idx == 0: s = np.sqrt(max(1.0 + rot[0, 0] - rot[1, 1] - rot[2, 2], 1e-8)) * 2.0 quat = np.array( [ (rot[2, 1] - rot[1, 2]) / s, 0.25 * s, (rot[0, 1] + rot[1, 0]) / s, (rot[0, 2] + rot[2, 0]) / s, ], dtype=np.float32, ) elif idx == 1: s = np.sqrt(max(1.0 + rot[1, 1] - rot[0, 0] - rot[2, 2], 1e-8)) * 2.0 quat = np.array( [ (rot[0, 2] - rot[2, 0]) / s, (rot[0, 1] + rot[1, 0]) / s, 0.25 * s, (rot[1, 2] + rot[2, 1]) / s, ], dtype=np.float32, ) else: s = np.sqrt(max(1.0 + rot[2, 2] - rot[0, 0] - rot[1, 1], 1e-8)) * 2.0 quat = np.array( [ (rot[1, 0] - rot[0, 1]) / s, (rot[0, 2] + rot[2, 0]) / s, (rot[1, 2] + rot[2, 1]) / s, 0.25 * s, ], dtype=np.float32, ) quats.append(quat) out = np.stack(quats, axis=0).reshape(matrix.shape[:-2] + (4,)) return normalize_quat_wxyz(out) def matrix_to_rot6d(matrix: np.ndarray) -> np.ndarray: matrix = np.asarray(matrix, dtype=np.float32) return np.concatenate([matrix[..., :, 0], matrix[..., :, 1]], axis=-1).astype(np.float32) def rot6d_to_matrix(rot6d: np.ndarray) -> np.ndarray: rot6d = np.asarray(rot6d, dtype=np.float32) a1 = rot6d[..., 0:3] a2 = rot6d[..., 3:6] b1 = a1 / np.maximum(np.linalg.norm(a1, axis=-1, keepdims=True), 1e-8) a2_orth = a2 - np.sum(b1 * a2, axis=-1, keepdims=True) * b1 b2 = a2_orth / np.maximum(np.linalg.norm(a2_orth, axis=-1, keepdims=True), 1e-8) b3 = np.cross(b1, b2) return np.stack([b1, b2, b3], axis=-1).astype(np.float32) def quat_delta_to_rot6d(prev_quat: np.ndarray, cur_quat: np.ndarray) -> np.ndarray: prev_quat = normalize_quat_wxyz(prev_quat) cur_quat = normalize_quat_wxyz(cur_quat) cur_quat = np.where(np.sum(prev_quat * cur_quat, axis=-1, keepdims=True) < 0.0, -cur_quat, cur_quat) rel_quat = quat_mul_wxyz(quat_inv_wxyz(prev_quat), cur_quat) return matrix_to_rot6d(quat_to_matrix_wxyz(rel_quat)) def rot6d_delta_to_quat(rot6d: np.ndarray) -> np.ndarray: return matrix_to_quat_wxyz(rot6d_to_matrix(rot6d)) def estimate_two_foot_contacts(joint_delta: np.ndarray, root_delta_local: np.ndarray) -> np.ndarray: left_leg_motion = np.linalg.norm(joint_delta[..., 0:6], axis=-1) right_leg_motion = np.linalg.norm(joint_delta[..., 6:12], axis=-1) root_z_motion = np.abs(root_delta_local[..., 2]) left = np.logical_and(left_leg_motion < 0.035, root_z_motion < 0.025) right = np.logical_and(right_leg_motion < 0.035, root_z_motion < 0.025) return np.stack([left, right], axis=-1).astype(np.float32) def build_seed_textop_action(actions_abs: np.ndarray, states_abs: np.ndarray) -> np.ndarray: actions_abs = np.asarray(actions_abs, dtype=np.float32) states_abs = np.asarray(states_abs, dtype=np.float32) if actions_abs.shape[-1] != SEED_ABS_DIM or states_abs.shape[-1] != SEED_ABS_DIM: raise ValueError("TextOp SEED representation requires 36D absolute action/state.") if states_abs.ndim == 1: states_abs = states_abs[None, :] prev_abs = np.concatenate([states_abs[-1:, :], actions_abs[:-1, :]], axis=0) joint_abs = actions_abs[:, :SEED_JOINT_DIM] joint_delta = joint_abs - prev_abs[:, :SEED_JOINT_DIM] root_delta_local = local_delta_from_world(prev_abs[:, 29:32], actions_abs[:, 29:32], prev_abs[:, 32:36]) root_delta_local_xy = root_delta_local[:, :2] root_rot5_delta = quat_delta_to_textop_rot5(prev_abs[:, 32:36], actions_abs[:, 32:36]) root_height = actions_abs[:, 31:32] foot_contact = estimate_two_foot_contacts(joint_delta, root_delta_local) return np.concatenate( [joint_abs, root_delta_local_xy, root_rot5_delta, root_height, foot_contact], axis=-1, ).astype(np.float32) def build_seed_textop_state(states_abs: np.ndarray) -> np.ndarray: states_abs = np.asarray(states_abs, dtype=np.float32) if states_abs.shape[-1] != SEED_ABS_DIM: raise ValueError("TextOp SEED state representation requires 36D absolute state.") if states_abs.ndim == 1: states_abs = states_abs[None, :] # state也使用同一套39D motion token;首帧没有更早上下文,delta置0。 prev_abs = np.concatenate([states_abs[:1, :], states_abs[:-1, :]], axis=0) joint_abs = states_abs[:, :SEED_JOINT_DIM] joint_delta = joint_abs - prev_abs[:, :SEED_JOINT_DIM] root_delta_local = local_delta_from_world(prev_abs[:, 29:32], states_abs[:, 29:32], prev_abs[:, 32:36]) root_delta_local_xy = root_delta_local[:, :2] root_rot5 = quat_delta_to_textop_rot5(prev_abs[:, 32:36], states_abs[:, 32:36]) root_height = states_abs[:, 31:32] foot_contact = estimate_two_foot_contacts(joint_delta, root_delta_local) return np.concatenate([joint_abs, root_delta_local_xy, root_rot5, root_height, foot_contact], axis=-1).astype(np.float32) def recover_seed_abs_from_textop_action(features: np.ndarray, current_abs: np.ndarray) -> np.ndarray: features = np.asarray(features, dtype=np.float32) current_abs = np.asarray(current_abs, dtype=np.float32).copy() if features.shape[-1] != SEED_TEXTOP_ACTION_DIM: raise ValueError(f"Expected textop action dim {SEED_TEXTOP_ACTION_DIM}, got {features.shape[-1]}") if current_abs.shape[-1] != SEED_ABS_DIM: raise ValueError(f"Expected current_abs dim {SEED_ABS_DIM}, got {current_abs.shape[-1]}") frames = [] prev_abs = current_abs.copy() for feat in features: joint_abs = feat[TEXTOP_JOINT_ABS] root_delta_local_xy = feat[TEXTOP_ROOT_DELTA_LOCAL_XY] root_rot_delta = feat[TEXTOP_ROOT_ROT5_DELTA] root_height = feat[TEXTOP_ROOT_HEIGHT][0] root_delta_local = np.array([root_delta_local_xy[0], root_delta_local_xy[1], 0.0], dtype=np.float32) root_pos = prev_abs[29:32] + world_delta_from_local(prev_abs[32:36], root_delta_local) root_pos[2] = root_height roll = float(np.arctan2(root_rot_delta[0], root_rot_delta[1])) pitch = float(np.arctan2(root_rot_delta[2], root_rot_delta[3])) yaw = float(quat_to_yaw_wxyz(prev_abs[32:36]) + root_rot_delta[4]) root_quat = rpy_to_quat_wxyz(roll, pitch, yaw) frame = np.concatenate([joint_abs, root_pos.astype(np.float32), root_quat.astype(np.float32)], axis=0) frame = frame.astype(np.float32) frames.append(frame) prev_abs = frame return np.stack(frames, axis=0).astype(np.float32)