Trying to transform vectors between local and global frames

Question

Here is a function to transform a vector from robot's local frame to global or from global to local (depending on bGlobalToLocal parameter). Here arrRobotOrientation is always in global frame.

def transform(self, arrVector, arrRobotOrientation, bGlobalToLocal):
    roll, pitch, yaw = arrRobotOrientation

    # Define rotation matrices for roll, pitch, and yaw
    R_x = np.array([
        [1, 0, 0],
        [0, np.cos(roll), -np.sin(roll)],
        [0, np.sin(roll), np.cos(roll)]
    ])

    R_y = np.array([
        [np.cos(pitch), 0, np.sin(pitch)],
        [0, 1, 0],
        [-np.sin(pitch), 0, np.cos(pitch)]
    ])

    # R_y = np.array([
    #     [np.cos(pitch), 0, -np.sin(pitch)],
    #     [0, 1, 0],
    #     [np.sin(pitch), 0, np.cos(pitch)]
    # ])        

    R_z = np.array([
        [np.cos(yaw), -np.sin(yaw), 0],
        [np.sin(yaw), np.cos(yaw), 0],
        [0, 0, 1]
    ])

    # Combine rotation matrices in Yaw -> Pitch -> Roll order
    R = R_z @ R_y @ R_x

    if bGlobalToLocal:
        # Invert the transformation by transposing the rotation matrix
        R = R.T

    # Transform the vector
    arrTransformed = R @ arrVector
    return arrTransformed

I have created test cases similar to this one (and it fails most of them):

    arrVector = np.array([1, 0, 0])
    arrRobotOrientation = np.array([0, 0, 0])  # No rotation
    bGlobalToLocal = True
    result = self.transform(arrVector, arrRobotOrientation, bGlobalToLocal)
    print("Expected: [1, 0, 0]:", result)

Please help.

Answer 1

I am very confused that no one answered this question. Anyway:

# ---

def get_rotation_matrix(rpy):
    roll, pitch, yaw = rpy
    
    # Create a rotation matrix from roll, pitch, yaw using tf_transformations
    return tf_transformations.euler_matrix(roll, pitch, yaw)[:3, :3]

# ---

def global_to_local(global_vec, rpy):
    # Get the rotation matrix for the robot's orientation (RPY)
    rotation_matrix = get_rotation_matrix(rpy)

    # Apply inverse (transpose) of the rotation matrix to the angular velocity
    local_vec = np.dot(rotation_matrix.T, global_vec)

    return local_vec

# ---

def local_to_global(local_vec, rpy):
    # Get the rotation matrix for the robot's orientation (RPY)
    rotation_matrix = get_rotation_matrix(rpy)

    # Apply the rotation matrix to the angular velocity
    global_vec = np.dot(rotation_matrix, local_vec)

    return global_vec

# ---    

def transform(arrVector, arrRobotOrientation, bGlobalToLocal):
    if bGlobalToLocal:
        return global_to_local(arrVector, arrRobotOrientation)
    else:    
        return local_to_global(arrVector, arrRobotOrientation)