def state_to_observation(self, state):
"""
Prepare state to be read as input to neural network.
"""
_, y, yaw, x_dot, y_dot, yaw_dot = state
yaw = normalize_angle(yaw)
return np.array([y, yaw, x_dot, y_dot, yaw_dot])
def project_line(state, x0, y0, angle):
"""
Note that this policy is trained to follow a straight line
to the right (y = 0). To follow an arbitrary line, use this
function to transform the current absolute state to a form
that makes the policy believe the car is moving to the right.
:param state: Current absolute state of robot
:param x0: x-value of start of line to follow
:param y0: y-value of start of line to follow
:param angle: Angle of line to follow
"""
x, y, yaw, x_dot, y_dot, yaw_dot = state
angle = normalize_angle(angle)
current_angle = np.arctan2((y - y0), (x - x0))
projected_angle = normalize_angle(current_angle - angle)
dist = np.sqrt((x - x0)**2 + (y - y0)**2)
new_x = dist * np.cos(projected_angle)
new_y = dist * np.sin(projected_angle)
new_yaw = normalize_angle(yaw - angle)
return np.array([new_x, new_y, new_yaw, x_dot, y_dot, yaw_dot])
def state_to_observation(self, state):
"""
Convert state [x, y, yaw, x_dot, y_dot, yaw_dot] to
[dx, theta, ddx, dtheta]
"""
r = self.radius
x, y, yaw, x_dot, y_dot, yaw_dot = state
dx = np.sqrt(x**2 + y**2) - r
theta = normalize_angle(np.arctan2(-x, y) + np.pi - yaw)
ddx = x/(x**2 + y**2)**0.5*x_dot + y/(x**2 + y**2)**0.5*y_dot
dtheta = -y/(x**2 + y**2)*x_dot + x/(x**2 + y**2)*y_dot - yaw_dot
# May want to rescale/normalize values to each other.
return np.array([dx, theta, ddx, dtheta])
