def rotation_error(self, goal): """ This method should be called when the robot doesn't need to translate itself because it's already in the (x, y) of the goal position. If the robot needs to translate to a different (x, y) position, use translation_error. :param goal: The position including the orientation the robot wants. The current position (x, y) and the goal (x, y) must be very near. :return: Angular error to reach the goal orientation. """ angerr1 = normalize(goal.theta - self.theta) angerr2 = normalize(goal.theta - (self.theta-180)) if abs(angerr1) < abs(angerr2): return angerr1 else: return angerr2
def __init__(self, x=0, y=0, theta=0): """ :param theta: The orientation of the robot. In degrees. Angle zero points to the positive axis X and it increments counter clockwise. :return: None. """ super(RobotPosition, self).__init__(x, y) self.theta = normalize(theta)
def rotation_error(self, goal): """ This method should be called when the robot doesn't need to translate itself because it's already in the (x, y) of the goal position. If the robot needs to translate to a different (x, y) position, use translation_error. :param goal: The position including the orientation the robot wants. The current position (x, y) and the goal (x, y) must be very near. :return: Angular error to reach the goal orientation. """ angerr1 = normalize(goal.theta - self.theta) angerr2 = normalize(goal.theta - (self.theta - 180)) if abs(angerr1) < abs(angerr2): return angerr1 else: return angerr2
def translation_error(self, goal): """ Calculate linear and angular distance errors to reach the goal position. :type goal: Position :param goal: The position where the robot wants to go. :return: Linear and angular errors to reach the goal position. """ linerr = self.distance_to(goal) angerr1 = normalize(self.angle_to(goal) - self.theta) # angerr2 = normalize(self.angle_to(goal) - (self.theta-180)) return linerr, angerr1