Function motion_anchor_pos_b_future

TextOpTracker/scripts/deploy_mujoco.py:390–426

Future N-step motion anchor position in body frame - matching Isaac Lab order

(sim_data, motion_loader, t)

def motion_anchor_pos_b_future(sim_data, motion_loader, t):
    """Future N-step motion anchor position in body frame - matching Isaac Lab order"""
    if t < 0:
        return np.zeros(15, dtype=np.float32)  # 3 * 5 = 15

    # Get robot anchor pose
    robot_pos = sim_data.body(motion_loader.anchor_body_name).xpos.copy().reshape(1, 3)
    robot_quat = sim_data.body(motion_loader.anchor_body_name).xquat.copy().reshape(1, 4)

    # Get future motion anchor poses - batch process like Isaac Lab
    future_positions = []
    future_orientations = []
    for i in range(motion_loader.future_steps):
        step_idx = min(t + i, motion_loader.T - 1)
        ref_pos = motion_loader.body_pos[step_idx][motion_loader.anchor_body_index].reshape(1, 3)
        ref_quat = motion_loader.body_ori[step_idx][motion_loader.anchor_body_index].reshape(1, 4)
        future_positions.append(ref_pos)
        future_orientations.append(ref_quat)

    # Stack to create [future_steps, 1, 3] then reshape to [future_steps, 3]
    future_anchor_pos_w = np.stack(future_positions, axis=0).squeeze(1)  # [5, 3]
    future_anchor_quat_w = np.stack(future_orientations, axis=0).squeeze(1)  # [5, 4]

    # Expand robot anchor for broadcasting: [future_steps, 3] and [future_steps, 4]
    robot_anchor_pos_w_exp = robot_pos.repeat(motion_loader.future_steps, axis=0)  # [5, 3]
    robot_anchor_quat_w_exp = robot_quat.repeat(motion_loader.future_steps, axis=0)  # [5, 4]

    # Transform all future steps at once
    pos_b, _ = subtract_frame_transforms(
        torch.from_numpy(robot_anchor_pos_w_exp).float(),
        torch.from_numpy(robot_anchor_quat_w_exp).float(),
        torch.from_numpy(future_anchor_pos_w).float(),
        torch.from_numpy(future_anchor_quat_w).float(),
    )

    pos_b_flat = np.array(pos_b, dtype=np.float32).flatten()  # [15]
    return pos_b_flat  # [15]


def motion_anchor_ori_b_future(sim_data, motion_loader, t):