def totalDist(self, pose, angleScale = 1):
        return self.distance(pose) + \
               abs(fixAnglePlusMinusPi(self.theta-pose.theta)) * angleScale
dataset/ETHPy150Open rll/lfd/lfd/rapprentice/util.py/Pose.totalDist/original