Function propagate_pose

src/core/transforms.py:125–140

Propagate one object's pose through another object's rigid motion. Returns: (new_pos2, new_quat2_wxyz)

(
    current_pos1: np.ndarray, current_quat1_wxyz: np.ndarray,
    prev_pos1: np.ndarray, prev_quat1_wxyz: np.ndarray,
    prev_pos2: np.ndarray, prev_quat2_wxyz: np.ndarray,
)

def propagate_pose(
    current_pos1: np.ndarray, current_quat1_wxyz: np.ndarray,
    prev_pos1: np.ndarray, prev_quat1_wxyz: np.ndarray,
    prev_pos2: np.ndarray, prev_quat2_wxyz: np.ndarray,
) -> Tuple[np.ndarray, np.ndarray]:
    """
    Propagate one object's pose through another object's rigid motion.

    Returns:
        (new_pos2, new_quat2_wxyz)
    """
    T_curr1 = pose_to_transform(current_pos1, current_quat1_wxyz)
    T_prev1 = pose_to_transform(prev_pos1, prev_quat1_wxyz)
    T_prev2 = pose_to_transform(prev_pos2, prev_quat2_wxyz)
    T_new2 = T_prev2 @ np.linalg.inv(T_prev1) @ T_curr1
    return transform_to_pose(T_new2)