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
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