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