def _in_pi(self, t):
    '\n        Reduce orientation from -pi..2pi to 0..pi\n        '
    if (t > np.pi):
        return (t - np.pi)
    if (t < 0):
        return (t + np.pi)
    return t
Reduce orientation from -pi..2pi to 0..pi
def _in_pi(self, t):
    '\n        \n        '
    if (t > np.pi):
        return (t - np.pi)
    if (t < 0):
        return (t + np.pi)
    return t