From f495ddc5bf1c63e7ac9face5eb37465391bbeec9 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Tue, 24 Dec 2024 19:16:49 -0800 Subject: [PATCH] finished stanley controller --- .../buggy/controller/stanley_controller.py | 150 +++++++ rb_ws/src/buggy/buggy/util/pose.py | 219 +++++++++++ rb_ws/src/buggy/buggy/util/trajectory.py | 365 ++++++++++++++++++ 3 files changed, 734 insertions(+) create mode 100644 rb_ws/src/buggy/buggy/util/pose.py create mode 100644 rb_ws/src/buggy/buggy/util/trajectory.py diff --git a/rb_ws/src/buggy/buggy/controller/stanley_controller.py b/rb_ws/src/buggy/buggy/controller/stanley_controller.py index e69de29b..d8be46b6 100644 --- a/rb_ws/src/buggy/buggy/controller/stanley_controller.py +++ b/rb_ws/src/buggy/buggy/controller/stanley_controller.py @@ -0,0 +1,150 @@ +import sys +import numpy as np + +from sensor_msgs.msg import NavSatFix +from geometry_msgs.msg import Pose as ROSPose +from nav_msgs.msg import Odometry + +sys.path.append("/rb_ws/src/buggy/buggy") +from rb_ws.src.buggy.buggy.util.trajectory import Trajectory +from controller.controller_superclass import Controller +from util.pose import Pose + +import utm + + +class StanleyController(Controller): + """ + Stanley Controller (front axle used as reference point) + Referenced from this paper: https://ai.stanford.edu/~gabeh/papers/hoffmann_stanley_control07.pdf + """ + + LOOK_AHEAD_DIST_CONST = 0.05 # s + MIN_LOOK_AHEAD_DIST = 0.1 #m + MAX_LOOK_AHEAD_DIST = 2.0 #m + + CROSS_TRACK_GAIN = 1.3 + K_SOFT = 1.0 # m/s + K_D_YAW = 0.012 # rad / (rad/s) + + def __init__(self, start_index, namespace, node): + super(StanleyController, self).__init__(start_index, namespace, node) + self.debug_reference_pos_publisher = self.node.create_publisher( + "/auton/debug/reference_navsat", NavSatFix, queue_size=1 + ) + self.debug_error_publisher = self.node.create_publisher( + "/auton/debug/error", ROSPose, queue_size=1 + ) + + def compute_control(self, state_msg : Odometry, trajectory : Trajectory): + """Computes the steering angle determined by Stanley controller. + Does this by looking at the crosstrack error + heading error + + Args: + state_msg: ros Odometry message + trajectory (Trajectory): reference trajectory + + Returns: + float (desired steering angle) + """ + if self.current_traj_index >= trajectory.get_num_points() - 1: + self.node.get_logger.error("[Stanley]: Ran out of path to follow!") + raise Exception("[Stanley]: Ran out of path to follow!") + + current_rospose = state_msg.pose.pose + current_speed = np.sqrt( + state_msg.twist.twist.linear.x**2 + state_msg.twist.twist.linear.y**2 + ) + yaw_rate = state_msg.twist.twist.angular.z + heading = current_rospose.orientation.z + x, y = current_rospose.position.x, current_rospose.position.y #(Easting, Northing) + + front_x = x + StanleyController.WHEELBASE * np.cos(heading) + front_y = y + StanleyController.WHEELBASE * np.sin(heading) + + # setting range of indices to search so we don't have to search the entire path + traj_index = trajectory.get_closest_index_on_path( + front_x, + front_y, + start_index=self.current_traj_index - 20, + end_index=self.current_traj_index + 50, + ) + self.current_traj_index = max(traj_index, self.current_traj_index) + + + lookahead_dist = np.clip( + self.LOOK_AHEAD_DIST_CONST * current_speed, + self.MIN_LOOK_AHEAD_DIST, + self.MAX_LOOK_AHEAD_DIST) + + traj_dist = trajectory.get_distance_from_index(self.current_traj_index) + lookahead_dist + + ref_heading = trajectory.get_heading_by_index( + trajectory.get_index_from_distance(traj_dist)) + + error_heading = ref_heading - heading + error_heading = np.arctan2(np.sin(error_heading), np.cos(error_heading)) #Bounds error_heading + + # Calculate cross track error by finding the distance from the buggy to the tangent line of + # the reference trajectory + closest_position = trajectory.get_position_by_index(self.current_traj_index) + next_position = trajectory.get_position_by_index( + self.current_traj_index + 0.0001 + ) + x1 = closest_position[0] + y1 = closest_position[1] + x2 = next_position[0] + y2 = next_position[1] + error_dist = -((x - x1) * (y2 - y1) - (y - y1) * (x2 - x1)) / np.sqrt( + (y2 - y1) ** 2 + (x2 - x1) ** 2 + ) + + + cross_track_component = -np.arctan2( + StanleyController.CROSS_TRACK_GAIN * error_dist, current_speed + StanleyController.K_SOFT + ) + + # Calculate yaw rate error + r_meas = yaw_rate + r_traj = current_speed * (trajectory.get_heading_by_index(trajectory.get_index_from_distance(traj_dist) + 0.05) + - trajectory.get_heading_by_index(trajectory.get_index_from_distance(traj_dist))) / 0.05 + + #Determine steering_command + steering_cmd = error_heading + cross_track_component + StanleyController.K_D_YAW * (r_traj - r_meas) + steering_cmd = np.clip(steering_cmd, -np.pi / 9, np.pi / 9) + + + #Calculate error, where x is in orientation of buggy, y is cross track error + current_pose = Pose(current_rospose.pose.x, current_rospose.pose.y, heading) + reference_error = current_pose.convert_point_from_global_to_local_frame(closest_position) + reference_error -= np.array([StanleyController.WHEELBASE, 0]) #Translae back to back wheel to get accurate error + + error_pose = ROSPose() + error_pose.position.x = reference_error[0] + error_pose.position.y = reference_error[1] + self.debug_error_publisher.publish(error_pose) + + # Publish reference position for debugging + try: + reference_navsat = NavSatFix() + lat, lon = utm.to_latlon(closest_position[0], closest_position[1], 17, "T") + reference_navsat.latitude = lat + reference_navsat.longitude = lon + self.debug_reference_pos_publisher.publish(reference_navsat) + except Exception as e: + self.node.get_logger().warn( + "[Stanley] Unable to convert closest track position lat lon; Error: " + str(e) + ) + + return steering_cmd + + + + + + + + + + + diff --git a/rb_ws/src/buggy/buggy/util/pose.py b/rb_ws/src/buggy/buggy/util/pose.py new file mode 100644 index 00000000..985755a2 --- /dev/null +++ b/rb_ws/src/buggy/buggy/util/pose.py @@ -0,0 +1,219 @@ +import numpy as np + +from geometry_msgs.msg import Pose as ROSPose +from tf.transformations import euler_from_quaternion +from tf.transformations import quaternion_from_euler + + +class Pose: + """ + A data structure for storing 2D poses, as well as a set of + convenience methods for transforming/manipulating poses + + TODO: All Pose objects are with respect to World coordinates. + """ + + __x = None + __y = None + __theta = None + + @staticmethod + def rospose_to_pose(rospose: ROSPose): + """ + Converts a geometry_msgs/Pose to a pose3d/Pose + + Args: + posestamped (geometry_msgs/Pose): pose to convert + + Returns: + Pose: converted pose + """ + (_, _, yaw) = euler_from_quaternion( + [ + rospose.orientation.x, + rospose.orientation.y, + rospose.orientation.z, + rospose.orientation.w, + ] + ) + + p = Pose(rospose.position.x, rospose.position.y, yaw) + return p + + def heading_to_quaternion(heading): + q = quaternion_from_euler(0, 0, heading) + return q + + def __init__(self, x, y, theta): + self.x = x + self.y = y + self.theta = theta + + def __repr__(self) -> str: + return f"Pose(x={self.x}, y={self.y}, theta={self.theta})" + + def copy(self): + return Pose(self.x, self.y, self.theta) + + @property + def x(self): + return self.__x + + @x.setter + def x(self, x): + self.__x = x + + @property + def y(self): + return self.__y + + @y.setter + def y(self, y): + self.__y = y + + @property + def theta(self): + if self.__theta > np.pi or self.__theta < -np.pi: + raise ValueError("Theta is not in [-pi, pi]") + return self.__theta + + @theta.setter + def theta(self, theta): + # normalize theta to [-pi, pi] + self.__theta = np.arctan2(np.sin(theta), np.cos(theta)) + + def to_mat(self): + """ + Returns the pose as a 3x3 homogeneous transformation matrix + """ + return np.array( + [ + [np.cos(self.theta), -np.sin(self.theta), self.x], + [np.sin(self.theta), np.cos(self.theta), self.y], + [0, 0, 1], + ] + ) + + @staticmethod + def from_mat(mat): + """ + Creates a pose from a 3x3 homogeneous transformation matrix + """ + return Pose(mat[0, 2], mat[1, 2], np.arctan2(mat[1, 0], mat[0, 0])) + + def invert(self): + """ + Inverts the pose + """ + return Pose.from_mat(np.linalg.inv(self.to_mat())) + + def convert_pose_from_global_to_local_frame(self, pose): + """ + Converts the inputted pose from the global frame to the local frame + (relative to the current pose) + """ + return pose / self + + def convert_pose_from_local_to_global_frame(self, pose): + """ + Converts the inputted pose from the local frame to the global frame + (relative to the current pose) + """ + return pose * self + + def convert_point_from_global_to_local_frame(self, point): + """ + Converts the inputted point from the global frame to the local frame + (relative to the current pose) + """ + point_hg = np.array([point[0], point[1], 1]) + point_hg_local = np.linalg.inv(self.to_mat()) @ point_hg + return ( + point_hg_local[0] / point_hg_local[2], + point_hg_local[1] / point_hg_local[2], + ) + + def convert_point_from_local_to_global_frame(self, point): + """ + Converts the inputted point from the local frame to the global frame + (relative to the current pose) + """ + point_hg = np.array([point[0], point[1], 1]) + point_hg_global = self.to_mat() @ point_hg + return ( + point_hg_global[0] / point_hg_global[2], + point_hg_global[1] / point_hg_global[2], + ) + + def convert_point_array_from_global_to_local_frame(self, points): + """ + Converts the inputted point array from the global frame to the local frame + (relative to the current pose) + + Args: + points (np.ndarray) [Size: (N,2)]: array of points to convert + """ + points_hg = np.array([points[:, 0], points[:, 1], np.ones(len(points))]) + points_hg_local = np.linalg.inv(self.to_mat()) @ points_hg.T + return (points_hg_local[:2, :] / points_hg_local[2, :]).T + + def convert_point_array_from_local_to_global_frame(self, points): + """ + Converts the inputted point array from the local frame to the global frame + (relative to the current pose) + + Args: + points (np.ndarray) [Size: (N,2)]: array of points to convert + """ + points_hg = np.array([points[:, 0], points[:, 1], np.ones(len(points))]) + points_hg_global = self.to_mat() @ points_hg.T + return (points_hg_global[:2, :] / points_hg_global[2, :]).T + + def rotate(self, angle): + """ + Rotates the pose by the given angle + """ + self.theta += angle + + def translate(self, x, y): + """ + Translates the pose by the given x and y distances + """ + self.x += x + self.y += y + + def __add__(self, other): + return Pose(self.x + other.x, self.y + other.y, self.theta + other.theta) + + def __sub__(self, other): + return Pose(self.x - other.x, self.y - other.y, self.theta - other.theta) + + def __neg__(self): + return Pose(-self.x, -self.y, -self.theta) + + def __mul__(self, other): + p1_mat = self.to_mat() + p2_mat = other.to_mat() + + return Pose.from_mat(p1_mat @ p2_mat) + + def __truediv__(self, other): + p1_mat = self.to_mat() + p2_mat = other.to_mat() + + return Pose.from_mat(np.linalg.inv(p2_mat) @ p1_mat) + + +if __name__ == "__main__": + # TODO: again do we want example code in these classes + rospose = ROSPose() + rospose.position.x = 1 + rospose.position.y = 2 + rospose.position.z = 3 + rospose.orientation.x = 0 + rospose.orientation.y = 0 + rospose.orientation.z = -0.061461 + rospose.orientation.w = 0.9981095 + + pose = Pose.rospose_to_pose(rospose) + print(pose) # Pose(x=1, y=2, theta=-0.123) diff --git a/rb_ws/src/buggy/buggy/util/trajectory.py b/rb_ws/src/buggy/buggy/util/trajectory.py new file mode 100644 index 00000000..a7da7dfb --- /dev/null +++ b/rb_ws/src/buggy/buggy/util/trajectory.py @@ -0,0 +1,365 @@ +import json +import time +import uuid +import matplotlib.pyplot as plt + +from buggy.msg import TrajectoryMsg + +import numpy as np +from scipy.interpolate import Akima1DInterpolator, CubicSpline + +import utm + + +class Trajectory: + """A wrapper around a trajectory JSON file that does some under-the-hood math. Will + interpolate the data points and calculate other information such as distance along the trajectory + and instantaneous curvature. + + Currently, the trajectory is assumed to be a straight line between each waypoint. This is + not a good assumption, but it's good enough for now. This means that the curvature is zero + anywhere between waypoints. + + This class also has a method that will find the closest point on the trajectory to a given + point in the world. This is useful for finding where the buggy is on the trajectory. + + Use https://rdudhagra.github.io/eracer-portal/ to make trajectories and save the JSON file + """ + + def __init__(self, json_filepath=None, positions = None, interpolator="CubicSpline") -> None: + """ + Args: + json_filepath (String): file path to the path json file (begins at /rb_ws) + positions [[float, float]]: reference trajectory in world coordinates + current_speed (float): current speed of the buggy + + Returns: + float (desired steering angle) + """ + self.distances = np.zeros((0, 1)) # (N/dt x 1) [d, d, ...] + self.positions = np.zeros((0, 2)) # (N x 2) [(x,y), (x,y), ...] + self.indices = None # (N x 1) [0, 1, 2, ...] + self.interpolation = None # scipy.interpolate.PPoly + + # read positions from file + if positions is None: + positions = [] + # Load the json file + with open(json_filepath, "r") as f: + data = json.load(f) + + # Iterate through the waypoints and extract the positions + num_waypoints = len(data) + for i in range(0, num_waypoints): + + waypoint = data[i] + + lat = waypoint["lat"] + lon = waypoint["lon"] + + # Convert to world coordinates + x, y = utm.from_latlon(lat, lon) #TODO: Before Merging, make sure that we can do this nonlocalized + positions.append([x, y]) + + positions = np.array(positions) + + num_indices = positions.shape[0] + + if interpolator == "Akima": + self.positions = positions + self.indices = np.arange(num_indices) + self.interpolation = Akima1DInterpolator(self.indices, self.positions) + self.interpolation.extrapolate = True + elif interpolator == "CubicSpline": + temp_traj = Trajectory(positions=positions, interpolator="Akima") + tot_len = temp_traj.distances[-1] + interp_dists = np.linspace(0, tot_len, num_indices) + + self.indices = np.arange(num_indices) + self.positions = [ + temp_traj.get_position_by_distance(interp_dist) + for interp_dist in interp_dists + ] + self.positions = np.array(self.positions) + + self.interpolation = CubicSpline(self.indices, self.positions) + self.interpolation.extrapolate = True + + # TODO: check units + # Calculate the distances along the trajectory + dt = 0.01 #dt is time step (in seconds (?)) + ts = np.arange(len(self.positions) - 1, step=dt) # times corresponding to each position (?) + drdt = self.interpolation( + ts, nu=1 + ) # Calculate derivatives of interpolated path wrt indices + ds = np.sqrt(drdt[:, 0] ** 2 + drdt[:, 1] ** 2) * dt # distances to each interpolated point + s = np.cumsum(np.hstack([[0], ds[:-1]])) + self.distances = s + self.dt = dt + + def get_num_points(self): + """Gets the number of points along the trajectory + + Returns: + int: number of points + """ + return len(self.positions) + + def get_heading_by_index(self, index): + """Gets the heading at given index along trajectory, + interpolating if necessary + + Args: + index (float): index along the trajectory + + Returns: + float: (theta) in rads + """ + # theta = np.interp(index, self.indices, self.positions[:, 2]) + dxdt, dydt = self.interpolation(index, nu=1).reshape((-1, 2)).T + theta = np.arctan2(dydt, dxdt) + + return theta + + def get_heading_by_distance(self, distance): + """Gets the heading at given distance along trajectory, + interpolating if necessary + + Args: + distance (float): distance along the trajectory in meters + + Returns: + float: (theta) in rads + """ + index = self.get_index_from_distance(distance) + return self.get_heading_by_index(index) + + def get_position_by_index(self, index): + """Gets the position at a given index along the trajectory, + interpolating if necessary + + Args: + index (float): index along the trajectory + + Returns: + tuple: (x, y) + """ + # Interpolate the position + + return self.interpolation(index) + + def get_position_by_distance(self, distance): + """Gets the position at a given distance along the trajectory, + interpolating if necessary + + Args: + distance (float): distance along the trajectory in meters + + Returns: + tuple: (x, y) + """ + # Interpolate the position + index = self.get_index_from_distance(distance) + return self.get_position_by_index(index) + + def get_steering_angle_by_index(self, index, wheelbase): + """Gets the bicycle-model steering angle at a given distance along the trajectory, + interpolating if necessary. Assumes that the origin point of the buggy is at the + center of the rear axle. + + Args: + index (float): index along the trajectory + wheelbase (float): wheelbase of the buggy in meters + + Returns: + float: steering angle in rads + """ + curvature = self.get_curvature_by_index(index) + return np.arctan(wheelbase * curvature) + + def get_steering_angle_by_distance(self, distance, wheelbase): + """Gets the bicycle-model steering angle at a given distance along the trajectory, + interpolating if necessary. Assumes that the origin point of the buggy is at the + center of the rear axle. + + Args: + distance (float): distance along the trajectory in meters + wheelbase (float): wheelbase of the buggy in meters + + Returns: + float: steering angle in rads + """ + index = self.get_index_from_distance(distance) + return self.get_steering_angle_by_index(index, wheelbase) + + def get_index_from_distance(self, distance): + """Gets the index at a given distance along the trajectory, + interpolating if necessary + + Args: + distance (float): distance along the trajectory in meters + + Returns: + int: index along the trajectory + """ + # Interpolate the index + index = np.interp( + distance, + self.distances, + np.linspace(0, len(self.distances), len(self.distances)), + ) + + return index * self.dt + + def get_distance_from_index(self, index): + """Gets the distance at a given index along the trajectory, + interpolating if necessary + + Args: + index (float): index along the trajectory + + Returns: + float: distance along the trajectory in meters + """ + # Interpolate the distance + distance = np.interp( + index / self.dt, np.arange(len(self.distances)), self.distances + ) + + return distance + + def get_curvature_by_index(self, index): + """Gets the curvature at a given index along the trajectory, + interpolating if necessary + + Args: + index (float): index along the trajectory + + Returns: + float: curvature + """ + # Interpolate the curvature + dxdt, dydt = self.interpolation(index, nu=1).reshape((-1, 2)).T + ddxdtt, ddydtt = self.interpolation(index, nu=2).reshape((-1, 2)).T + + curvature = (dxdt * ddydtt - dydt * ddxdtt) / ( + (dxdt**2 + dydt**2) ** (3 / 2) + ) + + return curvature + + def get_curvature_by_distance(self, distance): + """Gets the curvature at a given distance along the trajectory, + interpolating if necessary + + Args: + distance (float): distance along the trajectory in meters + + Returns: + float: curvature + """ + # Interpolate the curvature + index = self.get_index_from_distance(distance) + + return self.get_curvature_by_index(index) + + def get_dynamics_by_index(self, index, wheelbase): + """Gets the dynamics at a given index along the trajectory, + interpolating if necessary. Convenience function that returns + all of the dynamics at once rather than requiring multiple + calls to other helper functions. In this way, we can reuse calls + to the interpolator, improving performance. + + Args: + index (float): index along the trajectory + + Returns: + tuple: (x, y, theta, steering_angle) + """ + # Interpolate the dynamics + x, y = self.interpolation(index).reshape((-1, 2)).T + dxdt, dydt = self.interpolation(index, nu=1).reshape((-1, 2)).T + ddxdtt, ddydtt = self.interpolation(index, nu=2).reshape((-1, 2)).T + + curvature = (dxdt * ddydtt - dydt * ddxdtt) / ( + (dxdt**2 + dydt**2) ** (3 / 2) + ) + theta = np.arctan2(dydt, dxdt) + + return np.stack((x, y, theta, np.arctan(wheelbase * curvature)), axis=-1) + + def get_unit_normal_by_index(self, index): + """Gets the index of the closest point on the trajectory to the given point + + Args: + index: Nx1 numpy array: indexes along trajectory + Returns: + Nx2 numpy array: unit normal of the trajectory at index + """ + + derivative = self.interpolation(index, nu=1) + unit_derivative = derivative / np.linalg.norm(derivative, axis=1)[:, None] + + # (x, y), rotated by 90 deg ccw = (-y, x) + unit_normal = np.vstack((-unit_derivative[:, 1], unit_derivative[:, 0])).T + return unit_normal + + def get_closest_index_on_path( + self, x, y, start_index=0, end_index=None, subsample_resolution=1000 + ): + """Gets the index of the closest point on the trajectory to the given point + + Args: + x (float): x coordinate + y (float): y coordinate + start_index (int, optional): index to start searching from. Defaults to 0. + end_index (int, optional): index to end searching at. Defaults to None (disable). + subsample_resolution: resolution of the resulting interpolation + Returns: + float: index along the trajectory + """ + # If end_index is not specified, use the length of the trajectory + if end_index is None: + end_index = len(self.positions) #sketch, 0-indexing where?? + + # Floor/ceil the start/end indices + start_index = max(0, int(np.floor(start_index))) + end_index = int(np.ceil(end_index)) + + # Calculate the distance from the point to each point on the trajectory + distances = (self.positions[start_index : end_index + 1, 0] - x) ** 2 + ( + self.positions[start_index : end_index + 1, 1] - y + ) ** 2 #Don't need to squareroot as it is a relative distance + + min_ind = np.argmin(distances) + start_index + + start_index = max(0, min_ind - 1) #Protection in case min_ind is too low + end_index = min(len(self.positions), min_ind + 1) #Prtoecting in case min_ind too high + #Theoretically start_index and end_index are just two apart + + # Now interpolate at a higher resolution to get a more accurate result + r_interp = self.interpolation( + np.linspace(start_index, end_index, subsample_resolution + 1)) + x_interp, y_interp = r_interp[:, 0], r_interp[:, 1] #x_interp, y_interp are numpy column vectors + + distances = (x_interp - x) ** 2 + (y_interp - y) ** 2 #Again distances are relative + + # Return the rational index of the closest point + return ( + np.argmin(distances) / subsample_resolution * (end_index - start_index) + + start_index + ) + + def pack(self, x, y) -> TrajectoryMsg: + traj = TrajectoryMsg() + traj.easting = self.positions[:, 0] + traj.northing = self.positions[:, 1] + traj.time = time.time() + traj.cur_idx = self.get_closest_index_on_path(x,y) + return traj + + def unpack(trajMsg : TrajectoryMsg): + pos = np.array([trajMsg.easting, trajMsg.northing]).transpose(1, 0) + cur_idx = trajMsg.cur_idx + return Trajectory(positions=pos), cur_idx +