def filter_pairs_by_angular_speed(poses, timestamps, speed, tol, degrees=False): """ filters pairs in a list of SE(3) poses by the angular speed of the motion in between them :param poses: list of SE(3) poses :param timestamps: list of timestamps corresponding to the poses :param speed: in rad/s :param tol: tolerance to accept or reject velocities, in rad/s :param degrees: set to True to use deg/s :return: list of index tuples of the filtered pairs """ speeds = [trajectory.calc_angular_speed(poses[i], poses[i + 1], timestamps[i], timestamps[i + 1], degrees) for i in range(len(poses) - 1)] id_pairs = [(i, i+1) for i, v in enumerate(speeds) if speed - tol <= v <= speed + tol] return id_pairs