import math


def calculate_rosbot_velocities(x_new, y_new, rpy_new, x_prev, y_prev, rpy_prev, dt):
    """
    Calculates the linear and angular velocity of the rosbot
    """
    # get yaw from [roll, pitch, yaw]
    
    yaw_new = rpy_new[2]   
    yaw_prev = rpy_prev[2] 
    
    d_yaw = yaw_new - yaw_prev
    d_yaw = (d_yaw + math.pi) % (2 * math.pi) - math.pi
    w = d_yaw / dt

    vx = (x_new - x_prev) / dt
    vy = (y_new - y_prev) / dt
    v = math.sqrt(vx**2 + vy**2)

    alpha = math.atan2(vy,vx)
    v = v * math.cos(alpha - yaw_new)
    return v, w