Download src/psi/data/seed/motion_repr.py from Tinghong-Ye22/code-Psi0-yth-v21: direct link, hf CLI and curl.
- Browser
- Download file 12.7 kB
-
https://huggingface.co/Tinghong-Ye22/code-Psi0-yth-v21/resolve/main/src/psi/data/seed/motion_repr.py
- Command line
-
hf download hf://Tinghong-Ye22/code-Psi0-yth-v21/src/psi/data/seed/motion_repr.py
-
curl -L -o motion_repr.py https://huggingface.co/Tinghong-Ye22/code-Psi0-yth-v21/resolve/main/src/psi/data/seed/motion_repr.py
12.7 kB
| from __future__ import annotations | |
| import numpy as np | |
| # <modify_textop39_repr> 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) | |
| # <modify_textop39_repr> 兼容旧推理脚本的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, :] | |
| # <modify_textop39_repr> 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) | |