跳到主要内容

robot_flip_left_right

Module: GBC.utils.data_preparation.robot_flip_left_right

This module provides a sophisticated framework for bilateral symmetry transformations in humanoid robot action spaces. It implements mathematically rigorous left-right mirroring operations essential for data augmentation and symmetry preservation in motion retargeting pipelines. The framework handles complex joint transformations including sign flips, joint swaps, and Euler angle reflections with rotation matrix mathematics.

📚 Dependencies​

This module requires the following Python packages:

  • torch - PyTorch framework for tensor operations
  • numpy - Numerical computing library
  • scipy.spatial.transform - Rotation mathematics (Rotation class)
  • roma - Advanced rotation mathematics library
  • GBC.utils.base.base_fk - Robot forward kinematics
  • GBC.utils.base.math_utils - Mathematical utilities for robotics
  • GBC.utils.data_preparation.data_preparation_cfg - Configuration management

🧮 Mathematical Foundation​

🔄 Rotation Matrix Reflection​

The core mathematical operation for bilateral symmetry is reflection about the robot's sagittal plane (YZ-plane). For a rotation matrix R, the reflected rotation R' is computed as:

Mathematical Formulation:

R'[1,0] = -R[1,0]    R'[1,2] = -R[1,2]
R'[0,1] = -R[0,1]    R'[2,1] = -R[2,1]

🔧 Implementation:

def flip_rot_mat_left_right(rot_mat):
    """Reflect rotation matrix about sagittal plane"""
    shape = rot_mat.shape
    rot_mat = rot_mat.view(-1, 3, 3)
    rot_mat[:, 1, 0] *= -1    # Y-X reflection
    rot_mat[:, 1, 2] *= -1    # Y-Z reflection
    rot_mat[:, 0, 1] *= -1    # X-Y reflection
    rot_mat[:, 2, 1] *= -1    # Z-Y reflection
    return rot_mat.view(shape)

💡 Purpose: This transformation preserves the orthogonal properties of rotation matrices while implementing proper bilateral reflection for humanoid robots.

🏗️ Core Architecture​

🎯 RobotFlipLeftRight (Base Class)​

Module Name: GBC.utils.data_preparation.robot_flip_left_right.RobotFlipLeftRight

Definition:

class RobotFlipLeftRight:
    def __init__(self, cfg: AMASSActionConverterCfg | None = None):
        self.flip_sign_ids = []     # Joints that flip sign only
        self.swap_dict = {}         # Single-DOF joints that swap left-right
        self.flip_rpy_dict = {}     # Multi-DOF joints with Euler angle flipping
        self.is_prepared = False

⚙️ Core Data Structures:

  1. flip_sign_ids (List[int]):

    • Joint indices that require sign inversion (e.g., torso yaw, waist roll)
    • Operation: q'[i] = -q[i]
    • Use Case: Non-mirroring joints that reverse direction under reflection

  2. swap_dict (Dict[str, List[int]]):

    • Single-DOF joint pairs that swap positions
    • Operation: q'[left] = q[right], q'[right] = q[left]
    • Use Case: Simple joint pairs (e.g., left/right knee pitch)

  3. flip_rpy_dict (Dict[str, List[List]]):

    • Multi-DOF joints requiring Euler angle transformation
    • Operation: Rotation matrix reflection + Euler angle re-extraction
    • Use Case: Complex joints with roll-pitch-yaw degrees of freedom

🧠 Core Algorithm: flip() Method​

Method Signature:

def flip(self, q: torch.Tensor, is_velocity: bool = True) -> torch.Tensor

📥 Input Parameters:

  • q (torch.Tensor): Joint configurations or velocities [..., num_dofs]
  • is_velocity (bool): Whether input represents velocities (True) or positions (False)

🔧 Algorithm Steps:

Step 1: Sign Flipping​

# Apply sign inversion to non-mirroring joints
if self.flip_sign_ids.numel() > 0:
    new_q[:, self.flip_sign_ids] *= -1

Step 2: Simple Joint Swapping​

# Swap single-DOF left-right joint pairs
for li, ri in self.swap_dict.values():
    new_q[:, li] = q_flat[:, ri]  # Left gets right value
    new_q[:, ri] = q_flat[:, li]  # Right gets left value

Step 3: Euler Angle Transformation​

For Position Data (is_velocity=False):

# Convert to rotation matrices
rot_mat = euler_to_rot_mat(axis, q_flat[:, ids])

