跳到主要内容

math_utils

This module provides comprehensive mathematical utilities for robotics applications, including rotation mathematics, motion processing, quaternion operations, and various utility functions.

📚 Dependencies​

This module requires the following Python packages:

  • pytorch
  • numpy
  • scipy

Most functions are JIT-compiled with torch.jit for optimal performance in real-time applications.

📋 Utility Functions​

📝 list_expand​

Module Name: GBC.utils.base.math_utils.list_expand

Definition:

def list_expand(input: List, times: int, length: int) -> List

📥 Input:

  • input (List): The list to expand
  • times (int): Number of times to repeat the expansion
  • length (int): Length increment for each repetition

📤 Output:

  • result (List): Expanded list with incremented indices

🔧 Functionality: Expands a list by repeating it the specified number of times, incrementing indices by the length offset for each repetition.

💡 Example Usage:

indices = [0, 1, 2]
expanded = list_expand(indices, times=3, length=5)
# Result: [0, 1, 2, 5, 6, 7, 10, 11, 12]

🔍 hampel_filter​

Module Name: GBC.utils.base.math_utils.hampel_filter

Definition:

def hampel_filter(x: torch.Tensor, window_size: int = 3, n_sigma: float = 3.0) -> Tuple[torch.Tensor, bool]

📥 Input:

  • x (torch.Tensor): Input tensor of shape (T, D) where T is sequence length
  • window_size (int): Half window size for the filter. Default is 3
  • n_sigma (float): Number of standard deviations for threshold. Default is 3.0

📤 Output:

  • corrected_x (torch.Tensor): Filtered tensor of shape (T, D)
  • has_outlier (bool): Whether any outliers were detected

🔧 Functionality: Applies Hampel filter for robust outlier detection and removal using median and MAD (Median Absolute Deviation) statistics.

💡 Example Usage:

noisy_data = torch.randn(100, 5)
filtered_data, has_outliers = hampel_filter(noisy_data, window_size=5, n_sigma=2.5)

🔄 Rotation Mathematics​

🛠️ _build_single_axis_rot_mat​

Module Name: GBC.utils.base.math_utils._build_single_axis_rot_mat

Definition:

@torch.jit.script
def _build_single_axis_rot_mat(axis: str, angle: torch.Tensor) -> torch.Tensor

📥 Input:

  • axis (str): Rotation axis ('x', 'y', or 'z')
  • angle (torch.Tensor): Rotation angle in radians

📤 Output:

  • rotation_matrix (torch.Tensor): Rotation matrix of shape (..., 3, 3)

🔧 Functionality: Internal helper function to build a rotation matrix for a single axis rotation with proper batch handling.

⚠️ Note: This is an internal helper function used by euler_to_rot_mat for building single-axis rotation matrices.

🎯 euler_to_rot_mat​

Module Name: GBC.utils.base.math_utils.euler_to_rot_mat

Definition:

def euler_to_rot_mat(axis: str, angle: torch.Tensor) -> torch.Tensor

📥 Input:

  • axis (str): A string specifying the axes of rotation (e.g., 'x', 'xy', 'zyx')
  • angle (torch.Tensor): Euler angles with shape (..., N) where N = len(axis)

📤 Output:

  • rotation_matrices (torch.Tensor): Rotation matrices of shape (..., 3, 3)

🔧 Functionality: Converts a batch of Euler angles to rotation matrices with variable axis length, mimicking scipy.spatial.transform.Rotation behavior with extrinsic convention.

⚠️ Exceptions:

  • ValueError: If axis string is empty or doesn't match angle tensor dimensions

💡 Example Usage:

angles = torch.tensor([[0.1, 0.2, 0.3]])  # Single rotation
rot_mat = euler_to_rot_mat('xyz', angles)

📐 rot_mat_to_euler​

Module Name: GBC.utils.base.math_utils.rot_mat_to_euler

Definition:

def rot_mat_to_euler(mat: torch.Tensor, axis: str) -> torch.Tensor

📥 Input:

  • mat (torch.Tensor): Rotation matrices of shape (..., 3, 3)
  • axis (str): 3-character axis sequence ('xyz', 'xzy', 'yxz', 'yzx', 'zxy', 'zyx')

📤 Output:

  • euler_angles (torch.Tensor): Euler angles of shape (..., 3)

🔧 Functionality: Converts rotation matrices to Euler angles for any Tait-Bryan sequence, with proper gimbal lock handling.

⚠️ Exceptions:

  • ValueError: If input tensor doesn't have shape (..., 3, 3)
  • NotImplementedError: If axis sequence is not supported

💡 Example Usage:

rot_matrices = torch.eye(3).unsqueeze(0)  # Identity matrix
euler_angles = rot_mat_to_euler(rot_matrices, 'xyz')

⚡ euler_xyz_to_rot_mat​

Module Name: GBC.utils.base.math_utils.euler_xyz_to_rot_mat

Definition:

