def generate_trajectory(self, leader_positions, t):
arr = get_interval(self.leader_positions, t, config.SAMPLE_COUNT)
self.traj_interval = arr
if len(arr) == 0:
return None
known, last_x, last_y, last_t = self.polyfit_trajectory(arr, t)
if config.DISABLE_CIRCLES:
return known
else:
return self.extend_trajectory(known, last_x, last_y, last_t)
def lerp_precise_states(time, precise_leader_states):
orig_arr = get_interval(precise_leader_states, time, 3)
if len(orig_arr) < 2:
raise LerpError, "too few states available at given time"
a = orig_arr[-2]
b = orig_arr[-1]
# lerp nearest states to get orig_leader's state at moment t
coeff = (time - a.time) / (b.time - a.time)
leader_pos = lerp(a.pos, b.pos, coeff)
leader_theta = lerp_angles(a.theta, b.theta, coeff)
return State(x=leader_pos.x, y=leader_pos.y,
theta=leader_theta)