# Apply matrix reflection
rot_mats = flip_rot_mat_left_right(rot_mats)

# Swap left-right and convert back to Euler angles
rot_mats = torch.flip(rot_mats, dims=[0])
angles = roma.rotmat_to_euler(convention=full_joint_axis, rotmat=rot_mats[i])

For Velocity Data (is_velocity=True):

# Angular velocity reflection: ω = [ωx, ωy, ωz] → ω' = [-ωx, ωy, -ωz]
sign_flips = {'x': -1.0, 'y': 1.0, 'z': -1.0}
new_q[:, left_ids] = right_vel * sign_flips
new_q[:, right_ids] = left_vel * sign_flips

🎯 Output:

  • torch.Tensor: Transformed joint configurations with proper bilateral symmetry

🤖 Robot-Specific Implementations​

🦾 UnitreeH12FlipLeftRight​

Module Name: GBC.utils.data_preparation.robot_flip_left_right.UnitreeH12FlipLeftRight

🔧 Joint Parsing Strategy:

def prepare_flip_joint_ids(self, dof_names: list[str] | None = None):
    # Parse joint names following Unitree naming convention
    for joint_name in self.dof_names:
        if "left" in joint_name or "right" in joint_name:
            names = joint_name.split("_")
            
            if len(names) == 3:  # Single DOF: "left_knee_joint"
                self.swap_dict[names[1]].append(joint_index)
                
            else:  # Multi DOF: "left_shoulder_roll_joint"
                side_id = ["left", "right"].index(names[0])
                angle_id = names[2][0]  # 'r', 'p', or 'y'
                self.flip_rpy_dict[names[1]][side_id][0] += axis_char
                self.flip_rpy_dict[names[1]][side_id][1].append(joint_index)

🎯 Naming Convention:

  • Single DOF: {side}_{joint}_joint → Swap operation
  • Multi DOF: {side}_{joint}_{rpy}_joint → Euler transformation
  • Non-mirroring: torso_joint → Sign flip only

💡 Example Joints:

# Single DOF swapping
"left_knee_joint" ↔ "right_knee_joint"

# Multi DOF Euler transformation  
"left_shoulder_roll_joint", "left_shoulder_pitch_joint", "left_shoulder_yaw_joint"
↕
"right_shoulder_roll_joint", "right_shoulder_pitch_joint", "right_shoulder_yaw_joint"

# Sign flipping only
"torso_joint" → -"torso_joint"

🏃 TurinV3FlipLeftRight​

Module Name: GBC.utils.data_preparation.robot_flip_left_right.TurinV3FlipLeftRight

🔧 Joint Parsing Strategy:

def prepare_flip_joint_ids(self, dof_names: list[str] | None = None):
    # Turin V3 humanoid naming convention
    for joint_name in self.dof_names:
        names = joint_name.split("_")
        name = "_".join(names[1:])  # Remove l/r prefix
        
        if len(names) == 2:  # Single DOF: "l_knee"
            self.swap_dict[name].append(joint_index)
            
        else:  # Multi DOF: "l_arm_roll"
            side_id = ["l", "r"].index(names[0])
            angle_id = names[2][0]  # 'r', 'p', or 'y'
            self.flip_rpy_dict[name][side_id][0] += axis_char
            self.flip_rpy_dict[name][side_id][1].append(joint_index)

🎯 Naming Convention:

  • Single DOF: {l|r}_{joint} → Swap operation
  • Multi DOF: {l|r}_{joint}_{rpy} → Euler transformation
  • Non-mirroring: head_yaw, waist_yaw, waist_roll → Sign flip only

💡 Example Joints:

# Single DOF swapping
"l_knee" ↔ "r_knee"
"l_ankle_pitch" ↔ "r_ankle_pitch"

# Multi DOF Euler transformation
"l_arm_roll", "l_arm_pitch", "l_arm_yaw"
↕  
"r_arm_roll", "r_arm_pitch", "r_arm_yaw"

# Sign flipping only
"head_yaw" → -"head_yaw"
"waist_roll" → -"waist_roll"

🛠️ Implementation Guide​

📋 Creating Custom Robot Flippers​

To implement a flipper for your robot, follow this systematic approach:

Step 1: Analyze Joint Naming Convention​