@torch.jit.script
def euler_xyz_to_rot_mat(euler_angles: torch.Tensor) -> torch.Tensor

📥 Input:

  • euler_angles (torch.Tensor): Euler angles (roll, pitch, yaw) of shape (B, 3)

📤 Output:

  • rot_mats (torch.Tensor): Rotation matrices of shape (B, 3, 3)

🔧 Functionality: JIT-compiled function to convert Euler angles in 'xyz' order (roll, pitch, yaw) to rotation matrices with optimal performance.

💡 Example Usage:

euler_angles = torch.tensor([[0.1, 0.2, 0.3]])
rot_mats = euler_xyz_to_rot_mat(euler_angles)

🌀 rot_vec_to_mat​

Module Name: GBC.utils.base.math_utils.rot_vec_to_mat

Definition:

@torch.jit.script
def rot_vec_to_mat(rot_vec: torch.Tensor) -> torch.Tensor

📥 Input:

  • rot_vec (torch.Tensor): Rotation vectors of shape (B, 3)

📤 Output:

  • rotation_matrices (torch.Tensor): Rotation matrices of shape (B, 3, 3)

🔧 Functionality: Converts rotation vectors to rotation matrices using Rodrigues' formula with proper handling of small angle cases and numerical stability.

💡 Example Usage:

rot_vecs = torch.tensor([[0.1, 0.2, 0.3], [0.0, 0.0, 0.1]])
rot_mats = rot_vec_to_mat(rot_vecs)

🔙 rot_mat_to_vec​

Module Name: GBC.utils.base.math_utils.rot_mat_to_vec

Definition:

@torch.jit.script
def rot_mat_to_vec(rot_mat: torch.Tensor) -> torch.Tensor

📥 Input:

  • rot_mat (torch.Tensor): Rotation matrices of shape (B, 3, 3)

📤 Output:

  • rotation_vectors (torch.Tensor): Rotation vectors of shape (B, 3)

🔧 Functionality: Converts rotation matrices to axis-angle representation with robust handling of edge cases and numerical stability.

💡 Example Usage:

rot_mats = torch.eye(3).unsqueeze(0).repeat(5, 1, 1)
rot_vecs = rot_mat_to_vec(rot_mats)

📦 batch_rot_vec_to_mat​

Module Name: GBC.utils.base.math_utils.batch_rot_vec_to_mat

Definition:

@torch.jit.script
def batch_rot_vec_to_mat(rot_vec: torch.Tensor) -> torch.Tensor

📥 Input:

  • rot_vec (torch.Tensor): Rotation vectors of shape (..., 3)

📤 Output:

  • rotation_matrices (torch.Tensor): Rotation matrices of shape (..., 3, 3)

🔧 Functionality: Batch version of rotation vector to matrix conversion, supporting arbitrary batch dimensions.

💡 Example Usage:

batch_rot_vecs = torch.randn(10, 21, 3)  # 10 samples, 21 joints
batch_rot_mats = batch_rot_vec_to_mat(batch_rot_vecs)

📦 batch_rot_mat_to_vec​

Module Name: GBC.utils.base.math_utils.batch_rot_mat_to_vec

Definition:

@torch.jit.script
def batch_rot_mat_to_vec(rot_mat: torch.Tensor) -> torch.Tensor

📥 Input:

  • rot_mat (torch.Tensor): Rotation matrices of shape (..., 3, 3)

📤 Output:

  • rotation_vectors (torch.Tensor): Rotation vectors of shape (..., 3)

🔧 Functionality: Batch version of rotation matrix to vector conversion, supporting arbitrary batch dimensions.

💡 Example Usage:

batch_rot_mats = torch.randn(10, 21, 3, 3)
batch_rot_vecs = batch_rot_mat_to_vec(batch_rot_mats)

🧭 angle_axis_to_ypr​

Module Name: GBC.utils.base.math_utils.angle_axis_to_ypr

Definition:

@torch.jit.script
def angle_axis_to_ypr(angle_axis: torch.Tensor) -> torch.Tensor

📥 Input:

  • angle_axis (torch.Tensor): Angle-axis representation of shape (3,)

📤 Output:

  • ypr (torch.Tensor): Yaw, pitch, roll angles of shape (3,)

🔧 Functionality: Converts single angle-axis representation to yaw-pitch-roll Euler angles.

💡 Example Usage:

angle_axis = torch.tensor([0.1, 0.2, 0.3])
ypr = angle_axis_to_ypr(angle_axis)

📦 batch_angle_axis_to_ypr​

Module Name: GBC.utils.base.math_utils.batch_angle_axis_to_ypr

Definition:

@torch.jit.script
def batch_angle_axis_to_ypr(angle_axis: torch.Tensor) -> torch.Tensor

📥 Input:

  • angle_axis (torch.Tensor): Batch of angle-axis vectors of shape (B, 3)

📤 Output:

  • ypr (torch.Tensor): Yaw, pitch, roll angles of shape (B, 3)

