def act(self, state: State, snake_idx: int):
snake: Snake = state.snakes[snake_idx]
default_action, greedy = self.actor.act(state.observe(snake_idx))
possible_actions = [default_action]
possible_actions.extend([i for i in range(3) if i != default_action])
# Try all actions starting with the default action, chosen by the actor
for action in possible_actions:
next_head = snake._get_next_head(snake._get_direction(action))
collided = state._collided(snake, next_head)
if not collided:
return action
# Give up and return some random action
return action
def update(self, _x, _y):
"""
Updates the belief with one state-observation pair.
:param _x: state, as a Pose
:param _y: observation, as a real number
"""
# print _y, "_y"
self.need_recompute = True
if len(self.x) == 0:
self.x = np.array([State.to_array(_x)])
self.y = np.array([[_y]])
else:
self.x = np.append(self.x, np.array([State.to_array(_x)]), axis=0)
self.y = np.append(self.y, np.array([[_y]]), axis=0)
def get_samp_traj(self, steps, time_offset=0):
"""
Method used to retrieve a sampled trajectory.
:param steps: The number of steps for the sampling.
:param time_offset: when using time, world time at which the trajectory starts
:return: a matrix of positions corresponding to the given times.
"""
kmcf = KMCF(0.2, 15.0, 0.1, 0.00001)
# kmcf.set_anchor_pts(self.anchor_pts, self.times, 2 if self.restrict_angle else 1)
kmcf.set_anchor_pts(self.anchor_pts, self.times, 1, self.restrict_angle)
samp_times = np.atleast_2d(np.linspace(0.0, 1.0, steps + 1)).T
raw_coords = kmcf.test_mean(samp_times)
# Rescale points
coords = np.array([raw_coords[:, 0] * self.rescale_factor[0] + self.rescale_factor[2],
raw_coords[:, 1] * self.rescale_factor[1] + self.rescale_factor[3]]).T
# Add time
pos = np.append(coords, samp_times, axis=1)
# Compute the angle and angular velocity and convert them to states
dt = 1.0 / (steps + 1)
states = [None] * len(pos)
w_last = self.starting_state.w
states[0] = self.starting_state.clone()
for i in range(1, len(states)):
w = math.atan2(pos[i][1] - pos[i - 1][1], pos[i][0] - pos[i - 1][0])
w_vel = (w - w_last) / (2 * dt)
states[i] = State(pos[i, 0], pos[i, 1], t=pos[i, 2] + time_offset, w=w, w_vel=w_vel)
w_last = w
return states
def samp_traj_at(self, time, time_offset=0):
"""
Method used to sample the trajectory at a single location
:param time: time at which to sample. 0<=t<=1
:param time_offset: when using time, world time at which the trajectory starts
:return: a position corresponding to the given time
"""
kmcf = KMCF(0.2, 15.0)
# kmcf.set_anchor_pts(self.anchor_pts, self.times, 2 if self.restrict_angle else 1)
kmcf.set_anchor_pts(self.anchor_pts, self.times, 1, self.restrict_angle)
dt = 0.01
raw_pos = kmcf.test_mean(np.array([[0.0], [time - dt], [time], [time + dt]]))
# Rescale points
pos = np.array([raw_pos[:, 0] * self.rescale_factor[0] + self.rescale_factor[2],
raw_pos[:, 1] * self.rescale_factor[1] + self.rescale_factor[3]]).T
# Compute the angle and angular velocity
w1 = math.atan2(pos[2][1] - pos[1][1], pos[2][0] - pos[1][0])
w2 = math.atan2(pos[3][1] - pos[2][1], pos[3][0] - pos[2][0])
w_vel = (w2 - w1) / (2 * dt)
# Convert them to states
return State(pos[2][0], pos[2][1], w=w1, w_vel=w_vel, t=time + time_offset)
def pos_at(self, u):
"""
Get a position by sampling the spline at a specific time.
:param u: Time at which to sample.
:return: a state.
"""
_x = self.cx * u + self.dx
_y = self.ay * math.pow(u, 3) + self.by * math.pow(
u, 2) + self.cy * u + self.dy
der1 = (3.0 * self.ax * math.pow(u, 2) + 2.0 * self.bx * u + self.cx,
3.0 * self.ay * math.pow(u, 2) + 2.0 * self.by * u + self.cy)
der2 = (3.0 * self.ax * u + self.bx, 3.0 * self.ay * u + self.by)
return State(_x,
_y,
w=math.atan2(der1[1], der1[0]),
w_vel=math.atan2(der2[1], der2[0]))
def estimate(self, _x):
"""
Estimates the model value at a given pose, or multiple poses.
:param _x: Pose, or iterable of Poses, or array of positions, at which to estimate.
:return: Model estimation, as a real number
"""
if self.need_recompute:
self.___recompute___()
if hasattr(_x, '__iter__'):
if isinstance(_x[0], Pose):
mean, var = self.model.predict(np.array(map(State.to_array, _x)))
else:
mean, var = self.model.predict(np.array(_x))
else:
mean, var = self.model.predict(np.array([State.to_array(_x)]))
if math.isnan(var[0]):
var = np.zeros(var.shape)
return mean.reshape(-1), var.reshape(-1)
return State(pos[2][0], pos[2][1], w=w1, w_vel=w_vel, t=time + time_offset)
if __name__ == '__main__':
def jerk(x):
v = np.gradient(x, axis=0)
a = np.gradient(v, axis=0)
j = np.gradient(a, axis=0)
return np.sum(np.linalg.norm(j, 2, axis=1))
import matplotlib.pyplot as plt
plt.subplot(1, 1, 1)
nparams = 3
p = State(0, 0, w=0.0, t=0)
jerks = []
for _ in range(20):
params = np.random.uniform(-1, 1, (nparams, 1))
# p.w = np.random.uniform(-np.pi, np.pi)
ker_traj = KernelTraj(params, p, True)
traj = np.array(map(State.to_xy_array, ker_traj.get_samp_traj(100)))
plt.plot(traj[:, 0], traj[:, 1], 'b-')
anchors = ker_traj.anchor_pts
anchors = np.array([anchors[:, 0] * ker_traj.rescale_factor[0] + ker_traj.rescale_factor[2],
anchors[:, 1] * ker_traj.rescale_factor[1] + ker_traj.rescale_factor[3]]).T
jerks.append(jerk(traj))
# plt.plot(anchors[:, 0], anchors[:, 1], '*r')
# Translate
spl.dx = pos.x
spl.dy = pos.y
if __name__ == '__main__':
def jerk(x):
v = np.gradient(x, axis=0)
a = np.gradient(v, axis=0)
j = np.gradient(a, axis=0)
return np.sum(np.linalg.norm(j, 2, axis=1))
spl2d = DiscreteSplines2D()
pos = State(0, 0)
jerks = []
import matplotlib.pyplot as plt
import numpy as np
plt.figure()
spls = spl2d.get_splines(pos)
for spl in spls:
t = np.linspace(0, 1, 50)
states = [spl.pos_at(u) for u in t]
x = np.array([state.x for state in states])
y = np.array([state.y for state in states])
jerks.append(jerk(np.vstack((x, y)).T))
plt.plot(x, y, 'b')
# spls = spl2d.get_splines2(pos)