def velocity_vectors(self):
# linear_velocity in m/s, angular_velocity in rad/s
vh = radians_to_vec(self.pose.heading)
if self._speed is not None:
linear_velocity = np.array((vh[0], vh[1], 0.0)) * self._speed
else:
linear_velocity = None
angular_velocity = np.array((0.0, 0.0, 0.0))
if self._last_dt > 0:
angular_velocity = vh - radians_to_vec(self._last_heading)
angular_velocity = np.append(angular_velocity / self._last_dt, 0.0)
return (linear_velocity, angular_velocity)
def velocity_vectors(self):
# linear_velocity in m/s, angular_velocity in rad/s
vh = radians_to_vec(self.pose.heading)
if self._speed is not None:
linear_velocity = np.array((vh[0], vh[1], 0.0)) * self._speed
else:
linear_velocity = None
if self._last_dt and self._last_dt > 0:
av = (vh - radians_to_vec(self._last_heading)) / self._last_dt
angular_velocity = np.array((av[0], av[1], 0.0))
else:
angular_velocity = np.array((0.0, 0.0, 0.0))
return (linear_velocity, angular_velocity)
def discover_missions_of_traffic_histories(self) -> Dict[str, Mission]:
vehicle_missions = {}
map_offset = self._road_network.net_offset
for row in self._traffic_history.first_seen_times():
start_time = float(row[1])
pphs = self._traffic_history.vehicle_pose_at_time(row[0], start_time)
assert pphs
pos_x, pos_y, heading, speed = pphs
entry_tactic = default_entry_tactic(speed)
v_id = str(row[0])
veh_type = self._traffic_history.vehicle_type(v_id)
veh_length, veh_width, veh_height = self._traffic_history.vehicle_size(v_id)
# missions start from front bumper, but pos is center of vehicle
hhx, hhy = radians_to_vec(heading) * (0.5 * veh_length)
vehicle_missions[v_id] = Mission(
start=Start(
(pos_x + map_offset[0] + hhx, pos_y + map_offset[1] + hhy),
Heading(heading),
),
entry_tactic=entry_tactic,
goal=TraverseGoal(self.road_network),
start_time=start_time,
vehicle_spec=VehicleSpec(
veh_id=v_id,
veh_type=veh_type,
dimensions=BoundingBox(veh_length, veh_width, veh_height),
),
)
return vehicle_missions
def perform_action(cls, dt, vehicle, action):
chassis = vehicle.chassis
if isinstance(action, (int, float)):
# special case: setting the initial speed
if isinstance(chassis, BoxChassis):
vehicle.control(vehicle.pose, action, dt)
elif isinstance(chassis, AckermannChassis):
chassis.speed = action # hack that calls control internally
return
assert isinstance(action, (list, tuple)) and len(action) == 2
# acceleration is scalar in m/s^2, angular_velocity is scalar in rad/s
# acceleration is in the direction of the heading only.
acceleration, angular_velocity = action
# Note: we only use angular_velocity (not angular_acceleration) to determine
# the new heading and position in this action space. This sacrifices
# some realism/control, but helps simplify the imitation learning model.
target_heading = (vehicle.heading + angular_velocity * dt) % (2 *
math.pi)
if isinstance(chassis, BoxChassis):
# Since BoxChassis does not use pybullet for force-to-motion computations (only collision detection),
# we have to update the position and other state here (instead of pybullet.stepSimulation()).
heading_vec = radians_to_vec(vehicle.heading)
dpos = heading_vec * vehicle.speed * dt
new_pose = Pose(
position=vehicle.position + np.append(dpos, 0.0),
orientation=fast_quaternion_from_angle(target_heading),
)
target_speed = vehicle.speed + acceleration * dt
vehicle.control(new_pose, target_speed, dt)
elif isinstance(chassis, AckermannChassis):
mass = chassis.mass_and_inertia[0] # in kg
wheel_radius = chassis.wheel_radius
# XXX: should also take wheel inertia into account here
# XXX: ... or else we should apply this force directly to the main link point of the chassis.
if acceleration >= 0:
# necessary torque is N*m = kg*m*acceleration
torque_ratio = mass / (4 * wheel_radius * chassis.max_torque)
throttle = np.clip(acceleration * torque_ratio, 0, 1)
brake = 0
else:
throttle = 0
# necessary torque is N*m = kg*m*acceleration
torque_ratio = mass / (4 * wheel_radius * chassis.max_btorque)
brake = np.clip(acceleration * torque_ratio, 0, 1)
steering = np.clip(dt * -angular_velocity * chassis.steering_ratio,
-1, 1)
vehicle.control(throttle=throttle, brake=brake, steering=steering)
else:
raise Exception("unsupported chassis type")
def _get_vehicle_goal(self, vehicle_id: str) -> Point:
final_exit_time = self._traffic_history.vehicle_final_exit_time(vehicle_id)
final_pose = self._traffic_history.vehicle_pose_at_time(
vehicle_id, final_exit_time
)
assert final_pose
final_pos_x, final_pos_y, final_heading, _ = final_pose
# missions start from front bumper, but pos is center of vehicle
veh_dims = self._traffic_history.vehicle_dims(vehicle_id)
final_hhx, final_hhy = radians_to_vec(final_heading) * (0.5 * veh_dims.length)
return Point(final_pos_x + final_hhx, final_pos_y + final_hhy)
def as_sumo(self, length, local_heading):
"""Convert to SUMO (position of front bumper, cw_heading)
args:
heading:
The heading of the pose
length:
The length dimension of the object's physical bounds
local_heading:
An additional orientation that re-faces the length offset
"""
vprime = radians_to_vec(self.heading + local_heading) * 0.5 * length
return (
np.array([self.position[0] + vprime[0], self.position[1] + vprime[1], 0]),
self.heading.as_sumo,
)
def calulate_heading_lateral_error(
vehicle, trajectory, initial_look_ahead_distant, speed_reduction_activation
):
heading_error = min_angles_difference_signed(
(vehicle.heading % (2 * math.pi)), trajectory[2][0]
)
# Number of look ahead points to calculate the
# look ahead error.
# TODO: Find the number of points using the
# distance between trajectory points.
look_ahead_points = initial_look_ahead_distant
# If we are on the curvy portion of the road
# we need to decrease the values for look ahead calculation
# The value 30 for road curviness is obtained empirically
# for bullet model.
if (
abs(TrajectoryTrackingController.curvature_calculation(trajectory, 4)) < 30
and speed_reduction_activation
):
initial_look_ahead_distant = 1
look_ahead_points = 1
path_vector = radians_to_vec(
trajectory[2][min([look_ahead_points, len(trajectory[2]) - 1])]
)
vehicle_look_ahead_pt = [
vehicle.position[0]
- initial_look_ahead_distant * math.sin(vehicle.heading),
vehicle.position[1]
+ initial_look_ahead_distant * math.cos(vehicle.heading),
]
lateral_error = signed_dist_to_line(
vehicle_look_ahead_pt,
[
trajectory[0][min([look_ahead_points, len(trajectory[0]) - 1])],
trajectory[1][min([look_ahead_points, len(trajectory[1]) - 1])],
],
path_vector,
)
return (heading_error, lateral_error)
def _record_data(t: float, obs: Observation,
collected_data: Dict[str, Dict[float, Dict[str, Any]]]):
# just a hypothetical example of how we might collect some observations to save...
for car, car_obs in obs.items():
car_state = car_obs.ego_vehicle_state
collected_data.setdefault(car, {}).setdefault(t, {})
collected_data[car][t]["ego_pos"] = car_state.position
collected_data[car][t]["heading"] = car_state.heading
collected_data[car][t]["speed"] = car_state.speed
collected_data[car][t]["angular_velocity"] = car_state.yaw_rate
# note: acceleration is a 3-vector. convert it here to a scalar
# keeping only the acceleration in the direction of travel (the heading).
# we will miss angular acceleration effects, but hopefully angular velocity
# will be enough to "keep things real". This is simpler than using
# the angular acceleration vector because there are less degrees of
# freedom in the resulting model.
heading_vector = radians_to_vec(car_state.heading)
acc_scalar = car_state.linear_acceleration[:2].dot(heading_vector)
collected_data[car][t]["acceleration"] = acc_scalar
def get_vehicle_start_at_time(self, vehicle_id: str,
start_time: float) -> Tuple[Start, float]:
"""Returns a Start object that can be used to create a Mission for
a vehicle from a traffic history dataset starting at its location
at start_time. Also returns its speed at that time."""
pphs = self._traffic_history.vehicle_pose_at_time(
vehicle_id, start_time)
assert pphs
pos_x, pos_y, heading, speed = pphs
# missions start from front bumper, but pos is center of vehicle
veh_dims = self._traffic_history.vehicle_dims(vehicle_id)
hhx, hhy = radians_to_vec(heading) * (0.5 * veh_dims.length)
return (
Start(
(pos_x + hhx, pos_y + hhy),
Heading(heading),
),
speed,
)
def from_front_bumper(cls, front_bumper_position, heading, length):
"""Convert from front bumper location
Args:
front_bumper_position: The (x, y) position of the centre front of the front bumper
heading: The heading of the pose
length: The length dimension of the object's physical bounds
"""
assert isinstance(front_bumper_position, np.ndarray)
assert front_bumper_position.shape == (2,), f"{front_bumper_position.shape}"
_orientation = fast_quaternion_from_angle(heading)
lwh_offset = radians_to_vec(heading) * (0.5 * length)
pos_2d = front_bumper_position - lwh_offset
return cls(
position=np.array([pos_2d[0], pos_2d[1], 0]),
orientation=_orientation,
heading_=heading,
)
def _initialize_speed(self, speed: float):
self.speed = speed
velocity = radians_to_vec(self.pose.heading) * speed
self._client.resetBaseVelocity(self._bullet_id, [*velocity, 0])
def _equally_spaced_path(self, path, point):
continuous_variables = [
"ref_wp_positions_x",
"ref_wp_positions_y",
"ref_wp_headings",
"ref_wp_lane_width",
"ref_wp_speed_limit",
]
discrete_variables = ["ref_wp_lane_id", "ref_wp_lane_index"]
ref_waypoints_coordinates = {
parameter: []
for parameter in (continuous_variables + discrete_variables)
}
for idx, waypoint in enumerate(path):
if not waypoint.is_shape_wp and 0 < idx < len(path) - 1:
continue
ref_waypoints_coordinates["ref_wp_positions_x"].append(
waypoint.wp.pos[0])
ref_waypoints_coordinates["ref_wp_positions_y"].append(
waypoint.wp.pos[1])
ref_waypoints_coordinates["ref_wp_headings"].append(
waypoint.wp.heading.as_bullet)
ref_waypoints_coordinates["ref_wp_lane_id"].append(
waypoint.wp.lane_id)
ref_waypoints_coordinates["ref_wp_lane_index"].append(
waypoint.wp.lane_index)
ref_waypoints_coordinates["ref_wp_lane_width"].append(
waypoint.wp.lane_width)
ref_waypoints_coordinates["ref_wp_speed_limit"].append(
waypoint.wp.speed_limit)
ref_waypoints_coordinates["ref_wp_headings"] = inplace_unwrap(
ref_waypoints_coordinates["ref_wp_headings"])
first_wp_heading = ref_waypoints_coordinates["ref_wp_headings"][0]
wp_position = np.array([*path[0].wp.pos, 0])
vehicle_pos = np.array([point[0], point[1], 0])
heading_vector = np.array([
*radians_to_vec(first_wp_heading),
0,
])
projected_distant_wp_vehicle = np.inner((vehicle_pos - wp_position),
heading_vector)
ref_waypoints_coordinates["ref_wp_positions_x"][0] = (
wp_position[0] + projected_distant_wp_vehicle * heading_vector[0])
ref_waypoints_coordinates["ref_wp_positions_y"][0] = (
wp_position[1] + projected_distant_wp_vehicle * heading_vector[1])
# To ensure that the distance between waypoints are equal, we used
# interpolation approach inspired by:
# https://stackoverflow.com/a/51515357
cumulative_path_dist = np.cumsum(
np.sqrt(
np.ediff1d(ref_waypoints_coordinates["ref_wp_positions_x"],
to_begin=0)**2 +
np.ediff1d(ref_waypoints_coordinates["ref_wp_positions_y"],
to_begin=0)**2))
if len(cumulative_path_dist) <= 1:
return [path[0].wp]
evenly_spaced_cumulative_path_dist = np.linspace(
0, cumulative_path_dist[-1], len(path))
evenly_spaced_coordinates = {}
for variable in continuous_variables:
evenly_spaced_coordinates[variable] = np.interp(
evenly_spaced_cumulative_path_dist,
cumulative_path_dist,
ref_waypoints_coordinates[variable],
)
for variable in discrete_variables:
ref_coordinates = ref_waypoints_coordinates[variable]
evenly_spaced_coordinates[variable] = []
jdx = 0
for idx in range(len(path)):
while (jdx + 1 < len(cumulative_path_dist)
and evenly_spaced_cumulative_path_dist[idx] >
cumulative_path_dist[jdx + 1]):
jdx += 1
evenly_spaced_coordinates[variable].append(
ref_coordinates[jdx])
evenly_spaced_coordinates[variable].append(ref_coordinates[-1])
equally_spaced_path = []
for idx, waypoint in enumerate(path):
equally_spaced_path.append(
Waypoint(
pos=np.array([
evenly_spaced_coordinates["ref_wp_positions_x"][idx],
evenly_spaced_coordinates["ref_wp_positions_y"][idx],
]),
heading=Heading(
evenly_spaced_coordinates["ref_wp_headings"][idx]),
lane_width=evenly_spaced_coordinates["ref_wp_lane_width"]
[idx],
speed_limit=evenly_spaced_coordinates["ref_wp_speed_limit"]
[idx],
lane_id=evenly_spaced_coordinates["ref_wp_lane_id"][idx],
lane_index=evenly_spaced_coordinates["ref_wp_lane_index"]
[idx],
))
return equally_spaced_path
def direction_vector(self):
return radians_to_vec(self)
def direction_vector(self):
"""Convert to a 2D directional vector that aligns with Cartesian Coordinate System"""
return radians_to_vec(self)