🔧 Functionality: Batch version of angle-axis to yaw-pitch-roll conversion.

💡 Example Usage:

batch_angle_axis = torch.randn(100, 3)
batch_ypr = batch_angle_axis_to_ypr(batch_angle_axis)

🔄 Quaternion Operations​

🔀 q_mul​

Module Name: GBC.utils.base.math_utils.q_mul

Definition:

@torch.jit.script
def q_mul(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor

📥 Input:

  • q1 (torch.Tensor): First quaternion of shape (..., 4)
  • q2 (torch.Tensor): Second quaternion of shape (..., 4)

📤 Output:

  • result (torch.Tensor): Quaternion product of shape (..., 4)

🔧 Functionality: Performs quaternion multiplication using the standard quaternion product formula with JIT compilation for performance.

💡 Example Usage:

q1 = torch.tensor([[1.0, 0.0, 0.0, 0.0]])  # Identity quaternion
q2 = torch.tensor([[0.707, 0.707, 0.0, 0.0]])  # 90° rotation around x
result = q_mul(q1, q2)

🔄 euler_to_quaternion​

Module Name: GBC.utils.base.math_utils.euler_to_quaternion

Definition:

def euler_to_quaternion(e: torch.Tensor, order: str) -> torch.Tensor

📥 Input:

  • e (torch.Tensor): Euler angles of shape (..., 3)
  • order (str): Rotation order (e.g., 'xyz', 'zyx')

📤 Output:

  • quaternions (torch.Tensor): Quaternions of shape (..., 4)

🔧 Functionality: Converts Euler angles to quaternions using sequential rotation composition, with support for different rotation orders.

⚠️ Exceptions:

  • ValueError: If unknown coordinate in order string

💡 Example Usage:

euler = torch.tensor([[0.1, 0.2, 0.3]])
quat = euler_to_quaternion(euler, 'xyz')

🔧 quat_fix​

Module Name: GBC.utils.base.math_utils.quat_fix

Definition:

@torch.jit.script
def quat_fix(q: torch.Tensor) -> torch.Tensor

📥 Input:

  • q (torch.Tensor): Quaternions of shape (..., T, 4) where T is time dimension

📤 Output:

  • result (torch.Tensor): Continuity-enforced quaternions of same shape

🔧 Functionality: Enforces quaternion continuity across time by selecting the representation (q or -q) with minimal distance between consecutive frames, eliminating quaternion flipping artifacts.

💡 Example Usage:

quat_sequence = torch.randn(5, 10, 4)  # 5 batches, 10 time steps
fixed_sequence = quat_fix(quat_sequence)

📐 angle_axis_to_quaternion​

Module Name: GBC.utils.base.math_utils.angle_axis_to_quaternion

Definition:

@torch.jit.script
def angle_axis_to_quaternion(angle_axis: torch.Tensor) -> torch.Tensor

📥 Input:

  • angle_axis (torch.Tensor): Angle-axis vectors of shape (..., 3)

📤 Output:

  • quaternions (torch.Tensor): Quaternions of shape (..., 4)

🔧 Functionality: Converts angle-axis representation to quaternions with efficient JIT compilation.

💡 Example Usage:

angle_axis = torch.randn(100, 3)
quaternions = angle_axis_to_quaternion(angle_axis)

🌐 slerp​

Module Name: GBC.utils.base.math_utils.slerp

Definition:

@torch.jit.script
def slerp(q1: torch.Tensor, q2: torch.Tensor, t: torch.Tensor) -> torch.Tensor

📥 Input:

  • q1 (torch.Tensor): Start quaternions of shape (..., 4)
  • q2 (torch.Tensor): End quaternions of shape (..., 4)
  • t (torch.Tensor): Interpolation parameter in [0, 1]

📤 Output:

  • interpolated (torch.Tensor): Interpolated quaternions of shape (..., 4)

🔧 Functionality: Spherical Linear Interpolation (SLERP) between quaternions, providing smooth rotation interpolation.

💡 Example Usage:

q1 = torch.tensor([[1.0, 0.0, 0.0, 0.0]])
q2 = torch.tensor([[0.707, 0.707, 0.0, 0.0]])
t = torch.tensor([0.5])
interpolated = slerp(q1, q2, t)

🔙 quaternion_to_angle_axis​

Module Name: GBC.utils.base.math_utils.quaternion_to_angle_axis

Definition:

@torch.jit.script
def quaternion_to_angle_axis(quaternion: torch.Tensor) -> torch.Tensor

📥 Input:

  • quaternion (torch.Tensor): Quaternions of shape (..., 4)

📤 Output:

  • angle_axis (torch.Tensor): Angle-axis vectors of shape (..., 3)

🔧 Functionality: Converts quaternions to angle-axis representation with numerical stability for edge cases.

💡 Example Usage:

quaternions = torch.randn(100, 4)
quaternions = quaternions / torch.norm(quaternions, dim=-1, keepdim=True)
angle_axis = quaternion_to_angle_axis(quaternions)

🔄 convert_quat​

Module Name: GBC.utils.base.math_utils.convert_quat

Definition:

def convert_quat(quat: Union[torch.Tensor, np.ndarray], to: Literal["xyzw", "wxyz"] = "xyzw") -> Union[torch.Tensor, np.ndarray]

📥 Input:

  • quat (torch.Tensor | np.ndarray): Quaternions of shape (..., 4)
  • to (Literal["xyzw", "wxyz"]): Target convention. Default is "xyzw"

📤 Output:

  • converted_quat (torch.Tensor | np.ndarray): Converted quaternions

🔧 Functionality: Converts quaternions between different conventions (wxyz ↔ xyzw), supporting both PyTorch tensors and NumPy arrays.

⚠️ Exceptions:

  • ValueError: If invalid input shape or conversion target

💡 Example Usage:

wxyz_quat = torch.tensor([[1.0, 0.0, 0.0, 0.0]])
xyzw_quat = convert_quat(wxyz_quat, to="xyzw")

🔗 quat_conjugate​

Module Name: GBC.utils.base.math_utils.quat_conjugate

Definition:

@torch.jit.script
def quat_conjugate(q: torch.Tensor) -> torch.Tensor

📥 Input:

  • q (torch.Tensor): Quaternions in (w, x, y, z) format of shape (..., 4)

📤 Output:

  • q_conj (torch.Tensor): Conjugate quaternions of shape (..., 4)

🔧 Functionality: Computes the conjugate of quaternions by negating the vector part (x, y, z) while keeping the scalar part (w) unchanged.

💡 Example Usage:

q = torch.tensor([[1.0, 0.5, 0.3, 0.2]])
q_conj = quat_conjugate(q)  # [1.0, -0.5, -0.3, -0.2]

🔄 quat_inv​

Module Name: GBC.utils.base.math_utils.quat_inv

Definition:

@torch.jit.script
def quat_inv(q: torch.Tensor) -> torch.Tensor

📥 Input:

  • q (torch.Tensor): Quaternions in (w, x, y, z) format of shape (N, 4)

📤 Output:

  • q_inv (torch.Tensor): Inverse quaternions of shape (N, 4)

🔧 Functionality: Computes the inverse of quaternions using normalized conjugate, providing the rotation in the opposite direction.

💡 Example Usage:

q = torch.tensor([[1.0, 0.5, 0.3, 0.2]])
q_inv = quat_inv(q)

🎯 quat_from_euler_xyz​

Module Name: GBC.utils.base.math_utils.quat_from_euler_xyz

Definition:

@torch.jit.script
def quat_from_euler_xyz(roll: torch.Tensor, pitch: torch.Tensor, yaw: torch.Tensor) -> torch.Tensor

📥 Input:

  • roll (torch.Tensor): Rotation around x-axis in radians, shape (N,)
  • pitch (torch.Tensor): Rotation around y-axis in radians, shape (N,)
  • yaw (torch.Tensor): Rotation around z-axis in radians, shape (N,)

📤 Output:

  • quaternions (torch.Tensor): Quaternions in (w, x, y, z) format, shape (N, 4)

🔧 Functionality: Converts Euler angles in XYZ convention to quaternions using optimized trigonometric calculations.

💡 Example Usage:

roll = torch.tensor([0.1, 0.2])
pitch = torch.tensor([0.3, 0.4])
yaw = torch.tensor([0.5, 0.6])
quats = quat_from_euler_xyz(roll, pitch, yaw)

🔄 quat_from_matrix​

Module Name: GBC.utils.base.math_utils.quat_from_matrix

Definition:

@torch.jit.script
def quat_from_matrix(matrix: torch.Tensor) -> torch.Tensor

📥 Input:

  • matrix (torch.Tensor): Rotation matrices of shape (..., 3, 3)

📤 Output:

  • quaternions (torch.Tensor): Quaternions in (w, x, y, z) format, shape (..., 4)

🔧 Functionality: Converts rotation matrices to quaternions using the most numerically stable algorithm based on the largest diagonal element.

⚠️ Exceptions:

  • ValueError: If matrix doesn't have shape (..., 3, 3)

💡 Example Usage:

rot_mat = torch.eye(3).unsqueeze(0)
quat = quat_from_matrix(rot_mat)

🎯 quat_apply​

Module Name: GBC.utils.base.math_utils.quat_apply

Definition:

@torch.jit.script
def quat_apply(quat: torch.Tensor, vec: torch.Tensor) -> torch.Tensor

📥 Input:

  • quat (torch.Tensor): Quaternions in (w, x, y, z) format of shape (..., 4)
  • vec (torch.Tensor): Vectors in (x, y, z) format of shape (..., 3)

📤 Output:

  • rotated_vec (torch.Tensor): Rotated vectors of shape (..., 3)

🔧 Functionality: Applies quaternion rotation to vectors using efficient quaternion-vector multiplication formula.

💡 Example Usage:

quat = torch.tensor([[1.0, 0.0, 0.0, 0.0]])  # Identity
vec = torch.tensor([[1.0, 0.0, 0.0]])
rotated = quat_apply(quat, vec)

🧭 yaw_quat​

Module Name: GBC.utils.base.math_utils.yaw_quat

Definition:

@torch.jit.script
def yaw_quat(quat: torch.Tensor) -> torch.Tensor

📥 Input:

  • quat (torch.Tensor): Quaternions in (w, x, y, z) format of shape (..., 4)

📤 Output:

  • yaw_only_quat (torch.Tensor): Yaw-only quaternions of shape (..., 4)

🔧 Functionality: Extracts only the yaw component from a quaternion, zeroing out roll and pitch components.

💡 Example Usage:

full_quat = torch.tensor([[0.924, 0.383, 0.0, 0.0]])  # Some rotation
yaw_only = yaw_quat(full_quat)

🎯 quat_apply_yaw​

Module Name: GBC.utils.base.math_utils.quat_apply_yaw

Definition:

@torch.jit.script
def quat_apply_yaw(quat: torch.Tensor, vec: torch.Tensor) -> torch.Tensor

📥 Input:

  • quat (torch.Tensor): Quaternions in (w, x, y, z) format of shape (N, 4)
  • vec (torch.Tensor): Vectors in (x, y, z) format of shape (N, 3)

📤 Output:

  • rotated_vec (torch.Tensor): Yaw-rotated vectors of shape (N, 3)

🔧 Functionality: Rotates vectors using only the yaw component of the quaternion, useful for 2D rotations in 3D space.

💡 Example Usage:

quat = torch.tensor([[0.924, 0.383, 0.0, 0.0]])
vec = torch.tensor([[1.0, 0.0, 0.0]])
yaw_rotated = quat_apply_yaw(quat, vec)

🔄 quat_rotate​

Module Name: GBC.utils.base.math_utils.quat_rotate

Definition:

@torch.jit.script
def quat_rotate(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor

📥 Input:

  • q (torch.Tensor): Quaternions in (w, x, y, z) format of shape (..., 4)
  • v (torch.Tensor): Vectors in (x, y, z) format of shape (..., 3)

📤 Output:

  • rotated_vec (torch.Tensor): Rotated vectors of shape (..., 3)

🔧 Functionality: High-performance quaternion rotation using optimized vector operations and Einstein summation for arbitrary batch dimensions.

💡 Example Usage:

q = torch.randn(100, 4)
q = q / torch.norm(q, dim=-1, keepdim=True)
v = torch.randn(100, 3)
rotated = quat_rotate(q, v)

🔙 quat_rotate_inverse​

Module Name: GBC.utils.base.math_utils.quat_rotate_inverse

Definition:

@torch.jit.script
def quat_rotate_inverse(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor

📥 Input:

  • q (torch.Tensor): Quaternions in (w, x, y, z) format of shape (..., 4)
  • v (torch.Tensor): Vectors in (x, y, z) format of shape (..., 3)

📤 Output:

  • inverse_rotated_vec (torch.Tensor): Inverse-rotated vectors of shape (..., 3)

🔧 Functionality: Applies the inverse quaternion rotation to vectors, equivalent to rotating by the conjugate quaternion.

💡 Example Usage:

q = torch.randn(100, 4)
q = q / torch.norm(q, dim=-1, keepdim=True)
v = torch.randn(100, 3)
inv_rotated = quat_rotate_inverse(q, v)

🔄 matrix_from_quat​

Module Name: GBC.utils.base.math_utils.matrix_from_quat

Definition:

@torch.jit.script
def matrix_from_quat(quaternions: torch.Tensor) -> torch.Tensor

📥 Input:

  • quaternions (torch.Tensor): Quaternions of shape (..., 4)

📤 Output:

  • rotation_matrices (torch.Tensor): Rotation matrices of shape (..., 3, 3)

🔧 Functionality: Converts quaternions to rotation matrices using efficient JIT-compiled implementation.

💡 Example Usage:

quats = torch.randn(10, 4)
quats = quats / torch.norm(quats, dim=-1, keepdim=True)
rot_mats = matrix_from_quat(quats)

🤸 Pose and Motion Processing​

🔄 swap_order​

Module Name: GBC.utils.base.math_utils.swap_order

Definition:

def swap_order(x: torch.Tensor, swap_pairs: List[List[int]], axis: int) -> torch.Tensor

📥 Input:

  • x (torch.Tensor): Input tensor
  • swap_pairs (List[List[int]]): List of index pairs to swap
  • axis (int): Axis along which to perform swaps

📤 Output:

  • x_swapped (torch.Tensor): Tensor with swapped elements

🔧 Functionality: Swaps elements in a tensor based on specified pairs along a given axis, useful for pose symmetry operations.

💡 Example Usage:

tensor = torch.randn(10, 21, 3)
swap_pairs = [[0, 1], [2, 3]]  # Swap indices 0↔1, 2↔3
swapped = swap_order(tensor, swap_pairs, axis=1)

🤸 symmetry_smplh_pose​

Module Name: GBC.utils.base.math_utils.symmetry_smplh_pose

Definition:

def symmetry_smplh_pose(pose: torch.Tensor) -> torch.Tensor

📥 Input:

  • pose (torch.Tensor): SMPLH pose tensor of shape (B, 63) (21 joints × 3)

📤 Output:

  • pose_opp (torch.Tensor): Symmetrized pose tensor of shape (B, 63)

🔧 Functionality: Creates symmetric SMPLH poses by swapping left-right joint pairs and applying proper rotation transformations for human pose mirroring.

💡 Example Usage:

smplh_pose = torch.randn(10, 63)  # 10 poses
symmetric_pose = symmetry_smplh_pose(smplh_pose)

📈 Motion Interpolation and Processing​

📉 downsample_angle_axis​

Module Name: GBC.utils.base.math_utils.downsample_angle_axis

Definition:

def downsample_angle_axis(angle_axis_data: torch.Tensor, target_fps: int = 50, source_fps: int = 120) -> torch.Tensor

📥 Input:

  • angle_axis_data (torch.Tensor): Motion data of shape (num_frames, C) where C = num_joints × 3
  • target_fps (int): Target framerate. Default is 50
  • source_fps (int): Source framerate. Default is 120

📤 Output:

  • downsampled_data (torch.Tensor): Downsampled motion data

🔧 Functionality: Downsamples angle-axis motion capture data using SLERP interpolation to maintain smooth rotations while reducing temporal resolution.

💡 Example Usage:

mocap_data = torch.randn(1200, 63)  # 10 seconds at 120 FPS
downsampled = downsample_angle_axis(mocap_data, target_fps=30, source_fps=120)

📈 interpolate_angle_axis​

Module Name: GBC.utils.base.math_utils.interpolate_angle_axis

Definition:

def interpolate_angle_axis(angle_axis_data: torch.Tensor, target_fps: int = 1000, source_fps: int = 120) -> torch.Tensor

📥 Input:

  • angle_axis_data (torch.Tensor): Motion data of shape (num_frames, C)
  • target_fps (int): Target framerate. Default is 1000
  • source_fps (int): Source framerate. Default is 120

📤 Output:

  • interpolated_data (torch.Tensor): Upsampled motion data

🔧 Functionality: Upsamples angle-axis motion data using SLERP interpolation for smooth high-frequency motion generation.

💡 Example Usage:

low_fps_data = torch.randn(120, 63)  # 1 second at 120 FPS
high_fps_data = interpolate_angle_axis(low_fps_data, target_fps=1000)

📍 interpolate_trans​

Module Name: GBC.utils.base.math_utils.interpolate_trans

Definition:

def interpolate_trans(trans: torch.Tensor, target_fps: int = 1000, source_fps: int = 120) -> torch.Tensor

📥 Input:

  • trans (torch.Tensor): Translation data of shape (num_frames, D)
  • target_fps (int): Target framerate. Default is 1000
  • source_fps (int): Source framerate. Default is 120

📤 Output:

  • interpolated_trans (torch.Tensor): Interpolated translation data

🔧 Functionality: Interpolates translation data using linear interpolation for smooth positional trajectories.

💡 Example Usage:

translations = torch.randn(120, 3)  # 1 second of root positions
interpolated = interpolate_trans(translations, target_fps=1000)

📏 unwrap_angle​

Module Name: GBC.utils.base.math_utils.unwrap_angle

Definition:

@torch.jit.script
def unwrap_angle(angles: torch.Tensor, period: float = 2 * np.pi) -> torch.Tensor

📥 Input:

  • angles (torch.Tensor): 1D tensor of angles in radians, shape (N,)
  • period (float): Period of the angles. Default is 2π

📤 Output:

  • unwrapped_angles (torch.Tensor): Continuous unwrapped angle sequence

🔧 Functionality: Unwraps angle sequences by detecting and correcting phase jumps, creating continuous angle trajectories.

💡 Example Usage:

# Angles with jumps from 2π to 0
angles = torch.tensor([6.2, 6.3, 0.1, 0.2])
unwrapped = unwrap_angle(angles)

📊 simple_moving_average​

Module Name: GBC.utils.base.math_utils.simple_moving_average

Definition:

@torch.jit.script
def simple_moving_average(x: torch.Tensor, window_size: int) -> torch.Tensor

📥 Input:

  • x (torch.Tensor): Input tensor of shape (T, D) where T is sequence length
  • window_size (int): Size of the moving average window

📤 Output:

  • smoothed (torch.Tensor): Smoothed tensor of shape (T, D)

🔧 Functionality: Applies moving average smoothing to 2D tensors using efficient 1D convolution operations.

💡 Example Usage:

noisy_data = torch.randn(100, 5)  # 100 timesteps, 5 features
smoothed = simple_moving_average(noisy_data, window_size=7)

🔧 unwrap_and_smooth_rot_vecs​

Module Name: GBC.utils.base.math_utils.unwrap_and_smooth_rot_vecs

Definition:

@torch.jit.script
def unwrap_and_smooth_rot_vecs(rot_vecs: torch.Tensor, smoothing_window: int = 5) -> torch.Tensor

📥 Input:

  • rot_vecs (torch.Tensor): Rotation vector sequence of shape (T, 3)
  • smoothing_window (int): Size of the smoothing window. Default is 5

📤 Output:

  • fixed_rot_vecs (torch.Tensor): Smoothed rotation vectors of shape (T, 3)

🔧 Functionality: Unwraps and smooths rotation vector sequences by decomposing into angles and axes, unwrapping angle discontinuities, and applying moving average smoothing.

💡 Example Usage:

rot_vec_sequence = torch.randn(200, 3)  # Motion sequence
smoothed_sequence = unwrap_and_smooth_rot_vecs(rot_vec_sequence, smoothing_window=7)

🔄 Pattern Analysis and Contact​

🔄 find_longest_cyclic_subsequence​

Module Name: GBC.utils.base.math_utils.find_longest_cyclic_subsequence

Definition:

def find_longest_cyclic_subsequence(input_tensor: torch.Tensor, max_distance: float = np.inf) -> Tuple[int, int, int, int]

📥 Input:

  • input_tensor (torch.Tensor): Input tensor of shape (N, D)
  • max_distance (float): Maximum distance between start/end points. Default is np.inf

📤 Output:

  • best_start (int): Start index of longest cyclic subsequence
  • best_end (int): End index of longest cyclic subsequence
  • max_length (int): Length of the subsequence
  • seq_distance (float): Distance between start and end points

🔧 Functionality: Finds the longest subsequence where start and end points are within specified distance, useful for identifying repetitive motion patterns.

💡 Example Usage:

motion_sequence = torch.randn(1000, 29)
start, end, length, distance = find_longest_cyclic_subsequence(motion_sequence, max_distance=0.1)

📊 contact_to_phase​

Module Name: GBC.utils.base.math_utils.contact_to_phase

Definition:

def contact_to_phase(contact: torch.Tensor, threshold: float = 0.5) -> torch.Tensor

📥 Input:

  • contact (torch.Tensor): Binary contact sequence of shape (T,)
  • threshold (float): Phase transition threshold. Default is 0.5

📤 Output:

  • alpha (torch.Tensor): Phase values of shape (T,)

🔧 Functionality: Converts binary contact sequences to continuous phase values for smooth gait cycle representation.

💡 Example Usage:

contact_sequence = torch.randint(0, 2, (100,))
phase_values = contact_to_phase(contact_sequence)

👟 filt_feet_contact​

Module Name: GBC.utils.base.math_utils.filt_feet_contact

Definition:

def filt_feet_contact(actions: torch.Tensor, root_pos: torch.Tensor, root_rot: torch.Tensor, fk: object, foot_names: List[str], threshold: float = 0.1, foot_ground_offset: float = 0.045, disable_height: bool = False, debug_visualize: bool = False, debug_save_dir: str = None, debug_rv: object = None) -> torch.Tensor

📥 Input:

  • actions (torch.Tensor): Joint actions of shape (B, N, num_dof)
  • root_pos (torch.Tensor): Root positions of shape (B, N, 3)
  • root_rot (torch.Tensor): Root rotations of shape (B, N, 3)
  • fk (object): Forward kinematics model
  • foot_names (List[str]): Names of foot links
  • threshold (float): Contact detection threshold. Default is 0.1
  • foot_ground_offset (float): Ground offset. Default is 0.045
  • disable_height (bool): Disable height filtering. Default is False

📤 Output:

  • feet_contact_indices (torch.Tensor): Binary contact signals
  • filtered_root_poses (torch.Tensor): Filtered root positions

🔧 Functionality: Generates foot contact signals based on distance to ground with optional height filtering and visualization support.

💡 Example Usage:

contact_signals, filtered_poses = filt_feet_contact(
    actions, root_pos, root_rot, fk_model, 
    ['left_foot', 'right_foot'], threshold=0.05
)

🛠️ Tensor Utilities​

📏 pad_to_len​

Module Name: GBC.utils.base.math_utils.pad_to_len

Definition:

@torch.jit.script
def pad_to_len(tensor: torch.Tensor, target_len: int, pad_front: bool = False) -> torch.Tensor

📥 Input:

  • tensor (torch.Tensor): Input tensor
  • target_len (int): Target length
  • pad_front (bool): Whether to pad at the front. Default is False

📤 Output:

  • padded_tensor (torch.Tensor): Zero-padded tensor

🔧 Functionality: Pads tensor to target length with zeros, supporting both front and back padding.

💡 Example Usage:

short_tensor = torch.randn(5, 3)
padded = pad_to_len(short_tensor, target_len=10, pad_front=False)

📦 batch_pad_to_len​

Module Name: GBC.utils.base.math_utils.batch_pad_to_len

Definition:

@torch.jit.script
def batch_pad_to_len(tensor: torch.Tensor, target_len: int, pad_front: bool = False) -> torch.Tensor

📥 Input:

  • tensor (torch.Tensor): Batch tensor
  • target_len (int): Target length
  • pad_front (bool): Whether to pad at the front. Default is False

📤 Output:

  • padded_tensor (torch.Tensor): Batch-padded tensor

🔧 Functionality: Batch version of padding operation for multiple sequences simultaneously.

💡 Example Usage:

batch_tensor = torch.randn(32, 5, 3)  # 32 sequences of length 5
padded_batch = batch_pad_to_len(batch_tensor, target_len=10)

📐 normalize​

Module Name: GBC.utils.base.math_utils.normalize

Definition:

@torch.jit.script
def normalize(x: torch.Tensor, eps: float = 1e-9) -> torch.Tensor

📥 Input:

  • x (torch.Tensor): Input tensor
  • eps (float): Small epsilon for numerical stability. Default is 1e-9

📤 Output:

  • normalized (torch.Tensor): L2-normalized tensor

🔧 Functionality: Performs L2 normalization along the last dimension with numerical stability.

💡 Example Usage:

vectors = torch.randn(100, 3)
normalized_vectors = normalize(vectors)

🔄 wrap_to_pi​

Module Name: GBC.utils.base.math_utils.wrap_to_pi

Definition:

@torch.jit.script
def wrap_to_pi(angles: torch.Tensor) -> torch.Tensor

📥 Input:

  • angles (torch.Tensor): Input angles in radians

📤 Output:

  • wrapped_angles (torch.Tensor): Angles wrapped to [-π, π]

🔧 Functionality: Wraps angles to the range [-π, π] for consistent angle representation.

💡 Example Usage:

large_angles = torch.tensor([3.5, -4.2, 7.1])
wrapped = wrap_to_pi(large_angles)

🛠️ Internal Helper Functions​

🛠️ _sqrt_positive_part​

Module Name: GBC.utils.base.math_utils._sqrt_positive_part

Definition:

@torch.jit.script
def _sqrt_positive_part(x: torch.Tensor) -> torch.Tensor

📥 Input:

  • x (torch.Tensor): Input tensor

📤 Output:

  • result (torch.Tensor): Square root of positive part

🔧 Functionality: Internal helper function that computes the square root of the positive part of a tensor, ensuring numerical stability.

⚠️ Note: This is an internal helper function used by quat_from_matrix for numerical stability.

🎭 Complete Usage Example​

Here's a comprehensive example showcasing various mathematical utilities:

import torch
from GBC.utils.base.math_utils import *

# 🔄 Rotation conversions
euler_angles = torch.tensor([[0.1, 0.2, 0.3], [0.4, 0.5, 0.6]])
rot_mats = euler_to_rot_mat('xyz', euler_angles)
quaternions = euler_to_quaternion(euler_angles, 'xyz')

# 🧹 Data filtering and processing
noisy_data = torch.randn(100, 6) + torch.sin(torch.linspace(0, 10, 100)).unsqueeze(1)
filtered_data, had_outliers = hampel_filter(noisy_data, window_size=5)

# 🎯 Motion interpolation
low_fps_motion = torch.randn(60, 21*3)  # 1 second at 60 FPS, 21 joints
high_fps_motion = interpolate_angle_axis(low_fps_motion, target_fps=240, source_fps=60)

# 🔄 Quaternion operations
q1 = torch.tensor([[1.0, 0.0, 0.0, 0.0]])
q2 = torch.tensor([[0.707, 0.707, 0.0, 0.0]])
interpolated_quat = slerp(q1, q2, torch.tensor([0.5]))

# 📊 Motion processing
rot_vec_sequence = torch.randn(200, 3)
smoothed_sequence = unwrap_and_smooth_rot_vecs(rot_vec_sequence, smoothing_window=7)

print(f"🔄 Rotation matrices shape: {rot_mats.shape}")
print(f"🧹 Outliers detected: {had_outliers}")
print(f"📈 Upsampled motion shape: {high_fps_motion.shape}")
print(f"🌐 Interpolated quaternion: {interpolated_quat}")
print(f"📊 Smoothed sequence shape: {smoothed_sequence.shape}")

⚡ Performance Tip: Many functions are JIT-compiled for optimal performance. The first call may be slower due to compilation, but subsequent calls will be significantly faster!

🔧 Integration Note: These utilities are designed to work seamlessly with the GBC robotics pipeline, particularly with the RobotKinematics class for kinematic computations.