class CustomRobotFlipLeftRight(RobotFlipLeftRight):
    def prepare_flip_joint_ids(self, dof_names: list[str] | None = None):
        # Extract your robot's joint names
        if dof_names is None:
            self.fk = RobotKinematics(self.urdf_path)
            self.dof_names = self.fk.dof_names
        else:
            self.dof_names = dof_names
            
        self.num_dofs = len(self.dof_names)

Step 2: Identify Joint Categories​

# 1. Non-mirroring joints (sign flip only)
self.flip_sign_ids = [
    self.dof_names.index(name) for name in ("spine_yaw", "head_pan")
]

# 2. Single-DOF mirroring joints (swap only)
# 3. Multi-DOF mirroring joints (Euler transformation)
for joint_name in self.dof_names:
    # Parse your naming convention here
    if self.is_single_dof_joint(joint_name):
        # Add to swap_dict
    elif self.is_multi_dof_joint(joint_name):
        # Add to flip_rpy_dict

Step 3: Implement Parsing Logic​

def parse_joint_name(self, joint_name: str):
    """Extract side, joint type, and DOF from joint name"""
    # Example for "left_shoulder_roll_joint" format
    parts = joint_name.split("_")
    side = parts[0]  # "left" or "right"
    joint_type = parts[1]  # "shoulder"
    dof_type = parts[2] if len(parts) > 3 else None  # "roll"
    
    return side, joint_type, dof_type

🎯 Automatic URDF-Based Parsing​

Advanced Implementation Example:

class AutoFlipLeftRight(RobotFlipLeftRight):
    def prepare_flip_joint_ids(self, dof_names: list[str] | None = None):
        # Initialize data structures
        self.swap_dict = {}
        self.flip_rpy_dict = {}
        non_mirroring_keywords = ["spine", "torso", "waist", "head", "neck"]
        
        for i, joint_name in enumerate(self.dof_names):
            # Check if non-mirroring joint
            if any(keyword in joint_name.lower() for keyword in non_mirroring_keywords):
                if self.contains_yaw_or_roll(joint_name):
                    self.flip_sign_ids.append(i)
                continue
                
            # Parse left-right structure
            side, base_name, dof = self.parse_joint_structure(joint_name)
            
            if side in ["left", "right", "l", "r"]:
                if dof is None:  # Single DOF
                    if base_name not in self.swap_dict:
                        self.swap_dict[base_name] = []
                    self.swap_dict[base_name].append(i)
                else:  # Multi DOF
                    if base_name not in self.flip_rpy_dict:
                        self.flip_rpy_dict[base_name] = [["", []], ["", []]]
                    side_idx = self.get_side_index(side)
                    axis_char = self.dof_to_axis(dof)
                    self.flip_rpy_dict[base_name][side_idx][0] += axis_char
                    self.flip_rpy_dict[base_name][side_idx][1].append(i)

💡 Usage Examples​

🚀 Basic Usage​

from GBC.utils.data_preparation.robot_flip_left_right import TurinV3FlipLeftRight
from GBC.utils.base.base_fk import RobotKinematics

# Initialize flipper for Turin robot
flipper = TurinV3FlipLeftRight()

# Set up with robot kinematics
fk = RobotKinematics(urdf_path, device='cuda:0')
flipper.prepare_flip_joint_ids(fk.get_dof_names())

# Apply symmetry transformation
original_actions = torch.randn(batch_size, num_dofs)
mirrored_actions = flipper(original_actions)

# Verify bilateral symmetry
assert flipper(flipper(original_actions)).allclose(original_actions)

🔧 Integration with Training​

# In pose transformer training
from GBC.utils.data_preparation.robot_flip_left_right import TurinV3FlipLeftRight

class PoseFormerTrainer:
    def set_flip_left_right(self, flipper: RobotFlipLeftRight):
        """Configure bilateral symmetry transformation"""
        self.flip_left_right = flipper
        
    def symmetry_loss(self, actions, pose_body, pose_hand=None):
        """Compute symmetry consistency loss"""
        # Apply symmetry to input pose
        pose_body_flipped = symmetry_smplh_pose(pose_body)
        
        # Get actions for flipped pose
        flipped_input = self.prepare_model_input(pose_body_flipped, pose_hand)
        actions_flipped = self.model(flipped_input)
        
        # Apply action symmetry and compare
        actions_sym = self.flip_left_right.apply_action_flip(actions)
        return F.mse_loss(actions_sym, actions_flipped)

🧪 Validation and Testing​

