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)