def resample_trajectory(self, trajectory: ip.VelocityTrajectory, n: int, k: int = 3):
zeros = [id for id in np.argwhere(trajectory.velocity <= trajectory.velocity_stop)]
if zeros and len(zeros) < len(trajectory.velocity) - 1:
path = np.delete(trajectory.path, zeros, axis=0)
velocity = np.delete(trajectory.velocity, zeros)
heading = np.delete(trajectory.heading, zeros)
timesteps = np.delete(trajectory.timesteps, zeros)
else:
path = trajectory.path
velocity = trajectory.velocity
heading = trajectory.heading
timesteps = trajectory.timesteps
trajectory_nostop = ip.VelocityTrajectory(path, velocity, heading, timesteps)
u = trajectory_nostop.pathlength
u_new = linspace(0, u[-1], n)
k = min(k, len(velocity) - 1)
if k != 0:
tck, _ = splprep([path[:, 0], path[:, 1], velocity, heading], u=u, k=k, s=0)
tck[0] = self.fix_points(tck[0])
path_new = np.empty((n, 2), float)
path_new[:, 0], path_new[:, 1], velocity_new, heading_new = splev(u_new, tck)
trajectory_resampled = ip.VelocityTrajectory(path_new, velocity_new, heading_new)
else:
trajectory_resampled = trajectory
return trajectory_resampled
def set_trajectory(self, new_trajectory: ip.Trajectory):
""" Override current trajectory of the vehicle and resample to match execution frequency of the environment.
If the trajectory given is empty or None, then the vehicle will stay in place for 10 seconds. """
fps = self._vehicle.fps
if not new_trajectory:
self._trajectory = ip.VelocityTrajectory(
np.repeat([self._initial_state.position], 10 * fps, axis=0),
np.zeros(10 * fps),
np.repeat(self._initial_state.heading, 10 * fps),
np.arange(0.0, 10 * fps, 1 / fps))
elif isinstance(new_trajectory,
ip.StateTrajectory) and new_trajectory.fps == fps:
self._trajectory = ip.VelocityTrajectory(new_trajectory.path,
new_trajectory.velocity,
new_trajectory.heading,
new_trajectory.timesteps)
else:
num_frames = np.ceil(new_trajectory.duration * fps)
ts = new_trajectory.times
points = np.linspace(ts[0], ts[-1], int(num_frames))
xs_r = np.interp(points, ts, new_trajectory.path[:, 0])
ys_r = np.interp(points, ts, new_trajectory.path[:, 1])
v_r = np.interp(points, ts, new_trajectory.velocity)
path = np.c_[xs_r, ys_r]
self._trajectory = ip.VelocityTrajectory(path, v_r)
def trajectory_cost(self, trajectory: ip.Trajectory, goal: ip.Goal) -> float:
""" Calculate the total cost of the trajectory given a goal.
Args:
trajectory: The trajectory to examine
goal: The goal to reach
Returns:
A scalar floating-point cost value
"""
if isinstance(trajectory, ip.StateTrajectory):
trajectory = ip.VelocityTrajectory(trajectory.path, trajectory.velocity,
trajectory.heading, trajectory.timesteps)
goal_reached_i = self._goal_reached(trajectory, goal)
self._cost = (self.factors["time"] * abs(self._time_to_goal(trajectory, goal_reached_i)) +
self.factors["velocity"] * abs(self._velocity(trajectory, goal_reached_i)) +
self.factors["acceleration"] * abs(self._longitudinal_acceleration(trajectory, goal_reached_i)) +
self.factors["jerk"] * abs(self._longitudinal_jerk(trajectory, goal_reached_i)) +
self.factors["heading"] * abs(self._heading(trajectory, goal_reached_i)) +
self.factors["angular_velocity"] * abs(self._angular_velocity(trajectory, goal_reached_i)) +
self.factors["angular_acceleration"] * abs(
self._angular_acceleration(trajectory, goal_reached_i)) +
self.factors["curvature"] * abs(self._curvature(trajectory, goal_reached_i)))
return self._cost
def get_trajectory(self, frame: Dict[int, ip.AgentState],
scenario_map: ip.Map) -> ip.VelocityTrajectory:
state = frame[self.agent_id]
target_lane = self.lane_sequence[-1]
path = self._get_path(state, target_lane)
velocity = self.get_velocity(path, frame, [target_lane])
return ip.VelocityTrajectory(path, velocity)
def get_trajectory(self, frame: Dict[int, ip.AgentState],
scenario_map: ip.Map) -> ip.VelocityTrajectory:
state = frame[self.agent_id]
points = self._get_points(state, self.lane_sequence)
path = self._get_path(state, points, self.lane_sequence)
velocity = self.get_velocity(path, frame, self.lane_sequence)
return ip.VelocityTrajectory(path, velocity)
def cost_difference_resampled(self, trajectory1: ip.Trajectory, trajectory2: ip.Trajectory, goal: ip.Goal) -> float:
""" Calculate the sum of the cost elements differences between two trajectories given a goal,
at sampled points along the pathlength
Args:
trajectory1: The first trajectory to examine
trajectory2: The second trajectory to examine
goal: The goal to reach
Returns:
A scalar floating-point cost difference value
"""
trajectories_resampled = []
for trajectory in [trajectory1, trajectory2]:
goal_reached = self._goal_reached(trajectory, goal)
trajectory_resampled = ip.VelocityTrajectory(trajectory.path[:goal_reached],
trajectory.velocity[:goal_reached],
trajectory.heading[:goal_reached],
trajectory.timesteps[:goal_reached])
trajectories_resampled.append(trajectory_resampled)
n = min(len(trajectory.velocity) for trajectory in trajectories_resampled)
# handle edge case where goal is reached straight away
if n == 0:
self._cost = 0.
return self._cost
trajectories_resampled = [self.resample_trajectory(trajectory, n) for trajectory in trajectories_resampled]
# handle case when we reach the end of the trajectory
if len(trajectories_resampled[0].path) == 1 or len(trajectories_resampled[1].path) == 1:
self._cost = 0.
return self._cost
dcost_time_to_goal = self._d_time_to_goal(trajectories_resampled[0], trajectories_resampled[1])
dcost_velocity = self._d_velocity(trajectories_resampled[0], trajectories_resampled[1])
dcost_longitudinal_acceleration = self._d_longitudinal_acceleration(trajectories_resampled[0],
trajectories_resampled[1])
dcost_longitudinal_jerk = self._d_longitudinal_jerk(trajectories_resampled[0], trajectories_resampled[1])
dcost_heading = self._d_heading(trajectories_resampled[0], trajectories_resampled[1])
dcost_angular_velocity = self._d_angular_velocity(trajectories_resampled[0], trajectories_resampled[1])
dcost_angular_acceleration = self._d_angular_acceleration(trajectories_resampled[0], trajectories_resampled[1])
dcost_curvature = self._d_curvature(trajectories_resampled[0], trajectories_resampled[1])
self._cost = (self.factors["time"] * dcost_time_to_goal +
self.factors["velocity"] * dcost_velocity +
self.factors["acceleration"] * dcost_longitudinal_acceleration +
self.factors["jerk"] * dcost_longitudinal_jerk +
self.factors["heading"] * dcost_heading +
self.factors["angular_velocity"] * dcost_angular_velocity +
self.factors["angular_acceleration"] * dcost_angular_acceleration +
self.factors["curvature"] * dcost_curvature)
return self._cost
def get_trajectory(self, frame: Dict[int, ip.AgentState],
scenario_map: ip.Map) -> ip.VelocityTrajectory:
state = frame[self.agent_id]
points = self._get_points(state, self.lane_sequence)
path = self._get_path(state, points, self.lane_sequence)
velocity = self._get_const_deceleration_vel(state.speed, 2, path)
ego_time_to_junction = ip.VelocityTrajectory(path, velocity).duration
times_to_junction = self._get_times_to_junction(
frame, scenario_map, ego_time_to_junction)
time_until_clear = self._get_time_until_clear(ego_time_to_junction,
times_to_junction)
stop_time = time_until_clear - ego_time_to_junction
if stop_time > 0:
# insert waiting points
path = self._add_stop_points(path)
velocity = self._add_stop_velocities(path, velocity, stop_time)
return ip.VelocityTrajectory(path, velocity)
def get_trajectory(self) -> ip.VelocityTrajectory:
""" If open_loop is True then get the complete trajectory of the macro action.
Returns:
A VelocityTrajectory that describes the complete open loop trajectory of the macro action
"""
if self._maneuvers is None:
raise ValueError("Maneuver sequence of macro action was not initialised!")
points = None
velocity = None
for maneuver in self._maneuvers:
trajectory = maneuver.trajectory
points = trajectory.path if points is None else np.append(points, trajectory.path[1:], axis=0)
velocity = trajectory.velocity if velocity is None else np.append(velocity, trajectory.velocity[1:], axis=0)
return ip.VelocityTrajectory(points, velocity)