def test_normalize_angle(self):
self.assertTrue(
math.isclose(utils.normalize_angle(radians(360 + 1)), radians(1)))
self.assertTrue(
math.isclose(utils.normalize_angle(radians(-360 - 50)),
radians(-50)))
self.assertTrue(
math.isclose(utils.normalize_angle(radians(-360 - 190)),
radians(170)))
self.assertTrue(
math.isclose(utils.normalize_angle(radians(-360 * 5 - 190)),
radians(170)))
def get_ideal(self, current, t):
"""
Parameters:
----------
current: np.array(x, y, theta)
current pose (is not used in this agent)
t: float
elapsed time
Returns:
----------
np.array(x, y, theta)
ideal pose of t
"""
d0 = 0.0
d1 = d0 + 1.0 / INPUT_V
d2 = d1 + np.pi / 2.0 / INPUT_OMEGA
d3 = d2 + 2.0 / INPUT_V
d4 = d3 + np.pi / 2.0 / INPUT_OMEGA
d5 = d4 + 2.0 / INPUT_V
d6 = d5 + np.pi / 2.0 / INPUT_OMEGA
d7 = d6 + 2.0 / INPUT_V
d8 = d7 + np.pi / 2.0 / INPUT_OMEGA
d9 = d8 + 1.0 / INPUT_V
delta = t % d9
if d0 <= delta < d1:
ideal = np.array((1.0, 0 + INPUT_V * delta, np.pi / 2.0))
elif d1 <= delta < d2:
ideal = np.array(
(1.0, 1.0, np.pi / 2.0 + INPUT_OMEGA * (delta - d1)))
elif d2 <= delta < d3:
ideal = np.array((1.0 - INPUT_V * (delta - d2), 1.0, np.pi))
elif d3 <= delta < d4:
ideal = np.array((-1.0, 1.0, np.pi + INPUT_OMEGA * (delta - d3)))
elif d4 <= delta < d5:
ideal = np.array(
(-1.0, 1.0 - INPUT_V * (delta - d4), np.pi * 3.0 / 2.0))
elif d5 <= delta < d6:
ideal = np.array(
(-1.0, -1.0, np.pi * 3.0 / 2.0 + INPUT_OMEGA * (delta - d5)))
elif d6 <= delta < d7:
ideal = np.array((-1.0 + INPUT_V * (delta - d6), -1.0, 0.0))
elif d7 <= delta < d8:
ideal = np.array((1.0, -1.0, INPUT_OMEGA * (delta - d7)))
elif d8 <= delta < d9:
ideal = np.array((1.0, -1.0 + INPUT_V * (delta - d8), np.pi / 2.0))
else:
raise NotImplementedError
ideal[2] = utils.normalize_angle(ideal[2])
return ideal
def _eval_theta(cls, next, destination):
"""
Parameters:
----------
next: np.array(x, y, theta)
candidate pose calculated from the Robot's motion model
destination: np.array(x, y, theta)
destination pose
Returns:
----------
float
the difference angle between the destination's theta and candidate pose's theta
(smaller is better)
"""
return np.abs(utils.normalize_angle(next[2] - destination[2]))
def _eval_heading(cls, next, destination):
"""
Parameters:
----------
next: np.array(x, y, theta)
candidate pose calculated from the Robot's motion model
destination: np.array(x, y, theta)
destination pose
Returns:
----------
float
the difference angle between the direction to destination and candidate pose's theta
(smaller is better)
"""
angle = np.arctan2(destination[1] - next[1], destination[0] - next[0])
return np.abs(utils.normalize_angle(angle - next[2]))
def get_ideal(self, current, t):
"""
Parameters:
----------
current: np.array(x, y, theta)
current pose (is not used in this agent)
t: float
elapsed time
Returns:
----------
np.array(x, y, theta)
ideal pose of t
"""
ideal = np.array((0, 0, np.pi / 2.0)) + np.array((np.cos(
INPUT_OMEGA * t), np.sin(INPUT_OMEGA * t), INPUT_OMEGA * t))
ideal[2] = utils.normalize_angle(ideal[2])
return ideal
def get_ideal(self, current, t):
"""
Parameters:
----------
current: np.array(x, y, theta)
current pose
t: float
elapsed time (is not used in this agent)
Returns:
----------
np.array(x, y, theta)
ideal pose of t
"""
if np.linalg.norm(self.target[:2] - current[:2]) < DISTANCE_THRESHOLD and \
np.abs(utils.normalize_angle(self.target[2] - current[2])) < ANGLE_THRESHOLD:
self.target = next(self.itr)
return self.target
def move(cls, current, input, delta):
"""
Parameters:
----------
current: np.array(x, y, theta)
current pose
input: np.array(v, omega)
input vector
delta: float
time delta
Returns:
----------
np.array(x, y, theta)
predict pose of next tick
"""
theta = current[2]
omega = input[1]
next = current + Robot.T(theta, omega, delta).dot(input.T)
next[2] = utils.normalize_angle(next[2])
return next