def _extrapolate_to_now(vs: VehicleState, staleness: float,
lin_acc_slope: float, ang_acc_slope: float):
"""Here we just linearly extrapolate the acceleration to "now" from the previous state for
each vehicle and then use standard kinematics to project the velocity and position from that."""
# The following ~10 lines are a hack b/c I'm too stupid to figure out
# how to do calculus on quaternions...
heading = vs.pose.heading
heading_delta_vec = staleness * (
vs.angular_velocity + 0.5 * vs.angular_acceleration * staleness +
ang_acc_slope * staleness**2 / 6.0)
heading += vec_to_radians(heading_delta_vec[:2]) + (0.5 * math.pi)
heading %= 2 * math.pi
vs.pose.orientation = fast_quaternion_from_angle(heading)
# XXX: also need to remove the cached heading_ since we've changed orientation
vs.pose.heading_ = None
# I assume the following should be updated based on changing
# heading from above, but I'll leave that for now...
vs.pose.position += staleness * (
vs.linear_velocity + 0.5 * vs.linear_acceleration * staleness +
lin_acc_slope * staleness**2 / 6.0)
vs.linear_velocity += staleness * (vs.linear_acceleration +
0.5 * lin_acc_slope * staleness)
vs.speed = np.linalg.norm(vs.linear_velocity)
vs.angular_velocity += staleness * (vs.angular_acceleration +
0.5 * ang_acc_slope * staleness)
vs.linear_acceleration += staleness * lin_acc_slope
vs.angular_acceleration += staleness * ang_acc_slope
def from_explicit_offset(cls, offset_from_centre, base_position, heading,
local_heading):
"""Convert from an explicit offset
Args:
offset_from_centre: The offset away from the centre of the object's bounds
heading: The heading of the pose
base_position: The base position without offset
local_heading: An additional orientation that re-faces the center offset
"""
assert isinstance(heading, Heading)
assert isinstance(base_position, np.ndarray)
orientation = fast_quaternion_from_angle(heading)
oprime = heading + local_heading
# Calculate rotation on xy-plane only, given that fast_quaternion_from_angle is also on xy-plane
vprime = np.array([
offset_from_centre[0] * np.cos(oprime) -
offset_from_centre[1] * np.sin(oprime),
offset_from_centre[0] * np.sin(oprime) +
offset_from_centre[1] * np.cos(oprime),
offset_from_centre[2],
])
position = base_position + vprime
return cls(position=position,
orientation=orientation,
heading_=heading)
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 from_center(cls, base_position, heading):
"""Convert from centred location
Args:
base_position: The center of the object's bounds
heading: The heading of the object
"""
assert isinstance(heading, Heading)
position = np.array([*base_position, 0][:3])
orientation = fast_quaternion_from_angle(heading)
return cls(position=position, orientation=orientation, heading_=heading,)
def reset_with(self, position, heading: Heading):
"""Resets the pose with the given position and heading values."""
if self.position.dtype is not np.dtype(np.float64):
# The slice assignment below doesn't change self.position's dtype,
# which can be a problem if it was initialized with ints and
# now we are assigning it floats, so we just cast it...
self.position = np.float64(self.position)
self.position[:] = position
if "point" in self.__dict__:
# clear the cached_property
del self.__dict__["point"]
if heading != self.heading_:
self.orientation = fast_quaternion_from_angle(heading)
self.heading_ = heading
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 _process_interp_for_lane_lp(
shape_lp: LinkedLanePoint,
first_linked_lanepoint: LinkedLanePoint,
next_shape_lp: LinkedLanePoint,
spacing: float,
newly_created_lanepoints: List[LinkedLanePoint],
) -> LinkedLanePoint:
rmlane = shape_lp.lp.lane
curr_lanepoint = first_linked_lanepoint
lane_seg_vec = next_shape_lp.lp.pose.position - shape_lp.lp.pose.position
lane_seg_len = np.linalg.norm(lane_seg_vec)
# We set the initial distance into the lane at `spacing` because
# we already have a lanepoint along this segment (curr_lanepoint)
dist_into_lane_seg = spacing
while dist_into_lane_seg < lane_seg_len:
p = dist_into_lane_seg / lane_seg_len
pos = shape_lp.lp.pose.position + lane_seg_vec * p
# The thresholds for calculating last lanepoint. If the
# midpoint between the current lanepoint and the next shape
# lanepoint is less than the minimum distance then the last
# lanepoint position will be that midpoint. If the midpoint
# is closer than last spacing threshold to the next shape
# lanepoint, then the last lanepoint will be the current
# lanepoint.
# XXX: the map scale should be taken into account here.
last_spacing_threshold_dist = 0.8 * spacing
minimum_dist_next_shape_lp = 1.4
half_distant_current_next_shape_lp = np.linalg.norm(
0.5 * (curr_lanepoint.lp.pose.position -
next_shape_lp.lp.pose.position))
mid_point_current_next_shape_lp = 0.5 * (
next_shape_lp.lp.pose.position +
curr_lanepoint.lp.pose.position)
if half_distant_current_next_shape_lp < minimum_dist_next_shape_lp:
pos = mid_point_current_next_shape_lp
dist_pos_next_shape_lp = np.linalg.norm(
next_shape_lp.lp.pose.position - pos)
if dist_pos_next_shape_lp < last_spacing_threshold_dist:
break
heading = vec_to_radians(lane_seg_vec)
orientation = fast_quaternion_from_angle(heading)
rmlane_coord = rmlane.to_lane_coord(
Point(x=pos[0], y=pos[1], z=0.0))
linked_lanepoint = LinkedLanePoint(
lp=LanePoint(
lane=rmlane,
pose=Pose(position=pos, orientation=orientation),
lane_width=rmlane.width_at_offset(rmlane_coord.s),
),
nexts=[],
is_inferred=True,
)
curr_lanepoint.nexts.append(linked_lanepoint)
curr_lanepoint = linked_lanepoint
newly_created_lanepoints.append(linked_lanepoint)
dist_into_lane_seg += spacing
return curr_lanepoint
def _shape_lanepoints_along_lane(
road_map: OpenDriveRoadNetwork,
lane: RoadMap.Lane,
lanepoint_by_lane_memo: dict,
) -> Tuple[LinkedLanePoint, List[LinkedLanePoint]]:
lane_queue = queue.Queue()
lane_queue.put((lane, None))
shape_lanepoints = []
initial_lanepoint = None
while not lane_queue.empty():
curr_lane, previous_lp = lane_queue.get()
first_lanepoint = lanepoint_by_lane_memo.get(curr_lane.lane_id)
if first_lanepoint:
if previous_lp:
previous_lp.nexts.append(first_lanepoint)
continue
lane_shape = [np.array(p) for p in curr_lane.centerline_points]
assert len(lane_shape) >= 2, repr(lane_shape)
heading = vec_to_radians(lane_shape[1] - lane_shape[0])
heading = Heading(heading)
orientation = fast_quaternion_from_angle(heading)
first_lane_coord = curr_lane.to_lane_coord(
Point(x=lane_shape[0][0], y=lane_shape[0][1], z=0.0))
first_lanepoint = LinkedLanePoint(
lp=LanePoint(
lane=curr_lane,
pose=Pose(position=lane_shape[0],
orientation=orientation),
lane_width=curr_lane.width_at_offset(
first_lane_coord.s),
),
nexts=[],
is_inferred=False,
)
if previous_lp is not None:
previous_lp.nexts.append(first_lanepoint)
if initial_lanepoint is None:
initial_lanepoint = first_lanepoint
lanepoint_by_lane_memo[curr_lane.lane_id] = first_lanepoint
shape_lanepoints.append(first_lanepoint)
curr_lanepoint = first_lanepoint
for p1, p2 in zip(lane_shape[1:], lane_shape[2:]):
heading_ = vec_to_radians(p2 - p1)
heading_ = Heading(heading_)
orientation_ = fast_quaternion_from_angle(heading_)
lp_lane_coord = curr_lane.to_lane_coord(
Point(x=p1[0], y=p1[1], z=0.0))
linked_lanepoint = LinkedLanePoint(
lp=LanePoint(
lane=curr_lane,
pose=Pose(position=p1, orientation=orientation_),
lane_width=curr_lane.width_at_offset(
lp_lane_coord.s),
),
nexts=[],
is_inferred=False,
)
shape_lanepoints.append(linked_lanepoint)
curr_lanepoint.nexts.append(linked_lanepoint)
curr_lanepoint = linked_lanepoint
# Add a lanepoint for the last point of the current lane
last_lane_coord = curr_lanepoint.lp.lane.to_lane_coord(
Point(x=lane_shape[-1][0], y=lane_shape[-1][1], z=0.0))
last_linked_lanepoint = LinkedLanePoint(
lp=LanePoint(
lane=curr_lanepoint.lp.lane,
pose=Pose(
position=lane_shape[-1],
orientation=curr_lanepoint.lp.pose.orientation,
),
lane_width=curr_lanepoint.lp.lane.width_at_offset(
last_lane_coord.s),
),
nexts=[],
is_inferred=False,
)
shape_lanepoints.append(last_linked_lanepoint)
curr_lanepoint.nexts.append(last_linked_lanepoint)
curr_lanepoint = last_linked_lanepoint
outgoing_roads_added = []
for out_lane in curr_lane.outgoing_lanes:
if out_lane.is_drivable:
lane_queue.put((out_lane, curr_lanepoint))
outgoing_road = out_lane.road
if out_lane.road not in outgoing_roads_added:
outgoing_roads_added.append(outgoing_road)
for outgoing_road in outgoing_roads_added:
for next_lane in outgoing_road.lanes:
if (next_lane.is_drivable
and next_lane not in curr_lane.outgoing_lanes):
lane_queue.put((next_lane, curr_lanepoint))
return initial_lanepoint, shape_lanepoints
def _shape_lanepoints_along_lane(
road_map: SumoRoadNetwork, lane: RoadMap.Lane,
lanepoint_by_lane_memo: dict
) -> Tuple[LinkedLanePoint, List[LinkedLanePoint]]:
lane_queue = queue.Queue()
lane_queue.put((lane, None))
shape_lanepoints = []
initial_lanepoint = None
while not lane_queue.empty():
lane, previous_lp = lane_queue.get()
first_lanepoint = lanepoint_by_lane_memo.get(lane.getID())
if first_lanepoint:
if previous_lp:
previous_lp.nexts.append(first_lanepoint)
continue
lane_shape = [np.array(p) for p in lane.getShape(False)]
assert len(lane_shape) >= 2, repr(lane_shape)
heading = vec_to_radians(lane_shape[1] - lane_shape[0])
heading = Heading(heading)
orientation = fast_quaternion_from_angle(heading)
first_lanepoint = LinkedLanePoint(
lp=LanePoint(
lane=road_map.lane_by_id(lane.getID()),
pose=Pose(position=lane_shape[0],
orientation=orientation),
lane_width=road_map.lane_by_id(
lane.getID()).width_at_offset(0),
),
nexts=[],
is_inferred=False,
)
if previous_lp is not None:
previous_lp.nexts.append(first_lanepoint)
if initial_lanepoint is None:
initial_lanepoint = first_lanepoint
lanepoint_by_lane_memo[lane.getID()] = first_lanepoint
shape_lanepoints.append(first_lanepoint)
curr_lanepoint = first_lanepoint
for p1, p2 in zip(lane_shape[1:], lane_shape[2:]):
heading_ = vec_to_radians(p2 - p1)
heading_ = Heading(heading_)
orientation_ = fast_quaternion_from_angle(heading_)
linked_lanepoint = LinkedLanePoint(
lp=LanePoint(
lane=road_map.lane_by_id(lane.getID()),
pose=Pose(position=p1, orientation=orientation_),
lane_width=road_map.lane_by_id(
lane.getID()).width_at_offset(0),
),
nexts=[],
is_inferred=False,
)
shape_lanepoints.append(linked_lanepoint)
curr_lanepoint.nexts.append(linked_lanepoint)
curr_lanepoint = linked_lanepoint
# Add a lanepoint for the last point of the current lane
last_linked_lanepoint = LinkedLanePoint(
lp=LanePoint(
lane=curr_lanepoint.lp.lane,
pose=Pose(
position=lane_shape[-1],
orientation=curr_lanepoint.lp.pose.orientation,
),
lane_width=curr_lanepoint.lp.lane.width_at_offset(0),
),
nexts=[],
is_inferred=False,
)
shape_lanepoints.append(last_linked_lanepoint)
curr_lanepoint.nexts.append(last_linked_lanepoint)
curr_lanepoint = last_linked_lanepoint
for out_connection in lane.getOutgoing():
out_lane = out_connection.getToLane()
# Use internal lanes of junctions (if we're at a junction)
via_lane_id = out_connection.getViaLaneID()
if via_lane_id:
out_lane = road_map._graph.getLane(via_lane_id)
lane_queue.put((out_lane, curr_lanepoint))
return initial_lanepoint, shape_lanepoints
