code-Psi0-yth-v21 / src /psi /data /seed /motion_repr.py
Tinghong-Ye22's picture
Add files using upload-large-folder tool
6597efe verified
Raw History Blame Contribute Delete
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)