# Test flipper correctness
def validate_flipper(flipper, test_actions):
    """Validate flipper mathematical properties"""
    
    # Test 1: Involution property (f(f(x)) = x)
    twice_flipped = flipper(flipper(test_actions))
    assert torch.allclose(test_actions, twice_flipped, atol=1e-6)
    
    # Test 2: Linear transformation property
    X = torch.randn(10000, num_dofs)
    Y = flipper(X)
    
    # Fit linear model to verify linearity
    from sklearn.linear_model import LinearRegression
    model = LinearRegression()
    model.fit(X.cpu().numpy(), Y.cpu().numpy())
    
    # Check if transformation is purely linear
    predicted = torch.tensor(model.predict(X.cpu().numpy()))
    assert torch.allclose(Y.cpu(), predicted, atol=1e-5)
    
    print("Flipper validation passed! ✓")

# Usage
flipper = TurinV3FlipLeftRight()
flipper.prepare_flip_joint_ids(dof_names)
validate_flipper(flipper, torch.randn(100, len(dof_names)))

🔍 Advanced Features​

📊 Linear Transformation Analysis​

The framework provides tools to analyze the linear transformation matrix of the flip operation:

def analyze_flip_transformation(flipper, num_dofs, num_samples=100000):
    """Analyze the linear transformation matrix of flip operation"""
    
    # Generate random samples
    X = torch.randn(num_samples, num_dofs) * 2 - 1
    Y = flipper(X)
    
    # Fit linear regression to extract transformation matrix
    from sklearn.linear_model import LinearRegression
    model = LinearRegression()
    model.fit(X.cpu().numpy(), Y.cpu().numpy())
    
    # Analyze coefficient matrix
    coef_matrix = model.coef_  # Shape: (num_dofs, num_dofs)
    
    # Find non-zero coefficients (significant transformations)
    valid_coef = np.abs(coef_matrix) > 1e-5
    
    print("Transformation Analysis:")
    for i, source_joint in enumerate(flipper.dof_names):
        active_targets = np.where(valid_coef[i])[0]
        if len(active_targets) > 0:
            target_joint = flipper.dof_names[active_targets[0]]
            coefficient = coef_matrix[i, active_targets[0]]
            operation = "Sign Flip" if coefficient < 0 else "Swap"
            print(f"{source_joint} → {target_joint} ({operation}: {coefficient:.3f})")

🎯 Velocity vs Position Handling​

The framework automatically handles both position and velocity transformations:

# Position transformation (involves rotation matrix operations)
position_flipped = flipper(joint_positions, is_velocity=False)

# Velocity transformation (uses angular velocity reflection rules)
velocity_flipped = flipper(joint_velocities, is_velocity=True)

Key Differences:

  • Position: Requires Euler angle extraction, rotation matrix reflection, and re-conversion
  • Velocity: Uses direct angular velocity reflection rules (ω → [-ωx, ωy, -ωz])

🚨 Best Practices​

✅ Implementation Guidelines​

  1. Joint Naming Consistency: Ensure your robot's joint names follow a consistent left-right naming convention
  2. URDF Verification: Validate joint limits and axis definitions in your URDF file
  3. Testing: Always validate the involution property: flip(flip(x)) = x
  4. Performance: Prepare flip joint IDs once and reuse for batch processing
  5. Device Management: Ensure tensor device consistency throughout the pipeline

🔧 Common Pitfalls​

  1. Inconsistent Naming: Mixed naming conventions (left/right vs l/r) within the same robot
  2. Missing Joints: Forgetting to handle all DOFs in your robot
  3. Axis Confusion: Incorrect Euler angle axis ordering for multi-DOF joints
  4. Device Mismatch: Tensor device inconsistency between flipper and input data

🎯 Debugging Tips​

# Debug joint categorization
print("Flip Sign IDs:", flipper.flip_sign_ids)
print("Swap Dict:", flipper.swap_dict)
print("Flip RPY Dict:", flipper.flip_rpy_dict)

# Verify completeness
total_joints = len(flipper.flip_sign_ids) + \
               sum(len(v) for v in flipper.swap_dict.values()) + \
               sum(len(side[1]) for joint in flipper.flip_rpy_dict.values() for side in joint)
assert total_joints == len(flipper.dof_names), "Missing joint assignments!"

This comprehensive framework provides the mathematical foundation and practical tools for implementing robust bilateral symmetry transformations in humanoid robot control systems.