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