From 884206d9686391d911cdc4c3fda116ff7b188b26 Mon Sep 17 00:00:00 2001 From: hhxjqm Date: Sun, 30 Mar 2025 03:40:58 +0000 Subject: [PATCH 01/71] Update current.yaml and stanley.py --- GEMstack/knowledge/defaults/current.yaml | 39 ++- GEMstack/onboard/planning/stanley.py | 353 +++++++++++++++++++++++ 2 files changed, 379 insertions(+), 13 deletions(-) create mode 100644 GEMstack/onboard/planning/stanley.py diff --git a/GEMstack/knowledge/defaults/current.yaml b/GEMstack/knowledge/defaults/current.yaml index c886288be..e07edf881 100644 --- a/GEMstack/knowledge/defaults/current.yaml +++ b/GEMstack/knowledge/defaults/current.yaml @@ -7,28 +7,41 @@ vehicle: !include ../vehicle/gem_e4.yaml model_predictive_controller: dt: 0.1 lookahead: 20 + control: recovery: brake_amount : 0.5 brake_speed : 2.0 + + # Pure Pursuit controller parameters pure_pursuit: - lookahead: 2.0 - lookahead_scale: 3.0 - crosstrack_gain: 1.0 - desired_speed: trajectory + lookahead: 2.0 # Base lookahead distance (meters) + lookahead_scale: 3.0 # Velocity-dependent lookahead scaling factor + crosstrack_gain: 0.5 # Gain for crosstrack error correction (default: 1) + desired_speed: trajectory # Speed reference source: can be "trajectory", "path", or a constant value + + # Stanley controller parameters (fine tune this) + stanley: + control_gain: 1.0 + softening_gain: 1.0 + yaw_rate_gain: 0.0 + steering_damp_gain: 0.0 + desired_speed: trajectory # Speed reference source: can be "trajectory", "path", or a constant value + + # Shared longitudinal control parameters longitudinal_control: - pid_p: 1.0 - pid_i: 0.1 - pid_d: 0.0 + pid_p: 1.5 # Proportional gain for speed PID controller + pid_i: 0.1 # Integral gain for speed PID controller + pid_d: 0.0 # Derivative gain for speed PID controller #configure the simulator, if using simulator: dt: 0.01 - real_time_multiplier: 1.0 # make the simulator run faster than real time by making this > 1 + real_time_multiplier: 1.0 # make the simulator run faster than real time by making this > 1 gnss_emulator: - dt: 0.1 #10Hz - #position_noise: 0.1 #10cm noise - #orientation_noise: 0.04 #2.3 degrees noise + dt: 0.1 # 10Hz + #position_noise: 0.1 # 10cm noise + #orientation_noise: 0.04 # 2.3 degrees noise #velocity_noise: - # constant: 0.04 #4cm/s noise - # linear: 0.02 #2% noise \ No newline at end of file + # constant: 0.04 # 4cm/s noise + # linear: 0.02 # 2% noise diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley.py new file mode 100644 index 000000000..620ae917f --- /dev/null +++ b/GEMstack/onboard/planning/stanley.py @@ -0,0 +1,353 @@ +import numpy as np +from math import sin, cos, atan2 + +# These imports match your existing project structure +from ...mathutils.control import PID +from ...utils import settings +from ...mathutils import transforms +from ...knowledge.vehicle.geometry import front2steer +from ...knowledge.vehicle.dynamics import acceleration_to_pedal_positions +from ...state.vehicle import VehicleState, ObjectFrameEnum +from ...state.trajectory import Path, Trajectory +from ..interface.gem import GEMVehicleCommand +from ..component import Component + +##################################### +# 1. Angle normalization +##################################### +def normalise_angle(angle): + """Wraps an angle to the [-pi, pi] range.""" + while angle > np.pi: + angle -= 2.0 * np.pi + while angle < -np.pi: + angle += 2.0 * np.pi + return angle + +##################################### +# 2. Stanley-based controller with longitudinal PID +##################################### +class Stanley(object): + """ + A Stanley controller that handles lateral control (steering) + plus a basic longitudinal control (PID + optional feedforward). + It has been modified to reduce oscillations by: + 1) Lower gains + 2) Steering damping + 3) Low-pass filter on steering + 4) Gentler speed logic when cornering + """ + + def __init__( + self, + control_gain=None, + softening_gain=None, + yaw_rate_gain=None, + steering_damp_gain=None, + desired_speed=None + ): + """ + :param control_gain: Stanley lateral control gain k (lowered from the default to reduce overshoot). + :param softening_gain: Softening gain k_soft. + :param yaw_rate_gain: Yaw-rate gain k_yaw_rate (helps reduce oscillations). + :param steering_damp_gain: Steering damping gain k_damp_steer. + :param desired_speed: Desired speed (float) or 'path'/'trajectory'. + """ + + # 1) Lower Gains + # Reduced from 2.5 to 1.0 by default, to mitigate oscillations + self.k = control_gain if control_gain is not None else settings.get('control.stanley.control_gain', 1.0) + self.k_soft = softening_gain if softening_gain is not None else settings.get('control.stanley.softening_gain', 1.0) + self.k_yaw_rate = yaw_rate_gain if yaw_rate_gain is not None else settings.get('control.stanley.yaw_rate_gain', 0.0) + self.k_damp_steer = steering_damp_gain if steering_damp_gain is not None else settings.get('control.stanley.steering_damp_gain', 0.0) + + # Typically, this is the max front-wheel steering angle in radians + self.max_steer = settings.get('vehicle.geometry.max_wheel_angle', np.deg2rad(24)) + self.wheelbase = settings.get('vehicle.geometry.wheelbase', 2.0) + + # Basic longitudinal constraints + self.speed_limit = settings.get('vehicle.limits.max_speed', 10.0) + self.max_accel = settings.get('vehicle.limits.max_acceleration', 2.0) + self.max_decel = settings.get('vehicle.limits.max_deceleration', 2.0) + + # PID for longitudinal speed tracking + p = settings.get('control.longitudinal_control.pid_p', 0.5) + d = settings.get('control.longitudinal_control.pid_d', 0.0) + i = settings.get('control.longitudinal_control.pid_i', 0.1) + self.pid_speed = PID(p, d, i, windup_limit=20) + + # Speed source: numeric or derived from path/trajectory + if desired_speed is not None: + self.desired_speed_source = desired_speed + else: + self.desired_speed_source = settings.get('control.stanley.desired_speed', 'path') + + if isinstance(self.desired_speed_source, (int, float)): + self.desired_speed = self.desired_speed_source + else: + self.desired_speed = None + + # For path/trajectory + self.path_arg = None + self.path = None + self.trajectory = None + self.current_path_parameter = 0.0 + self.current_traj_parameter = 0.0 + self.t_last = None + + # 2) Steering damping memory + self.prev_steering_angle = 0.0 + + # 3) Low-pass filter memory: final front-wheel angle + self.prev_front_wheel_angle = 0.0 + + def set_path(self, path: Path): + """Sets the path or trajectory to track.""" + if path == self.path_arg: + return + self.path_arg = path + + # If the path has more than 2D, reduce to (x,y) + if len(path.points[0]) > 2: + path = path.get_dims([0,1]) + + # If no timing info, we can't rely on 'path'/'trajectory' speed + if not isinstance(path, Trajectory): + self.path = path.arc_length_parameterize() + self.trajectory = None + self.current_traj_parameter = 0.0 + if self.desired_speed_source in ['path', 'trajectory']: + raise ValueError("Stanley: Provided path has no timing. Either provide a Trajectory or set a constant desired_speed.") + else: + self.path = path.arc_length_parameterize() + self.trajectory = path + self.current_traj_parameter = self.trajectory.domain()[0] + + self.current_path_parameter = 0.0 + + def _find_front_axle_position(self, x, y, yaw): + """Compute front-axle world position from the center/rear and yaw.""" + fx = x + self.wheelbase * cos(yaw) + fy = y + self.wheelbase * sin(yaw) + return fx, fy + + def compute(self, state: VehicleState, component: Component = None): + """Compute the control outputs: (longitudinal acceleration, front wheel angle).""" + t = state.pose.t + if self.t_last is None: + self.t_last = t + dt = t - self.t_last + + # Current vehicle states + curr_x = state.pose.x + curr_y = state.pose.y + curr_yaw = state.pose.yaw if state.pose.yaw is not None else 0.0 + speed = state.v + + if self.path is None: + if component: + component.debug_event("No path provided to Stanley controller. Doing nothing.") + return (0.0, 0.0) + + # Ensure same frame + if self.path.frame != state.pose.frame: + if component: + component.debug_event(f"Transforming path from {self.path.frame.name} to {state.pose.frame.name}") + self.path = self.path.to_frame(state.pose.frame, current_pose=state.pose) + + if self.trajectory and self.trajectory.frame != state.pose.frame: + if component: + component.debug_event(f"Transforming trajectory from {self.trajectory.frame.name} to {state.pose.frame.name}") + self.trajectory = self.trajectory.to_frame(state.pose.frame, current_pose=state.pose) + + # 1) Closest point + fx, fy = self._find_front_axle_position(curr_x, curr_y, curr_yaw) + search_start = self.current_path_parameter - 5.0 + search_end = self.current_path_parameter + 5.0 + closest_dist, closest_parameter = self.path.closest_point_local((fx, fy), [search_start, search_end]) + self.current_path_parameter = closest_parameter + + # 2) Path heading + target_x, target_y = self.path.eval(closest_parameter) + tangent = self.path.eval_tangent(closest_parameter) + path_yaw = atan2(tangent[1], tangent[0]) + + # 3) Lateral error + dx = fx - target_x + dy = fy - target_y + left_vec = np.array([sin(curr_yaw), -cos(curr_yaw)]) + cross_track_error = np.sign(np.dot(np.array([dx, dy]), left_vec)) * closest_dist + + # 4) Heading error + yaw_error = normalise_angle(path_yaw - curr_yaw) + + # 5) Standard Stanley terms + cross_term = atan2(self.k * cross_track_error, self.k_soft + speed) + desired_steering_angle = yaw_error + cross_term + + # 5.1) Yaw-rate damping (optional if k_yaw_rate != 0) + # This term penalizes high yaw rates, helps reduce overshoot + yaw_rate_damping = self.k_yaw_rate * (-speed * sin(desired_steering_angle)) / self.wheelbase + desired_steering_angle += yaw_rate_damping + + # 5.2) Steering damping (penalize rapid changes) + # Resist big steering jumps from one cycle to the next + steering_damp_term = self.k_damp_steer * (desired_steering_angle - self.prev_steering_angle) + desired_steering_angle += steering_damp_term + + # 6) Clip the raw desired steering + unfiltered_steering = np.clip(desired_steering_angle, -self.max_steer, self.max_steer) + + # 7) Low-pass filter for final steering + # alpha in [0..1], smaller -> more smoothing + alpha = 0.2 + final_steering = alpha * unfiltered_steering + (1.0 - alpha) * self.prev_front_wheel_angle + final_steering = np.clip(final_steering, -self.max_steer, self.max_steer) + + # Update memories + self.prev_steering_angle = unfiltered_steering + self.prev_front_wheel_angle = final_steering + + ################################################################ + # Speed Logic - Slightly gentler on cornering deceleration + ################################################################ + + # 8) Determine desired_speed from path/trajectory or fallback + desired_speed = self.desired_speed + feedforward_accel = 0.0 + + if self.trajectory and self.desired_speed_source in ['path', 'trajectory']: + if len(self.trajectory.points) < 2 or self.current_path_parameter >= self.path.domain()[1]: + # End of trajectory -> stop + if component: + component.debug_event("Stanley: Past the end of trajectory, stopping.") + desired_speed = 0.0 + feedforward_accel = -2.0 + else: + if self.desired_speed_source == 'path': + current_trajectory_time = self.trajectory.parameter_to_time(self.current_path_parameter) + else: + self.current_traj_parameter += dt + current_trajectory_time = self.current_traj_parameter + + deriv = self.trajectory.eval_derivative(current_trajectory_time) + desired_speed = min(np.linalg.norm(deriv), self.speed_limit) + + difference_dt = 0.1 + future_t = current_trajectory_time + difference_dt + if future_t > self.trajectory.domain()[1]: + future_t = self.trajectory.domain()[1] + future_deriv = self.trajectory.eval_derivative(future_t) + next_desired_speed = min(np.linalg.norm(future_deriv), self.speed_limit) + feedforward_accel = (next_desired_speed - desired_speed) / difference_dt + feedforward_accel = np.clip(feedforward_accel, -self.max_decel, self.max_accel) + else: + if desired_speed is None: + desired_speed = 4.0 + + # Cross-track-based slowdown (less aggressive than before). + desired_speed *= np.exp(-abs(cross_track_error) * 0.6) + + # Steering-based slowdown: reduce speed if the steering angle is large + angle_ratio = abs(final_steering) / self.max_steer + # If angle_ratio is 1.0, we'll cut speed to ~ 50% of original + speed_scale = 1.0 - 0.5 * angle_ratio + # Keep speed_scale at least 0.4 + speed_scale = max(speed_scale, 0.4) + desired_speed *= speed_scale + + # Clip to speed limit + if desired_speed > self.speed_limit: + desired_speed = self.speed_limit + + # 9) PID for longitudinal control + speed_error = desired_speed - speed + output_accel = self.pid_speed.advance(e=speed_error, t=t, feedforward_term=feedforward_accel) + + # Clip acceleration + if output_accel > self.max_accel: + output_accel = self.max_accel + elif output_accel < -self.max_decel: + output_accel = -self.max_decel + + # Avoid negative accel when fully stopped + if desired_speed == 0 and abs(speed) < 0.01 and output_accel < 0: + output_accel = 0.0 + + # Debug + if component: + component.debug("Stanley: fx, fy", (fx, fy)) + component.debug("Stanley: path param", self.current_path_parameter) + component.debug("Stanley: crosstrack dist", closest_dist) + component.debug("Stanley: cross_track_error", cross_track_error) + component.debug("Stanley: yaw_error", yaw_error) + component.debug("Stanley: unfiltered_steering (rad)", unfiltered_steering) + component.debug("Stanley: final_steering (rad)", final_steering) + component.debug("Stanley: angle_ratio", angle_ratio) + component.debug("Stanley: speed_scale", speed_scale) + component.debug("Stanley: desired_speed (m/s)", desired_speed) + component.debug("Stanley: feedforward_accel (m/s^2)", feedforward_accel) + component.debug("Stanley: output_accel (m/s^2)", output_accel) + component.debug("Stanley: current speed (m/s)", speed) + + self.t_last = t + return (output_accel, final_steering) + +##################################### +# 3. Tracker component +##################################### +class StanleyTrajectoryTracker(Component): + """ + A trajectory-tracking Component that uses the above Stanley controller + for lateral control plus PID-based longitudinal control. + It now includes measures to mitigate oscillations. + """ + + def __init__(self, vehicle_interface=None, **kwargs): + """ + :param vehicle_interface: The low-level interface to send commands to the vehicle. + :param kwargs: Optional parameters to pass into the Stanley(...) constructor. + """ + self.stanley = Stanley(**kwargs) + self.vehicle_interface = vehicle_interface + + def rate(self): + """Control frequency in Hz.""" + return 50.0 + + def state_inputs(self): + """ + Required state inputs: + - Vehicle state + - Trajectory + """ + return ["vehicle", "trajectory"] + + def state_outputs(self): + """No direct output state here.""" + return [] + + def update(self, vehicle: VehicleState, trajectory: Trajectory): + """ + Per control cycle: + 1) Set the path/trajectory + 2) Compute (acceleration, front wheel angle) + 3) Convert front wheel angle to steering wheel angle (if necessary) + 4) Send command to the vehicle + """ + self.stanley.set_path(trajectory) + accel, f_delta = self.stanley.compute(vehicle, self) + + # If your low-level interface expects steering wheel angle: + steering_angle = front2steer(f_delta) + steering_angle = np.clip( + steering_angle, + settings.get('vehicle.geometry.min_steering_angle', -0.5), + settings.get('vehicle.geometry.max_steering_angle', 0.5) + ) + + cmd = self.vehicle_interface.simple_command(accel, steering_angle, vehicle) + self.vehicle_interface.send_command(cmd) + + def healthy(self): + """Optional: check if the controller has a valid path.""" + return self.stanley.path is not None From f587a81d3dff1ab90d8543c535c1a2898b205971 Mon Sep 17 00:00:00 2001 From: hhxjqm Date: Mon, 31 Mar 2025 16:48:21 +0000 Subject: [PATCH 02/71] Update current.yaml and stanley.py --- GEMstack/knowledge/defaults/current.yaml | 4 +- GEMstack/onboard/planning/stanley.py | 75 ++++-------------------- 2 files changed, 13 insertions(+), 66 deletions(-) diff --git a/GEMstack/knowledge/defaults/current.yaml b/GEMstack/knowledge/defaults/current.yaml index e07edf881..5efa5b5bb 100644 --- a/GEMstack/knowledge/defaults/current.yaml +++ b/GEMstack/knowledge/defaults/current.yaml @@ -23,9 +23,7 @@ control: # Stanley controller parameters (fine tune this) stanley: control_gain: 1.0 - softening_gain: 1.0 - yaw_rate_gain: 0.0 - steering_damp_gain: 0.0 + softening_gain: 0.01 desired_speed: trajectory # Speed reference source: can be "trajectory", "path", or a constant value # Shared longitudinal control parameters diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley.py index 620ae917f..a2390ea71 100644 --- a/GEMstack/onboard/planning/stanley.py +++ b/GEMstack/onboard/planning/stanley.py @@ -41,8 +41,6 @@ def __init__( self, control_gain=None, softening_gain=None, - yaw_rate_gain=None, - steering_damp_gain=None, desired_speed=None ): """ @@ -54,25 +52,22 @@ def __init__( """ # 1) Lower Gains - # Reduced from 2.5 to 1.0 by default, to mitigate oscillations - self.k = control_gain if control_gain is not None else settings.get('control.stanley.control_gain', 1.0) - self.k_soft = softening_gain if softening_gain is not None else settings.get('control.stanley.softening_gain', 1.0) - self.k_yaw_rate = yaw_rate_gain if yaw_rate_gain is not None else settings.get('control.stanley.yaw_rate_gain', 0.0) - self.k_damp_steer = steering_damp_gain if steering_damp_gain is not None else settings.get('control.stanley.steering_damp_gain', 0.0) + self.k = control_gain if control_gain is not None else settings.get('control.stanley.control_gain') + self.k_soft = softening_gain if softening_gain is not None else settings.get('control.stanley.softening_gain') # Typically, this is the max front-wheel steering angle in radians - self.max_steer = settings.get('vehicle.geometry.max_wheel_angle', np.deg2rad(24)) - self.wheelbase = settings.get('vehicle.geometry.wheelbase', 2.0) + self.max_steer = settings.get('vehicle.geometry.max_wheel_angle') + self.wheelbase = settings.get('vehicle.geometry.wheelbase') # Basic longitudinal constraints - self.speed_limit = settings.get('vehicle.limits.max_speed', 10.0) - self.max_accel = settings.get('vehicle.limits.max_acceleration', 2.0) - self.max_decel = settings.get('vehicle.limits.max_deceleration', 2.0) + self.speed_limit = settings.get('vehicle.limits.max_speed') + self.max_accel = settings.get('vehicle.limits.max_acceleration') + self.max_decel = settings.get('vehicle.limits.max_deceleration') # PID for longitudinal speed tracking - p = settings.get('control.longitudinal_control.pid_p', 0.5) - d = settings.get('control.longitudinal_control.pid_d', 0.0) - i = settings.get('control.longitudinal_control.pid_i', 0.1) + p = settings.get('control.longitudinal_control.pid_p') + d = settings.get('control.longitudinal_control.pid_d') + i = settings.get('control.longitudinal_control.pid_i') self.pid_speed = PID(p, d, i, windup_limit=20) # Speed source: numeric or derived from path/trajectory @@ -94,12 +89,6 @@ def __init__( self.current_traj_parameter = 0.0 self.t_last = None - # 2) Steering damping memory - self.prev_steering_angle = 0.0 - - # 3) Low-pass filter memory: final front-wheel angle - self.prev_front_wheel_angle = 0.0 - def set_path(self, path: Path): """Sets the path or trajectory to track.""" if path == self.path_arg: @@ -184,34 +173,6 @@ def compute(self, state: VehicleState, component: Component = None): cross_term = atan2(self.k * cross_track_error, self.k_soft + speed) desired_steering_angle = yaw_error + cross_term - # 5.1) Yaw-rate damping (optional if k_yaw_rate != 0) - # This term penalizes high yaw rates, helps reduce overshoot - yaw_rate_damping = self.k_yaw_rate * (-speed * sin(desired_steering_angle)) / self.wheelbase - desired_steering_angle += yaw_rate_damping - - # 5.2) Steering damping (penalize rapid changes) - # Resist big steering jumps from one cycle to the next - steering_damp_term = self.k_damp_steer * (desired_steering_angle - self.prev_steering_angle) - desired_steering_angle += steering_damp_term - - # 6) Clip the raw desired steering - unfiltered_steering = np.clip(desired_steering_angle, -self.max_steer, self.max_steer) - - # 7) Low-pass filter for final steering - # alpha in [0..1], smaller -> more smoothing - alpha = 0.2 - final_steering = alpha * unfiltered_steering + (1.0 - alpha) * self.prev_front_wheel_angle - final_steering = np.clip(final_steering, -self.max_steer, self.max_steer) - - # Update memories - self.prev_steering_angle = unfiltered_steering - self.prev_front_wheel_angle = final_steering - - ################################################################ - # Speed Logic - Slightly gentler on cornering deceleration - ################################################################ - - # 8) Determine desired_speed from path/trajectory or fallback desired_speed = self.desired_speed feedforward_accel = 0.0 @@ -247,19 +208,11 @@ def compute(self, state: VehicleState, component: Component = None): # Cross-track-based slowdown (less aggressive than before). desired_speed *= np.exp(-abs(cross_track_error) * 0.6) - # Steering-based slowdown: reduce speed if the steering angle is large - angle_ratio = abs(final_steering) / self.max_steer - # If angle_ratio is 1.0, we'll cut speed to ~ 50% of original - speed_scale = 1.0 - 0.5 * angle_ratio - # Keep speed_scale at least 0.4 - speed_scale = max(speed_scale, 0.4) - desired_speed *= speed_scale - # Clip to speed limit if desired_speed > self.speed_limit: desired_speed = self.speed_limit - # 9) PID for longitudinal control + # PID for longitudinal control speed_error = desired_speed - speed output_accel = self.pid_speed.advance(e=speed_error, t=t, feedforward_term=feedforward_accel) @@ -280,17 +233,13 @@ def compute(self, state: VehicleState, component: Component = None): component.debug("Stanley: crosstrack dist", closest_dist) component.debug("Stanley: cross_track_error", cross_track_error) component.debug("Stanley: yaw_error", yaw_error) - component.debug("Stanley: unfiltered_steering (rad)", unfiltered_steering) - component.debug("Stanley: final_steering (rad)", final_steering) - component.debug("Stanley: angle_ratio", angle_ratio) - component.debug("Stanley: speed_scale", speed_scale) component.debug("Stanley: desired_speed (m/s)", desired_speed) component.debug("Stanley: feedforward_accel (m/s^2)", feedforward_accel) component.debug("Stanley: output_accel (m/s^2)", output_accel) component.debug("Stanley: current speed (m/s)", speed) self.t_last = t - return (output_accel, final_steering) + return (output_accel, desired_steering_angle) ##################################### # 3. Tracker component From c19280234059c174187af6d6f6a057745cdf49b2 Mon Sep 17 00:00:00 2001 From: hhxjqm Date: Mon, 31 Mar 2025 20:22:31 +0000 Subject: [PATCH 03/71] Fix Stanley controller debug logging --- GEMstack/onboard/planning/stanley.py | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley.py index a2390ea71..b660861d7 100644 --- a/GEMstack/onboard/planning/stanley.py +++ b/GEMstack/onboard/planning/stanley.py @@ -227,17 +227,24 @@ def compute(self, state: VehicleState, component: Component = None): output_accel = 0.0 # Debug - if component: - component.debug("Stanley: fx, fy", (fx, fy)) + if component is not None: + # component.debug("Stanley: fx, fy", (fx, fy)) + component.debug('curr pt',(curr_x,curr_y)) component.debug("Stanley: path param", self.current_path_parameter) component.debug("Stanley: crosstrack dist", closest_dist) - component.debug("Stanley: cross_track_error", cross_track_error) + component.debug("crosstrack error", cross_track_error) component.debug("Stanley: yaw_error", yaw_error) + component.debug('steering_angle', desired_steering_angle) component.debug("Stanley: desired_speed (m/s)", desired_speed) component.debug("Stanley: feedforward_accel (m/s^2)", feedforward_accel) component.debug("Stanley: output_accel (m/s^2)", output_accel) - component.debug("Stanley: current speed (m/s)", speed) + if output_accel > self.max_accel: + output_accel = self.max_accel + + if output_accel < -self.max_decel: + output_accel = -self.max_decel + self.t_last = t return (output_accel, desired_steering_angle) From b7ced521ce1caf0003df5c65da3d3af16415cf52 Mon Sep 17 00:00:00 2001 From: Jugal Date: Thu, 10 Apr 2025 15:41:45 -0500 Subject: [PATCH 04/71] Update fixed_route.yaml --- launch/fixed_route.yaml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/launch/fixed_route.yaml b/launch/fixed_route.yaml index c05de8ff7..abd8242e6 100644 --- a/launch/fixed_route.yaml +++ b/launch/fixed_route.yaml @@ -21,7 +21,8 @@ drive: type: RouteToTrajectoryPlanner args: [null] #desired speed in m/s. If null, this will keep the route untimed for the trajectory tracker trajectory_tracking: - type: pure_pursuit.PurePursuitTrajectoryTracker + #type: pure_pursuit.PurePursuitTrajectoryTracker + type: stanley.StanleyTrajectoryTracker args: {desired_speed: 2.5} #approximately 5mph print: False log: @@ -76,4 +77,4 @@ variants: #visualization: !include "mpl_visualization.yaml" log_ros: log: - ros_topics : !include "../GEMstack/knowledge/defaults/standard_ros_topics.yaml" \ No newline at end of file + ros_topics : !include "../GEMstack/knowledge/defaults/standard_ros_topics.yaml" From 0ed85b3b1b98a6f8566cafc5a7605c5e8ca0dc85 Mon Sep 17 00:00:00 2001 From: Jugal Date: Thu, 10 Apr 2025 15:42:24 -0500 Subject: [PATCH 05/71] Update stanley.py --- GEMstack/onboard/planning/stanley.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley.py index b660861d7..8f871f488 100644 --- a/GEMstack/onboard/planning/stanley.py +++ b/GEMstack/onboard/planning/stanley.py @@ -159,7 +159,8 @@ def compute(self, state: VehicleState, component: Component = None): target_x, target_y = self.path.eval(closest_parameter) tangent = self.path.eval_tangent(closest_parameter) path_yaw = atan2(tangent[1], tangent[0]) - + desired_x = target_x + desired_y = target_y # 3) Lateral error dx = fx - target_x dy = fy - target_y @@ -230,6 +231,8 @@ def compute(self, state: VehicleState, component: Component = None): if component is not None: # component.debug("Stanley: fx, fy", (fx, fy)) component.debug('curr pt',(curr_x,curr_y)) + component.debug("desired_x",desired_x) + component.debug("desired_y",desired_y) component.debug("Stanley: path param", self.current_path_parameter) component.debug("Stanley: crosstrack dist", closest_dist) component.debug("crosstrack error", cross_track_error) From ec5c28c60e80809fce05cef11dcebce21e290bd2 Mon Sep 17 00:00:00 2001 From: Jugal Date: Thu, 10 Apr 2025 15:43:17 -0500 Subject: [PATCH 06/71] Add files via upload --- logs/logplot_pp.py | 120 +++++++++++++++++++++++++++++++++++++++++++++ logs/logplot_s.py | 119 ++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 239 insertions(+) create mode 100644 logs/logplot_pp.py create mode 100644 logs/logplot_s.py diff --git a/logs/logplot_pp.py b/logs/logplot_pp.py new file mode 100644 index 000000000..74025c291 --- /dev/null +++ b/logs/logplot_pp.py @@ -0,0 +1,120 @@ +####Following code can be used to visualize performance of Pure Pursuit after using it.### +###How to use?- After running the simulation or on actual vehicle you get logs in log folder### +### You can run the command- python3 logplot_pp.py "logfilename".### + + +import json +import sys +import os +import matplotlib.pyplot as plt +import numpy as np + +# Safety thresholds (not used in the current plots) +ACC_SAFE = 0.8 # |acceleration| <= 0.8 m/s² is safe +ACC_UNSAFE = 2.0 # |acceleration| >= 2.0 m/s² is unsafe +HR_SAFE = 0.05 # |heading_rate| <= 0.05 rad/s is safe +HR_UNSAFE = 0.2 # |heading_rate| >= 0.2 rad/s is unsafe + +def parse_behavior_log(filename): + times = [] + accelerations = [] + heading_rates = [] + + with open(filename, 'r') as f: + first_time = None # Store the first timestamp for shifting + for line in f: + try: + entry = json.loads(line) + except json.JSONDecodeError: + print(f"Skipping invalid JSON line: {line.strip()}") + continue + if "vehicle" in entry: + t = entry.get("time") + vehicle_data = entry["vehicle"].get("data", {}) + acceleration = vehicle_data.get("acceleration") + heading_rate = vehicle_data.get("heading_rate") + if t is not None and acceleration is not None and heading_rate is not None: + if first_time is None: + first_time = t # Set the first timestamp + times.append(t - first_time) # Shift time to start from 0 + accelerations.append(acceleration) + heading_rates.append(heading_rate) + + return np.array(times), np.array(accelerations), np.array(heading_rates) + +def parse_tracker_csv(filename): + data = np.genfromtxt(filename, delimiter=',', skip_header=1) + first_time = data[0, 18] # Normalize time to start from 0 + cte_time = data[:, 18] - first_time + cte = data[:, 20] + x_actual, y_actual = data[:, 2], data[:, 5] + x_desired, y_desired = data[:, 11], data[:, 14] + return cte_time, cte, x_actual, y_actual, x_desired, y_desired + +def compute_derivative(times, values): + dt = np.diff(times) + dv = np.diff(values) + derivative = dv / dt + return times[1:], derivative + +def plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired): + global_min_time = min(time_jerk[0], time_heading_acc[0], cte_time[0]) + global_max_time = max(time_jerk[-1], time_heading_acc[-1], cte_time[-1]) + + fig, axs = plt.subplots(2, 2, figsize=(12, 8)) + fig.subplots_adjust(hspace=0.4, wspace=0.3) + + axs[0,0].plot(time_jerk, jerk, marker='', linestyle='-', color='blue') + axs[0,0].set_ylabel("Jerk (m/s³)") + axs[0,0].set_title("Vehicle Jerk Over Time") + axs[0,0].grid(True) + axs[0,0].set_xlim(global_min_time, global_max_time) + + axs[0,1].plot(time_heading_acc, heading_acc, marker='', linestyle='-', color='orange') + axs[0,1].set_ylabel("Heading Acceleration (rad/s²)") + axs[0,1].set_title("Vehicle Heading Acceleration Over Time") + axs[0,1].grid(True) + axs[0,1].set_xlim(global_min_time, global_max_time) + + axs[1,0].plot(cte_time, cte, marker='', linestyle='-', color='green') + axs[1,0].set_xlabel("Time (s)") + axs[1,0].set_ylabel("Cross Track Error") + axs[1,0].set_title("Vehicle Cross Track Error Over Time") + axs[1,0].grid(True) + axs[1,0].set_xlim(global_min_time, global_max_time) + + axs[1,1].plot(x_actual, y_actual, marker='', linestyle='-', color='blue', label='Actual') + axs[1,1].plot(x_desired, y_desired, marker='', linestyle='-', color='orange', label='Desired') + axs[1,1].set_xlabel("X Position (m)") + axs[1,1].set_ylabel("Y Position (m)") + axs[1,1].set_title("Vehicle Position") + axs[1,1].legend() + axs[1,1].grid(True) + + plt.show() + +if __name__=='__main__': + if len(sys.argv) != 2: + print("Usage: python test_comfort_metrics.py ") + sys.exit(1) + + log_dir = sys.argv[1] + behavior_file = os.path.join(log_dir, "behavior.json") + tracker_file = os.path.join(log_dir, "PurePursuitTrajectoryTracker_debug.csv") + #tracker_file = os.path.join(log_dir, "StanleyTrajectoryTracker_debug.csv") + + times, accelerations, heading_rates = parse_behavior_log(behavior_file) + time_jerk, jerk = compute_derivative(times, accelerations) + time_heading_acc, heading_acc = compute_derivative(times, heading_rates) + + cte_time, cte, x_actual, y_actual, x_desired, y_desired = parse_tracker_csv(tracker_file) + + plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired) + + print("Max (abs) jerk:", np.max(np.abs(jerk))) + print("Avg jerk:", np.mean(np.abs(jerk))) + print("Max (abs) heading acceleration:", np.max(np.abs(heading_acc))) + print("Avg heading acceleration:", np.mean(np.abs(heading_acc))) + print("Max (abs) cross track error:", np.max(np.abs(cte))) + print("Avg cross track error:", np.mean(np.abs(cte))) + diff --git a/logs/logplot_s.py b/logs/logplot_s.py new file mode 100644 index 000000000..f534bbd27 --- /dev/null +++ b/logs/logplot_s.py @@ -0,0 +1,119 @@ +####Following code can be used to visualize performance of Stanley after using it.### +###How to use?- After running the simulation or on actual vehicle you get logs in log folder### +### You can run the command- python3 logplot_s.py "logfilename".### + +import json +import sys +import os +import matplotlib.pyplot as plt +import numpy as np + +# Safety thresholds (not used in the current plots) +ACC_SAFE = 0.8 # |acceleration| <= 0.8 m/s² is safe +ACC_UNSAFE = 2.0 # |acceleration| >= 2.0 m/s² is unsafe +HR_SAFE = 0.05 # |heading_rate| <= 0.05 rad/s is safe +HR_UNSAFE = 0.2 # |heading_rate| >= 0.2 rad/s is unsafe + +def parse_behavior_log(filename): + times = [] + accelerations = [] + heading_rates = [] + + with open(filename, 'r') as f: + first_time = None # Store the first timestamp for shifting + for line in f: + try: + entry = json.loads(line) + except json.JSONDecodeError: + print(f"Skipping invalid JSON line: {line.strip()}") + continue + if "vehicle" in entry: + t = entry.get("time") + vehicle_data = entry["vehicle"].get("data", {}) + acceleration = vehicle_data.get("acceleration") + heading_rate = vehicle_data.get("heading_rate") + if t is not None and acceleration is not None and heading_rate is not None: + if first_time is None: + first_time = t # Set the first timestamp + times.append(t - first_time) # Shift time to start from 0 + accelerations.append(acceleration) + heading_rates.append(heading_rate) + + return np.array(times), np.array(accelerations), np.array(heading_rates) + +def parse_tracker_csv(filename): + data = np.genfromtxt(filename, delimiter=',', skip_header=1) + first_time = data[0, 18] # Normalize time to start from 0 + cte_time = data[:, 18] - first_time + cte = data[:, 20] + x_actual, y_actual = data[:, 2], data[:, 5] + x_desired, y_desired = data[:, 8], data[:, 11] + return cte_time, cte, x_actual, y_actual, x_desired, y_desired + +def compute_derivative(times, values): + dt = np.diff(times) + dv = np.diff(values) + derivative = dv / dt + return times[1:], derivative + +def plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired): + global_min_time = min(time_jerk[0], time_heading_acc[0], cte_time[0]) + global_max_time = max(time_jerk[-1], time_heading_acc[-1], cte_time[-1]) + + fig, axs = plt.subplots(2, 2, figsize=(12, 8)) + fig.subplots_adjust(hspace=0.4, wspace=0.3) + + axs[0,0].plot(time_jerk, jerk, marker='', linestyle='-', color='blue') + axs[0,0].set_ylabel("Jerk (m/s³)") + axs[0,0].set_title("Vehicle Jerk Over Time") + axs[0,0].grid(True) + axs[0,0].set_xlim(global_min_time, global_max_time) + + axs[0,1].plot(time_heading_acc, heading_acc, marker='', linestyle='-', color='orange') + axs[0,1].set_ylabel("Heading Acceleration (rad/s²)") + axs[0,1].set_title("Vehicle Heading Acceleration Over Time") + axs[0,1].grid(True) + axs[0,1].set_xlim(global_min_time, global_max_time) + + axs[1,0].plot(cte_time, cte, marker='', linestyle='-', color='green') + axs[1,0].set_xlabel("Time (s)") + axs[1,0].set_ylabel("Cross Track Error") + axs[1,0].set_title("Vehicle Cross Track Error Over Time") + axs[1,0].grid(True) + axs[1,0].set_xlim(global_min_time, global_max_time) + + axs[1,1].plot(x_actual, y_actual, marker='', linestyle='-', color='blue', label='Actual') + axs[1,1].plot(x_desired, y_desired, marker='', linestyle='-', color='orange', label='Desired') + axs[1,1].set_xlabel("X Position (m)") + axs[1,1].set_ylabel("Y Position (m)") + axs[1,1].set_title("Vehicle Position") + axs[1,1].legend() + axs[1,1].grid(True) + + plt.show() + +if __name__=='__main__': + if len(sys.argv) != 2: + print("Usage: python test_comfort_metrics.py ") + sys.exit(1) + + log_dir = sys.argv[1] + behavior_file = os.path.join(log_dir, "behavior.json") + #tracker_file = os.path.join(log_dir, "PurePursuitTrajectoryTracker_debug.csv") + tracker_file = os.path.join(log_dir, "StanleyTrajectoryTracker_debug.csv") + + times, accelerations, heading_rates = parse_behavior_log(behavior_file) + time_jerk, jerk = compute_derivative(times, accelerations) + time_heading_acc, heading_acc = compute_derivative(times, heading_rates) + + cte_time, cte, x_actual, y_actual, x_desired, y_desired = parse_tracker_csv(tracker_file) + + plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired) + + print("Max (abs) jerk:", np.max(np.abs(jerk))) + print("Avg jerk:", np.mean(np.abs(jerk))) + print("Max (abs) heading acceleration:", np.max(np.abs(heading_acc))) + print("Avg heading acceleration:", np.mean(np.abs(heading_acc))) + print("Max (abs) cross track error:", np.max(np.abs(cte))) + print("Avg cross track error:", np.mean(np.abs(cte))) + From 01d2f921fd807abf769656df03f3402d7b179d08 Mon Sep 17 00:00:00 2001 From: Jugal Date: Thu, 10 Apr 2025 15:49:14 -0500 Subject: [PATCH 07/71] Update settings.py --- GEMstack/utils/settings.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/GEMstack/utils/settings.py b/GEMstack/utils/settings.py index b2470ce25..a3d193658 100644 --- a/GEMstack/utils/settings.py +++ b/GEMstack/utils/settings.py @@ -37,14 +37,14 @@ def load_settings(): def settings(): """Returns all global settings, loading them if necessary.""" - global SETTINGS + #global SETTINGS load_settings() return SETTINGS def get(path : Union[str,List[str]], defaultValue=KeyError) -> Any: """Retrieves a setting by a list of keys or a '.'-separated string.""" - global SETTINGS + #global SETTINGS load_settings() if isinstance(path,str): path = path.split('.') @@ -65,7 +65,7 @@ def set(path : Union[str,List[str]], value : Any, leaf_only=True) -> None: If leaf_only=True (default), we prevent inadvertently deleting parts of the settings dictionary. """ - global SETTINGS + #global SETTINGS load_settings() if isinstance(path,str): path = path.split('.') From 66e83d35d1fd908e23705762761d8c37c8bad2ea Mon Sep 17 00:00:00 2001 From: Patrick Wu Date: Sat, 12 Apr 2025 23:21:39 -0500 Subject: [PATCH 08/71] add mpc.py --- .github/workflows/python-app.yml | 124 ++--- GEMstack/knowledge/defaults/current.yaml | 90 ++-- GEMstack/onboard/planning/mpc.py | 162 ++++++ GEMstack/onboard/planning/stanley.py | 624 +++++++++++------------ GEMstack/utils/klampt_visualization.py | 598 +++++++++++----------- GEMstack/utils/settings.py | 160 +++--- launch/fixed_route.yaml | 160 +++--- logs/logplot_pp.py | 240 ++++----- logs/logplot_s.py | 238 ++++----- 9 files changed, 1279 insertions(+), 1117 deletions(-) create mode 100644 GEMstack/onboard/planning/mpc.py diff --git a/.github/workflows/python-app.yml b/.github/workflows/python-app.yml index 3de575d8e..84dc183c0 100644 --- a/.github/workflows/python-app.yml +++ b/.github/workflows/python-app.yml @@ -1,62 +1,62 @@ -# This workflow will install Python dependencies, run tests and lint with a single version of Python -# For more information see: https://docs.github.com/en/actions/automating-builds-and-tests/building-and-testing-python - -name: Python application - -on: - push: - branches: - - '**' - - -permissions: - contents: read - -jobs: - PEP-Guidelines: - - runs-on: ubuntu-latest - - steps: - - uses: actions/checkout@v3 - - name: Set up Python 3.10 - uses: actions/setup-python@v3 - with: - python-version: "3.10" - - name: Install dependencies - run: | - python -m pip install --upgrade pip - pip install flake8 flake8-docstrings pep8-naming - - name: Lint with flake8 - run: | - # stop the build if there are Python syntax errors or undefined names - flake8 ./GEMstack --count --select=E9,F63,F7,F82 --show-source --statistics --exclude=__init__.py || exit 1 - # to enable more advanced checks on the repo, uncomment the lines below (There are around 3000 violations) - # flake8 ./GEMstack --ignore=D,C901,E402,E231 --count --max-complexity=10 --max-line-length=127 --statistics --exclude=__init__.py || exit 1 - # if we want to enable documentation checks, uncomment the line below - # flake8 ./GEMstack --ignore=E128,E402,E501,F401 --docstring-convention pep257 --max-line-length=120 --exclude=__init__.py || exit 1 - continue-on-error: false - - Documentation: - - runs-on: ubuntu-latest - - steps: - - uses: actions/checkout@v3 - - name: Set up Python 3.10 - uses: actions/setup-python@v3 - with: - python-version: "3.10" - - name: Install dependencies - run: | - python -m pip install --upgrade pip - pip install sphinx sphinx-rtd-theme - - name: Generate Documentation - run: | - # stop the build if there are Python syntax errors or undefined names - sphinx-build -b html docs docs/build - - name: Save Documentation as Artifact - uses: actions/upload-artifact@v4 - with: - name: documentation - path: docs/build +# This workflow will install Python dependencies, run tests and lint with a single version of Python +# For more information see: https://docs.github.com/en/actions/automating-builds-and-tests/building-and-testing-python + +name: Python application + +on: + push: + branches: + - '**' + + +permissions: + contents: read + +jobs: + PEP-Guidelines: + + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v3 + - name: Set up Python 3.10 + uses: actions/setup-python@v3 + with: + python-version: "3.10" + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install flake8 flake8-docstrings pep8-naming + - name: Lint with flake8 + run: | + # stop the build if there are Python syntax errors or undefined names + flake8 ./GEMstack --count --select=E9,F63,F7,F82 --show-source --statistics --exclude=__init__.py || exit 1 + # to enable more advanced checks on the repo, uncomment the lines below (There are around 3000 violations) + # flake8 ./GEMstack --ignore=D,C901,E402,E231 --count --max-complexity=10 --max-line-length=127 --statistics --exclude=__init__.py || exit 1 + # if we want to enable documentation checks, uncomment the line below + # flake8 ./GEMstack --ignore=E128,E402,E501,F401 --docstring-convention pep257 --max-line-length=120 --exclude=__init__.py || exit 1 + continue-on-error: false + + Documentation: + + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v3 + - name: Set up Python 3.10 + uses: actions/setup-python@v3 + with: + python-version: "3.10" + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install sphinx sphinx-rtd-theme + - name: Generate Documentation + run: | + # stop the build if there are Python syntax errors or undefined names + sphinx-build -b html docs docs/build + - name: Save Documentation as Artifact + uses: actions/upload-artifact@v4 + with: + name: documentation + path: docs/build diff --git a/GEMstack/knowledge/defaults/current.yaml b/GEMstack/knowledge/defaults/current.yaml index 5efa5b5bb..1350e5204 100644 --- a/GEMstack/knowledge/defaults/current.yaml +++ b/GEMstack/knowledge/defaults/current.yaml @@ -1,45 +1,45 @@ -# ********* Main settings entry point for behavior stack *********** - -# Configure settings for the vehicle / vehicle model -vehicle: !include ../vehicle/gem_e4.yaml - -#arguments for algorithm components here -model_predictive_controller: - dt: 0.1 - lookahead: 20 - -control: - recovery: - brake_amount : 0.5 - brake_speed : 2.0 - - # Pure Pursuit controller parameters - pure_pursuit: - lookahead: 2.0 # Base lookahead distance (meters) - lookahead_scale: 3.0 # Velocity-dependent lookahead scaling factor - crosstrack_gain: 0.5 # Gain for crosstrack error correction (default: 1) - desired_speed: trajectory # Speed reference source: can be "trajectory", "path", or a constant value - - # Stanley controller parameters (fine tune this) - stanley: - control_gain: 1.0 - softening_gain: 0.01 - desired_speed: trajectory # Speed reference source: can be "trajectory", "path", or a constant value - - # Shared longitudinal control parameters - longitudinal_control: - pid_p: 1.5 # Proportional gain for speed PID controller - pid_i: 0.1 # Integral gain for speed PID controller - pid_d: 0.0 # Derivative gain for speed PID controller - -#configure the simulator, if using -simulator: - dt: 0.01 - real_time_multiplier: 1.0 # make the simulator run faster than real time by making this > 1 - gnss_emulator: - dt: 0.1 # 10Hz - #position_noise: 0.1 # 10cm noise - #orientation_noise: 0.04 # 2.3 degrees noise - #velocity_noise: - # constant: 0.04 # 4cm/s noise - # linear: 0.02 # 2% noise +# ********* Main settings entry point for behavior stack *********** + +# Configure settings for the vehicle / vehicle model +vehicle: !include ../vehicle/gem_e4.yaml + +#arguments for algorithm components here +model_predictive_controller: + dt: 0.1 + lookahead: 20 + +control: + recovery: + brake_amount : 0.5 + brake_speed : 2.0 + + # Pure Pursuit controller parameters + pure_pursuit: + lookahead: 2.0 # Base lookahead distance (meters) + lookahead_scale: 3.0 # Velocity-dependent lookahead scaling factor + crosstrack_gain: 0.5 # Gain for crosstrack error correction (default: 1) + desired_speed: trajectory # Speed reference source: can be "trajectory", "path", or a constant value + + # Stanley controller parameters (fine tune this) + stanley: + control_gain: 1.0 + softening_gain: 0.01 + desired_speed: trajectory # Speed reference source: can be "trajectory", "path", or a constant value + + # Shared longitudinal control parameters + longitudinal_control: + pid_p: 1.5 # Proportional gain for speed PID controller + pid_i: 0.1 # Integral gain for speed PID controller + pid_d: 0.0 # Derivative gain for speed PID controller + +#configure the simulator, if using +simulator: + dt: 0.01 + real_time_multiplier: 1.0 # make the simulator run faster than real time by making this > 1 + gnss_emulator: + dt: 0.1 # 10Hz + #position_noise: 0.1 # 10cm noise + #orientation_noise: 0.04 # 2.3 degrees noise + #velocity_noise: + # constant: 0.04 # 4cm/s noise + # linear: 0.02 # 2% noise diff --git a/GEMstack/onboard/planning/mpc.py b/GEMstack/onboard/planning/mpc.py new file mode 100644 index 000000000..fe511bbfa --- /dev/null +++ b/GEMstack/onboard/planning/mpc.py @@ -0,0 +1,162 @@ +from ...utils import settings +from ...state.vehicle import VehicleState,ObjectFrameEnum +from ...state.trajectory import Trajectory, Path +from ...knowledge.vehicle.geometry import front2steer +from ..component import Component +import numpy as np +import casadi + +class MPCController(object): + """Model Predictive Controller for trajectory tracking.""" + def __init__(self, T=None, dt=None): + self.T = T if T is not None else settings.get('control.mpc.horizon', 10) + self.dt = dt if dt is not None else settings.get('control.mpc.dt', 0.1) + self.L = settings.get('vehicle.geometry.wheelbase') + self.v_bounds = [-settings.get('vehicle.limits.max_reverse_speed'), settings.get('vehicle.limits.max_speed')] + self.delta_bounds = [settings.get('vehicle.geometry.min_wheel_angle'),settings.get('vehicle.geometry.max_wheel_angle')] + self.steering_angle_range = [settings.get('vehicle.geometry.min_steering_angle'),settings.get('vehicle.geometry.max_steering_angle')] + self.a_bounds = [-settings.get('vehicle.limits.max_deceleration'), settings.get('vehicle.limits.max_acceleration')] + self.trajectory = None + self.prev_x = None # Previous state trajectory + self.prev_u = None # Previous control inputs + self.path = None + self.path_arg = None + + # def set_path(self, trajectory: Trajectory): + # """Set the trajectory for the MPC controller.""" + # # Assume the argument can only be trajectory + # assert isinstance(trajectory, Trajectory), "Invalid trajectory type." + # print("*"*10) + # print("set_path") + # self.trajectory = trajectory + + def set_path(self, path : Path): + print("Get new path!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!") + if path == self.path_arg: + return + self.path_arg = path + if len(path.points[0]) > 2: + path = path.get_dims([0,1]) + self.path = path.arc_length_parameterize() + self.trajectory = self.path + self.current_traj_parameter = self.trajectory.domain()[0] + self.current_path_parameter = 0.0 + + def compute(self, state: VehicleState, component: Component = None): + """Compute the control commands using MPC.""" + + if self.trajectory is not None: + if self.trajectory.frame != state.pose.frame: + print("Transforming trajectory from",self.trajectory.frame.name,"to",state.pose.frame.name) + self.trajectory = self.trajectory.to_frame(state.pose.frame, current_pose=state.pose) + + x0 = np.array([state.pose.x, state.pose.y, state.pose.yaw, state.v]) + + # Interpolate trajectory points to match MPC time horizon + times = self.trajectory.times + points = self.trajectory.points + traj_points = [] + j = 0 + for i in range(self.T + 1): + t_query = times[0] + i * self.dt + if t_query <= times[0]: + traj_points.append(points[0]) + elif t_query >= times[-1]: + traj_points.append(points[-1]) + else: + while j < len(times) - 2 and times[j+1] < t_query: + j += 1 + alpha = (t_query - times[j]) / (times[j+1] - times[j]) + pt = (1 - alpha) * np.array(points[j]) + alpha * np.array(points[j+1]) + traj_points.append(pt) + + # Optimization setup + opti = casadi.Opti() + x = opti.variable(self.T+1, 4) # [x, y, theta, v] + u = opti.variable(self.T, 2) # [a, delta] + + def model(x, u): + """Dynamic model of the vehicle using kinematic bicycle model""" + px, py, theta, v = x[0], x[1], x[2], x[3] + a, delta = u[0], u[1] + beta = casadi.atan(casadi.tan(delta) / 2.0) + dx = v * casadi.cos(theta + beta) + dy = v * casadi.sin(theta + beta) + dtheta = v * casadi.tan(delta) / self.L + dv = a + return casadi.vertcat(dx, dy, dtheta, dv) + + obj = 0 + for t in range(self.T): + # Vehicle dynamics + x_next = x[t,:] + self.dt * model(x[t,:], u[t,:]).T + opti.subject_to(x[t+1,:] == x_next) + target = traj_points[t+1] + # Cost function + obj += casadi.sumsqr(x[t+1,0:2] - casadi.reshape(target[0:2], 1, 2)) + obj += 0.1 * casadi.sumsqr(u[t,:]) + + # Initial condition + opti.subject_to(x[0, :] == casadi.reshape(x0[:4], 1, 4)) + + # Constraints + for t in range(self.T): + opti.subject_to(opti.bounded(self.a_bounds[0], u[t,0], self.a_bounds[1])) + opti.subject_to(opti.bounded(self.delta_bounds[0], u[t,1], self.delta_bounds[1])) + opti.subject_to(opti.bounded(self.v_bounds[0], x[t+1,3], self.v_bounds[1])) + + # Initial guess + if self.prev_x is not None and self.prev_u is not None: + if len(self.prev_x) == self.T+1 and len(self.prev_u) == self.T: + opti.set_initial(x, np.vstack((self.prev_x[1:], self.prev_x[-1]))) + opti.set_initial(u, np.vstack((self.prev_u[1:], self.prev_u[-1]))) + + # Solver settings + opti.minimize(obj) + opti.solver("ipopt", {"expand": True}, {"max_iter": 100}) + + try: + # Solve the optimization problem + sol = opti.solve() + acc = float(sol.value(u[0,0])) + delta = float(sol.value(u[0,1])) + self.prev_x = sol.value(x) + self.prev_u = sol.value(u) + print(acc, delta) + return acc, delta + except RuntimeError: + # Handle optimization failure + print("MPC optimization failed.") + return 0.0, 0.0 + + +class MPCTrajectoryTracker(Component): + def __init__(self, vehicle_interface=None, **args): + self.mpc = MPCController(**args) + self.vehicle_interface = vehicle_interface + print(type(vehicle_interface)) + + def rate(self): + return 5.0 + + def state_inputs(self): + return ['vehicle', 'trajectory'] + + def state_outputs(self): + return [] + + def update(self, vehicle: VehicleState, trajectory: Trajectory): + self.mpc.set_path(trajectory) + accel, delta = self.mpc.compute(vehicle, self) + + # Clip acceleration and steering angle to vehicle limits + accel = np.clip(accel, self.mpc.a_bounds[0], self.mpc.a_bounds[1]) + delta = np.clip(delta, self.mpc.delta_bounds[0], self.mpc.delta_bounds[1]) + + # Convert delta to steering angle + steering_angle = np.clip(front2steer(delta), + self.mpc.steering_angle_range[0], self.mpc.steering_angle_range[1]) + self.vehicle_interface.send_command(self.vehicle_interface.simple_command(accel, steering_angle, vehicle)) + + def healthy(self): + return self.mpc.path is not None \ No newline at end of file diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley.py index 8f871f488..19efbfbf4 100644 --- a/GEMstack/onboard/planning/stanley.py +++ b/GEMstack/onboard/planning/stanley.py @@ -1,312 +1,312 @@ -import numpy as np -from math import sin, cos, atan2 - -# These imports match your existing project structure -from ...mathutils.control import PID -from ...utils import settings -from ...mathutils import transforms -from ...knowledge.vehicle.geometry import front2steer -from ...knowledge.vehicle.dynamics import acceleration_to_pedal_positions -from ...state.vehicle import VehicleState, ObjectFrameEnum -from ...state.trajectory import Path, Trajectory -from ..interface.gem import GEMVehicleCommand -from ..component import Component - -##################################### -# 1. Angle normalization -##################################### -def normalise_angle(angle): - """Wraps an angle to the [-pi, pi] range.""" - while angle > np.pi: - angle -= 2.0 * np.pi - while angle < -np.pi: - angle += 2.0 * np.pi - return angle - -##################################### -# 2. Stanley-based controller with longitudinal PID -##################################### -class Stanley(object): - """ - A Stanley controller that handles lateral control (steering) - plus a basic longitudinal control (PID + optional feedforward). - It has been modified to reduce oscillations by: - 1) Lower gains - 2) Steering damping - 3) Low-pass filter on steering - 4) Gentler speed logic when cornering - """ - - def __init__( - self, - control_gain=None, - softening_gain=None, - desired_speed=None - ): - """ - :param control_gain: Stanley lateral control gain k (lowered from the default to reduce overshoot). - :param softening_gain: Softening gain k_soft. - :param yaw_rate_gain: Yaw-rate gain k_yaw_rate (helps reduce oscillations). - :param steering_damp_gain: Steering damping gain k_damp_steer. - :param desired_speed: Desired speed (float) or 'path'/'trajectory'. - """ - - # 1) Lower Gains - self.k = control_gain if control_gain is not None else settings.get('control.stanley.control_gain') - self.k_soft = softening_gain if softening_gain is not None else settings.get('control.stanley.softening_gain') - - # Typically, this is the max front-wheel steering angle in radians - self.max_steer = settings.get('vehicle.geometry.max_wheel_angle') - self.wheelbase = settings.get('vehicle.geometry.wheelbase') - - # Basic longitudinal constraints - self.speed_limit = settings.get('vehicle.limits.max_speed') - self.max_accel = settings.get('vehicle.limits.max_acceleration') - self.max_decel = settings.get('vehicle.limits.max_deceleration') - - # PID for longitudinal speed tracking - p = settings.get('control.longitudinal_control.pid_p') - d = settings.get('control.longitudinal_control.pid_d') - i = settings.get('control.longitudinal_control.pid_i') - self.pid_speed = PID(p, d, i, windup_limit=20) - - # Speed source: numeric or derived from path/trajectory - if desired_speed is not None: - self.desired_speed_source = desired_speed - else: - self.desired_speed_source = settings.get('control.stanley.desired_speed', 'path') - - if isinstance(self.desired_speed_source, (int, float)): - self.desired_speed = self.desired_speed_source - else: - self.desired_speed = None - - # For path/trajectory - self.path_arg = None - self.path = None - self.trajectory = None - self.current_path_parameter = 0.0 - self.current_traj_parameter = 0.0 - self.t_last = None - - def set_path(self, path: Path): - """Sets the path or trajectory to track.""" - if path == self.path_arg: - return - self.path_arg = path - - # If the path has more than 2D, reduce to (x,y) - if len(path.points[0]) > 2: - path = path.get_dims([0,1]) - - # If no timing info, we can't rely on 'path'/'trajectory' speed - if not isinstance(path, Trajectory): - self.path = path.arc_length_parameterize() - self.trajectory = None - self.current_traj_parameter = 0.0 - if self.desired_speed_source in ['path', 'trajectory']: - raise ValueError("Stanley: Provided path has no timing. Either provide a Trajectory or set a constant desired_speed.") - else: - self.path = path.arc_length_parameterize() - self.trajectory = path - self.current_traj_parameter = self.trajectory.domain()[0] - - self.current_path_parameter = 0.0 - - def _find_front_axle_position(self, x, y, yaw): - """Compute front-axle world position from the center/rear and yaw.""" - fx = x + self.wheelbase * cos(yaw) - fy = y + self.wheelbase * sin(yaw) - return fx, fy - - def compute(self, state: VehicleState, component: Component = None): - """Compute the control outputs: (longitudinal acceleration, front wheel angle).""" - t = state.pose.t - if self.t_last is None: - self.t_last = t - dt = t - self.t_last - - # Current vehicle states - curr_x = state.pose.x - curr_y = state.pose.y - curr_yaw = state.pose.yaw if state.pose.yaw is not None else 0.0 - speed = state.v - - if self.path is None: - if component: - component.debug_event("No path provided to Stanley controller. Doing nothing.") - return (0.0, 0.0) - - # Ensure same frame - if self.path.frame != state.pose.frame: - if component: - component.debug_event(f"Transforming path from {self.path.frame.name} to {state.pose.frame.name}") - self.path = self.path.to_frame(state.pose.frame, current_pose=state.pose) - - if self.trajectory and self.trajectory.frame != state.pose.frame: - if component: - component.debug_event(f"Transforming trajectory from {self.trajectory.frame.name} to {state.pose.frame.name}") - self.trajectory = self.trajectory.to_frame(state.pose.frame, current_pose=state.pose) - - # 1) Closest point - fx, fy = self._find_front_axle_position(curr_x, curr_y, curr_yaw) - search_start = self.current_path_parameter - 5.0 - search_end = self.current_path_parameter + 5.0 - closest_dist, closest_parameter = self.path.closest_point_local((fx, fy), [search_start, search_end]) - self.current_path_parameter = closest_parameter - - # 2) Path heading - target_x, target_y = self.path.eval(closest_parameter) - tangent = self.path.eval_tangent(closest_parameter) - path_yaw = atan2(tangent[1], tangent[0]) - desired_x = target_x - desired_y = target_y - # 3) Lateral error - dx = fx - target_x - dy = fy - target_y - left_vec = np.array([sin(curr_yaw), -cos(curr_yaw)]) - cross_track_error = np.sign(np.dot(np.array([dx, dy]), left_vec)) * closest_dist - - # 4) Heading error - yaw_error = normalise_angle(path_yaw - curr_yaw) - - # 5) Standard Stanley terms - cross_term = atan2(self.k * cross_track_error, self.k_soft + speed) - desired_steering_angle = yaw_error + cross_term - - desired_speed = self.desired_speed - feedforward_accel = 0.0 - - if self.trajectory and self.desired_speed_source in ['path', 'trajectory']: - if len(self.trajectory.points) < 2 or self.current_path_parameter >= self.path.domain()[1]: - # End of trajectory -> stop - if component: - component.debug_event("Stanley: Past the end of trajectory, stopping.") - desired_speed = 0.0 - feedforward_accel = -2.0 - else: - if self.desired_speed_source == 'path': - current_trajectory_time = self.trajectory.parameter_to_time(self.current_path_parameter) - else: - self.current_traj_parameter += dt - current_trajectory_time = self.current_traj_parameter - - deriv = self.trajectory.eval_derivative(current_trajectory_time) - desired_speed = min(np.linalg.norm(deriv), self.speed_limit) - - difference_dt = 0.1 - future_t = current_trajectory_time + difference_dt - if future_t > self.trajectory.domain()[1]: - future_t = self.trajectory.domain()[1] - future_deriv = self.trajectory.eval_derivative(future_t) - next_desired_speed = min(np.linalg.norm(future_deriv), self.speed_limit) - feedforward_accel = (next_desired_speed - desired_speed) / difference_dt - feedforward_accel = np.clip(feedforward_accel, -self.max_decel, self.max_accel) - else: - if desired_speed is None: - desired_speed = 4.0 - - # Cross-track-based slowdown (less aggressive than before). - desired_speed *= np.exp(-abs(cross_track_error) * 0.6) - - # Clip to speed limit - if desired_speed > self.speed_limit: - desired_speed = self.speed_limit - - # PID for longitudinal control - speed_error = desired_speed - speed - output_accel = self.pid_speed.advance(e=speed_error, t=t, feedforward_term=feedforward_accel) - - # Clip acceleration - if output_accel > self.max_accel: - output_accel = self.max_accel - elif output_accel < -self.max_decel: - output_accel = -self.max_decel - - # Avoid negative accel when fully stopped - if desired_speed == 0 and abs(speed) < 0.01 and output_accel < 0: - output_accel = 0.0 - - # Debug - if component is not None: - # component.debug("Stanley: fx, fy", (fx, fy)) - component.debug('curr pt',(curr_x,curr_y)) - component.debug("desired_x",desired_x) - component.debug("desired_y",desired_y) - component.debug("Stanley: path param", self.current_path_parameter) - component.debug("Stanley: crosstrack dist", closest_dist) - component.debug("crosstrack error", cross_track_error) - component.debug("Stanley: yaw_error", yaw_error) - component.debug('steering_angle', desired_steering_angle) - component.debug("Stanley: desired_speed (m/s)", desired_speed) - component.debug("Stanley: feedforward_accel (m/s^2)", feedforward_accel) - component.debug("Stanley: output_accel (m/s^2)", output_accel) - - if output_accel > self.max_accel: - output_accel = self.max_accel - - if output_accel < -self.max_decel: - output_accel = -self.max_decel - - self.t_last = t - return (output_accel, desired_steering_angle) - -##################################### -# 3. Tracker component -##################################### -class StanleyTrajectoryTracker(Component): - """ - A trajectory-tracking Component that uses the above Stanley controller - for lateral control plus PID-based longitudinal control. - It now includes measures to mitigate oscillations. - """ - - def __init__(self, vehicle_interface=None, **kwargs): - """ - :param vehicle_interface: The low-level interface to send commands to the vehicle. - :param kwargs: Optional parameters to pass into the Stanley(...) constructor. - """ - self.stanley = Stanley(**kwargs) - self.vehicle_interface = vehicle_interface - - def rate(self): - """Control frequency in Hz.""" - return 50.0 - - def state_inputs(self): - """ - Required state inputs: - - Vehicle state - - Trajectory - """ - return ["vehicle", "trajectory"] - - def state_outputs(self): - """No direct output state here.""" - return [] - - def update(self, vehicle: VehicleState, trajectory: Trajectory): - """ - Per control cycle: - 1) Set the path/trajectory - 2) Compute (acceleration, front wheel angle) - 3) Convert front wheel angle to steering wheel angle (if necessary) - 4) Send command to the vehicle - """ - self.stanley.set_path(trajectory) - accel, f_delta = self.stanley.compute(vehicle, self) - - # If your low-level interface expects steering wheel angle: - steering_angle = front2steer(f_delta) - steering_angle = np.clip( - steering_angle, - settings.get('vehicle.geometry.min_steering_angle', -0.5), - settings.get('vehicle.geometry.max_steering_angle', 0.5) - ) - - cmd = self.vehicle_interface.simple_command(accel, steering_angle, vehicle) - self.vehicle_interface.send_command(cmd) - - def healthy(self): - """Optional: check if the controller has a valid path.""" - return self.stanley.path is not None +import numpy as np +from math import sin, cos, atan2 + +# These imports match your existing project structure +from ...mathutils.control import PID +from ...utils import settings +from ...mathutils import transforms +from ...knowledge.vehicle.geometry import front2steer +from ...knowledge.vehicle.dynamics import acceleration_to_pedal_positions +from ...state.vehicle import VehicleState, ObjectFrameEnum +from ...state.trajectory import Path, Trajectory +from ..interface.gem import GEMVehicleCommand +from ..component import Component + +##################################### +# 1. Angle normalization +##################################### +def normalise_angle(angle): + """Wraps an angle to the [-pi, pi] range.""" + while angle > np.pi: + angle -= 2.0 * np.pi + while angle < -np.pi: + angle += 2.0 * np.pi + return angle + +##################################### +# 2. Stanley-based controller with longitudinal PID +##################################### +class Stanley(object): + """ + A Stanley controller that handles lateral control (steering) + plus a basic longitudinal control (PID + optional feedforward). + It has been modified to reduce oscillations by: + 1) Lower gains + 2) Steering damping + 3) Low-pass filter on steering + 4) Gentler speed logic when cornering + """ + + def __init__( + self, + control_gain=None, + softening_gain=None, + desired_speed=None + ): + """ + :param control_gain: Stanley lateral control gain k (lowered from the default to reduce overshoot). + :param softening_gain: Softening gain k_soft. + :param yaw_rate_gain: Yaw-rate gain k_yaw_rate (helps reduce oscillations). + :param steering_damp_gain: Steering damping gain k_damp_steer. + :param desired_speed: Desired speed (float) or 'path'/'trajectory'. + """ + + # 1) Lower Gains + self.k = control_gain if control_gain is not None else settings.get('control.stanley.control_gain') + self.k_soft = softening_gain if softening_gain is not None else settings.get('control.stanley.softening_gain') + + # Typically, this is the max front-wheel steering angle in radians + self.max_steer = settings.get('vehicle.geometry.max_wheel_angle') + self.wheelbase = settings.get('vehicle.geometry.wheelbase') + + # Basic longitudinal constraints + self.speed_limit = settings.get('vehicle.limits.max_speed') + self.max_accel = settings.get('vehicle.limits.max_acceleration') + self.max_decel = settings.get('vehicle.limits.max_deceleration') + + # PID for longitudinal speed tracking + p = settings.get('control.longitudinal_control.pid_p') + d = settings.get('control.longitudinal_control.pid_d') + i = settings.get('control.longitudinal_control.pid_i') + self.pid_speed = PID(p, d, i, windup_limit=20) + + # Speed source: numeric or derived from path/trajectory + if desired_speed is not None: + self.desired_speed_source = desired_speed + else: + self.desired_speed_source = settings.get('control.stanley.desired_speed', 'path') + + if isinstance(self.desired_speed_source, (int, float)): + self.desired_speed = self.desired_speed_source + else: + self.desired_speed = None + + # For path/trajectory + self.path_arg = None + self.path = None + self.trajectory = None + self.current_path_parameter = 0.0 + self.current_traj_parameter = 0.0 + self.t_last = None + + def set_path(self, path: Path): + """Sets the path or trajectory to track.""" + if path == self.path_arg: + return + self.path_arg = path + + # If the path has more than 2D, reduce to (x,y) + if len(path.points[0]) > 2: + path = path.get_dims([0,1]) + + # If no timing info, we can't rely on 'path'/'trajectory' speed + if not isinstance(path, Trajectory): + self.path = path.arc_length_parameterize() + self.trajectory = None + self.current_traj_parameter = 0.0 + if self.desired_speed_source in ['path', 'trajectory']: + raise ValueError("Stanley: Provided path has no timing. Either provide a Trajectory or set a constant desired_speed.") + else: + self.path = path.arc_length_parameterize() + self.trajectory = path + self.current_traj_parameter = self.trajectory.domain()[0] + + self.current_path_parameter = 0.0 + + def _find_front_axle_position(self, x, y, yaw): + """Compute front-axle world position from the center/rear and yaw.""" + fx = x + self.wheelbase * cos(yaw) + fy = y + self.wheelbase * sin(yaw) + return fx, fy + + def compute(self, state: VehicleState, component: Component = None): + """Compute the control outputs: (longitudinal acceleration, front wheel angle).""" + t = state.pose.t + if self.t_last is None: + self.t_last = t + dt = t - self.t_last + + # Current vehicle states + curr_x = state.pose.x + curr_y = state.pose.y + curr_yaw = state.pose.yaw if state.pose.yaw is not None else 0.0 + speed = state.v + + if self.path is None: + if component: + component.debug_event("No path provided to Stanley controller. Doing nothing.") + return (0.0, 0.0) + + # Ensure same frame + if self.path.frame != state.pose.frame: + if component: + component.debug_event(f"Transforming path from {self.path.frame.name} to {state.pose.frame.name}") + self.path = self.path.to_frame(state.pose.frame, current_pose=state.pose) + + if self.trajectory and self.trajectory.frame != state.pose.frame: + if component: + component.debug_event(f"Transforming trajectory from {self.trajectory.frame.name} to {state.pose.frame.name}") + self.trajectory = self.trajectory.to_frame(state.pose.frame, current_pose=state.pose) + + # 1) Closest point + fx, fy = self._find_front_axle_position(curr_x, curr_y, curr_yaw) + search_start = self.current_path_parameter - 5.0 + search_end = self.current_path_parameter + 5.0 + closest_dist, closest_parameter = self.path.closest_point_local((fx, fy), [search_start, search_end]) + self.current_path_parameter = closest_parameter + + # 2) Path heading + target_x, target_y = self.path.eval(closest_parameter) + tangent = self.path.eval_tangent(closest_parameter) + path_yaw = atan2(tangent[1], tangent[0]) + desired_x = target_x + desired_y = target_y + # 3) Lateral error + dx = fx - target_x + dy = fy - target_y + left_vec = np.array([sin(curr_yaw), -cos(curr_yaw)]) + cross_track_error = np.sign(np.dot(np.array([dx, dy]), left_vec)) * closest_dist + + # 4) Heading error + yaw_error = normalise_angle(path_yaw - curr_yaw) + + # 5) Standard Stanley terms + cross_term = atan2(self.k * cross_track_error, self.k_soft + speed) + desired_steering_angle = yaw_error + cross_term + + desired_speed = self.desired_speed + feedforward_accel = 0.0 + + if self.trajectory and self.desired_speed_source in ['path', 'trajectory']: + if len(self.trajectory.points) < 2 or self.current_path_parameter >= self.path.domain()[1]: + # End of trajectory -> stop + if component: + component.debug_event("Stanley: Past the end of trajectory, stopping.") + desired_speed = 0.0 + feedforward_accel = -2.0 + else: + if self.desired_speed_source == 'path': + current_trajectory_time = self.trajectory.parameter_to_time(self.current_path_parameter) + else: + self.current_traj_parameter += dt + current_trajectory_time = self.current_traj_parameter + + deriv = self.trajectory.eval_derivative(current_trajectory_time) + desired_speed = min(np.linalg.norm(deriv), self.speed_limit) + + difference_dt = 0.1 + future_t = current_trajectory_time + difference_dt + if future_t > self.trajectory.domain()[1]: + future_t = self.trajectory.domain()[1] + future_deriv = self.trajectory.eval_derivative(future_t) + next_desired_speed = min(np.linalg.norm(future_deriv), self.speed_limit) + feedforward_accel = (next_desired_speed - desired_speed) / difference_dt + feedforward_accel = np.clip(feedforward_accel, -self.max_decel, self.max_accel) + else: + if desired_speed is None: + desired_speed = 4.0 + + # Cross-track-based slowdown (less aggressive than before). + desired_speed *= np.exp(-abs(cross_track_error) * 0.6) + + # Clip to speed limit + if desired_speed > self.speed_limit: + desired_speed = self.speed_limit + + # PID for longitudinal control + speed_error = desired_speed - speed + output_accel = self.pid_speed.advance(e=speed_error, t=t, feedforward_term=feedforward_accel) + + # Clip acceleration + if output_accel > self.max_accel: + output_accel = self.max_accel + elif output_accel < -self.max_decel: + output_accel = -self.max_decel + + # Avoid negative accel when fully stopped + if desired_speed == 0 and abs(speed) < 0.01 and output_accel < 0: + output_accel = 0.0 + + # Debug + if component is not None: + # component.debug("Stanley: fx, fy", (fx, fy)) + component.debug('curr pt',(curr_x,curr_y)) + component.debug("desired_x",desired_x) + component.debug("desired_y",desired_y) + component.debug("Stanley: path param", self.current_path_parameter) + component.debug("Stanley: crosstrack dist", closest_dist) + component.debug("crosstrack error", cross_track_error) + component.debug("Stanley: yaw_error", yaw_error) + component.debug('steering_angle', desired_steering_angle) + component.debug("Stanley: desired_speed (m/s)", desired_speed) + component.debug("Stanley: feedforward_accel (m/s^2)", feedforward_accel) + component.debug("Stanley: output_accel (m/s^2)", output_accel) + + if output_accel > self.max_accel: + output_accel = self.max_accel + + if output_accel < -self.max_decel: + output_accel = -self.max_decel + + self.t_last = t + return (output_accel, desired_steering_angle) + +##################################### +# 3. Tracker component +##################################### +class StanleyTrajectoryTracker(Component): + """ + A trajectory-tracking Component that uses the above Stanley controller + for lateral control plus PID-based longitudinal control. + It now includes measures to mitigate oscillations. + """ + + def __init__(self, vehicle_interface=None, **kwargs): + """ + :param vehicle_interface: The low-level interface to send commands to the vehicle. + :param kwargs: Optional parameters to pass into the Stanley(...) constructor. + """ + self.stanley = Stanley(**kwargs) + self.vehicle_interface = vehicle_interface + + def rate(self): + """Control frequency in Hz.""" + return 50.0 + + def state_inputs(self): + """ + Required state inputs: + - Vehicle state + - Trajectory + """ + return ["vehicle", "trajectory"] + + def state_outputs(self): + """No direct output state here.""" + return [] + + def update(self, vehicle: VehicleState, trajectory: Trajectory): + """ + Per control cycle: + 1) Set the path/trajectory + 2) Compute (acceleration, front wheel angle) + 3) Convert front wheel angle to steering wheel angle (if necessary) + 4) Send command to the vehicle + """ + self.stanley.set_path(trajectory) + accel, f_delta = self.stanley.compute(vehicle, self) + + # If your low-level interface expects steering wheel angle: + steering_angle = front2steer(f_delta) + steering_angle = np.clip( + steering_angle, + settings.get('vehicle.geometry.min_steering_angle', -0.5), + settings.get('vehicle.geometry.max_steering_angle', 0.5) + ) + + cmd = self.vehicle_interface.simple_command(accel, steering_angle, vehicle) + self.vehicle_interface.send_command(cmd) + + def healthy(self): + """Optional: check if the controller has a valid path.""" + return self.stanley.path is not None diff --git a/GEMstack/utils/klampt_visualization.py b/GEMstack/utils/klampt_visualization.py index 063f690c7..188c8d4f9 100644 --- a/GEMstack/utils/klampt_visualization.py +++ b/GEMstack/utils/klampt_visualization.py @@ -1,299 +1,299 @@ -from klampt import vis -from klampt.math import vectorops,so3,se3 -from klampt.model.trajectory import Trajectory -from klampt import Geometry3D, GeometricPrimitive, TriangleMesh -import numpy as np -from . import settings -from ..state import ObjectFrameEnum,ObjectPose,PhysicalObject,VehicleState,VehicleGearEnum,Path,Obstacle,AgentState,AgentEnum,Roadgraph,RoadgraphLane,RoadgraphLaneEnum,RoadgraphCurve,RoadgraphCurveEnum,RoadgraphRegion,RoadgraphRegionEnum,RoadgraphSurfaceEnum,Trajectory,Route,SceneState,AllState - -#KH: there is a bug on some system where the visualization crashes with an OpenGL error when drawing curves -#this is a workaround. We really should find the source of the bug! -MAX_POINTS_IN_CURVE = 50 - -OBJECT_COLORS = { - AgentEnum.CAR : (1,1,0,1), - AgentEnum.PEDESTRIAN : (0,1,0,1), - AgentEnum.BICYCLIST : (0,0,1,1), - AgentEnum.MEDIUM_TRUCK : (1,0,1,1), - AgentEnum.LARGE_TRUCK : (0,1,1,1), - None: (0.7,0.7,0.7,1), -} - -AUX_BBOX_COLOR = (0,0,1,1) - -CURVE_TO_STYLE = { - RoadgraphCurveEnum.LANE_BOUNDARY : {'color':(1,1,1,1),'width':2,'pointSize':0}, - RoadgraphCurveEnum.CURB : {'color':(0.5,0.5,0.5,1),'width':2,'pointSize':0}, - RoadgraphCurveEnum.CLIFF : {'color':(1,0,0,1),'width':2,'pointSize':0}, - RoadgraphCurveEnum.CROSSING_BOUNDARY : {'color':(1,1,1,1),'width':1,'pointSize':0}, - RoadgraphCurveEnum.PARKING_SPOT_BOUNDARY : {'color':(1,1,1,1),'width':1,'pointSize':0}, - RoadgraphCurveEnum.STOP_LINE : {'color':(1,1,1,1),'width':2,'pointSize':0}, - RoadgraphCurveEnum.WALL : {'color':(0,0,1,1),'width':2,'pointSize':0}, - None : {'color':(1,1,1,1),'width':1,'pointSize':0}, -} - -SURFACE_TO_STYLE = { - RoadgraphSurfaceEnum.PAVEMENT : {'color':(0.5,0.5,0.5,0.2),'pointSize':0}, - RoadgraphSurfaceEnum.DIRT : {'color':(160/255.0,82/255.0,45/255.0,0.2),'pointSize':0}, - RoadgraphSurfaceEnum.GRASS : {'color':(50/255.0,255/255.0,50/255.0,0.2),'pointSize':0}, - RoadgraphSurfaceEnum.GRAVEL : {'color':(0.7,0.7,0.7,0.2),'pointSize':0}, - None: {'color':(1,0,0,0.2),'pointSize':0}, -} - -REGION_TO_STYLE = { - RoadgraphRegionEnum.INTERSECTION : {'color':(0,1,0,0.5),'width':1,'pointSize':0}, - RoadgraphRegionEnum.PARKING_LOT : {'color':(0,0,1,0.5),'width':1,'pointSize':0}, - RoadgraphRegionEnum.CLOSED_COURSE : {'color':(1,0,0,0.5),'width':1,'pointSize':0}, - RoadgraphRegionEnum.VIRTUAL : {'color':(0,0,0,0.5),'width':1,'pointSize':0}, -} - -def plot_pose(name : str, pose : ObjectPose, axis_len=0.1, label=True): - """Plots the pose in the given axes. The coordinates - of the pose are plotted in the pose's indicated frame.""" - R = pose.rotation() - t = pose.translation() - T = (so3.from_matrix(R),t) - vis.add(name, T, length=axis_len, hide_label=(not label)) - if pose.frame == ObjectFrameEnum.START: - vis.add(name+"_origin",t,color=(0,0,1,1),size=5,hide_label=True) - elif pose.frame == ObjectFrameEnum.CURRENT: - vis.add(name+"_origin",t,color=(1,0,0,1),size=5,hide_label=True) - elif pose.frame in [ObjectFrameEnum.GLOBAL,ObjectFrameEnum.ABSOLUTE_CARTESIAN]: - vis.add(name+"_origin",t,color=(0,1,0,1),size=5,hide_label=True) - else: - raise ValueError("Unknown frame %s" % pose.frame) - -def plot_object(name : str, obj : PhysicalObject, type=None, axis_len=None, outline=True, solid=True, bbox=True, label=True): - """Shows an object in the given axes. - - If axis_len is given, shows the object's pose with - a coordinate frame of the given length. - - If outline is True, shows the object's outline. - - If bbox is True, shows the object's bounding box. - """ - height = obj.dimensions[2] - core_color = OBJECT_COLORS[type] - bbox_color = AUX_BBOX_COLOR - if label: - #add a point at the object's origin - vis.add(name,obj.pose.translation(),size=5,color=(0,0,0,1)) - if axis_len: - plot_pose(name+"_pose", obj.pose, axis_len=0) - #plot bounding box - R = obj.pose.rotation() - t = obj.pose.translation() - if bbox or (outline and obj.outline is None): - bounds = obj.bounds() - (xmin,xmax),(ymin,ymax),(zmin,zmax) = bounds - if not solid: - corners = [[xmin,ymin,0.01],[xmin,ymax,0.01],[xmax,ymax,0.01],[xmax,ymin,0.01]] - corners = [list(R.dot(c)+t) for c in corners] - corners.append(corners[0]) - if not bbox: - vis.add(name+"_bbox1",corners,width=1,color=core_color,pointSize=0,hide_label=True) - else: - vis.add(name+"_bbox1",corners,width=1,color=AUX_BBOX_COLOR,pointSize=0,hide_label=True) - if height > 0: - corners = [[xmin,ymin,height],[xmin,ymax,height],[xmax,ymax,height],[xmax,ymin,height]] - corners = [list(R.dot(c)+t) for c in corners] - corners.append(corners[0]) - if not bbox: - vis.add(name+"_bbox2",corners,width=1,color=core_color,pointSize=0,hide_label=True) - else: - vis.add(name+"_bbox2",corners,width=1,color=AUX_BBOX_COLOR,pointSize=0,hide_label=True) - else: - prim = GeometricPrimitive() - prim.setAABB([xmin,ymin,0],[xmax,ymax,height]) - g = Geometry3D(prim) - g.setCurrentTransform(so3.from_matrix(R),t) - vis.add(name+"_bbox",g,color=core_color[:3]+(0.5,),hide_label=True) - - #plot outline - if outline and obj.outline: - outline = [R.dot((p[0],p[1],0))+t for p in obj.outline] - outline.append(outline[0]) - vis.add(name+"_outline1",outline,width=1,color=core_color,hide_label=True) - if height > 0: - outline = [R.dot((p[0],p[1],height))+t for p in obj.outline] - outline.append(outline[0]) - vis.add(name+"_outline2",outline,width=1,color=core_color,hide_label=True) - - -def plot_vehicle(vehicle : VehicleState, vehicle_model=None, axis_len=1.0): - """Plots the vehicle in the given axes. The coordinates - of the vehicle are plotted in the vehicle's indicated frame.""" - plot_object("vehicle",vehicle.to_object(), type=None, axis_len=0, solid=False, label=False) - R = vehicle.pose.rotation() - t = vehicle.pose.translation() - T = (so3.from_matrix(R),t) - vis.add("vehicle", T, length=axis_len, hide_label=True) - - xbounds,ybounds,zbounds = settings.get('vehicle.geometry.bounds') - #plot velocity arrow - R = vehicle.pose.rotation2d() - t = np.array([vehicle.pose.x,vehicle.pose.y]) - v = R.dot([vehicle.v,0]) - front_pt = vehicle.pose.apply((xbounds[1],0.0)) - h = (vehicle.pose.z if vehicle.pose.z is not None else 0) + zbounds[1] - vis.add("vehicle_velocity",[[front_pt[0],front_pt[1],h],[front_pt[0]+v[0],front_pt[1]+v[1],h]],width=2,color=(0,1,0,1),pointSize=0) - - #plot front wheel angles - wheelbase = settings.get('vehicle.geometry.wheelbase') - wheel_spacing = 0.8*settings.get('vehicle.geometry.width') / 2 - phi = vehicle.front_wheel_angle - if vehicle_model: - q = vehicle_model.getConfig() - lwhindex = vehicle_model.link("left_steering_hinge_link").index - rwhindex = vehicle_model.link("right_steering_hinge_link").index - lwindex = vehicle_model.link("front_left_wheel_link").index - rwindex = vehicle_model.link("front_right_wheel_link").index - rlwindex = vehicle_model.link("rear_left_wheel_link").index - rrwindex = vehicle_model.link("rear_right_wheel_link").index - q[lwhindex] = phi - q[rwhindex] = phi - q[lwindex] += vehicle.v * 0.2 - q[rwindex] += vehicle.v * 0.2 - q[rlwindex] += vehicle.v * 0.2 - q[rrwindex] += vehicle.v * 0.2 - vehicle_model.setConfig(q) - else: - left_wheel_origin = t + R.dot([wheelbase,wheel_spacing]) - right_wheel_origin = t + R.dot([wheelbase,-wheel_spacing]) - wheel_width = 0.5 #meters - wheel_offset = R.dot(np.array([np.cos(phi),np.sin(phi)]))*wheel_width*0.5 - vis.add("left_wheel",[vectorops.sub(left_wheel_origin,wheel_offset),vectorops.add(left_wheel_origin,wheel_offset)],color=(0,0,0,1),width=2) - vis.add("right_wheel",[vectorops.sub(right_wheel_origin,wheel_offset),vectorops.add(right_wheel_origin,wheel_offset)],color=(0,0,0,1),width=2) - - #plot gear - if vehicle.gear in [VehicleGearEnum.NEUTRAL,VehicleGearEnum.REVERSE,VehicleGearEnum.PARK]: - if vehicle.gear == VehicleGearEnum.NEUTRAL: - gear = 'N' - elif vehicle.gear == VehicleGearEnum.REVERSE: - gear = 'R' - else: - gear = 'P' - vis.addText("gear",gear,position=(t[0],t[1],1.5),color=(0,0,0,1)) - - #plot lights - light_point_size = 4 - light_size = 0.15 - light_color = (1,1,0,1) - turn_indicator_height = 0.7 - headlight_height = 0.6 - if vehicle.left_turn_indicator: - lp = vehicle.pose.apply([xbounds[0],ybounds[0]+light_size,turn_indicator_height]) - vis.add("left_turn_indicator",list(lp),size=light_point_size,color=light_color) - if vehicle.right_turn_indicator: - lp = vehicle.pose.apply([xbounds[0],ybounds[1]-light_size,turn_indicator_height]) - vis.add("right_turn_indicator",list(lp),size=light_point_size,color=light_color) - if vehicle.headlights_on: - lp = vehicle.pose.apply([xbounds[1],ybounds[0]+light_size*2,headlight_height]) - vis.add("left_headlight",list(lp),size=light_point_size,color=light_color) - lp = vehicle.pose.apply([xbounds[1],ybounds[1]-light_size*2,headlight_height]) - vis.add("right_headlight",list(lp),size=light_point_size,color=light_color) - if vehicle_model is not None: - if vehicle.brake_pedal_position > 0.1: - vehicle_model.link('rear_right_stop_light_link').appearance().setColor(1,0,0,1) - vehicle_model.link('rear_left_stop_light_link').appearance().setColor(1,0,0,1) - else: - vehicle_model.link('rear_right_stop_light_link').appearance().setColor(0.3,0,0,1) - vehicle_model.link('rear_left_stop_light_link').appearance().setColor(0.3,0,0,1) - -def plot_path(name : str, path : Path, color=(0,0,0), width=1): - if len(path.points) > MAX_POINTS_IN_CURVE: # downsample due to OpenGL error? - vis.add(name,[list(p) for p in path.points[::len(path.points)//MAX_POINTS_IN_CURVE]],color=color,width=width) - else: - vis.add(name,[list(p) for p in path.points],color=color,width=width) - -def plot_curve(name : str, curve : RoadgraphCurve, color=None, width=None): - style = CURVE_TO_STYLE.get(curve.type,CURVE_TO_STYLE[None]) - if curve.crossable and curve.type == RoadgraphCurveEnum.LANE_BOUNDARY: - #style['linestyle'] = '--' - #TODO: how to indicate crossable lines? - pass - if color is not None: - style['color'] = color - if width is not None: - style['width'] = width - for i,seg in enumerate(curve.segments): - if len(seg) > MAX_POINTS_IN_CURVE: # downsample due to OpenGL error? - vis.add(name+"_%d" % i,seg[::len(seg)//MAX_POINTS_IN_CURVE],**style) - else: - vis.add(name+"_%d" % i,seg,**style) - -def plot_lane(name : str, lane : RoadgraphLane, on_route=False): - if lane.surface != RoadgraphSurfaceEnum.PAVEMENT: - style = SURFACE_TO_STYLE.get(lane.surface,SURFACE_TO_STYLE[None]) - outline = lane.outline() - vis.add(name, outline, **style) - if lane.left is not None: - plot_curve(name+"_left", lane.left) - if lane.right is not None: - plot_curve(name+"_right", lane.right) - -def plot_region(name : str, region : RoadgraphRegion, color=None, width=None): - style = REGION_TO_STYLE.get(region.type,REGION_TO_STYLE[None]) - points = region.outline() - pts = points + points[0] - if color is not None: - style['color'] = color - if width is not None: - style['width'] = width - vis.add(name, [list(p) for p in pts], **style) - -def plot_roadgraph(roadgraph : Roadgraph, route : Route = None): - #plot lanes - for k,l in roadgraph.lanes.items(): - if route is not None and k in route.lanes: - plot_lane(k,l,on_route=True) - else: - plot_lane(k,l) - for k,c in roadgraph.curves.items(): - plot_curve(k,c,color=(0,0,0,1)) - #plot intersections - for k,r in roadgraph.regions.items(): - plot_region(k,r) - #plot - for k,o in roadgraph.static_obstacles.items(): - plot_object(k,o) - -def plot_scene(scene : SceneState, ground_truth_vehicle=None, vehicle_model = None, title = None, show=True): - for i in list(vis.scene().items.keys()): - if not i.startswith("vehicle"): - if not isinstance(vis.scene().items[i],vis.VisPlot): - vis.remove(i) - #set plot range - #TODO - if vehicle_model is not None: - vis.add("vehicle_model",vehicle_model) - if ground_truth_vehicle is not None: - xform = ground_truth_vehicle.to_object().pose.transform() - else: - xform = scene.vehicle.to_object().pose.transform() - vehicle_model.link(0).setParentTransform(*se3.from_ndarray(xform)) - vehicle_model.setConfig(vehicle_model.getConfig()) - - #plot roadgraph - plot_roadgraph(scene.roadgraph,scene.route) - #plot vehicle and objects - plot_vehicle(scene.vehicle, vehicle_model) - for k,a in scene.agents.items(): - plot_object(k,a,type=a.type) - for k,o in scene.obstacles.items(): - plot_object(k,o) - if title is None: - if show: - vis.add("title","Scene at t = %.2f" % scene.t, position=(10,10)) - else: - vis.add("title",title) - if show: - vis.show() - -def plot(state : AllState, ground_truth_vehicle = None, vehicle_model=None, title=None, show=True): - plot_scene(state, ground_truth_vehicle=ground_truth_vehicle, vehicle_model=vehicle_model, title=title, show=show) - if state.route is not None: - plot_path("route",state.route,color=(1,0.5,0,1),width=2) - if state.trajectory is not None: - plot_path("trajectory",state.trajectory,color=(1,0,0,1),width=3) +from klampt import vis +from klampt.math import vectorops,so3,se3 +from klampt.model.trajectory import Trajectory +from klampt import Geometry3D, GeometricPrimitive, TriangleMesh +import numpy as np +from . import settings +from ..state import ObjectFrameEnum,ObjectPose,PhysicalObject,VehicleState,VehicleGearEnum,Path,Obstacle,AgentState,AgentEnum,Roadgraph,RoadgraphLane,RoadgraphLaneEnum,RoadgraphCurve,RoadgraphCurveEnum,RoadgraphRegion,RoadgraphRegionEnum,RoadgraphSurfaceEnum,Trajectory,Route,SceneState,AllState + +#KH: there is a bug on some system where the visualization crashes with an OpenGL error when drawing curves +#this is a workaround. We really should find the source of the bug! +MAX_POINTS_IN_CURVE = 50 + +OBJECT_COLORS = { + AgentEnum.CAR : (1,1,0,1), + AgentEnum.PEDESTRIAN : (0,1,0,1), + AgentEnum.BICYCLIST : (0,0,1,1), + AgentEnum.MEDIUM_TRUCK : (1,0,1,1), + AgentEnum.LARGE_TRUCK : (0,1,1,1), + None: (0.7,0.7,0.7,1), +} + +AUX_BBOX_COLOR = (0,0,1,1) + +CURVE_TO_STYLE = { + RoadgraphCurveEnum.LANE_BOUNDARY : {'color':(1,1,1,1),'width':2,'pointSize':0}, + RoadgraphCurveEnum.CURB : {'color':(0.5,0.5,0.5,1),'width':2,'pointSize':0}, + RoadgraphCurveEnum.CLIFF : {'color':(1,0,0,1),'width':2,'pointSize':0}, + RoadgraphCurveEnum.CROSSING_BOUNDARY : {'color':(1,1,1,1),'width':1,'pointSize':0}, + RoadgraphCurveEnum.PARKING_SPOT_BOUNDARY : {'color':(1,1,1,1),'width':1,'pointSize':0}, + RoadgraphCurveEnum.STOP_LINE : {'color':(1,1,1,1),'width':2,'pointSize':0}, + RoadgraphCurveEnum.WALL : {'color':(0,0,1,1),'width':2,'pointSize':0}, + None : {'color':(1,1,1,1),'width':1,'pointSize':0}, +} + +SURFACE_TO_STYLE = { + RoadgraphSurfaceEnum.PAVEMENT : {'color':(0.5,0.5,0.5,0.2),'pointSize':0}, + RoadgraphSurfaceEnum.DIRT : {'color':(160/255.0,82/255.0,45/255.0,0.2),'pointSize':0}, + RoadgraphSurfaceEnum.GRASS : {'color':(50/255.0,255/255.0,50/255.0,0.2),'pointSize':0}, + RoadgraphSurfaceEnum.GRAVEL : {'color':(0.7,0.7,0.7,0.2),'pointSize':0}, + None: {'color':(1,0,0,0.2),'pointSize':0}, +} + +REGION_TO_STYLE = { + RoadgraphRegionEnum.INTERSECTION : {'color':(0,1,0,0.5),'width':1,'pointSize':0}, + RoadgraphRegionEnum.PARKING_LOT : {'color':(0,0,1,0.5),'width':1,'pointSize':0}, + RoadgraphRegionEnum.CLOSED_COURSE : {'color':(1,0,0,0.5),'width':1,'pointSize':0}, + RoadgraphRegionEnum.VIRTUAL : {'color':(0,0,0,0.5),'width':1,'pointSize':0}, +} + +def plot_pose(name : str, pose : ObjectPose, axis_len=0.1, label=True): + """Plots the pose in the given axes. The coordinates + of the pose are plotted in the pose's indicated frame.""" + R = pose.rotation() + t = pose.translation() + T = (so3.from_matrix(R),t) + vis.add(name, T, length=axis_len, hide_label=(not label)) + if pose.frame == ObjectFrameEnum.START: + vis.add(name+"_origin",t,color=(0,0,1,1),size=5,hide_label=True) + elif pose.frame == ObjectFrameEnum.CURRENT: + vis.add(name+"_origin",t,color=(1,0,0,1),size=5,hide_label=True) + elif pose.frame in [ObjectFrameEnum.GLOBAL,ObjectFrameEnum.ABSOLUTE_CARTESIAN]: + vis.add(name+"_origin",t,color=(0,1,0,1),size=5,hide_label=True) + else: + raise ValueError("Unknown frame %s" % pose.frame) + +def plot_object(name : str, obj : PhysicalObject, type=None, axis_len=None, outline=True, solid=True, bbox=True, label=True): + """Shows an object in the given axes. + + If axis_len is given, shows the object's pose with + a coordinate frame of the given length. + + If outline is True, shows the object's outline. + + If bbox is True, shows the object's bounding box. + """ + height = obj.dimensions[2] + core_color = OBJECT_COLORS[type] + bbox_color = AUX_BBOX_COLOR + if label: + #add a point at the object's origin + vis.add(name,obj.pose.translation(),size=5,color=(0,0,0,1)) + if axis_len: + plot_pose(name+"_pose", obj.pose, axis_len=0) + #plot bounding box + R = obj.pose.rotation() + t = obj.pose.translation() + if bbox or (outline and obj.outline is None): + bounds = obj.bounds() + (xmin,xmax),(ymin,ymax),(zmin,zmax) = bounds + if not solid: + corners = [[xmin,ymin,0.01],[xmin,ymax,0.01],[xmax,ymax,0.01],[xmax,ymin,0.01]] + corners = [list(R.dot(c)+t) for c in corners] + corners.append(corners[0]) + if not bbox: + vis.add(name+"_bbox1",corners,width=1,color=core_color,pointSize=0,hide_label=True) + else: + vis.add(name+"_bbox1",corners,width=1,color=AUX_BBOX_COLOR,pointSize=0,hide_label=True) + if height > 0: + corners = [[xmin,ymin,height],[xmin,ymax,height],[xmax,ymax,height],[xmax,ymin,height]] + corners = [list(R.dot(c)+t) for c in corners] + corners.append(corners[0]) + if not bbox: + vis.add(name+"_bbox2",corners,width=1,color=core_color,pointSize=0,hide_label=True) + else: + vis.add(name+"_bbox2",corners,width=1,color=AUX_BBOX_COLOR,pointSize=0,hide_label=True) + else: + prim = GeometricPrimitive() + prim.setAABB([xmin,ymin,0],[xmax,ymax,height]) + g = Geometry3D(prim) + g.setCurrentTransform(so3.from_matrix(R),t) + vis.add(name+"_bbox",g,color=core_color[:3]+(0.5,),hide_label=True) + + #plot outline + if outline and obj.outline: + outline = [R.dot((p[0],p[1],0))+t for p in obj.outline] + outline.append(outline[0]) + vis.add(name+"_outline1",outline,width=1,color=core_color,hide_label=True) + if height > 0: + outline = [R.dot((p[0],p[1],height))+t for p in obj.outline] + outline.append(outline[0]) + vis.add(name+"_outline2",outline,width=1,color=core_color,hide_label=True) + + +def plot_vehicle(vehicle : VehicleState, vehicle_model=None, axis_len=1.0): + """Plots the vehicle in the given axes. The coordinates + of the vehicle are plotted in the vehicle's indicated frame.""" + plot_object("vehicle",vehicle.to_object(), type=None, axis_len=0, solid=False, label=False) + R = vehicle.pose.rotation() + t = vehicle.pose.translation() + T = (so3.from_matrix(R),t) + vis.add("vehicle", T, length=axis_len, hide_label=True) + + xbounds,ybounds,zbounds = settings.get('vehicle.geometry.bounds') + #plot velocity arrow + R = vehicle.pose.rotation2d() + t = np.array([vehicle.pose.x,vehicle.pose.y]) + v = R.dot([vehicle.v,0]) + front_pt = vehicle.pose.apply((xbounds[1],0.0)) + h = (vehicle.pose.z if vehicle.pose.z is not None else 0) + zbounds[1] + vis.add("vehicle_velocity",[[front_pt[0],front_pt[1],h],[front_pt[0]+v[0],front_pt[1]+v[1],h]],width=2,color=(0,1,0,1),pointSize=0) + + #plot front wheel angles + wheelbase = settings.get('vehicle.geometry.wheelbase') + wheel_spacing = 0.8*settings.get('vehicle.geometry.width') / 2 + phi = vehicle.front_wheel_angle + if vehicle_model: + q = vehicle_model.getConfig() + lwhindex = vehicle_model.link("left_steering_hinge_link").index + rwhindex = vehicle_model.link("right_steering_hinge_link").index + lwindex = vehicle_model.link("front_left_wheel_link").index + rwindex = vehicle_model.link("front_right_wheel_link").index + rlwindex = vehicle_model.link("rear_left_wheel_link").index + rrwindex = vehicle_model.link("rear_right_wheel_link").index + q[lwhindex] = phi + q[rwhindex] = phi + q[lwindex] += vehicle.v * 0.2 + q[rwindex] += vehicle.v * 0.2 + q[rlwindex] += vehicle.v * 0.2 + q[rrwindex] += vehicle.v * 0.2 + vehicle_model.setConfig(q) + else: + left_wheel_origin = t + R.dot([wheelbase,wheel_spacing]) + right_wheel_origin = t + R.dot([wheelbase,-wheel_spacing]) + wheel_width = 0.5 #meters + wheel_offset = R.dot(np.array([np.cos(phi),np.sin(phi)]))*wheel_width*0.5 + vis.add("left_wheel",[vectorops.sub(left_wheel_origin,wheel_offset),vectorops.add(left_wheel_origin,wheel_offset)],color=(0,0,0,1),width=2) + vis.add("right_wheel",[vectorops.sub(right_wheel_origin,wheel_offset),vectorops.add(right_wheel_origin,wheel_offset)],color=(0,0,0,1),width=2) + + #plot gear + if vehicle.gear in [VehicleGearEnum.NEUTRAL,VehicleGearEnum.REVERSE,VehicleGearEnum.PARK]: + if vehicle.gear == VehicleGearEnum.NEUTRAL: + gear = 'N' + elif vehicle.gear == VehicleGearEnum.REVERSE: + gear = 'R' + else: + gear = 'P' + vis.addText("gear",gear,position=(t[0],t[1],1.5),color=(0,0,0,1)) + + #plot lights + light_point_size = 4 + light_size = 0.15 + light_color = (1,1,0,1) + turn_indicator_height = 0.7 + headlight_height = 0.6 + if vehicle.left_turn_indicator: + lp = vehicle.pose.apply([xbounds[0],ybounds[0]+light_size,turn_indicator_height]) + vis.add("left_turn_indicator",list(lp),size=light_point_size,color=light_color) + if vehicle.right_turn_indicator: + lp = vehicle.pose.apply([xbounds[0],ybounds[1]-light_size,turn_indicator_height]) + vis.add("right_turn_indicator",list(lp),size=light_point_size,color=light_color) + if vehicle.headlights_on: + lp = vehicle.pose.apply([xbounds[1],ybounds[0]+light_size*2,headlight_height]) + vis.add("left_headlight",list(lp),size=light_point_size,color=light_color) + lp = vehicle.pose.apply([xbounds[1],ybounds[1]-light_size*2,headlight_height]) + vis.add("right_headlight",list(lp),size=light_point_size,color=light_color) + if vehicle_model is not None: + if vehicle.brake_pedal_position > 0.1: + vehicle_model.link('rear_right_stop_light_link').appearance().setColor(1,0,0,1) + vehicle_model.link('rear_left_stop_light_link').appearance().setColor(1,0,0,1) + else: + vehicle_model.link('rear_right_stop_light_link').appearance().setColor(0.3,0,0,1) + vehicle_model.link('rear_left_stop_light_link').appearance().setColor(0.3,0,0,1) + +def plot_path(name : str, path : Path, color=(0,0,0), width=1): + if len(path.points) > MAX_POINTS_IN_CURVE: # downsample due to OpenGL error? + vis.add(name,[list(p) for p in path.points[::len(path.points)//MAX_POINTS_IN_CURVE]],color=color,width=width) + else: + vis.add(name,[list(p) for p in path.points],color=color,width=width) + +def plot_curve(name : str, curve : RoadgraphCurve, color=None, width=None): + style = CURVE_TO_STYLE.get(curve.type,CURVE_TO_STYLE[None]) + if curve.crossable and curve.type == RoadgraphCurveEnum.LANE_BOUNDARY: + #style['linestyle'] = '--' + #TODO: how to indicate crossable lines? + pass + if color is not None: + style['color'] = color + if width is not None: + style['width'] = width + for i,seg in enumerate(curve.segments): + if len(seg) > MAX_POINTS_IN_CURVE: # downsample due to OpenGL error? + vis.add(name+"_%d" % i,seg[::len(seg)//MAX_POINTS_IN_CURVE],**style) + else: + vis.add(name+"_%d" % i,seg,**style) + +def plot_lane(name : str, lane : RoadgraphLane, on_route=False): + if lane.surface != RoadgraphSurfaceEnum.PAVEMENT: + style = SURFACE_TO_STYLE.get(lane.surface,SURFACE_TO_STYLE[None]) + outline = lane.outline() + vis.add(name, outline, **style) + if lane.left is not None: + plot_curve(name+"_left", lane.left) + if lane.right is not None: + plot_curve(name+"_right", lane.right) + +def plot_region(name : str, region : RoadgraphRegion, color=None, width=None): + style = REGION_TO_STYLE.get(region.type,REGION_TO_STYLE[None]) + points = region.outline() + pts = points + points[0] + if color is not None: + style['color'] = color + if width is not None: + style['width'] = width + vis.add(name, [list(p) for p in pts], **style) + +def plot_roadgraph(roadgraph : Roadgraph, route : Route = None): + #plot lanes + for k,l in roadgraph.lanes.items(): + if route is not None and k in route.lanes: + plot_lane(k,l,on_route=True) + else: + plot_lane(k,l) + for k,c in roadgraph.curves.items(): + plot_curve(k,c,color=(0,0,0,1)) + #plot intersections + for k,r in roadgraph.regions.items(): + plot_region(k,r) + #plot + for k,o in roadgraph.static_obstacles.items(): + plot_object(k,o) + +def plot_scene(scene : SceneState, ground_truth_vehicle=None, vehicle_model = None, title = None, show=True): + for i in list(vis.scene().items.keys()): + if not i.startswith("vehicle"): + if not isinstance(vis.scene().items[i],vis.VisPlot): + vis.remove(i) + #set plot range + #TODO + if vehicle_model is not None: + vis.add("vehicle_model",vehicle_model) + if ground_truth_vehicle is not None: + xform = ground_truth_vehicle.to_object().pose.transform() + else: + xform = scene.vehicle.to_object().pose.transform() + vehicle_model.link(0).setParentTransform(*se3.from_ndarray(xform)) + vehicle_model.setConfig(vehicle_model.getConfig()) + + #plot roadgraph + plot_roadgraph(scene.roadgraph,scene.route) + #plot vehicle and objects + plot_vehicle(scene.vehicle, vehicle_model) + for k,a in scene.agents.items(): + plot_object(k,a,type=a.type) + for k,o in scene.obstacles.items(): + plot_object(k,o) + if title is None: + if show: + vis.add("title","Scene at t = %.2f" % scene.t, position=(10,10)) + else: + vis.add("title",title) + if show: + vis.show() + +def plot(state : AllState, ground_truth_vehicle = None, vehicle_model=None, title=None, show=True): + plot_scene(state, ground_truth_vehicle=ground_truth_vehicle, vehicle_model=vehicle_model, title=title, show=show) + if state.route is not None: + plot_path("route",state.route,color=(1,0.5,0,1),width=2) + if state.trajectory is not None: + plot_path("trajectory",state.trajectory,color=(1,0,0,1),width=3) diff --git a/GEMstack/utils/settings.py b/GEMstack/utils/settings.py index a3d193658..9486b3bf2 100644 --- a/GEMstack/utils/settings.py +++ b/GEMstack/utils/settings.py @@ -1,80 +1,80 @@ -import json -from ..knowledge import defaults -import copy -from typing import List,Union,Any - -SETTINGS = None - -def load_settings(): - """Loads the settings object for the first time. - - Order of operations is to look into defaults.SETTINGS, and then - look through the command line arguments to determine whether the user has - overridden any settings using --KEY=VALUE. - """ - global SETTINGS - if SETTINGS is not None: - return - import os - import sys - SETTINGS = copy.deepcopy(defaults.SETTINGS) - for arg in sys.argv: - if arg.startswith('--'): - k,v = arg.split('=',1) - k = k[2:] - v = v.strip('"') - try: - v = json.loads(v) - except json.decoder.JSONDecodeError: - pass - if v.startswith('{'): - set(k,v,leaf_only=False) - else: - set(k,v) - - return - - -def settings(): - """Returns all global settings, loading them if necessary.""" - #global SETTINGS - load_settings() - return SETTINGS - - -def get(path : Union[str,List[str]], defaultValue=KeyError) -> Any: - """Retrieves a setting by a list of keys or a '.'-separated string.""" - #global SETTINGS - load_settings() - if isinstance(path,str): - path = path.split('.') - try: - val = SETTINGS - for key in path: - val = val[key] - return val - except KeyError: - if defaultValue is KeyError: - print("settings.py: Unable to get",path,"available keys:",val.keys()) - raise - return defaultValue - -def set(path : Union[str,List[str]], value : Any, leaf_only=True) -> None: - """Sets a setting by a list of keys or a '.'-separated string. - - If leaf_only=True (default), we prevent inadvertently deleting parts of the - settings dictionary. - """ - #global SETTINGS - load_settings() - if isinstance(path,str): - path = path.split('.') - val = SETTINGS - if len(path) == 0: - raise KeyError("Cannot set top-level settings") - for key in path[:-1]: - val = val[key] - if leaf_only: - if path[-1] in val and isinstance(val[path[-1]],dict): - raise ValueError("Can only set leaves of the settings dictionary when leaf_only=True is given") - val[path[-1]] = value +import json +from ..knowledge import defaults +import copy +from typing import List,Union,Any + +SETTINGS = None + +def load_settings(): + """Loads the settings object for the first time. + + Order of operations is to look into defaults.SETTINGS, and then + look through the command line arguments to determine whether the user has + overridden any settings using --KEY=VALUE. + """ + global SETTINGS + if SETTINGS is not None: + return + import os + import sys + SETTINGS = copy.deepcopy(defaults.SETTINGS) + for arg in sys.argv: + if arg.startswith('--'): + k,v = arg.split('=',1) + k = k[2:] + v = v.strip('"') + try: + v = json.loads(v) + except json.decoder.JSONDecodeError: + pass + if v.startswith('{'): + set(k,v,leaf_only=False) + else: + set(k,v) + + return + + +def settings(): + """Returns all global settings, loading them if necessary.""" + #global SETTINGS + load_settings() + return SETTINGS + + +def get(path : Union[str,List[str]], defaultValue=KeyError) -> Any: + """Retrieves a setting by a list of keys or a '.'-separated string.""" + #global SETTINGS + load_settings() + if isinstance(path,str): + path = path.split('.') + try: + val = SETTINGS + for key in path: + val = val[key] + return val + except KeyError: + if defaultValue is KeyError: + print("settings.py: Unable to get",path,"available keys:",val.keys()) + raise + return defaultValue + +def set(path : Union[str,List[str]], value : Any, leaf_only=True) -> None: + """Sets a setting by a list of keys or a '.'-separated string. + + If leaf_only=True (default), we prevent inadvertently deleting parts of the + settings dictionary. + """ + #global SETTINGS + load_settings() + if isinstance(path,str): + path = path.split('.') + val = SETTINGS + if len(path) == 0: + raise KeyError("Cannot set top-level settings") + for key in path[:-1]: + val = val[key] + if leaf_only: + if path[-1] in val and isinstance(val[path[-1]],dict): + raise ValueError("Can only set leaves of the settings dictionary when leaf_only=True is given") + val[path[-1]] = value diff --git a/launch/fixed_route.yaml b/launch/fixed_route.yaml index abd8242e6..3e28ea668 100644 --- a/launch/fixed_route.yaml +++ b/launch/fixed_route.yaml @@ -1,80 +1,80 @@ -description: "Drive the GEM vehicle along a fixed route (currently xyhead_highbay_backlot_p.csv)" -mode: hardware -vehicle_interface: gem_hardware.GEMHardwareInterface -mission_execution: StandardExecutor -# Recovery behavior after a component failure -recovery: - planning: - trajectory_tracking: - type: recovery.StopTrajectoryTracker - print: False -# Driving behavior for the GEM vehicle following a fixed route -drive: - perception: - state_estimation : GNSSStateEstimator - perception_normalization : StandardPerceptionNormalizer - planning: - route_planning: - type: StaticRoutePlanner - args: [!relative_path '../GEMstack/knowledge/routes/xyhead_highbay_backlot_p.csv'] - motion_planning: - type: RouteToTrajectoryPlanner - args: [null] #desired speed in m/s. If null, this will keep the route untimed for the trajectory tracker - trajectory_tracking: - #type: pure_pursuit.PurePursuitTrajectoryTracker - type: stanley.StanleyTrajectoryTracker - args: {desired_speed: 2.5} #approximately 5mph - print: False -log: - # Specify the top-level folder to save the log files. Default is 'logs' - #folder : 'logs' - # If prefix is specified, then the log folder will be named with the prefix followed by the date and time. Default no prefix - #prefix : 'fixed_route_' - # If suffix is specified, then logs will output to folder/prefix+suffix. Default uses date and time as the suffix - #suffix : 'test3' - # Specify which ros topics to record to vehicle.bag. Default records nothing. This records the "standard" ROS topics. - ros_topics : [] - # Specify options to pass to rosbag record. Default is no options. - #rosbag_options : '--split --size=1024' - # If True, then record all readings / commands of the vehicle interface. Default False - vehicle_interface : True - # Specify which components to record to behavior.json. Default records nothing - components : ['state_estimation','trajectory_tracking'] - # Specify which components of state to record to state.json. Default records nothing - #state: ['all'] - # Specify the rate in Hz at which to record state to state.json. Default records at the pipeline's rate - #state_rate: 10 -replay: # Add items here to set certain topics / inputs to be replayed from logs - # Specify which log folder to replay from - log: - # For replaying sensor data, try !include "../knowledge/defaults/standard_sensor_ros_topics.yaml" - ros_topics : [] - components : [] - -#usually can keep this constant -computation_graph: !include "../GEMstack/knowledge/defaults/computation_graph.yaml" - -after: - show_log_folder: True #set to false to avoid showing the log folder - -#on load, variants will overload the settings structure -variants: - #sim variant doesn't execute on the real vehicle - #real variant executes on the real robot - sim: - run: - mode: simulation - vehicle_interface: - type: gem_simulator.GEMDoubleIntegratorSimulationInterface - args: - scene: !relative_path '../scenes/xyhead_demo.yaml' - - drive: - perception: - state_estimation : OmniscientStateEstimator - agent_detection : OmniscientAgentDetector - visualization: !include "klampt_visualization.yaml" - #visualization: !include "mpl_visualization.yaml" - log_ros: - log: - ros_topics : !include "../GEMstack/knowledge/defaults/standard_ros_topics.yaml" +description: "Drive the GEM vehicle along a fixed route (currently xyhead_highbay_backlot_p.csv)" +mode: hardware +vehicle_interface: gem_hardware.GEMHardwareInterface +mission_execution: StandardExecutor +# Recovery behavior after a component failure +recovery: + planning: + trajectory_tracking: + type: recovery.StopTrajectoryTracker + print: False +# Driving behavior for the GEM vehicle following a fixed route +drive: + perception: + state_estimation : GNSSStateEstimator + perception_normalization : StandardPerceptionNormalizer + planning: + route_planning: + type: StaticRoutePlanner + args: [!relative_path '../GEMstack/knowledge/routes/xyhead_highbay_backlot_p.csv'] + motion_planning: + type: RouteToTrajectoryPlanner + args: [null] #desired speed in m/s. If null, this will keep the route untimed for the trajectory tracker + trajectory_tracking: + #type: pure_pursuit.PurePursuitTrajectoryTracker + type: stanley.StanleyTrajectoryTracker + args: {desired_speed: 2.5} #approximately 5mph + print: False +log: + # Specify the top-level folder to save the log files. Default is 'logs' + #folder : 'logs' + # If prefix is specified, then the log folder will be named with the prefix followed by the date and time. Default no prefix + #prefix : 'fixed_route_' + # If suffix is specified, then logs will output to folder/prefix+suffix. Default uses date and time as the suffix + #suffix : 'test3' + # Specify which ros topics to record to vehicle.bag. Default records nothing. This records the "standard" ROS topics. + ros_topics : [] + # Specify options to pass to rosbag record. Default is no options. + #rosbag_options : '--split --size=1024' + # If True, then record all readings / commands of the vehicle interface. Default False + vehicle_interface : True + # Specify which components to record to behavior.json. Default records nothing + components : ['state_estimation','trajectory_tracking'] + # Specify which components of state to record to state.json. Default records nothing + #state: ['all'] + # Specify the rate in Hz at which to record state to state.json. Default records at the pipeline's rate + #state_rate: 10 +replay: # Add items here to set certain topics / inputs to be replayed from logs + # Specify which log folder to replay from + log: + # For replaying sensor data, try !include "../knowledge/defaults/standard_sensor_ros_topics.yaml" + ros_topics : [] + components : [] + +#usually can keep this constant +computation_graph: !include "../GEMstack/knowledge/defaults/computation_graph.yaml" + +after: + show_log_folder: True #set to false to avoid showing the log folder + +#on load, variants will overload the settings structure +variants: + #sim variant doesn't execute on the real vehicle + #real variant executes on the real robot + sim: + run: + mode: simulation + vehicle_interface: + type: gem_simulator.GEMDoubleIntegratorSimulationInterface + args: + scene: !relative_path '../scenes/xyhead_demo.yaml' + + drive: + perception: + state_estimation : OmniscientStateEstimator + agent_detection : OmniscientAgentDetector + visualization: !include "klampt_visualization.yaml" + #visualization: !include "mpl_visualization.yaml" + log_ros: + log: + ros_topics : !include "../GEMstack/knowledge/defaults/standard_ros_topics.yaml" diff --git a/logs/logplot_pp.py b/logs/logplot_pp.py index 74025c291..2a6533afc 100644 --- a/logs/logplot_pp.py +++ b/logs/logplot_pp.py @@ -1,120 +1,120 @@ -####Following code can be used to visualize performance of Pure Pursuit after using it.### -###How to use?- After running the simulation or on actual vehicle you get logs in log folder### -### You can run the command- python3 logplot_pp.py "logfilename".### - - -import json -import sys -import os -import matplotlib.pyplot as plt -import numpy as np - -# Safety thresholds (not used in the current plots) -ACC_SAFE = 0.8 # |acceleration| <= 0.8 m/s² is safe -ACC_UNSAFE = 2.0 # |acceleration| >= 2.0 m/s² is unsafe -HR_SAFE = 0.05 # |heading_rate| <= 0.05 rad/s is safe -HR_UNSAFE = 0.2 # |heading_rate| >= 0.2 rad/s is unsafe - -def parse_behavior_log(filename): - times = [] - accelerations = [] - heading_rates = [] - - with open(filename, 'r') as f: - first_time = None # Store the first timestamp for shifting - for line in f: - try: - entry = json.loads(line) - except json.JSONDecodeError: - print(f"Skipping invalid JSON line: {line.strip()}") - continue - if "vehicle" in entry: - t = entry.get("time") - vehicle_data = entry["vehicle"].get("data", {}) - acceleration = vehicle_data.get("acceleration") - heading_rate = vehicle_data.get("heading_rate") - if t is not None and acceleration is not None and heading_rate is not None: - if first_time is None: - first_time = t # Set the first timestamp - times.append(t - first_time) # Shift time to start from 0 - accelerations.append(acceleration) - heading_rates.append(heading_rate) - - return np.array(times), np.array(accelerations), np.array(heading_rates) - -def parse_tracker_csv(filename): - data = np.genfromtxt(filename, delimiter=',', skip_header=1) - first_time = data[0, 18] # Normalize time to start from 0 - cte_time = data[:, 18] - first_time - cte = data[:, 20] - x_actual, y_actual = data[:, 2], data[:, 5] - x_desired, y_desired = data[:, 11], data[:, 14] - return cte_time, cte, x_actual, y_actual, x_desired, y_desired - -def compute_derivative(times, values): - dt = np.diff(times) - dv = np.diff(values) - derivative = dv / dt - return times[1:], derivative - -def plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired): - global_min_time = min(time_jerk[0], time_heading_acc[0], cte_time[0]) - global_max_time = max(time_jerk[-1], time_heading_acc[-1], cte_time[-1]) - - fig, axs = plt.subplots(2, 2, figsize=(12, 8)) - fig.subplots_adjust(hspace=0.4, wspace=0.3) - - axs[0,0].plot(time_jerk, jerk, marker='', linestyle='-', color='blue') - axs[0,0].set_ylabel("Jerk (m/s³)") - axs[0,0].set_title("Vehicle Jerk Over Time") - axs[0,0].grid(True) - axs[0,0].set_xlim(global_min_time, global_max_time) - - axs[0,1].plot(time_heading_acc, heading_acc, marker='', linestyle='-', color='orange') - axs[0,1].set_ylabel("Heading Acceleration (rad/s²)") - axs[0,1].set_title("Vehicle Heading Acceleration Over Time") - axs[0,1].grid(True) - axs[0,1].set_xlim(global_min_time, global_max_time) - - axs[1,0].plot(cte_time, cte, marker='', linestyle='-', color='green') - axs[1,0].set_xlabel("Time (s)") - axs[1,0].set_ylabel("Cross Track Error") - axs[1,0].set_title("Vehicle Cross Track Error Over Time") - axs[1,0].grid(True) - axs[1,0].set_xlim(global_min_time, global_max_time) - - axs[1,1].plot(x_actual, y_actual, marker='', linestyle='-', color='blue', label='Actual') - axs[1,1].plot(x_desired, y_desired, marker='', linestyle='-', color='orange', label='Desired') - axs[1,1].set_xlabel("X Position (m)") - axs[1,1].set_ylabel("Y Position (m)") - axs[1,1].set_title("Vehicle Position") - axs[1,1].legend() - axs[1,1].grid(True) - - plt.show() - -if __name__=='__main__': - if len(sys.argv) != 2: - print("Usage: python test_comfort_metrics.py ") - sys.exit(1) - - log_dir = sys.argv[1] - behavior_file = os.path.join(log_dir, "behavior.json") - tracker_file = os.path.join(log_dir, "PurePursuitTrajectoryTracker_debug.csv") - #tracker_file = os.path.join(log_dir, "StanleyTrajectoryTracker_debug.csv") - - times, accelerations, heading_rates = parse_behavior_log(behavior_file) - time_jerk, jerk = compute_derivative(times, accelerations) - time_heading_acc, heading_acc = compute_derivative(times, heading_rates) - - cte_time, cte, x_actual, y_actual, x_desired, y_desired = parse_tracker_csv(tracker_file) - - plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired) - - print("Max (abs) jerk:", np.max(np.abs(jerk))) - print("Avg jerk:", np.mean(np.abs(jerk))) - print("Max (abs) heading acceleration:", np.max(np.abs(heading_acc))) - print("Avg heading acceleration:", np.mean(np.abs(heading_acc))) - print("Max (abs) cross track error:", np.max(np.abs(cte))) - print("Avg cross track error:", np.mean(np.abs(cte))) - +####Following code can be used to visualize performance of Pure Pursuit after using it.### +###How to use?- After running the simulation or on actual vehicle you get logs in log folder### +### You can run the command- python3 logplot_pp.py "logfilename".### + + +import json +import sys +import os +import matplotlib.pyplot as plt +import numpy as np + +# Safety thresholds (not used in the current plots) +ACC_SAFE = 0.8 # |acceleration| <= 0.8 m/s² is safe +ACC_UNSAFE = 2.0 # |acceleration| >= 2.0 m/s² is unsafe +HR_SAFE = 0.05 # |heading_rate| <= 0.05 rad/s is safe +HR_UNSAFE = 0.2 # |heading_rate| >= 0.2 rad/s is unsafe + +def parse_behavior_log(filename): + times = [] + accelerations = [] + heading_rates = [] + + with open(filename, 'r') as f: + first_time = None # Store the first timestamp for shifting + for line in f: + try: + entry = json.loads(line) + except json.JSONDecodeError: + print(f"Skipping invalid JSON line: {line.strip()}") + continue + if "vehicle" in entry: + t = entry.get("time") + vehicle_data = entry["vehicle"].get("data", {}) + acceleration = vehicle_data.get("acceleration") + heading_rate = vehicle_data.get("heading_rate") + if t is not None and acceleration is not None and heading_rate is not None: + if first_time is None: + first_time = t # Set the first timestamp + times.append(t - first_time) # Shift time to start from 0 + accelerations.append(acceleration) + heading_rates.append(heading_rate) + + return np.array(times), np.array(accelerations), np.array(heading_rates) + +def parse_tracker_csv(filename): + data = np.genfromtxt(filename, delimiter=',', skip_header=1) + first_time = data[0, 18] # Normalize time to start from 0 + cte_time = data[:, 18] - first_time + cte = data[:, 20] + x_actual, y_actual = data[:, 2], data[:, 5] + x_desired, y_desired = data[:, 11], data[:, 14] + return cte_time, cte, x_actual, y_actual, x_desired, y_desired + +def compute_derivative(times, values): + dt = np.diff(times) + dv = np.diff(values) + derivative = dv / dt + return times[1:], derivative + +def plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired): + global_min_time = min(time_jerk[0], time_heading_acc[0], cte_time[0]) + global_max_time = max(time_jerk[-1], time_heading_acc[-1], cte_time[-1]) + + fig, axs = plt.subplots(2, 2, figsize=(12, 8)) + fig.subplots_adjust(hspace=0.4, wspace=0.3) + + axs[0,0].plot(time_jerk, jerk, marker='', linestyle='-', color='blue') + axs[0,0].set_ylabel("Jerk (m/s³)") + axs[0,0].set_title("Vehicle Jerk Over Time") + axs[0,0].grid(True) + axs[0,0].set_xlim(global_min_time, global_max_time) + + axs[0,1].plot(time_heading_acc, heading_acc, marker='', linestyle='-', color='orange') + axs[0,1].set_ylabel("Heading Acceleration (rad/s²)") + axs[0,1].set_title("Vehicle Heading Acceleration Over Time") + axs[0,1].grid(True) + axs[0,1].set_xlim(global_min_time, global_max_time) + + axs[1,0].plot(cte_time, cte, marker='', linestyle='-', color='green') + axs[1,0].set_xlabel("Time (s)") + axs[1,0].set_ylabel("Cross Track Error") + axs[1,0].set_title("Vehicle Cross Track Error Over Time") + axs[1,0].grid(True) + axs[1,0].set_xlim(global_min_time, global_max_time) + + axs[1,1].plot(x_actual, y_actual, marker='', linestyle='-', color='blue', label='Actual') + axs[1,1].plot(x_desired, y_desired, marker='', linestyle='-', color='orange', label='Desired') + axs[1,1].set_xlabel("X Position (m)") + axs[1,1].set_ylabel("Y Position (m)") + axs[1,1].set_title("Vehicle Position") + axs[1,1].legend() + axs[1,1].grid(True) + + plt.show() + +if __name__=='__main__': + if len(sys.argv) != 2: + print("Usage: python test_comfort_metrics.py ") + sys.exit(1) + + log_dir = sys.argv[1] + behavior_file = os.path.join(log_dir, "behavior.json") + tracker_file = os.path.join(log_dir, "PurePursuitTrajectoryTracker_debug.csv") + #tracker_file = os.path.join(log_dir, "StanleyTrajectoryTracker_debug.csv") + + times, accelerations, heading_rates = parse_behavior_log(behavior_file) + time_jerk, jerk = compute_derivative(times, accelerations) + time_heading_acc, heading_acc = compute_derivative(times, heading_rates) + + cte_time, cte, x_actual, y_actual, x_desired, y_desired = parse_tracker_csv(tracker_file) + + plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired) + + print("Max (abs) jerk:", np.max(np.abs(jerk))) + print("Avg jerk:", np.mean(np.abs(jerk))) + print("Max (abs) heading acceleration:", np.max(np.abs(heading_acc))) + print("Avg heading acceleration:", np.mean(np.abs(heading_acc))) + print("Max (abs) cross track error:", np.max(np.abs(cte))) + print("Avg cross track error:", np.mean(np.abs(cte))) + diff --git a/logs/logplot_s.py b/logs/logplot_s.py index f534bbd27..ecff7f118 100644 --- a/logs/logplot_s.py +++ b/logs/logplot_s.py @@ -1,119 +1,119 @@ -####Following code can be used to visualize performance of Stanley after using it.### -###How to use?- After running the simulation or on actual vehicle you get logs in log folder### -### You can run the command- python3 logplot_s.py "logfilename".### - -import json -import sys -import os -import matplotlib.pyplot as plt -import numpy as np - -# Safety thresholds (not used in the current plots) -ACC_SAFE = 0.8 # |acceleration| <= 0.8 m/s² is safe -ACC_UNSAFE = 2.0 # |acceleration| >= 2.0 m/s² is unsafe -HR_SAFE = 0.05 # |heading_rate| <= 0.05 rad/s is safe -HR_UNSAFE = 0.2 # |heading_rate| >= 0.2 rad/s is unsafe - -def parse_behavior_log(filename): - times = [] - accelerations = [] - heading_rates = [] - - with open(filename, 'r') as f: - first_time = None # Store the first timestamp for shifting - for line in f: - try: - entry = json.loads(line) - except json.JSONDecodeError: - print(f"Skipping invalid JSON line: {line.strip()}") - continue - if "vehicle" in entry: - t = entry.get("time") - vehicle_data = entry["vehicle"].get("data", {}) - acceleration = vehicle_data.get("acceleration") - heading_rate = vehicle_data.get("heading_rate") - if t is not None and acceleration is not None and heading_rate is not None: - if first_time is None: - first_time = t # Set the first timestamp - times.append(t - first_time) # Shift time to start from 0 - accelerations.append(acceleration) - heading_rates.append(heading_rate) - - return np.array(times), np.array(accelerations), np.array(heading_rates) - -def parse_tracker_csv(filename): - data = np.genfromtxt(filename, delimiter=',', skip_header=1) - first_time = data[0, 18] # Normalize time to start from 0 - cte_time = data[:, 18] - first_time - cte = data[:, 20] - x_actual, y_actual = data[:, 2], data[:, 5] - x_desired, y_desired = data[:, 8], data[:, 11] - return cte_time, cte, x_actual, y_actual, x_desired, y_desired - -def compute_derivative(times, values): - dt = np.diff(times) - dv = np.diff(values) - derivative = dv / dt - return times[1:], derivative - -def plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired): - global_min_time = min(time_jerk[0], time_heading_acc[0], cte_time[0]) - global_max_time = max(time_jerk[-1], time_heading_acc[-1], cte_time[-1]) - - fig, axs = plt.subplots(2, 2, figsize=(12, 8)) - fig.subplots_adjust(hspace=0.4, wspace=0.3) - - axs[0,0].plot(time_jerk, jerk, marker='', linestyle='-', color='blue') - axs[0,0].set_ylabel("Jerk (m/s³)") - axs[0,0].set_title("Vehicle Jerk Over Time") - axs[0,0].grid(True) - axs[0,0].set_xlim(global_min_time, global_max_time) - - axs[0,1].plot(time_heading_acc, heading_acc, marker='', linestyle='-', color='orange') - axs[0,1].set_ylabel("Heading Acceleration (rad/s²)") - axs[0,1].set_title("Vehicle Heading Acceleration Over Time") - axs[0,1].grid(True) - axs[0,1].set_xlim(global_min_time, global_max_time) - - axs[1,0].plot(cte_time, cte, marker='', linestyle='-', color='green') - axs[1,0].set_xlabel("Time (s)") - axs[1,0].set_ylabel("Cross Track Error") - axs[1,0].set_title("Vehicle Cross Track Error Over Time") - axs[1,0].grid(True) - axs[1,0].set_xlim(global_min_time, global_max_time) - - axs[1,1].plot(x_actual, y_actual, marker='', linestyle='-', color='blue', label='Actual') - axs[1,1].plot(x_desired, y_desired, marker='', linestyle='-', color='orange', label='Desired') - axs[1,1].set_xlabel("X Position (m)") - axs[1,1].set_ylabel("Y Position (m)") - axs[1,1].set_title("Vehicle Position") - axs[1,1].legend() - axs[1,1].grid(True) - - plt.show() - -if __name__=='__main__': - if len(sys.argv) != 2: - print("Usage: python test_comfort_metrics.py ") - sys.exit(1) - - log_dir = sys.argv[1] - behavior_file = os.path.join(log_dir, "behavior.json") - #tracker_file = os.path.join(log_dir, "PurePursuitTrajectoryTracker_debug.csv") - tracker_file = os.path.join(log_dir, "StanleyTrajectoryTracker_debug.csv") - - times, accelerations, heading_rates = parse_behavior_log(behavior_file) - time_jerk, jerk = compute_derivative(times, accelerations) - time_heading_acc, heading_acc = compute_derivative(times, heading_rates) - - cte_time, cte, x_actual, y_actual, x_desired, y_desired = parse_tracker_csv(tracker_file) - - plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired) - - print("Max (abs) jerk:", np.max(np.abs(jerk))) - print("Avg jerk:", np.mean(np.abs(jerk))) - print("Max (abs) heading acceleration:", np.max(np.abs(heading_acc))) - print("Avg heading acceleration:", np.mean(np.abs(heading_acc))) - print("Max (abs) cross track error:", np.max(np.abs(cte))) - print("Avg cross track error:", np.mean(np.abs(cte))) - +####Following code can be used to visualize performance of Stanley after using it.### +###How to use?- After running the simulation or on actual vehicle you get logs in log folder### +### You can run the command- python3 logplot_s.py "logfilename".### + +import json +import sys +import os +import matplotlib.pyplot as plt +import numpy as np + +# Safety thresholds (not used in the current plots) +ACC_SAFE = 0.8 # |acceleration| <= 0.8 m/s² is safe +ACC_UNSAFE = 2.0 # |acceleration| >= 2.0 m/s² is unsafe +HR_SAFE = 0.05 # |heading_rate| <= 0.05 rad/s is safe +HR_UNSAFE = 0.2 # |heading_rate| >= 0.2 rad/s is unsafe + +def parse_behavior_log(filename): + times = [] + accelerations = [] + heading_rates = [] + + with open(filename, 'r') as f: + first_time = None # Store the first timestamp for shifting + for line in f: + try: + entry = json.loads(line) + except json.JSONDecodeError: + print(f"Skipping invalid JSON line: {line.strip()}") + continue + if "vehicle" in entry: + t = entry.get("time") + vehicle_data = entry["vehicle"].get("data", {}) + acceleration = vehicle_data.get("acceleration") + heading_rate = vehicle_data.get("heading_rate") + if t is not None and acceleration is not None and heading_rate is not None: + if first_time is None: + first_time = t # Set the first timestamp + times.append(t - first_time) # Shift time to start from 0 + accelerations.append(acceleration) + heading_rates.append(heading_rate) + + return np.array(times), np.array(accelerations), np.array(heading_rates) + +def parse_tracker_csv(filename): + data = np.genfromtxt(filename, delimiter=',', skip_header=1) + first_time = data[0, 18] # Normalize time to start from 0 + cte_time = data[:, 18] - first_time + cte = data[:, 20] + x_actual, y_actual = data[:, 2], data[:, 5] + x_desired, y_desired = data[:, 8], data[:, 11] + return cte_time, cte, x_actual, y_actual, x_desired, y_desired + +def compute_derivative(times, values): + dt = np.diff(times) + dv = np.diff(values) + derivative = dv / dt + return times[1:], derivative + +def plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired): + global_min_time = min(time_jerk[0], time_heading_acc[0], cte_time[0]) + global_max_time = max(time_jerk[-1], time_heading_acc[-1], cte_time[-1]) + + fig, axs = plt.subplots(2, 2, figsize=(12, 8)) + fig.subplots_adjust(hspace=0.4, wspace=0.3) + + axs[0,0].plot(time_jerk, jerk, marker='', linestyle='-', color='blue') + axs[0,0].set_ylabel("Jerk (m/s³)") + axs[0,0].set_title("Vehicle Jerk Over Time") + axs[0,0].grid(True) + axs[0,0].set_xlim(global_min_time, global_max_time) + + axs[0,1].plot(time_heading_acc, heading_acc, marker='', linestyle='-', color='orange') + axs[0,1].set_ylabel("Heading Acceleration (rad/s²)") + axs[0,1].set_title("Vehicle Heading Acceleration Over Time") + axs[0,1].grid(True) + axs[0,1].set_xlim(global_min_time, global_max_time) + + axs[1,0].plot(cte_time, cte, marker='', linestyle='-', color='green') + axs[1,0].set_xlabel("Time (s)") + axs[1,0].set_ylabel("Cross Track Error") + axs[1,0].set_title("Vehicle Cross Track Error Over Time") + axs[1,0].grid(True) + axs[1,0].set_xlim(global_min_time, global_max_time) + + axs[1,1].plot(x_actual, y_actual, marker='', linestyle='-', color='blue', label='Actual') + axs[1,1].plot(x_desired, y_desired, marker='', linestyle='-', color='orange', label='Desired') + axs[1,1].set_xlabel("X Position (m)") + axs[1,1].set_ylabel("Y Position (m)") + axs[1,1].set_title("Vehicle Position") + axs[1,1].legend() + axs[1,1].grid(True) + + plt.show() + +if __name__=='__main__': + if len(sys.argv) != 2: + print("Usage: python test_comfort_metrics.py ") + sys.exit(1) + + log_dir = sys.argv[1] + behavior_file = os.path.join(log_dir, "behavior.json") + #tracker_file = os.path.join(log_dir, "PurePursuitTrajectoryTracker_debug.csv") + tracker_file = os.path.join(log_dir, "StanleyTrajectoryTracker_debug.csv") + + times, accelerations, heading_rates = parse_behavior_log(behavior_file) + time_jerk, jerk = compute_derivative(times, accelerations) + time_heading_acc, heading_acc = compute_derivative(times, heading_rates) + + cte_time, cte, x_actual, y_actual, x_desired, y_desired = parse_tracker_csv(tracker_file) + + plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired) + + print("Max (abs) jerk:", np.max(np.abs(jerk))) + print("Avg jerk:", np.mean(np.abs(jerk))) + print("Max (abs) heading acceleration:", np.max(np.abs(heading_acc))) + print("Avg heading acceleration:", np.mean(np.abs(heading_acc))) + print("Max (abs) cross track error:", np.max(np.abs(cte))) + print("Avg cross track error:", np.mean(np.abs(cte))) + From ab45c45d27c74a5ba55f764ef963e07b73072880 Mon Sep 17 00:00:00 2001 From: hhxjqm Date: Wed, 23 Apr 2025 19:26:58 +0000 Subject: [PATCH 09/71] update by Bo-Hao Wu's idea --- GEMstack/onboard/interface/gem_simulator.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/onboard/interface/gem_simulator.py b/GEMstack/onboard/interface/gem_simulator.py index 2abfde71f..bb49054b5 100644 --- a/GEMstack/onboard/interface/gem_simulator.py +++ b/GEMstack/onboard/interface/gem_simulator.py @@ -172,7 +172,7 @@ def simulate(self, T : float, command : Optional[GEMVehicleCommand]): #simulate actuators accelerator_pedal_position = np.clip(self.last_command.accelerator_pedal_position,0.0,1.0) brake_pedal_position = np.clip(self.last_command.brake_pedal_position,0.0,1.0) - acceleration = pedal_positions_to_acceleration(accelerator_pedal_position,brake_pedal_position,v,0,1) + acceleration = pedal_positions_to_acceleration(accelerator_pedal_position,brake_pedal_position,v,0,self.last_command.gear) acceleration = np.clip(acceleration,*self.dubins.accelRange) phides = steer2front(self.last_command.steering_wheel_angle) phides = np.clip(phides,*self.dubins.wheelAngleRange) From 7a9824fce3953a2cd4de08f894887244b5a45e60 Mon Sep 17 00:00:00 2001 From: hhxjqm Date: Wed, 23 Apr 2025 19:29:20 +0000 Subject: [PATCH 10/71] implement reverse stanley in reverse_stanley.py (need to merge with stanley.py --- GEMstack/knowledge/routes/reverse_15m.csv | 20 + GEMstack/onboard/planning/reverse_stanley.py | 405 +++++++++++++++++++ launch/fixed_route.yaml | 7 +- 3 files changed, 429 insertions(+), 3 deletions(-) create mode 100644 GEMstack/knowledge/routes/reverse_15m.csv create mode 100644 GEMstack/onboard/planning/reverse_stanley.py diff --git a/GEMstack/knowledge/routes/reverse_15m.csv b/GEMstack/knowledge/routes/reverse_15m.csv new file mode 100644 index 000000000..2311d2fc5 --- /dev/null +++ b/GEMstack/knowledge/routes/reverse_15m.csv @@ -0,0 +1,20 @@ +0.0,0,0 +-1.0,0,0 +-2.0,0,0 +-3.0,1.0,0 +-4,1.0,0 +-5,1.0,0 +-6,1.0,0 +-7,1.0,0 +-8,1.0,0 +-9,1.0,0 +-10,0,0 +-11,-1,0 +-12,-1,0 +-13,-1,0 +-14,-1,0 +-15,-1,0 +-16,-1,0 +-17,-1,0 +-18,-1,0 +-19,-1,0 diff --git a/GEMstack/onboard/planning/reverse_stanley.py b/GEMstack/onboard/planning/reverse_stanley.py new file mode 100644 index 000000000..197cbc537 --- /dev/null +++ b/GEMstack/onboard/planning/reverse_stanley.py @@ -0,0 +1,405 @@ +import numpy as np +from math import sin, cos, atan2 + +from ...mathutils.control import PID +from ...utils import settings +from ...mathutils import transforms +from ...knowledge.vehicle.geometry import front2steer +from ...knowledge.vehicle.dynamics import acceleration_to_pedal_positions +from ...state.vehicle import VehicleState, ObjectFrameEnum +from ...state.trajectory import Path, Trajectory +from ..interface.gem import GEMVehicleCommand +from ..component import Component + +##################################### +# 1. Angle normalization +##################################### +def normalise_angle(angle): + """Wraps an angle to the [-pi, pi] range.""" + while angle > np.pi: + angle -= 2.0 * np.pi + while angle < -np.pi: + angle += 2.0 * np.pi + return angle + +##################################### +# 2. Reverse-only Stanley controller with longitudinal PID +##################################### +class Stanley(object): + """ + A Stanley controller modified to ONLY perform reverse path tracking. + It uses the rear axle as the control reference and calculates + commands for backward motion. + """ + + def __init__( + self, + control_gain=None, + softening_gain=None, + desired_speed=None # This will be interpreted as desired speed magnitude + ): + """ + :param control_gain: Stanley lateral control gain k. + :param softening_gain: Softening gain k_soft. + :param desired_speed: Desired speed magnitude (float) or 'path'/'trajectory'. + The controller will always target a negative speed. + """ + + self.k = control_gain if control_gain is not None else settings.get('control.stanley.control_gain') + self.k_soft = softening_gain if softening_gain is not None else settings.get('control.stanley.softening_gain') + + self.max_steer = settings.get('vehicle.geometry.max_wheel_angle') + self.wheelbase = settings.get('vehicle.geometry.wheelbase') + + self.speed_limit = settings.get('vehicle.limits.max_speed') + self.reverse_speed_limit = settings.get('vehicle.limits.max_reverse_speed') # add for reve + self.max_accel = settings.get('vehicle.limits.max_acceleration') # Max positive acceleration magnitude + self.max_decel = settings.get('vehicle.limits.max_deceleration') # Max deceleration magnitude (used for max n egative accel) + + # PID for longitudinal speed tracking + p = settings.get('control.longitudinal_control.pid_p') + d = settings.get('control.longitudinal_control.pid_d') + i = settings.get('control.longitudinal_control.pid_i') + self.pid_speed = PID(p, d, i, windup_limit=20) + + # Speed source: numeric or derived from path/trajectory + if desired_speed is not None: + self.desired_speed_source = -abs(desired_speed) + else: + self.desired_speed_source = settings.get('control.stanley.desired_speed', 'path') + + if isinstance(self.desired_speed_source, (int, float)): + self.desired_speed_magnitude = self.desired_speed_source + else: + self.desired_speed_magnitude = None + + + # For path/trajectory + self.path_arg = None + self.path = None + self.trajectory = None + self.current_path_parameter = 0.0 + self.current_traj_parameter = 0.0 + self.t_last = None + + def set_path(self, path: Path): + """Sets the path or trajectory to track.""" + if path == self.path_arg: + return + self.path_arg = path + + # If the path has more than 2D, reduce to (x,y) + if len(path.points[0]) > 2: + path = path.get_dims([0,1]) + + # If no timing info, cannot use 'path'/'trajectory' speed + if not isinstance(path, Trajectory): + self.path = path.arc_length_parameterize() + self.trajectory = None + self.current_traj_parameter = 0.0 + # Error if speed source requires trajectory but none provided + if self.desired_speed_source in ['path', 'trajectory']: + raise ValueError("Stanley: Provided path has no timing. Either provide a Trajectory or set a constant desired_speed.") + else: + self.path = path.arc_length_parameterize() # Parameterize by arc length for lateral tracking + self.trajectory = path # Store the original trajectory object + self.current_traj_parameter = self.trajectory.domain()[0] + + # Reset path parameter to the start of the parameterized path + self.current_path_parameter = self.path.domain()[0] + + + # Helper to find rear axle position (Front axle not needed in reverse-only) + def _find_rear_axle_position(self, x, y, yaw): + """Compute rear-axle world position from the vehicle's reference point and yaw.""" + rx = x - self.wheelbase * cos(yaw) + ry = y - self.wheelbase * sin(yaw) + return rx, ry + + + # compute method hardcoded for reverse + def compute(self, state: VehicleState, component: Component = None): + """Compute the control outputs for reverse tracking: + (longitudinal acceleration, front wheel angle). + + :param state: Current vehicle state. + :param component: Optional component for debugging. + # No 'reverse' parameter here, always reverse + """ + t = state.pose.t + if self.t_last is None: + self.t_last = t + dt = 0 # Initialize dt to 0 on first iteration + else: + dt = t - self.t_last + + # Current vehicle states + curr_x = state.pose.x + curr_y = state.pose.y + curr_yaw = state.pose.yaw if state.pose.yaw is not None else 0.0 + # Use signed longitudinal velocity. Assuming state.v is signed. + velocity = state.v + + + # Must have a trajectory to track in ReverseStanley if speed source is 'path'/'trajectory' + # or if we need path domain/evaluation. Let's assume Path is always needed. + if self.path is None: + if component: + component.debug_event("No path provided to ReverseStanley controller. Doing nothing.") + self.t_last = t # Update t_last even if doing nothing + self.pid_speed.reset() # Reset PID if path is lost + return (0.0, 0.0) + + # Ensure same frame + if self.path.frame != state.pose.frame: + if component: + component.debug_event(f"Transforming path from {self.path.frame.name} to {state.pose.frame.name}") + # Use transforms utility correctly + self.path = transforms.transform_path(self.path, self.path.frame, state.pose.frame, current_pose=state.pose) + + # Must transform trajectory if using trajectory speed source + if self.trajectory and self.trajectory.frame != state.pose.frame: + if component: + component.debug_event(f"Transforming trajectory from {self.trajectory.frame.name} to {state.pose.frame.name}") + # Use transforms utility correctly for trajectory + self.trajectory = transforms.transform_path(self.trajectory, self.trajectory.frame, state.pose.frame, current_pose=state.pose) + + + # Reverse always uses the rear axle position as the reference + ref_x, ref_y = self._find_rear_axle_position(curr_x, curr_y, curr_yaw) + + + # 1) Closest point on the arc-length parameterized path + # Find closest point relative to the rear axle + # Search range around the current path parameter. + search_start = self.current_path_parameter - 5.0 # Search range behind current parameter + search_end = self.current_path_parameter + 5.0 # Search range ahead of current parameter + + # Pass ref_x, ref_y to closest_point_local + closest_dist, closest_parameter = self.path.closest_point_local((ref_x, ref_y), [search_start, search_end]) + + # Update path parameter based on closest point. + # Note: This simple update might not be ideal for transitions between forward/reverse segments + # if using a trajectory with mixed gears, but it works for full-path reverse. + self.current_path_parameter = closest_parameter + + + # 2) Path heading + target_x, target_y = self.path.eval(self.current_path_parameter) # Use updated parameter + tangent = self.path.eval_tangent(self.current_path_parameter) # Use updated parameter + path_yaw = atan2(tangent[1], tangent[0]) + + # 3) Lateral error calculation + # Calculate cross-track error from the reference point (rear axle) to the path. + dx = ref_x - target_x + dy = ref_y - target_y + # Calculate signed cross-track error relative to path tangent + # This gives signed distance to the "left" of the path tangent + cross_track_error = dx * (-tangent[1]) + dy * tangent[0] + + + # 4) Heading error + # The heading error is the difference between the path tangent's orientation + # and the vehicle's current orientation. + yaw_error = normalise_angle(path_yaw - curr_yaw) + + # 5) Stanley lateral control terms (hardcoded for reverse) + # For reverse, heading error term is typically inverted + heading_term = -yaw_error + # For reverse, cross-track error term's effect is inverted. + # Negate the cross_track_error input to atan2. + # Use absolute velocity in the denominator. Avoid division by small velocity. + cross_term_input = self.k * (-cross_track_error) / (self.k_soft + abs(velocity)) + cross_term = atan2(cross_term_input, 1.0) # atan2(y, x) gives angle of vector (x, y) + + # The final steering angle is the sum of adjusted terms + desired_steering_angle = heading_term + cross_term + + # Longitudinal Control - Hardcoded for reverse (negative speed) + desired_speed_magnitude = self.desired_speed_magnitude # Start with base desired speed magnitude + feedforward_accel = 0.0 # Initialize feedforward + + # If speed source is trajectory, get magnitude from trajectory + if self.desired_speed_source in ['path', 'trajectory']: + # Get speed magnitude from trajectory at current parameter + # Assuming trajectory.eval_derivative(s) returns velocity vector at path parameter 's' + # or we map s to trajectory's native parameter (like time) if needed + current_trajectory_param_for_eval = self.current_path_parameter # Start with s + if hasattr(self.trajectory, 'parameter_to_time'): # Check if time mapping is possible + current_trajectory_param_for_eval = self.trajectory.parameter_to_time(self.current_path_parameter) + # Fallback to using path parameter 's' if time mapping fails is removed + + # Evaluate derivative (assumed to return velocity vector along path) + if self.trajectory is not None: # Ensure trajectory object exists + deriv = self.trajectory.eval_derivative(current_trajectory_param_for_eval) + path_speed_magnitude = np.linalg.norm(deriv) # Get speed magnitude + else: + path_speed_magnitude = 0.0 # Default if trajectory object somehow missing + + # Set desired speed magnitude, clipped by limit + desired_speed_magnitude = min(path_speed_magnitude, self.reverse_speed_limit) + + + # Apply direction sign (always negative for ReverseStanley) + desired_speed = desired_speed_magnitude * -1.0 + + + # Check for reaching the START of the path (for stopping in reverse) + is_at_start_backward = self.current_path_parameter <= self.path.domain()[0] + 1e-3 # Close to start parameter + + if is_at_start_backward: + # Reached the start of the path in reverse -> stop + # if component: + # component.debug_event(f"ReverseStanley: Reached start of path, stopping.") + desired_speed = 0.0 + # Apply braking force when moving negatively (positive acceleration) + feedforward_accel = -2.0 * -1.0 # = +2.0 + + else: + # Calculate feedforward acceleration based on speed change (still assuming magnitude from trajectory) + difference_dt = 0.1 + # Estimate future path parameter based on current speed magnitude and reverse direction + current_speed_abs = abs(velocity) # Use current vehicle speed magnitude + # Avoid division by near zero when estimating future parameter distance + if current_speed_abs < 0.1: estimated_path_step = 0.1 * difference_dt * -1.0 # Fixed step backwards + else: estimated_path_step = current_speed_abs * difference_dt * -1.0 # Step backwards based on speed + + future_parameter = self.current_path_parameter + estimated_path_step # Move along path towards start + + # Clip future parameter to path domain (ensure it doesn't go before start) + future_parameter = np.clip(future_parameter, self.path.domain()[0], self.path.domain()[-1]) + + # Need to map future_parameter (s) back to trajectory's native parameter if necessary + future_trajectory_param_for_eval = future_parameter # Start with s + if hasattr(self.trajectory, 'parameter_to_time'): # Check if time mapping is possible + future_trajectory_param_for_eval = self.trajectory.parameter_to_time(future_parameter) + # Fallback to using path parameter 's' if time mapping fails is removed + + # Evaluate derivative at future point (assumed to return velocity vector along path) + if self.trajectory is not None: # Ensure trajectory object exists + future_deriv = self.trajectory.eval_derivative(future_trajectory_param_for_eval) + next_path_speed_magnitude = min(np.linalg.norm(future_deriv), self.speed_limit) + else: + next_path_speed_magnitude = 0.0 # Default if trajectory object somehow missing + + # Apply direction sign (always negative) + next_desired_speed = next_path_speed_magnitude * -1.0 + + # Estimate acceleration = delta_v / delta_t + # Use the fixed difference_dt for acceleration calculation + feedforward_accel = (next_desired_speed - desired_speed) / difference_dt + feedforward_accel = np.clip(feedforward_accel, -self.max_decel, self.max_accel) + + + # Cross-track-based slowdown applied to speed magnitude (uses abs error) + desired_speed_magnitude_slowed = abs(desired_speed) * np.exp(-abs(cross_track_error) * 0.6) + + # Reapply direction sign (always negative) + desired_speed = desired_speed_magnitude_slowed * -1.0 if desired_speed_magnitude_slowed != 0 else 0.0 + + + # Clip to speed limit (magnitude) + if abs(desired_speed) > self.speed_limit: + desired_speed = self.speed_limit * -1.0 if desired_speed != 0 else 0.0 + + + # PID for longitudinal control + # Use signed velocity for error calculation + speed_error = desired_speed - velocity + output_accel = self.pid_speed.advance(e=speed_error, t=t, feedforward_term=feedforward_accel) + + # Clip acceleration + # output_accel can be positive (forward accel / reverse brake) or negative (forward brake / reverse accel) + if output_accel > self.max_accel: # Max positive accel + output_accel = self.max_accel + elif output_accel < -self.max_decel: # Max negative accel (corresponding to max decel magnitude) + output_accel = -self.max_decel + + + # Avoid acceleration from near zero if desired speed is zero + # This prevents "creep" when stopped. + if desired_speed == 0 and abs(velocity) < 0.05: # Use small epsilon for near zero velocity + # Check if output_accel would increase speed magnitude away from zero + # If current velocity > 0, positive accel increases speed. If velocity < 0, negative accel increases speed (makes it more negative). + # If velocity is 0, any non-zero accel would move away from zero. + if (velocity * output_accel > 0) or (abs(velocity) < 1e-3 and output_accel != 0): # Use smaller epsilon for the zero check + output_accel = 0.0 + + + + + # Debug + if component is not None: + component.debug('curr pt',(curr_x,curr_y)) + component.debug(" ", self.current_path_parameter) + component.debug("crosstrack error", cross_track_error) + component.debug("crosstrack dist", closest_dist) + component.debug("yaw_error", yaw_error) + component.debug('steering_angle', desired_steering_angle) + component.debug("desired_speed (m/s)", desired_speed) + component.debug("feedforward_accel (m/s^2)", feedforward_accel) + component.debug(" output_accel (m/s^2)", output_accel) + + self.t_last = t + # Return signed acceleration and desired front wheel steering angle + return (output_accel, desired_steering_angle) + +##################################### +# 3. Tracker component +##################################### +class StanleyTrajectoryTracker(Component): + """ + A trajectory-tracking Component that uses the above Stanley controller + for lateral control plus PID-based longitudinal control. + It now includes measures to mitigate oscillations. + """ + + def __init__(self, vehicle_interface=None, **kwargs): + """ + :param vehicle_interface: The low-level interface to send commands to the vehicle. + :param kwargs: Optional parameters to pass into the Stanley(...) constructor. + """ + self.stanley = Stanley(**kwargs) + self.vehicle_interface = vehicle_interface + + def rate(self): + """Control frequency in Hz.""" + return 50.0 + + def state_inputs(self): + """ + Required state inputs: + - Vehicle state + - Trajectory + """ + return ["vehicle", "trajectory"] + + def state_outputs(self): + """No direct output state here.""" + return [] + + def update(self, vehicle: VehicleState, trajectory: Trajectory): + """ + Per control cycle: + 1) Set the path/trajectory + 2) Compute (acceleration, front wheel angle) + 3) Convert front wheel angle to steering wheel angle (if necessary) + 4) Send command to the vehicle + """ + self.stanley.set_path(trajectory) + accel, f_delta = self.stanley.compute(vehicle, self) + + # If your low-level interface expects steering wheel angle: + steering_angle = front2steer(f_delta) + steering_angle = np.clip( + steering_angle, + settings.get('vehicle.geometry.min_steering_angle', -0.5), + settings.get('vehicle.geometry.max_steering_angle', 0.5) + ) + + cmd = self.vehicle_interface.simple_command(accel, steering_angle, vehicle) + self.vehicle_interface.send_command(cmd) + + def healthy(self): + """Optional: check if the controller has a valid path.""" + return self.stanley.path is not None diff --git a/launch/fixed_route.yaml b/launch/fixed_route.yaml index 3e28ea668..904e82f2e 100644 --- a/launch/fixed_route.yaml +++ b/launch/fixed_route.yaml @@ -16,13 +16,14 @@ drive: planning: route_planning: type: StaticRoutePlanner - args: [!relative_path '../GEMstack/knowledge/routes/xyhead_highbay_backlot_p.csv'] + args: [!relative_path '../GEMstack/knowledge/routes/forward_15m.csv'] + # args: [!relative_path '../GEMstack/knowledge/routes/reverse_15m.csv'] motion_planning: type: RouteToTrajectoryPlanner args: [null] #desired speed in m/s. If null, this will keep the route untimed for the trajectory tracker trajectory_tracking: - #type: pure_pursuit.PurePursuitTrajectoryTracker - type: stanley.StanleyTrajectoryTracker + type: pure_pursuit.PurePursuitTrajectoryTracker + # type: reverse_stanley.StanleyTrajectoryTracker args: {desired_speed: 2.5} #approximately 5mph print: False log: From d95582d7e2aaef647db069c0ea5bc2a089baea14 Mon Sep 17 00:00:00 2001 From: hhxjqm Date: Fri, 25 Apr 2025 05:48:52 +0000 Subject: [PATCH 11/71] Merge reverse_stanley.py and stanley.py --- GEMstack/onboard/planning/reverse_stanley.py | 405 ------------------- GEMstack/onboard/planning/stanley.py | 239 ++++++----- launch/fixed_route.yaml | 8 +- 3 files changed, 133 insertions(+), 519 deletions(-) delete mode 100644 GEMstack/onboard/planning/reverse_stanley.py diff --git a/GEMstack/onboard/planning/reverse_stanley.py b/GEMstack/onboard/planning/reverse_stanley.py deleted file mode 100644 index 197cbc537..000000000 --- a/GEMstack/onboard/planning/reverse_stanley.py +++ /dev/null @@ -1,405 +0,0 @@ -import numpy as np -from math import sin, cos, atan2 - -from ...mathutils.control import PID -from ...utils import settings -from ...mathutils import transforms -from ...knowledge.vehicle.geometry import front2steer -from ...knowledge.vehicle.dynamics import acceleration_to_pedal_positions -from ...state.vehicle import VehicleState, ObjectFrameEnum -from ...state.trajectory import Path, Trajectory -from ..interface.gem import GEMVehicleCommand -from ..component import Component - -##################################### -# 1. Angle normalization -##################################### -def normalise_angle(angle): - """Wraps an angle to the [-pi, pi] range.""" - while angle > np.pi: - angle -= 2.0 * np.pi - while angle < -np.pi: - angle += 2.0 * np.pi - return angle - -##################################### -# 2. Reverse-only Stanley controller with longitudinal PID -##################################### -class Stanley(object): - """ - A Stanley controller modified to ONLY perform reverse path tracking. - It uses the rear axle as the control reference and calculates - commands for backward motion. - """ - - def __init__( - self, - control_gain=None, - softening_gain=None, - desired_speed=None # This will be interpreted as desired speed magnitude - ): - """ - :param control_gain: Stanley lateral control gain k. - :param softening_gain: Softening gain k_soft. - :param desired_speed: Desired speed magnitude (float) or 'path'/'trajectory'. - The controller will always target a negative speed. - """ - - self.k = control_gain if control_gain is not None else settings.get('control.stanley.control_gain') - self.k_soft = softening_gain if softening_gain is not None else settings.get('control.stanley.softening_gain') - - self.max_steer = settings.get('vehicle.geometry.max_wheel_angle') - self.wheelbase = settings.get('vehicle.geometry.wheelbase') - - self.speed_limit = settings.get('vehicle.limits.max_speed') - self.reverse_speed_limit = settings.get('vehicle.limits.max_reverse_speed') # add for reve - self.max_accel = settings.get('vehicle.limits.max_acceleration') # Max positive acceleration magnitude - self.max_decel = settings.get('vehicle.limits.max_deceleration') # Max deceleration magnitude (used for max n egative accel) - - # PID for longitudinal speed tracking - p = settings.get('control.longitudinal_control.pid_p') - d = settings.get('control.longitudinal_control.pid_d') - i = settings.get('control.longitudinal_control.pid_i') - self.pid_speed = PID(p, d, i, windup_limit=20) - - # Speed source: numeric or derived from path/trajectory - if desired_speed is not None: - self.desired_speed_source = -abs(desired_speed) - else: - self.desired_speed_source = settings.get('control.stanley.desired_speed', 'path') - - if isinstance(self.desired_speed_source, (int, float)): - self.desired_speed_magnitude = self.desired_speed_source - else: - self.desired_speed_magnitude = None - - - # For path/trajectory - self.path_arg = None - self.path = None - self.trajectory = None - self.current_path_parameter = 0.0 - self.current_traj_parameter = 0.0 - self.t_last = None - - def set_path(self, path: Path): - """Sets the path or trajectory to track.""" - if path == self.path_arg: - return - self.path_arg = path - - # If the path has more than 2D, reduce to (x,y) - if len(path.points[0]) > 2: - path = path.get_dims([0,1]) - - # If no timing info, cannot use 'path'/'trajectory' speed - if not isinstance(path, Trajectory): - self.path = path.arc_length_parameterize() - self.trajectory = None - self.current_traj_parameter = 0.0 - # Error if speed source requires trajectory but none provided - if self.desired_speed_source in ['path', 'trajectory']: - raise ValueError("Stanley: Provided path has no timing. Either provide a Trajectory or set a constant desired_speed.") - else: - self.path = path.arc_length_parameterize() # Parameterize by arc length for lateral tracking - self.trajectory = path # Store the original trajectory object - self.current_traj_parameter = self.trajectory.domain()[0] - - # Reset path parameter to the start of the parameterized path - self.current_path_parameter = self.path.domain()[0] - - - # Helper to find rear axle position (Front axle not needed in reverse-only) - def _find_rear_axle_position(self, x, y, yaw): - """Compute rear-axle world position from the vehicle's reference point and yaw.""" - rx = x - self.wheelbase * cos(yaw) - ry = y - self.wheelbase * sin(yaw) - return rx, ry - - - # compute method hardcoded for reverse - def compute(self, state: VehicleState, component: Component = None): - """Compute the control outputs for reverse tracking: - (longitudinal acceleration, front wheel angle). - - :param state: Current vehicle state. - :param component: Optional component for debugging. - # No 'reverse' parameter here, always reverse - """ - t = state.pose.t - if self.t_last is None: - self.t_last = t - dt = 0 # Initialize dt to 0 on first iteration - else: - dt = t - self.t_last - - # Current vehicle states - curr_x = state.pose.x - curr_y = state.pose.y - curr_yaw = state.pose.yaw if state.pose.yaw is not None else 0.0 - # Use signed longitudinal velocity. Assuming state.v is signed. - velocity = state.v - - - # Must have a trajectory to track in ReverseStanley if speed source is 'path'/'trajectory' - # or if we need path domain/evaluation. Let's assume Path is always needed. - if self.path is None: - if component: - component.debug_event("No path provided to ReverseStanley controller. Doing nothing.") - self.t_last = t # Update t_last even if doing nothing - self.pid_speed.reset() # Reset PID if path is lost - return (0.0, 0.0) - - # Ensure same frame - if self.path.frame != state.pose.frame: - if component: - component.debug_event(f"Transforming path from {self.path.frame.name} to {state.pose.frame.name}") - # Use transforms utility correctly - self.path = transforms.transform_path(self.path, self.path.frame, state.pose.frame, current_pose=state.pose) - - # Must transform trajectory if using trajectory speed source - if self.trajectory and self.trajectory.frame != state.pose.frame: - if component: - component.debug_event(f"Transforming trajectory from {self.trajectory.frame.name} to {state.pose.frame.name}") - # Use transforms utility correctly for trajectory - self.trajectory = transforms.transform_path(self.trajectory, self.trajectory.frame, state.pose.frame, current_pose=state.pose) - - - # Reverse always uses the rear axle position as the reference - ref_x, ref_y = self._find_rear_axle_position(curr_x, curr_y, curr_yaw) - - - # 1) Closest point on the arc-length parameterized path - # Find closest point relative to the rear axle - # Search range around the current path parameter. - search_start = self.current_path_parameter - 5.0 # Search range behind current parameter - search_end = self.current_path_parameter + 5.0 # Search range ahead of current parameter - - # Pass ref_x, ref_y to closest_point_local - closest_dist, closest_parameter = self.path.closest_point_local((ref_x, ref_y), [search_start, search_end]) - - # Update path parameter based on closest point. - # Note: This simple update might not be ideal for transitions between forward/reverse segments - # if using a trajectory with mixed gears, but it works for full-path reverse. - self.current_path_parameter = closest_parameter - - - # 2) Path heading - target_x, target_y = self.path.eval(self.current_path_parameter) # Use updated parameter - tangent = self.path.eval_tangent(self.current_path_parameter) # Use updated parameter - path_yaw = atan2(tangent[1], tangent[0]) - - # 3) Lateral error calculation - # Calculate cross-track error from the reference point (rear axle) to the path. - dx = ref_x - target_x - dy = ref_y - target_y - # Calculate signed cross-track error relative to path tangent - # This gives signed distance to the "left" of the path tangent - cross_track_error = dx * (-tangent[1]) + dy * tangent[0] - - - # 4) Heading error - # The heading error is the difference between the path tangent's orientation - # and the vehicle's current orientation. - yaw_error = normalise_angle(path_yaw - curr_yaw) - - # 5) Stanley lateral control terms (hardcoded for reverse) - # For reverse, heading error term is typically inverted - heading_term = -yaw_error - # For reverse, cross-track error term's effect is inverted. - # Negate the cross_track_error input to atan2. - # Use absolute velocity in the denominator. Avoid division by small velocity. - cross_term_input = self.k * (-cross_track_error) / (self.k_soft + abs(velocity)) - cross_term = atan2(cross_term_input, 1.0) # atan2(y, x) gives angle of vector (x, y) - - # The final steering angle is the sum of adjusted terms - desired_steering_angle = heading_term + cross_term - - # Longitudinal Control - Hardcoded for reverse (negative speed) - desired_speed_magnitude = self.desired_speed_magnitude # Start with base desired speed magnitude - feedforward_accel = 0.0 # Initialize feedforward - - # If speed source is trajectory, get magnitude from trajectory - if self.desired_speed_source in ['path', 'trajectory']: - # Get speed magnitude from trajectory at current parameter - # Assuming trajectory.eval_derivative(s) returns velocity vector at path parameter 's' - # or we map s to trajectory's native parameter (like time) if needed - current_trajectory_param_for_eval = self.current_path_parameter # Start with s - if hasattr(self.trajectory, 'parameter_to_time'): # Check if time mapping is possible - current_trajectory_param_for_eval = self.trajectory.parameter_to_time(self.current_path_parameter) - # Fallback to using path parameter 's' if time mapping fails is removed - - # Evaluate derivative (assumed to return velocity vector along path) - if self.trajectory is not None: # Ensure trajectory object exists - deriv = self.trajectory.eval_derivative(current_trajectory_param_for_eval) - path_speed_magnitude = np.linalg.norm(deriv) # Get speed magnitude - else: - path_speed_magnitude = 0.0 # Default if trajectory object somehow missing - - # Set desired speed magnitude, clipped by limit - desired_speed_magnitude = min(path_speed_magnitude, self.reverse_speed_limit) - - - # Apply direction sign (always negative for ReverseStanley) - desired_speed = desired_speed_magnitude * -1.0 - - - # Check for reaching the START of the path (for stopping in reverse) - is_at_start_backward = self.current_path_parameter <= self.path.domain()[0] + 1e-3 # Close to start parameter - - if is_at_start_backward: - # Reached the start of the path in reverse -> stop - # if component: - # component.debug_event(f"ReverseStanley: Reached start of path, stopping.") - desired_speed = 0.0 - # Apply braking force when moving negatively (positive acceleration) - feedforward_accel = -2.0 * -1.0 # = +2.0 - - else: - # Calculate feedforward acceleration based on speed change (still assuming magnitude from trajectory) - difference_dt = 0.1 - # Estimate future path parameter based on current speed magnitude and reverse direction - current_speed_abs = abs(velocity) # Use current vehicle speed magnitude - # Avoid division by near zero when estimating future parameter distance - if current_speed_abs < 0.1: estimated_path_step = 0.1 * difference_dt * -1.0 # Fixed step backwards - else: estimated_path_step = current_speed_abs * difference_dt * -1.0 # Step backwards based on speed - - future_parameter = self.current_path_parameter + estimated_path_step # Move along path towards start - - # Clip future parameter to path domain (ensure it doesn't go before start) - future_parameter = np.clip(future_parameter, self.path.domain()[0], self.path.domain()[-1]) - - # Need to map future_parameter (s) back to trajectory's native parameter if necessary - future_trajectory_param_for_eval = future_parameter # Start with s - if hasattr(self.trajectory, 'parameter_to_time'): # Check if time mapping is possible - future_trajectory_param_for_eval = self.trajectory.parameter_to_time(future_parameter) - # Fallback to using path parameter 's' if time mapping fails is removed - - # Evaluate derivative at future point (assumed to return velocity vector along path) - if self.trajectory is not None: # Ensure trajectory object exists - future_deriv = self.trajectory.eval_derivative(future_trajectory_param_for_eval) - next_path_speed_magnitude = min(np.linalg.norm(future_deriv), self.speed_limit) - else: - next_path_speed_magnitude = 0.0 # Default if trajectory object somehow missing - - # Apply direction sign (always negative) - next_desired_speed = next_path_speed_magnitude * -1.0 - - # Estimate acceleration = delta_v / delta_t - # Use the fixed difference_dt for acceleration calculation - feedforward_accel = (next_desired_speed - desired_speed) / difference_dt - feedforward_accel = np.clip(feedforward_accel, -self.max_decel, self.max_accel) - - - # Cross-track-based slowdown applied to speed magnitude (uses abs error) - desired_speed_magnitude_slowed = abs(desired_speed) * np.exp(-abs(cross_track_error) * 0.6) - - # Reapply direction sign (always negative) - desired_speed = desired_speed_magnitude_slowed * -1.0 if desired_speed_magnitude_slowed != 0 else 0.0 - - - # Clip to speed limit (magnitude) - if abs(desired_speed) > self.speed_limit: - desired_speed = self.speed_limit * -1.0 if desired_speed != 0 else 0.0 - - - # PID for longitudinal control - # Use signed velocity for error calculation - speed_error = desired_speed - velocity - output_accel = self.pid_speed.advance(e=speed_error, t=t, feedforward_term=feedforward_accel) - - # Clip acceleration - # output_accel can be positive (forward accel / reverse brake) or negative (forward brake / reverse accel) - if output_accel > self.max_accel: # Max positive accel - output_accel = self.max_accel - elif output_accel < -self.max_decel: # Max negative accel (corresponding to max decel magnitude) - output_accel = -self.max_decel - - - # Avoid acceleration from near zero if desired speed is zero - # This prevents "creep" when stopped. - if desired_speed == 0 and abs(velocity) < 0.05: # Use small epsilon for near zero velocity - # Check if output_accel would increase speed magnitude away from zero - # If current velocity > 0, positive accel increases speed. If velocity < 0, negative accel increases speed (makes it more negative). - # If velocity is 0, any non-zero accel would move away from zero. - if (velocity * output_accel > 0) or (abs(velocity) < 1e-3 and output_accel != 0): # Use smaller epsilon for the zero check - output_accel = 0.0 - - - - - # Debug - if component is not None: - component.debug('curr pt',(curr_x,curr_y)) - component.debug(" ", self.current_path_parameter) - component.debug("crosstrack error", cross_track_error) - component.debug("crosstrack dist", closest_dist) - component.debug("yaw_error", yaw_error) - component.debug('steering_angle', desired_steering_angle) - component.debug("desired_speed (m/s)", desired_speed) - component.debug("feedforward_accel (m/s^2)", feedforward_accel) - component.debug(" output_accel (m/s^2)", output_accel) - - self.t_last = t - # Return signed acceleration and desired front wheel steering angle - return (output_accel, desired_steering_angle) - -##################################### -# 3. Tracker component -##################################### -class StanleyTrajectoryTracker(Component): - """ - A trajectory-tracking Component that uses the above Stanley controller - for lateral control plus PID-based longitudinal control. - It now includes measures to mitigate oscillations. - """ - - def __init__(self, vehicle_interface=None, **kwargs): - """ - :param vehicle_interface: The low-level interface to send commands to the vehicle. - :param kwargs: Optional parameters to pass into the Stanley(...) constructor. - """ - self.stanley = Stanley(**kwargs) - self.vehicle_interface = vehicle_interface - - def rate(self): - """Control frequency in Hz.""" - return 50.0 - - def state_inputs(self): - """ - Required state inputs: - - Vehicle state - - Trajectory - """ - return ["vehicle", "trajectory"] - - def state_outputs(self): - """No direct output state here.""" - return [] - - def update(self, vehicle: VehicleState, trajectory: Trajectory): - """ - Per control cycle: - 1) Set the path/trajectory - 2) Compute (acceleration, front wheel angle) - 3) Convert front wheel angle to steering wheel angle (if necessary) - 4) Send command to the vehicle - """ - self.stanley.set_path(trajectory) - accel, f_delta = self.stanley.compute(vehicle, self) - - # If your low-level interface expects steering wheel angle: - steering_angle = front2steer(f_delta) - steering_angle = np.clip( - steering_angle, - settings.get('vehicle.geometry.min_steering_angle', -0.5), - settings.get('vehicle.geometry.max_steering_angle', 0.5) - ) - - cmd = self.vehicle_interface.simple_command(accel, steering_angle, vehicle) - self.vehicle_interface.send_command(cmd) - - def healthy(self): - """Optional: check if the controller has a valid path.""" - return self.stanley.path is not None diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley.py index 19efbfbf4..f820ff9b9 100644 --- a/GEMstack/onboard/planning/stanley.py +++ b/GEMstack/onboard/planning/stanley.py @@ -1,7 +1,6 @@ import numpy as np from math import sin, cos, atan2 -# These imports match your existing project structure from ...mathutils.control import PID from ...utils import settings from ...mathutils import transforms @@ -27,15 +26,6 @@ def normalise_angle(angle): # 2. Stanley-based controller with longitudinal PID ##################################### class Stanley(object): - """ - A Stanley controller that handles lateral control (steering) - plus a basic longitudinal control (PID + optional feedforward). - It has been modified to reduce oscillations by: - 1) Lower gains - 2) Steering damping - 3) Low-pass filter on steering - 4) Gentler speed logic when cornering - """ def __init__( self, @@ -46,33 +36,32 @@ def __init__( """ :param control_gain: Stanley lateral control gain k (lowered from the default to reduce overshoot). :param softening_gain: Softening gain k_soft. - :param yaw_rate_gain: Yaw-rate gain k_yaw_rate (helps reduce oscillations). - :param steering_damp_gain: Steering damping gain k_damp_steer. :param desired_speed: Desired speed (float) or 'path'/'trajectory'. """ - - # 1) Lower Gains self.k = control_gain if control_gain is not None else settings.get('control.stanley.control_gain') self.k_soft = softening_gain if softening_gain is not None else settings.get('control.stanley.softening_gain') - # Typically, this is the max front-wheel steering angle in radians self.max_steer = settings.get('vehicle.geometry.max_wheel_angle') self.wheelbase = settings.get('vehicle.geometry.wheelbase') - # Basic longitudinal constraints self.speed_limit = settings.get('vehicle.limits.max_speed') - self.max_accel = settings.get('vehicle.limits.max_acceleration') - self.max_decel = settings.get('vehicle.limits.max_deceleration') + self.reverse_speed_limit = settings.get('vehicle.limits.max_reverse_speed') + self.max_accel = settings.get('vehicle.limits.max_acceleration') + self.max_decel = settings.get('vehicle.limits.max_deceleration') - # PID for longitudinal speed tracking p = settings.get('control.longitudinal_control.pid_p') d = settings.get('control.longitudinal_control.pid_d') i = settings.get('control.longitudinal_control.pid_i') self.pid_speed = PID(p, d, i, windup_limit=20) - # Speed source: numeric or derived from path/trajectory + reverse = True + # reverse = something_here() # TODO: Implement reverse logic + if desired_speed is not None: - self.desired_speed_source = desired_speed + if reverse: + self.desired_speed_source = -abs(desired_speed) + else: + self.desired_speed_source = abs(desired_speed) else: self.desired_speed_source = settings.get('control.stanley.desired_speed', 'path') @@ -81,7 +70,6 @@ def __init__( else: self.desired_speed = None - # For path/trajectory self.path_arg = None self.path = None self.trajectory = None @@ -90,16 +78,13 @@ def __init__( self.t_last = None def set_path(self, path: Path): - """Sets the path or trajectory to track.""" if path == self.path_arg: return self.path_arg = path - # If the path has more than 2D, reduce to (x,y) if len(path.points[0]) > 2: path = path.get_dims([0,1]) - # If no timing info, we can't rely on 'path'/'trajectory' speed if not isinstance(path, Trajectory): self.path = path.arc_length_parameterize() self.trajectory = None @@ -111,7 +96,7 @@ def set_path(self, path: Path): self.trajectory = path self.current_traj_parameter = self.trajectory.domain()[0] - self.current_path_parameter = 0.0 + self.current_path_parameter = self.path.domain()[0] def _find_front_axle_position(self, x, y, yaw): """Compute front-axle world position from the center/rear and yaw.""" @@ -119,14 +104,21 @@ def _find_front_axle_position(self, x, y, yaw): fy = y + self.wheelbase * sin(yaw) return fx, fy - def compute(self, state: VehicleState, component: Component = None): + def _find_rear_axle_position(self, x, y, yaw): + """Compute rear-axle world position from the vehicle's reference point and yaw.""" + rx = x - self.wheelbase * cos(yaw) + ry = y - self.wheelbase * sin(yaw) + return rx, ry + + def compute(self, state: VehicleState, component: Component = None, reverse = True): # TODO change only during debugging """Compute the control outputs: (longitudinal acceleration, front wheel angle).""" t = state.pose.t if self.t_last is None: self.t_last = t - dt = t - self.t_last + dt = 0 + else: + dt = t - self.t_last - # Current vehicle states curr_x = state.pose.x curr_y = state.pose.y curr_yaw = state.pose.yaw if state.pose.yaw is not None else 0.0 @@ -135,104 +127,157 @@ def compute(self, state: VehicleState, component: Component = None): if self.path is None: if component: component.debug_event("No path provided to Stanley controller. Doing nothing.") + self.t_last = t + self.pid_speed.reset() return (0.0, 0.0) - # Ensure same frame if self.path.frame != state.pose.frame: if component: component.debug_event(f"Transforming path from {self.path.frame.name} to {state.pose.frame.name}") - self.path = self.path.to_frame(state.pose.frame, current_pose=state.pose) + self.path = transforms.transform_path(self.path, self.path.frame, state.pose.frame, current_pose=state.pose) if self.trajectory and self.trajectory.frame != state.pose.frame: if component: component.debug_event(f"Transforming trajectory from {self.trajectory.frame.name} to {state.pose.frame.name}") - self.trajectory = self.trajectory.to_frame(state.pose.frame, current_pose=state.pose) + self.trajectory = transforms.transform_path(self.trajectory, self.trajectory.frame, state.pose.frame, current_pose=state.pose) - # 1) Closest point - fx, fy = self._find_front_axle_position(curr_x, curr_y, curr_yaw) + if reverse: + fx, fy = self._find_rear_axle_position(curr_x, curr_y, curr_yaw) + else: + fx, fy = self._find_front_axle_position(curr_x, curr_y, curr_yaw) search_start = self.current_path_parameter - 5.0 search_end = self.current_path_parameter + 5.0 closest_dist, closest_parameter = self.path.closest_point_local((fx, fy), [search_start, search_end]) self.current_path_parameter = closest_parameter - # 2) Path heading - target_x, target_y = self.path.eval(closest_parameter) - tangent = self.path.eval_tangent(closest_parameter) + target_x, target_y = self.path.eval(self.current_path_parameter) + tangent = self.path.eval_tangent(self.current_path_parameter) path_yaw = atan2(tangent[1], tangent[0]) - desired_x = target_x - desired_y = target_y - # 3) Lateral error + dx = fx - target_x dy = fy - target_y - left_vec = np.array([sin(curr_yaw), -cos(curr_yaw)]) - cross_track_error = np.sign(np.dot(np.array([dx, dy]), left_vec)) * closest_dist - # 4) Heading error - yaw_error = normalise_angle(path_yaw - curr_yaw) + if reverse: + cross_track_error = dx * (-tangent[1]) + dy * tangent[0] + else: + left_vec = np.array([sin(curr_yaw), -cos(curr_yaw)]) + cross_track_error = np.sign(np.dot(np.array([dx, dy]), left_vec)) * closest_dist - # 5) Standard Stanley terms - cross_term = atan2(self.k * cross_track_error, self.k_soft + speed) - desired_steering_angle = yaw_error + cross_term + yaw_error = normalise_angle(path_yaw - curr_yaw) desired_speed = self.desired_speed feedforward_accel = 0.0 - if self.trajectory and self.desired_speed_source in ['path', 'trajectory']: - if len(self.trajectory.points) < 2 or self.current_path_parameter >= self.path.domain()[1]: - # End of trajectory -> stop - if component: - component.debug_event("Stanley: Past the end of trajectory, stopping.") + if reverse: + heading_term = -yaw_error + cross_term_input = self.k * (-cross_track_error) / (self.k_soft + abs(speed)) + cross_term = atan2(cross_term_input, 1.0) + desired_steering_angle = heading_term + cross_term + + if self.trajectory and self.desired_speed_source in ['path', 'trajectory']: + current_trajectory_param_for_eval = self.current_path_parameter + if hasattr(self.trajectory, 'parameter_to_time'): + current_trajectory_param_for_eval = self.trajectory.parameter_to_time(self.current_path_parameter) + + if self.trajectory is not None: + deriv = self.trajectory.eval_derivative(current_trajectory_param_for_eval) + path_speed_magnitude = np.linalg.norm(deriv) + else: + path_speed_magnitude = 0.0 + + desired_speed = min(path_speed_magnitude, self.reverse_speed_limit) + + desired_speed = desired_speed * -1.0 + + is_at_start_backward = self.current_path_parameter <= self.path.domain()[0] + 1e-3 + + if is_at_start_backward: desired_speed = 0.0 - feedforward_accel = -2.0 + feedforward_accel = -2.0 * -1.0 # = +2.0 + else: - if self.desired_speed_source == 'path': - current_trajectory_time = self.trajectory.parameter_to_time(self.current_path_parameter) + difference_dt = 0.1 + current_speed_abs = abs(speed) + if current_speed_abs < 0.1: estimated_path_step = 0.1 * difference_dt * -1.0 + else: estimated_path_step = current_speed_abs * difference_dt * -1.0 + + future_parameter = self.current_path_parameter + estimated_path_step + + future_parameter = np.clip(future_parameter, self.path.domain()[0], self.path.domain()[-1]) + + future_trajectory_param_for_eval = future_parameter + if hasattr(self.trajectory, 'parameter_to_time'): + future_trajectory_param_for_eval = self.trajectory.parameter_to_time(future_parameter) + + if self.trajectory is not None: + future_deriv = self.trajectory.eval_derivative(future_trajectory_param_for_eval) + next_path_speed_magnitude = min(np.linalg.norm(future_deriv), self.speed_limit) else: - self.current_traj_parameter += dt - current_trajectory_time = self.current_traj_parameter + next_path_speed_magnitude = 0.0 - deriv = self.trajectory.eval_derivative(current_trajectory_time) - desired_speed = min(np.linalg.norm(deriv), self.speed_limit) + next_desired_speed = next_path_speed_magnitude * -1.0 - difference_dt = 0.1 - future_t = current_trajectory_time + difference_dt - if future_t > self.trajectory.domain()[1]: - future_t = self.trajectory.domain()[1] - future_deriv = self.trajectory.eval_derivative(future_t) - next_desired_speed = min(np.linalg.norm(future_deriv), self.speed_limit) feedforward_accel = (next_desired_speed - desired_speed) / difference_dt feedforward_accel = np.clip(feedforward_accel, -self.max_decel, self.max_accel) + + desired_speed_magnitude_slowed = abs(desired_speed) * np.exp(-abs(cross_track_error) * 0.6) + desired_speed = desired_speed_magnitude_slowed * -1.0 if desired_speed_magnitude_slowed != 0 else 0.0 + else: - if desired_speed is None: - desired_speed = 4.0 + cross_term = atan2(self.k * cross_track_error, self.k_soft + speed) + desired_steering_angle = yaw_error + cross_term + + if self.trajectory and self.desired_speed_source in ['path', 'trajectory']: + if len(self.trajectory.points) < 2 or self.current_path_parameter >= self.path.domain()[1]: + if component: + component.debug_event("Stanley: Past the end of trajectory, stopping.") + desired_speed = 0.0 + feedforward_accel = -2.0 + else: + if self.desired_speed_source == 'path': + current_trajectory_time = self.trajectory.parameter_to_time(self.current_path_parameter) + else: + self.current_traj_parameter += dt + current_trajectory_time = self.current_traj_parameter + + deriv = self.trajectory.eval_derivative(current_trajectory_time) + desired_speed = min(np.linalg.norm(deriv), self.speed_limit) + + difference_dt = 0.1 + future_t = current_trajectory_time + difference_dt + if future_t > self.trajectory.domain()[1]: + future_t = self.trajectory.domain()[1] + future_deriv = self.trajectory.eval_derivative(future_t) + next_desired_speed = min(np.linalg.norm(future_deriv), self.speed_limit) + feedforward_accel = (next_desired_speed - desired_speed) / difference_dt + feedforward_accel = np.clip(feedforward_accel, -self.max_decel, self.max_accel) + else: + if desired_speed is None: + desired_speed = 4.0 - # Cross-track-based slowdown (less aggressive than before). - desired_speed *= np.exp(-abs(cross_track_error) * 0.6) + desired_speed *= np.exp(-abs(cross_track_error) * 0.6) - # Clip to speed limit - if desired_speed > self.speed_limit: - desired_speed = self.speed_limit + if abs(desired_speed) > self.speed_limit: + if reverse: + desired_speed = self.speed_limit * -1.0 if desired_speed != 0 else 0.0 + else: + desired_speed = self.speed_limit - # PID for longitudinal control speed_error = desired_speed - speed output_accel = self.pid_speed.advance(e=speed_error, t=t, feedforward_term=feedforward_accel) - # Clip acceleration if output_accel > self.max_accel: output_accel = self.max_accel elif output_accel < -self.max_decel: output_accel = -self.max_decel - # Avoid negative accel when fully stopped if desired_speed == 0 and abs(speed) < 0.01 and output_accel < 0: output_accel = 0.0 - # Debug if component is not None: - # component.debug("Stanley: fx, fy", (fx, fy)) component.debug('curr pt',(curr_x,curr_y)) - component.debug("desired_x",desired_x) - component.debug("desired_y",desired_y) + component.debug("desired_x",target_x) + component.debug("desired_y",target_y) component.debug("Stanley: path param", self.current_path_parameter) component.debug("Stanley: crosstrack dist", closest_dist) component.debug("crosstrack error", cross_track_error) @@ -242,12 +287,6 @@ def compute(self, state: VehicleState, component: Component = None): component.debug("Stanley: feedforward_accel (m/s^2)", feedforward_accel) component.debug("Stanley: output_accel (m/s^2)", output_accel) - if output_accel > self.max_accel: - output_accel = self.max_accel - - if output_accel < -self.max_decel: - output_accel = -self.max_decel - self.t_last = t return (output_accel, desired_steering_angle) @@ -255,48 +294,28 @@ def compute(self, state: VehicleState, component: Component = None): # 3. Tracker component ##################################### class StanleyTrajectoryTracker(Component): - """ - A trajectory-tracking Component that uses the above Stanley controller - for lateral control plus PID-based longitudinal control. - It now includes measures to mitigate oscillations. - """ def __init__(self, vehicle_interface=None, **kwargs): - """ - :param vehicle_interface: The low-level interface to send commands to the vehicle. - :param kwargs: Optional parameters to pass into the Stanley(...) constructor. - """ self.stanley = Stanley(**kwargs) self.vehicle_interface = vehicle_interface def rate(self): - """Control frequency in Hz.""" return 50.0 def state_inputs(self): - """ - Required state inputs: - - Vehicle state - - Trajectory - """ return ["vehicle", "trajectory"] def state_outputs(self): - """No direct output state here.""" return [] def update(self, vehicle: VehicleState, trajectory: Trajectory): - """ - Per control cycle: - 1) Set the path/trajectory - 2) Compute (acceleration, front wheel angle) - 3) Convert front wheel angle to steering wheel angle (if necessary) - 4) Send command to the vehicle - """ self.stanley.set_path(trajectory) + + # TODO accel, f_delta = self.stanley.compute(vehicle, self) + # reverse = some_function_here() + # accel, f_delta = self.stanley.compute(vehicle, self, reverse) - # If your low-level interface expects steering wheel angle: steering_angle = front2steer(f_delta) steering_angle = np.clip( steering_angle, @@ -309,4 +328,4 @@ def update(self, vehicle: VehicleState, trajectory: Trajectory): def healthy(self): """Optional: check if the controller has a valid path.""" - return self.stanley.path is not None + return self.stanley.path is not None \ No newline at end of file diff --git a/launch/fixed_route.yaml b/launch/fixed_route.yaml index 904e82f2e..b773b20fd 100644 --- a/launch/fixed_route.yaml +++ b/launch/fixed_route.yaml @@ -16,15 +16,15 @@ drive: planning: route_planning: type: StaticRoutePlanner - args: [!relative_path '../GEMstack/knowledge/routes/forward_15m.csv'] + args: [!relative_path '../GEMstack/knowledge/routes/xyhead_highbay_backlot_p.csv'] # args: [!relative_path '../GEMstack/knowledge/routes/reverse_15m.csv'] motion_planning: type: RouteToTrajectoryPlanner args: [null] #desired speed in m/s. If null, this will keep the route untimed for the trajectory tracker trajectory_tracking: - type: pure_pursuit.PurePursuitTrajectoryTracker - # type: reverse_stanley.StanleyTrajectoryTracker - args: {desired_speed: 2.5} #approximately 5mph + # type: pure_pursuit.PurePursuitTrajectoryTracker + type: stanley.StanleyTrajectoryTracker + args: {desired_speed: 5.0} #approximately 5mph print: False log: # Specify the top-level folder to save the log files. Default is 'logs' From 6f36be424819155bfaa8cbed0ed202713878de85 Mon Sep 17 00:00:00 2001 From: hhxjqm Date: Fri, 25 Apr 2025 05:51:28 +0000 Subject: [PATCH 12/71] Set up stanley --- GEMstack/onboard/planning/stanley.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley.py index f820ff9b9..bb4f58749 100644 --- a/GEMstack/onboard/planning/stanley.py +++ b/GEMstack/onboard/planning/stanley.py @@ -54,7 +54,7 @@ def __init__( i = settings.get('control.longitudinal_control.pid_i') self.pid_speed = PID(p, d, i, windup_limit=20) - reverse = True + reverse = False # reverse = something_here() # TODO: Implement reverse logic if desired_speed is not None: @@ -110,7 +110,7 @@ def _find_rear_axle_position(self, x, y, yaw): ry = y - self.wheelbase * sin(yaw) return rx, ry - def compute(self, state: VehicleState, component: Component = None, reverse = True): # TODO change only during debugging + def compute(self, state: VehicleState, component: Component = None, reverse = False): # TODO change only during debugging """Compute the control outputs: (longitudinal acceleration, front wheel angle).""" t = state.pose.t if self.t_last is None: From aef87b147248f7470a1bc0fc564414500ef2d290 Mon Sep 17 00:00:00 2001 From: hhxjqm Date: Fri, 25 Apr 2025 06:42:19 +0000 Subject: [PATCH 13/71] update stanley.py --- GEMstack/onboard/planning/stanley.py | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley.py index bb4f58749..132c29306 100644 --- a/GEMstack/onboard/planning/stanley.py +++ b/GEMstack/onboard/planning/stanley.py @@ -53,15 +53,9 @@ def __init__( d = settings.get('control.longitudinal_control.pid_d') i = settings.get('control.longitudinal_control.pid_i') self.pid_speed = PID(p, d, i, windup_limit=20) - - reverse = False - # reverse = something_here() # TODO: Implement reverse logic if desired_speed is not None: - if reverse: - self.desired_speed_source = -abs(desired_speed) - else: - self.desired_speed_source = abs(desired_speed) + self.desired_speed_source = desired_speed else: self.desired_speed_source = settings.get('control.stanley.desired_speed', 'path') @@ -165,7 +159,7 @@ def compute(self, state: VehicleState, component: Component = None, reverse = Fa yaw_error = normalise_angle(path_yaw - curr_yaw) - desired_speed = self.desired_speed + desired_speed = abs(self.desired_speed) feedforward_accel = 0.0 if reverse: @@ -193,7 +187,7 @@ def compute(self, state: VehicleState, component: Component = None, reverse = Fa if is_at_start_backward: desired_speed = 0.0 - feedforward_accel = -2.0 * -1.0 # = +2.0 + feedforward_accel = -2.0 * -1.0 else: difference_dt = 0.1 From 789d36ee6e482fbb60853d87fbde59f20e66471c Mon Sep 17 00:00:00 2001 From: hhxjqm Date: Fri, 25 Apr 2025 06:46:18 +0000 Subject: [PATCH 14/71] update --- GEMstack/onboard/planning/stanley.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley.py index 132c29306..72bf8b05f 100644 --- a/GEMstack/onboard/planning/stanley.py +++ b/GEMstack/onboard/planning/stanley.py @@ -104,7 +104,7 @@ def _find_rear_axle_position(self, x, y, yaw): ry = y - self.wheelbase * sin(yaw) return rx, ry - def compute(self, state: VehicleState, component: Component = None, reverse = False): # TODO change only during debugging + def compute(self, state: VehicleState, component: Component = None, reverse = False): """Compute the control outputs: (longitudinal acceleration, front wheel angle).""" t = state.pose.t if self.t_last is None: @@ -306,9 +306,9 @@ def update(self, vehicle: VehicleState, trajectory: Trajectory): self.stanley.set_path(trajectory) # TODO - accel, f_delta = self.stanley.compute(vehicle, self) + reverse = True # reverse = some_function_here() - # accel, f_delta = self.stanley.compute(vehicle, self, reverse) + accel, f_delta = self.stanley.compute(vehicle, self, reverse) steering_angle = front2steer(f_delta) steering_angle = np.clip( From 67e0708f8356f3b755c4db67c8d8c7c6d414e52e Mon Sep 17 00:00:00 2001 From: hhxjqm Date: Fri, 25 Apr 2025 06:51:33 +0000 Subject: [PATCH 15/71] update --- GEMstack/onboard/planning/stanley.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley.py index 72bf8b05f..5f7fc91fd 100644 --- a/GEMstack/onboard/planning/stanley.py +++ b/GEMstack/onboard/planning/stanley.py @@ -306,7 +306,7 @@ def update(self, vehicle: VehicleState, trajectory: Trajectory): self.stanley.set_path(trajectory) # TODO - reverse = True + reverse = False # reverse = some_function_here() accel, f_delta = self.stanley.compute(vehicle, self, reverse) From f0d0e091129d3c416f68226638c153f78c8499cd Mon Sep 17 00:00:00 2001 From: hhxjqm Date: Sun, 30 Mar 2025 03:40:58 +0000 Subject: [PATCH 16/71] Update current.yaml and stanley.py --- GEMstack/knowledge/defaults/current.yaml | 39 ++- GEMstack/onboard/planning/stanley.py | 353 +++++++++++++++++++++++ 2 files changed, 379 insertions(+), 13 deletions(-) create mode 100644 GEMstack/onboard/planning/stanley.py diff --git a/GEMstack/knowledge/defaults/current.yaml b/GEMstack/knowledge/defaults/current.yaml index c886288be..e07edf881 100644 --- a/GEMstack/knowledge/defaults/current.yaml +++ b/GEMstack/knowledge/defaults/current.yaml @@ -7,28 +7,41 @@ vehicle: !include ../vehicle/gem_e4.yaml model_predictive_controller: dt: 0.1 lookahead: 20 + control: recovery: brake_amount : 0.5 brake_speed : 2.0 + + # Pure Pursuit controller parameters pure_pursuit: - lookahead: 2.0 - lookahead_scale: 3.0 - crosstrack_gain: 1.0 - desired_speed: trajectory + lookahead: 2.0 # Base lookahead distance (meters) + lookahead_scale: 3.0 # Velocity-dependent lookahead scaling factor + crosstrack_gain: 0.5 # Gain for crosstrack error correction (default: 1) + desired_speed: trajectory # Speed reference source: can be "trajectory", "path", or a constant value + + # Stanley controller parameters (fine tune this) + stanley: + control_gain: 1.0 + softening_gain: 1.0 + yaw_rate_gain: 0.0 + steering_damp_gain: 0.0 + desired_speed: trajectory # Speed reference source: can be "trajectory", "path", or a constant value + + # Shared longitudinal control parameters longitudinal_control: - pid_p: 1.0 - pid_i: 0.1 - pid_d: 0.0 + pid_p: 1.5 # Proportional gain for speed PID controller + pid_i: 0.1 # Integral gain for speed PID controller + pid_d: 0.0 # Derivative gain for speed PID controller #configure the simulator, if using simulator: dt: 0.01 - real_time_multiplier: 1.0 # make the simulator run faster than real time by making this > 1 + real_time_multiplier: 1.0 # make the simulator run faster than real time by making this > 1 gnss_emulator: - dt: 0.1 #10Hz - #position_noise: 0.1 #10cm noise - #orientation_noise: 0.04 #2.3 degrees noise + dt: 0.1 # 10Hz + #position_noise: 0.1 # 10cm noise + #orientation_noise: 0.04 # 2.3 degrees noise #velocity_noise: - # constant: 0.04 #4cm/s noise - # linear: 0.02 #2% noise \ No newline at end of file + # constant: 0.04 # 4cm/s noise + # linear: 0.02 # 2% noise diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley.py new file mode 100644 index 000000000..620ae917f --- /dev/null +++ b/GEMstack/onboard/planning/stanley.py @@ -0,0 +1,353 @@ +import numpy as np +from math import sin, cos, atan2 + +# These imports match your existing project structure +from ...mathutils.control import PID +from ...utils import settings +from ...mathutils import transforms +from ...knowledge.vehicle.geometry import front2steer +from ...knowledge.vehicle.dynamics import acceleration_to_pedal_positions +from ...state.vehicle import VehicleState, ObjectFrameEnum +from ...state.trajectory import Path, Trajectory +from ..interface.gem import GEMVehicleCommand +from ..component import Component + +##################################### +# 1. Angle normalization +##################################### +def normalise_angle(angle): + """Wraps an angle to the [-pi, pi] range.""" + while angle > np.pi: + angle -= 2.0 * np.pi + while angle < -np.pi: + angle += 2.0 * np.pi + return angle + +##################################### +# 2. Stanley-based controller with longitudinal PID +##################################### +class Stanley(object): + """ + A Stanley controller that handles lateral control (steering) + plus a basic longitudinal control (PID + optional feedforward). + It has been modified to reduce oscillations by: + 1) Lower gains + 2) Steering damping + 3) Low-pass filter on steering + 4) Gentler speed logic when cornering + """ + + def __init__( + self, + control_gain=None, + softening_gain=None, + yaw_rate_gain=None, + steering_damp_gain=None, + desired_speed=None + ): + """ + :param control_gain: Stanley lateral control gain k (lowered from the default to reduce overshoot). + :param softening_gain: Softening gain k_soft. + :param yaw_rate_gain: Yaw-rate gain k_yaw_rate (helps reduce oscillations). + :param steering_damp_gain: Steering damping gain k_damp_steer. + :param desired_speed: Desired speed (float) or 'path'/'trajectory'. + """ + + # 1) Lower Gains + # Reduced from 2.5 to 1.0 by default, to mitigate oscillations + self.k = control_gain if control_gain is not None else settings.get('control.stanley.control_gain', 1.0) + self.k_soft = softening_gain if softening_gain is not None else settings.get('control.stanley.softening_gain', 1.0) + self.k_yaw_rate = yaw_rate_gain if yaw_rate_gain is not None else settings.get('control.stanley.yaw_rate_gain', 0.0) + self.k_damp_steer = steering_damp_gain if steering_damp_gain is not None else settings.get('control.stanley.steering_damp_gain', 0.0) + + # Typically, this is the max front-wheel steering angle in radians + self.max_steer = settings.get('vehicle.geometry.max_wheel_angle', np.deg2rad(24)) + self.wheelbase = settings.get('vehicle.geometry.wheelbase', 2.0) + + # Basic longitudinal constraints + self.speed_limit = settings.get('vehicle.limits.max_speed', 10.0) + self.max_accel = settings.get('vehicle.limits.max_acceleration', 2.0) + self.max_decel = settings.get('vehicle.limits.max_deceleration', 2.0) + + # PID for longitudinal speed tracking + p = settings.get('control.longitudinal_control.pid_p', 0.5) + d = settings.get('control.longitudinal_control.pid_d', 0.0) + i = settings.get('control.longitudinal_control.pid_i', 0.1) + self.pid_speed = PID(p, d, i, windup_limit=20) + + # Speed source: numeric or derived from path/trajectory + if desired_speed is not None: + self.desired_speed_source = desired_speed + else: + self.desired_speed_source = settings.get('control.stanley.desired_speed', 'path') + + if isinstance(self.desired_speed_source, (int, float)): + self.desired_speed = self.desired_speed_source + else: + self.desired_speed = None + + # For path/trajectory + self.path_arg = None + self.path = None + self.trajectory = None + self.current_path_parameter = 0.0 + self.current_traj_parameter = 0.0 + self.t_last = None + + # 2) Steering damping memory + self.prev_steering_angle = 0.0 + + # 3) Low-pass filter memory: final front-wheel angle + self.prev_front_wheel_angle = 0.0 + + def set_path(self, path: Path): + """Sets the path or trajectory to track.""" + if path == self.path_arg: + return + self.path_arg = path + + # If the path has more than 2D, reduce to (x,y) + if len(path.points[0]) > 2: + path = path.get_dims([0,1]) + + # If no timing info, we can't rely on 'path'/'trajectory' speed + if not isinstance(path, Trajectory): + self.path = path.arc_length_parameterize() + self.trajectory = None + self.current_traj_parameter = 0.0 + if self.desired_speed_source in ['path', 'trajectory']: + raise ValueError("Stanley: Provided path has no timing. Either provide a Trajectory or set a constant desired_speed.") + else: + self.path = path.arc_length_parameterize() + self.trajectory = path + self.current_traj_parameter = self.trajectory.domain()[0] + + self.current_path_parameter = 0.0 + + def _find_front_axle_position(self, x, y, yaw): + """Compute front-axle world position from the center/rear and yaw.""" + fx = x + self.wheelbase * cos(yaw) + fy = y + self.wheelbase * sin(yaw) + return fx, fy + + def compute(self, state: VehicleState, component: Component = None): + """Compute the control outputs: (longitudinal acceleration, front wheel angle).""" + t = state.pose.t + if self.t_last is None: + self.t_last = t + dt = t - self.t_last + + # Current vehicle states + curr_x = state.pose.x + curr_y = state.pose.y + curr_yaw = state.pose.yaw if state.pose.yaw is not None else 0.0 + speed = state.v + + if self.path is None: + if component: + component.debug_event("No path provided to Stanley controller. Doing nothing.") + return (0.0, 0.0) + + # Ensure same frame + if self.path.frame != state.pose.frame: + if component: + component.debug_event(f"Transforming path from {self.path.frame.name} to {state.pose.frame.name}") + self.path = self.path.to_frame(state.pose.frame, current_pose=state.pose) + + if self.trajectory and self.trajectory.frame != state.pose.frame: + if component: + component.debug_event(f"Transforming trajectory from {self.trajectory.frame.name} to {state.pose.frame.name}") + self.trajectory = self.trajectory.to_frame(state.pose.frame, current_pose=state.pose) + + # 1) Closest point + fx, fy = self._find_front_axle_position(curr_x, curr_y, curr_yaw) + search_start = self.current_path_parameter - 5.0 + search_end = self.current_path_parameter + 5.0 + closest_dist, closest_parameter = self.path.closest_point_local((fx, fy), [search_start, search_end]) + self.current_path_parameter = closest_parameter + + # 2) Path heading + target_x, target_y = self.path.eval(closest_parameter) + tangent = self.path.eval_tangent(closest_parameter) + path_yaw = atan2(tangent[1], tangent[0]) + + # 3) Lateral error + dx = fx - target_x + dy = fy - target_y + left_vec = np.array([sin(curr_yaw), -cos(curr_yaw)]) + cross_track_error = np.sign(np.dot(np.array([dx, dy]), left_vec)) * closest_dist + + # 4) Heading error + yaw_error = normalise_angle(path_yaw - curr_yaw) + + # 5) Standard Stanley terms + cross_term = atan2(self.k * cross_track_error, self.k_soft + speed) + desired_steering_angle = yaw_error + cross_term + + # 5.1) Yaw-rate damping (optional if k_yaw_rate != 0) + # This term penalizes high yaw rates, helps reduce overshoot + yaw_rate_damping = self.k_yaw_rate * (-speed * sin(desired_steering_angle)) / self.wheelbase + desired_steering_angle += yaw_rate_damping + + # 5.2) Steering damping (penalize rapid changes) + # Resist big steering jumps from one cycle to the next + steering_damp_term = self.k_damp_steer * (desired_steering_angle - self.prev_steering_angle) + desired_steering_angle += steering_damp_term + + # 6) Clip the raw desired steering + unfiltered_steering = np.clip(desired_steering_angle, -self.max_steer, self.max_steer) + + # 7) Low-pass filter for final steering + # alpha in [0..1], smaller -> more smoothing + alpha = 0.2 + final_steering = alpha * unfiltered_steering + (1.0 - alpha) * self.prev_front_wheel_angle + final_steering = np.clip(final_steering, -self.max_steer, self.max_steer) + + # Update memories + self.prev_steering_angle = unfiltered_steering + self.prev_front_wheel_angle = final_steering + + ################################################################ + # Speed Logic - Slightly gentler on cornering deceleration + ################################################################ + + # 8) Determine desired_speed from path/trajectory or fallback + desired_speed = self.desired_speed + feedforward_accel = 0.0 + + if self.trajectory and self.desired_speed_source in ['path', 'trajectory']: + if len(self.trajectory.points) < 2 or self.current_path_parameter >= self.path.domain()[1]: + # End of trajectory -> stop + if component: + component.debug_event("Stanley: Past the end of trajectory, stopping.") + desired_speed = 0.0 + feedforward_accel = -2.0 + else: + if self.desired_speed_source == 'path': + current_trajectory_time = self.trajectory.parameter_to_time(self.current_path_parameter) + else: + self.current_traj_parameter += dt + current_trajectory_time = self.current_traj_parameter + + deriv = self.trajectory.eval_derivative(current_trajectory_time) + desired_speed = min(np.linalg.norm(deriv), self.speed_limit) + + difference_dt = 0.1 + future_t = current_trajectory_time + difference_dt + if future_t > self.trajectory.domain()[1]: + future_t = self.trajectory.domain()[1] + future_deriv = self.trajectory.eval_derivative(future_t) + next_desired_speed = min(np.linalg.norm(future_deriv), self.speed_limit) + feedforward_accel = (next_desired_speed - desired_speed) / difference_dt + feedforward_accel = np.clip(feedforward_accel, -self.max_decel, self.max_accel) + else: + if desired_speed is None: + desired_speed = 4.0 + + # Cross-track-based slowdown (less aggressive than before). + desired_speed *= np.exp(-abs(cross_track_error) * 0.6) + + # Steering-based slowdown: reduce speed if the steering angle is large + angle_ratio = abs(final_steering) / self.max_steer + # If angle_ratio is 1.0, we'll cut speed to ~ 50% of original + speed_scale = 1.0 - 0.5 * angle_ratio + # Keep speed_scale at least 0.4 + speed_scale = max(speed_scale, 0.4) + desired_speed *= speed_scale + + # Clip to speed limit + if desired_speed > self.speed_limit: + desired_speed = self.speed_limit + + # 9) PID for longitudinal control + speed_error = desired_speed - speed + output_accel = self.pid_speed.advance(e=speed_error, t=t, feedforward_term=feedforward_accel) + + # Clip acceleration + if output_accel > self.max_accel: + output_accel = self.max_accel + elif output_accel < -self.max_decel: + output_accel = -self.max_decel + + # Avoid negative accel when fully stopped + if desired_speed == 0 and abs(speed) < 0.01 and output_accel < 0: + output_accel = 0.0 + + # Debug + if component: + component.debug("Stanley: fx, fy", (fx, fy)) + component.debug("Stanley: path param", self.current_path_parameter) + component.debug("Stanley: crosstrack dist", closest_dist) + component.debug("Stanley: cross_track_error", cross_track_error) + component.debug("Stanley: yaw_error", yaw_error) + component.debug("Stanley: unfiltered_steering (rad)", unfiltered_steering) + component.debug("Stanley: final_steering (rad)", final_steering) + component.debug("Stanley: angle_ratio", angle_ratio) + component.debug("Stanley: speed_scale", speed_scale) + component.debug("Stanley: desired_speed (m/s)", desired_speed) + component.debug("Stanley: feedforward_accel (m/s^2)", feedforward_accel) + component.debug("Stanley: output_accel (m/s^2)", output_accel) + component.debug("Stanley: current speed (m/s)", speed) + + self.t_last = t + return (output_accel, final_steering) + +##################################### +# 3. Tracker component +##################################### +class StanleyTrajectoryTracker(Component): + """ + A trajectory-tracking Component that uses the above Stanley controller + for lateral control plus PID-based longitudinal control. + It now includes measures to mitigate oscillations. + """ + + def __init__(self, vehicle_interface=None, **kwargs): + """ + :param vehicle_interface: The low-level interface to send commands to the vehicle. + :param kwargs: Optional parameters to pass into the Stanley(...) constructor. + """ + self.stanley = Stanley(**kwargs) + self.vehicle_interface = vehicle_interface + + def rate(self): + """Control frequency in Hz.""" + return 50.0 + + def state_inputs(self): + """ + Required state inputs: + - Vehicle state + - Trajectory + """ + return ["vehicle", "trajectory"] + + def state_outputs(self): + """No direct output state here.""" + return [] + + def update(self, vehicle: VehicleState, trajectory: Trajectory): + """ + Per control cycle: + 1) Set the path/trajectory + 2) Compute (acceleration, front wheel angle) + 3) Convert front wheel angle to steering wheel angle (if necessary) + 4) Send command to the vehicle + """ + self.stanley.set_path(trajectory) + accel, f_delta = self.stanley.compute(vehicle, self) + + # If your low-level interface expects steering wheel angle: + steering_angle = front2steer(f_delta) + steering_angle = np.clip( + steering_angle, + settings.get('vehicle.geometry.min_steering_angle', -0.5), + settings.get('vehicle.geometry.max_steering_angle', 0.5) + ) + + cmd = self.vehicle_interface.simple_command(accel, steering_angle, vehicle) + self.vehicle_interface.send_command(cmd) + + def healthy(self): + """Optional: check if the controller has a valid path.""" + return self.stanley.path is not None From 79792821e7acc3dc1112a385817698148bc944df Mon Sep 17 00:00:00 2001 From: hhxjqm Date: Mon, 31 Mar 2025 16:48:21 +0000 Subject: [PATCH 17/71] Update current.yaml and stanley.py --- GEMstack/knowledge/defaults/current.yaml | 4 +- GEMstack/onboard/planning/stanley.py | 75 ++++-------------------- 2 files changed, 13 insertions(+), 66 deletions(-) diff --git a/GEMstack/knowledge/defaults/current.yaml b/GEMstack/knowledge/defaults/current.yaml index e07edf881..5efa5b5bb 100644 --- a/GEMstack/knowledge/defaults/current.yaml +++ b/GEMstack/knowledge/defaults/current.yaml @@ -23,9 +23,7 @@ control: # Stanley controller parameters (fine tune this) stanley: control_gain: 1.0 - softening_gain: 1.0 - yaw_rate_gain: 0.0 - steering_damp_gain: 0.0 + softening_gain: 0.01 desired_speed: trajectory # Speed reference source: can be "trajectory", "path", or a constant value # Shared longitudinal control parameters diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley.py index 620ae917f..a2390ea71 100644 --- a/GEMstack/onboard/planning/stanley.py +++ b/GEMstack/onboard/planning/stanley.py @@ -41,8 +41,6 @@ def __init__( self, control_gain=None, softening_gain=None, - yaw_rate_gain=None, - steering_damp_gain=None, desired_speed=None ): """ @@ -54,25 +52,22 @@ def __init__( """ # 1) Lower Gains - # Reduced from 2.5 to 1.0 by default, to mitigate oscillations - self.k = control_gain if control_gain is not None else settings.get('control.stanley.control_gain', 1.0) - self.k_soft = softening_gain if softening_gain is not None else settings.get('control.stanley.softening_gain', 1.0) - self.k_yaw_rate = yaw_rate_gain if yaw_rate_gain is not None else settings.get('control.stanley.yaw_rate_gain', 0.0) - self.k_damp_steer = steering_damp_gain if steering_damp_gain is not None else settings.get('control.stanley.steering_damp_gain', 0.0) + self.k = control_gain if control_gain is not None else settings.get('control.stanley.control_gain') + self.k_soft = softening_gain if softening_gain is not None else settings.get('control.stanley.softening_gain') # Typically, this is the max front-wheel steering angle in radians - self.max_steer = settings.get('vehicle.geometry.max_wheel_angle', np.deg2rad(24)) - self.wheelbase = settings.get('vehicle.geometry.wheelbase', 2.0) + self.max_steer = settings.get('vehicle.geometry.max_wheel_angle') + self.wheelbase = settings.get('vehicle.geometry.wheelbase') # Basic longitudinal constraints - self.speed_limit = settings.get('vehicle.limits.max_speed', 10.0) - self.max_accel = settings.get('vehicle.limits.max_acceleration', 2.0) - self.max_decel = settings.get('vehicle.limits.max_deceleration', 2.0) + self.speed_limit = settings.get('vehicle.limits.max_speed') + self.max_accel = settings.get('vehicle.limits.max_acceleration') + self.max_decel = settings.get('vehicle.limits.max_deceleration') # PID for longitudinal speed tracking - p = settings.get('control.longitudinal_control.pid_p', 0.5) - d = settings.get('control.longitudinal_control.pid_d', 0.0) - i = settings.get('control.longitudinal_control.pid_i', 0.1) + p = settings.get('control.longitudinal_control.pid_p') + d = settings.get('control.longitudinal_control.pid_d') + i = settings.get('control.longitudinal_control.pid_i') self.pid_speed = PID(p, d, i, windup_limit=20) # Speed source: numeric or derived from path/trajectory @@ -94,12 +89,6 @@ def __init__( self.current_traj_parameter = 0.0 self.t_last = None - # 2) Steering damping memory - self.prev_steering_angle = 0.0 - - # 3) Low-pass filter memory: final front-wheel angle - self.prev_front_wheel_angle = 0.0 - def set_path(self, path: Path): """Sets the path or trajectory to track.""" if path == self.path_arg: @@ -184,34 +173,6 @@ def compute(self, state: VehicleState, component: Component = None): cross_term = atan2(self.k * cross_track_error, self.k_soft + speed) desired_steering_angle = yaw_error + cross_term - # 5.1) Yaw-rate damping (optional if k_yaw_rate != 0) - # This term penalizes high yaw rates, helps reduce overshoot - yaw_rate_damping = self.k_yaw_rate * (-speed * sin(desired_steering_angle)) / self.wheelbase - desired_steering_angle += yaw_rate_damping - - # 5.2) Steering damping (penalize rapid changes) - # Resist big steering jumps from one cycle to the next - steering_damp_term = self.k_damp_steer * (desired_steering_angle - self.prev_steering_angle) - desired_steering_angle += steering_damp_term - - # 6) Clip the raw desired steering - unfiltered_steering = np.clip(desired_steering_angle, -self.max_steer, self.max_steer) - - # 7) Low-pass filter for final steering - # alpha in [0..1], smaller -> more smoothing - alpha = 0.2 - final_steering = alpha * unfiltered_steering + (1.0 - alpha) * self.prev_front_wheel_angle - final_steering = np.clip(final_steering, -self.max_steer, self.max_steer) - - # Update memories - self.prev_steering_angle = unfiltered_steering - self.prev_front_wheel_angle = final_steering - - ################################################################ - # Speed Logic - Slightly gentler on cornering deceleration - ################################################################ - - # 8) Determine desired_speed from path/trajectory or fallback desired_speed = self.desired_speed feedforward_accel = 0.0 @@ -247,19 +208,11 @@ def compute(self, state: VehicleState, component: Component = None): # Cross-track-based slowdown (less aggressive than before). desired_speed *= np.exp(-abs(cross_track_error) * 0.6) - # Steering-based slowdown: reduce speed if the steering angle is large - angle_ratio = abs(final_steering) / self.max_steer - # If angle_ratio is 1.0, we'll cut speed to ~ 50% of original - speed_scale = 1.0 - 0.5 * angle_ratio - # Keep speed_scale at least 0.4 - speed_scale = max(speed_scale, 0.4) - desired_speed *= speed_scale - # Clip to speed limit if desired_speed > self.speed_limit: desired_speed = self.speed_limit - # 9) PID for longitudinal control + # PID for longitudinal control speed_error = desired_speed - speed output_accel = self.pid_speed.advance(e=speed_error, t=t, feedforward_term=feedforward_accel) @@ -280,17 +233,13 @@ def compute(self, state: VehicleState, component: Component = None): component.debug("Stanley: crosstrack dist", closest_dist) component.debug("Stanley: cross_track_error", cross_track_error) component.debug("Stanley: yaw_error", yaw_error) - component.debug("Stanley: unfiltered_steering (rad)", unfiltered_steering) - component.debug("Stanley: final_steering (rad)", final_steering) - component.debug("Stanley: angle_ratio", angle_ratio) - component.debug("Stanley: speed_scale", speed_scale) component.debug("Stanley: desired_speed (m/s)", desired_speed) component.debug("Stanley: feedforward_accel (m/s^2)", feedforward_accel) component.debug("Stanley: output_accel (m/s^2)", output_accel) component.debug("Stanley: current speed (m/s)", speed) self.t_last = t - return (output_accel, final_steering) + return (output_accel, desired_steering_angle) ##################################### # 3. Tracker component From 969aef5a14956d13b5568f8a1d3ed5b02fa6d377 Mon Sep 17 00:00:00 2001 From: hhxjqm Date: Mon, 31 Mar 2025 20:22:31 +0000 Subject: [PATCH 18/71] Fix Stanley controller debug logging --- GEMstack/onboard/planning/stanley.py | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley.py index a2390ea71..b660861d7 100644 --- a/GEMstack/onboard/planning/stanley.py +++ b/GEMstack/onboard/planning/stanley.py @@ -227,17 +227,24 @@ def compute(self, state: VehicleState, component: Component = None): output_accel = 0.0 # Debug - if component: - component.debug("Stanley: fx, fy", (fx, fy)) + if component is not None: + # component.debug("Stanley: fx, fy", (fx, fy)) + component.debug('curr pt',(curr_x,curr_y)) component.debug("Stanley: path param", self.current_path_parameter) component.debug("Stanley: crosstrack dist", closest_dist) - component.debug("Stanley: cross_track_error", cross_track_error) + component.debug("crosstrack error", cross_track_error) component.debug("Stanley: yaw_error", yaw_error) + component.debug('steering_angle', desired_steering_angle) component.debug("Stanley: desired_speed (m/s)", desired_speed) component.debug("Stanley: feedforward_accel (m/s^2)", feedforward_accel) component.debug("Stanley: output_accel (m/s^2)", output_accel) - component.debug("Stanley: current speed (m/s)", speed) + if output_accel > self.max_accel: + output_accel = self.max_accel + + if output_accel < -self.max_decel: + output_accel = -self.max_decel + self.t_last = t return (output_accel, desired_steering_angle) From 3cb0ab456ee2040fc2af059c15fe2f74f0c72a50 Mon Sep 17 00:00:00 2001 From: Jugal Date: Thu, 10 Apr 2025 15:41:45 -0500 Subject: [PATCH 19/71] Update fixed_route.yaml --- launch/fixed_route.yaml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/launch/fixed_route.yaml b/launch/fixed_route.yaml index c05de8ff7..abd8242e6 100644 --- a/launch/fixed_route.yaml +++ b/launch/fixed_route.yaml @@ -21,7 +21,8 @@ drive: type: RouteToTrajectoryPlanner args: [null] #desired speed in m/s. If null, this will keep the route untimed for the trajectory tracker trajectory_tracking: - type: pure_pursuit.PurePursuitTrajectoryTracker + #type: pure_pursuit.PurePursuitTrajectoryTracker + type: stanley.StanleyTrajectoryTracker args: {desired_speed: 2.5} #approximately 5mph print: False log: @@ -76,4 +77,4 @@ variants: #visualization: !include "mpl_visualization.yaml" log_ros: log: - ros_topics : !include "../GEMstack/knowledge/defaults/standard_ros_topics.yaml" \ No newline at end of file + ros_topics : !include "../GEMstack/knowledge/defaults/standard_ros_topics.yaml" From 30527ec23d3ba9d8a5cd2f4f4642618448ce5b94 Mon Sep 17 00:00:00 2001 From: Jugal Date: Thu, 10 Apr 2025 15:42:24 -0500 Subject: [PATCH 20/71] Update stanley.py --- GEMstack/onboard/planning/stanley.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley.py index b660861d7..8f871f488 100644 --- a/GEMstack/onboard/planning/stanley.py +++ b/GEMstack/onboard/planning/stanley.py @@ -159,7 +159,8 @@ def compute(self, state: VehicleState, component: Component = None): target_x, target_y = self.path.eval(closest_parameter) tangent = self.path.eval_tangent(closest_parameter) path_yaw = atan2(tangent[1], tangent[0]) - + desired_x = target_x + desired_y = target_y # 3) Lateral error dx = fx - target_x dy = fy - target_y @@ -230,6 +231,8 @@ def compute(self, state: VehicleState, component: Component = None): if component is not None: # component.debug("Stanley: fx, fy", (fx, fy)) component.debug('curr pt',(curr_x,curr_y)) + component.debug("desired_x",desired_x) + component.debug("desired_y",desired_y) component.debug("Stanley: path param", self.current_path_parameter) component.debug("Stanley: crosstrack dist", closest_dist) component.debug("crosstrack error", cross_track_error) From 6aa8dca90538d4c1bd2d7f342283aaefc2c73148 Mon Sep 17 00:00:00 2001 From: Jugal Date: Thu, 10 Apr 2025 15:43:17 -0500 Subject: [PATCH 21/71] Add files via upload --- logs/logplot_pp.py | 120 +++++++++++++++++++++++++++++++++++++++++++++ logs/logplot_s.py | 119 ++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 239 insertions(+) create mode 100644 logs/logplot_pp.py create mode 100644 logs/logplot_s.py diff --git a/logs/logplot_pp.py b/logs/logplot_pp.py new file mode 100644 index 000000000..74025c291 --- /dev/null +++ b/logs/logplot_pp.py @@ -0,0 +1,120 @@ +####Following code can be used to visualize performance of Pure Pursuit after using it.### +###How to use?- After running the simulation or on actual vehicle you get logs in log folder### +### You can run the command- python3 logplot_pp.py "logfilename".### + + +import json +import sys +import os +import matplotlib.pyplot as plt +import numpy as np + +# Safety thresholds (not used in the current plots) +ACC_SAFE = 0.8 # |acceleration| <= 0.8 m/s² is safe +ACC_UNSAFE = 2.0 # |acceleration| >= 2.0 m/s² is unsafe +HR_SAFE = 0.05 # |heading_rate| <= 0.05 rad/s is safe +HR_UNSAFE = 0.2 # |heading_rate| >= 0.2 rad/s is unsafe + +def parse_behavior_log(filename): + times = [] + accelerations = [] + heading_rates = [] + + with open(filename, 'r') as f: + first_time = None # Store the first timestamp for shifting + for line in f: + try: + entry = json.loads(line) + except json.JSONDecodeError: + print(f"Skipping invalid JSON line: {line.strip()}") + continue + if "vehicle" in entry: + t = entry.get("time") + vehicle_data = entry["vehicle"].get("data", {}) + acceleration = vehicle_data.get("acceleration") + heading_rate = vehicle_data.get("heading_rate") + if t is not None and acceleration is not None and heading_rate is not None: + if first_time is None: + first_time = t # Set the first timestamp + times.append(t - first_time) # Shift time to start from 0 + accelerations.append(acceleration) + heading_rates.append(heading_rate) + + return np.array(times), np.array(accelerations), np.array(heading_rates) + +def parse_tracker_csv(filename): + data = np.genfromtxt(filename, delimiter=',', skip_header=1) + first_time = data[0, 18] # Normalize time to start from 0 + cte_time = data[:, 18] - first_time + cte = data[:, 20] + x_actual, y_actual = data[:, 2], data[:, 5] + x_desired, y_desired = data[:, 11], data[:, 14] + return cte_time, cte, x_actual, y_actual, x_desired, y_desired + +def compute_derivative(times, values): + dt = np.diff(times) + dv = np.diff(values) + derivative = dv / dt + return times[1:], derivative + +def plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired): + global_min_time = min(time_jerk[0], time_heading_acc[0], cte_time[0]) + global_max_time = max(time_jerk[-1], time_heading_acc[-1], cte_time[-1]) + + fig, axs = plt.subplots(2, 2, figsize=(12, 8)) + fig.subplots_adjust(hspace=0.4, wspace=0.3) + + axs[0,0].plot(time_jerk, jerk, marker='', linestyle='-', color='blue') + axs[0,0].set_ylabel("Jerk (m/s³)") + axs[0,0].set_title("Vehicle Jerk Over Time") + axs[0,0].grid(True) + axs[0,0].set_xlim(global_min_time, global_max_time) + + axs[0,1].plot(time_heading_acc, heading_acc, marker='', linestyle='-', color='orange') + axs[0,1].set_ylabel("Heading Acceleration (rad/s²)") + axs[0,1].set_title("Vehicle Heading Acceleration Over Time") + axs[0,1].grid(True) + axs[0,1].set_xlim(global_min_time, global_max_time) + + axs[1,0].plot(cte_time, cte, marker='', linestyle='-', color='green') + axs[1,0].set_xlabel("Time (s)") + axs[1,0].set_ylabel("Cross Track Error") + axs[1,0].set_title("Vehicle Cross Track Error Over Time") + axs[1,0].grid(True) + axs[1,0].set_xlim(global_min_time, global_max_time) + + axs[1,1].plot(x_actual, y_actual, marker='', linestyle='-', color='blue', label='Actual') + axs[1,1].plot(x_desired, y_desired, marker='', linestyle='-', color='orange', label='Desired') + axs[1,1].set_xlabel("X Position (m)") + axs[1,1].set_ylabel("Y Position (m)") + axs[1,1].set_title("Vehicle Position") + axs[1,1].legend() + axs[1,1].grid(True) + + plt.show() + +if __name__=='__main__': + if len(sys.argv) != 2: + print("Usage: python test_comfort_metrics.py ") + sys.exit(1) + + log_dir = sys.argv[1] + behavior_file = os.path.join(log_dir, "behavior.json") + tracker_file = os.path.join(log_dir, "PurePursuitTrajectoryTracker_debug.csv") + #tracker_file = os.path.join(log_dir, "StanleyTrajectoryTracker_debug.csv") + + times, accelerations, heading_rates = parse_behavior_log(behavior_file) + time_jerk, jerk = compute_derivative(times, accelerations) + time_heading_acc, heading_acc = compute_derivative(times, heading_rates) + + cte_time, cte, x_actual, y_actual, x_desired, y_desired = parse_tracker_csv(tracker_file) + + plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired) + + print("Max (abs) jerk:", np.max(np.abs(jerk))) + print("Avg jerk:", np.mean(np.abs(jerk))) + print("Max (abs) heading acceleration:", np.max(np.abs(heading_acc))) + print("Avg heading acceleration:", np.mean(np.abs(heading_acc))) + print("Max (abs) cross track error:", np.max(np.abs(cte))) + print("Avg cross track error:", np.mean(np.abs(cte))) + diff --git a/logs/logplot_s.py b/logs/logplot_s.py new file mode 100644 index 000000000..f534bbd27 --- /dev/null +++ b/logs/logplot_s.py @@ -0,0 +1,119 @@ +####Following code can be used to visualize performance of Stanley after using it.### +###How to use?- After running the simulation or on actual vehicle you get logs in log folder### +### You can run the command- python3 logplot_s.py "logfilename".### + +import json +import sys +import os +import matplotlib.pyplot as plt +import numpy as np + +# Safety thresholds (not used in the current plots) +ACC_SAFE = 0.8 # |acceleration| <= 0.8 m/s² is safe +ACC_UNSAFE = 2.0 # |acceleration| >= 2.0 m/s² is unsafe +HR_SAFE = 0.05 # |heading_rate| <= 0.05 rad/s is safe +HR_UNSAFE = 0.2 # |heading_rate| >= 0.2 rad/s is unsafe + +def parse_behavior_log(filename): + times = [] + accelerations = [] + heading_rates = [] + + with open(filename, 'r') as f: + first_time = None # Store the first timestamp for shifting + for line in f: + try: + entry = json.loads(line) + except json.JSONDecodeError: + print(f"Skipping invalid JSON line: {line.strip()}") + continue + if "vehicle" in entry: + t = entry.get("time") + vehicle_data = entry["vehicle"].get("data", {}) + acceleration = vehicle_data.get("acceleration") + heading_rate = vehicle_data.get("heading_rate") + if t is not None and acceleration is not None and heading_rate is not None: + if first_time is None: + first_time = t # Set the first timestamp + times.append(t - first_time) # Shift time to start from 0 + accelerations.append(acceleration) + heading_rates.append(heading_rate) + + return np.array(times), np.array(accelerations), np.array(heading_rates) + +def parse_tracker_csv(filename): + data = np.genfromtxt(filename, delimiter=',', skip_header=1) + first_time = data[0, 18] # Normalize time to start from 0 + cte_time = data[:, 18] - first_time + cte = data[:, 20] + x_actual, y_actual = data[:, 2], data[:, 5] + x_desired, y_desired = data[:, 8], data[:, 11] + return cte_time, cte, x_actual, y_actual, x_desired, y_desired + +def compute_derivative(times, values): + dt = np.diff(times) + dv = np.diff(values) + derivative = dv / dt + return times[1:], derivative + +def plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired): + global_min_time = min(time_jerk[0], time_heading_acc[0], cte_time[0]) + global_max_time = max(time_jerk[-1], time_heading_acc[-1], cte_time[-1]) + + fig, axs = plt.subplots(2, 2, figsize=(12, 8)) + fig.subplots_adjust(hspace=0.4, wspace=0.3) + + axs[0,0].plot(time_jerk, jerk, marker='', linestyle='-', color='blue') + axs[0,0].set_ylabel("Jerk (m/s³)") + axs[0,0].set_title("Vehicle Jerk Over Time") + axs[0,0].grid(True) + axs[0,0].set_xlim(global_min_time, global_max_time) + + axs[0,1].plot(time_heading_acc, heading_acc, marker='', linestyle='-', color='orange') + axs[0,1].set_ylabel("Heading Acceleration (rad/s²)") + axs[0,1].set_title("Vehicle Heading Acceleration Over Time") + axs[0,1].grid(True) + axs[0,1].set_xlim(global_min_time, global_max_time) + + axs[1,0].plot(cte_time, cte, marker='', linestyle='-', color='green') + axs[1,0].set_xlabel("Time (s)") + axs[1,0].set_ylabel("Cross Track Error") + axs[1,0].set_title("Vehicle Cross Track Error Over Time") + axs[1,0].grid(True) + axs[1,0].set_xlim(global_min_time, global_max_time) + + axs[1,1].plot(x_actual, y_actual, marker='', linestyle='-', color='blue', label='Actual') + axs[1,1].plot(x_desired, y_desired, marker='', linestyle='-', color='orange', label='Desired') + axs[1,1].set_xlabel("X Position (m)") + axs[1,1].set_ylabel("Y Position (m)") + axs[1,1].set_title("Vehicle Position") + axs[1,1].legend() + axs[1,1].grid(True) + + plt.show() + +if __name__=='__main__': + if len(sys.argv) != 2: + print("Usage: python test_comfort_metrics.py ") + sys.exit(1) + + log_dir = sys.argv[1] + behavior_file = os.path.join(log_dir, "behavior.json") + #tracker_file = os.path.join(log_dir, "PurePursuitTrajectoryTracker_debug.csv") + tracker_file = os.path.join(log_dir, "StanleyTrajectoryTracker_debug.csv") + + times, accelerations, heading_rates = parse_behavior_log(behavior_file) + time_jerk, jerk = compute_derivative(times, accelerations) + time_heading_acc, heading_acc = compute_derivative(times, heading_rates) + + cte_time, cte, x_actual, y_actual, x_desired, y_desired = parse_tracker_csv(tracker_file) + + plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired) + + print("Max (abs) jerk:", np.max(np.abs(jerk))) + print("Avg jerk:", np.mean(np.abs(jerk))) + print("Max (abs) heading acceleration:", np.max(np.abs(heading_acc))) + print("Avg heading acceleration:", np.mean(np.abs(heading_acc))) + print("Max (abs) cross track error:", np.max(np.abs(cte))) + print("Avg cross track error:", np.mean(np.abs(cte))) + From 815b3fa5c9e801744f688727843996ccb80739e3 Mon Sep 17 00:00:00 2001 From: Jugal Date: Thu, 10 Apr 2025 15:49:14 -0500 Subject: [PATCH 22/71] Update settings.py --- GEMstack/utils/settings.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/GEMstack/utils/settings.py b/GEMstack/utils/settings.py index b2470ce25..a3d193658 100644 --- a/GEMstack/utils/settings.py +++ b/GEMstack/utils/settings.py @@ -37,14 +37,14 @@ def load_settings(): def settings(): """Returns all global settings, loading them if necessary.""" - global SETTINGS + #global SETTINGS load_settings() return SETTINGS def get(path : Union[str,List[str]], defaultValue=KeyError) -> Any: """Retrieves a setting by a list of keys or a '.'-separated string.""" - global SETTINGS + #global SETTINGS load_settings() if isinstance(path,str): path = path.split('.') @@ -65,7 +65,7 @@ def set(path : Union[str,List[str]], value : Any, leaf_only=True) -> None: If leaf_only=True (default), we prevent inadvertently deleting parts of the settings dictionary. """ - global SETTINGS + #global SETTINGS load_settings() if isinstance(path,str): path = path.split('.') From 63b8994631087768684421f54062ef5ae7df1f1a Mon Sep 17 00:00:00 2001 From: Patrick Wu Date: Sat, 12 Apr 2025 23:21:39 -0500 Subject: [PATCH 23/71] add mpc.py --- .github/workflows/python-app.yml | 124 ++--- GEMstack/knowledge/defaults/current.yaml | 90 ++-- GEMstack/onboard/planning/mpc.py | 162 ++++++ GEMstack/onboard/planning/stanley.py | 624 +++++++++++------------ GEMstack/utils/klampt_visualization.py | 598 +++++++++++----------- GEMstack/utils/settings.py | 160 +++--- launch/fixed_route.yaml | 160 +++--- logs/logplot_pp.py | 240 ++++----- logs/logplot_s.py | 238 ++++----- 9 files changed, 1279 insertions(+), 1117 deletions(-) create mode 100644 GEMstack/onboard/planning/mpc.py diff --git a/.github/workflows/python-app.yml b/.github/workflows/python-app.yml index 3de575d8e..84dc183c0 100644 --- a/.github/workflows/python-app.yml +++ b/.github/workflows/python-app.yml @@ -1,62 +1,62 @@ -# This workflow will install Python dependencies, run tests and lint with a single version of Python -# For more information see: https://docs.github.com/en/actions/automating-builds-and-tests/building-and-testing-python - -name: Python application - -on: - push: - branches: - - '**' - - -permissions: - contents: read - -jobs: - PEP-Guidelines: - - runs-on: ubuntu-latest - - steps: - - uses: actions/checkout@v3 - - name: Set up Python 3.10 - uses: actions/setup-python@v3 - with: - python-version: "3.10" - - name: Install dependencies - run: | - python -m pip install --upgrade pip - pip install flake8 flake8-docstrings pep8-naming - - name: Lint with flake8 - run: | - # stop the build if there are Python syntax errors or undefined names - flake8 ./GEMstack --count --select=E9,F63,F7,F82 --show-source --statistics --exclude=__init__.py || exit 1 - # to enable more advanced checks on the repo, uncomment the lines below (There are around 3000 violations) - # flake8 ./GEMstack --ignore=D,C901,E402,E231 --count --max-complexity=10 --max-line-length=127 --statistics --exclude=__init__.py || exit 1 - # if we want to enable documentation checks, uncomment the line below - # flake8 ./GEMstack --ignore=E128,E402,E501,F401 --docstring-convention pep257 --max-line-length=120 --exclude=__init__.py || exit 1 - continue-on-error: false - - Documentation: - - runs-on: ubuntu-latest - - steps: - - uses: actions/checkout@v3 - - name: Set up Python 3.10 - uses: actions/setup-python@v3 - with: - python-version: "3.10" - - name: Install dependencies - run: | - python -m pip install --upgrade pip - pip install sphinx sphinx-rtd-theme - - name: Generate Documentation - run: | - # stop the build if there are Python syntax errors or undefined names - sphinx-build -b html docs docs/build - - name: Save Documentation as Artifact - uses: actions/upload-artifact@v4 - with: - name: documentation - path: docs/build +# This workflow will install Python dependencies, run tests and lint with a single version of Python +# For more information see: https://docs.github.com/en/actions/automating-builds-and-tests/building-and-testing-python + +name: Python application + +on: + push: + branches: + - '**' + + +permissions: + contents: read + +jobs: + PEP-Guidelines: + + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v3 + - name: Set up Python 3.10 + uses: actions/setup-python@v3 + with: + python-version: "3.10" + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install flake8 flake8-docstrings pep8-naming + - name: Lint with flake8 + run: | + # stop the build if there are Python syntax errors or undefined names + flake8 ./GEMstack --count --select=E9,F63,F7,F82 --show-source --statistics --exclude=__init__.py || exit 1 + # to enable more advanced checks on the repo, uncomment the lines below (There are around 3000 violations) + # flake8 ./GEMstack --ignore=D,C901,E402,E231 --count --max-complexity=10 --max-line-length=127 --statistics --exclude=__init__.py || exit 1 + # if we want to enable documentation checks, uncomment the line below + # flake8 ./GEMstack --ignore=E128,E402,E501,F401 --docstring-convention pep257 --max-line-length=120 --exclude=__init__.py || exit 1 + continue-on-error: false + + Documentation: + + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v3 + - name: Set up Python 3.10 + uses: actions/setup-python@v3 + with: + python-version: "3.10" + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install sphinx sphinx-rtd-theme + - name: Generate Documentation + run: | + # stop the build if there are Python syntax errors or undefined names + sphinx-build -b html docs docs/build + - name: Save Documentation as Artifact + uses: actions/upload-artifact@v4 + with: + name: documentation + path: docs/build diff --git a/GEMstack/knowledge/defaults/current.yaml b/GEMstack/knowledge/defaults/current.yaml index 5efa5b5bb..1350e5204 100644 --- a/GEMstack/knowledge/defaults/current.yaml +++ b/GEMstack/knowledge/defaults/current.yaml @@ -1,45 +1,45 @@ -# ********* Main settings entry point for behavior stack *********** - -# Configure settings for the vehicle / vehicle model -vehicle: !include ../vehicle/gem_e4.yaml - -#arguments for algorithm components here -model_predictive_controller: - dt: 0.1 - lookahead: 20 - -control: - recovery: - brake_amount : 0.5 - brake_speed : 2.0 - - # Pure Pursuit controller parameters - pure_pursuit: - lookahead: 2.0 # Base lookahead distance (meters) - lookahead_scale: 3.0 # Velocity-dependent lookahead scaling factor - crosstrack_gain: 0.5 # Gain for crosstrack error correction (default: 1) - desired_speed: trajectory # Speed reference source: can be "trajectory", "path", or a constant value - - # Stanley controller parameters (fine tune this) - stanley: - control_gain: 1.0 - softening_gain: 0.01 - desired_speed: trajectory # Speed reference source: can be "trajectory", "path", or a constant value - - # Shared longitudinal control parameters - longitudinal_control: - pid_p: 1.5 # Proportional gain for speed PID controller - pid_i: 0.1 # Integral gain for speed PID controller - pid_d: 0.0 # Derivative gain for speed PID controller - -#configure the simulator, if using -simulator: - dt: 0.01 - real_time_multiplier: 1.0 # make the simulator run faster than real time by making this > 1 - gnss_emulator: - dt: 0.1 # 10Hz - #position_noise: 0.1 # 10cm noise - #orientation_noise: 0.04 # 2.3 degrees noise - #velocity_noise: - # constant: 0.04 # 4cm/s noise - # linear: 0.02 # 2% noise +# ********* Main settings entry point for behavior stack *********** + +# Configure settings for the vehicle / vehicle model +vehicle: !include ../vehicle/gem_e4.yaml + +#arguments for algorithm components here +model_predictive_controller: + dt: 0.1 + lookahead: 20 + +control: + recovery: + brake_amount : 0.5 + brake_speed : 2.0 + + # Pure Pursuit controller parameters + pure_pursuit: + lookahead: 2.0 # Base lookahead distance (meters) + lookahead_scale: 3.0 # Velocity-dependent lookahead scaling factor + crosstrack_gain: 0.5 # Gain for crosstrack error correction (default: 1) + desired_speed: trajectory # Speed reference source: can be "trajectory", "path", or a constant value + + # Stanley controller parameters (fine tune this) + stanley: + control_gain: 1.0 + softening_gain: 0.01 + desired_speed: trajectory # Speed reference source: can be "trajectory", "path", or a constant value + + # Shared longitudinal control parameters + longitudinal_control: + pid_p: 1.5 # Proportional gain for speed PID controller + pid_i: 0.1 # Integral gain for speed PID controller + pid_d: 0.0 # Derivative gain for speed PID controller + +#configure the simulator, if using +simulator: + dt: 0.01 + real_time_multiplier: 1.0 # make the simulator run faster than real time by making this > 1 + gnss_emulator: + dt: 0.1 # 10Hz + #position_noise: 0.1 # 10cm noise + #orientation_noise: 0.04 # 2.3 degrees noise + #velocity_noise: + # constant: 0.04 # 4cm/s noise + # linear: 0.02 # 2% noise diff --git a/GEMstack/onboard/planning/mpc.py b/GEMstack/onboard/planning/mpc.py new file mode 100644 index 000000000..fe511bbfa --- /dev/null +++ b/GEMstack/onboard/planning/mpc.py @@ -0,0 +1,162 @@ +from ...utils import settings +from ...state.vehicle import VehicleState,ObjectFrameEnum +from ...state.trajectory import Trajectory, Path +from ...knowledge.vehicle.geometry import front2steer +from ..component import Component +import numpy as np +import casadi + +class MPCController(object): + """Model Predictive Controller for trajectory tracking.""" + def __init__(self, T=None, dt=None): + self.T = T if T is not None else settings.get('control.mpc.horizon', 10) + self.dt = dt if dt is not None else settings.get('control.mpc.dt', 0.1) + self.L = settings.get('vehicle.geometry.wheelbase') + self.v_bounds = [-settings.get('vehicle.limits.max_reverse_speed'), settings.get('vehicle.limits.max_speed')] + self.delta_bounds = [settings.get('vehicle.geometry.min_wheel_angle'),settings.get('vehicle.geometry.max_wheel_angle')] + self.steering_angle_range = [settings.get('vehicle.geometry.min_steering_angle'),settings.get('vehicle.geometry.max_steering_angle')] + self.a_bounds = [-settings.get('vehicle.limits.max_deceleration'), settings.get('vehicle.limits.max_acceleration')] + self.trajectory = None + self.prev_x = None # Previous state trajectory + self.prev_u = None # Previous control inputs + self.path = None + self.path_arg = None + + # def set_path(self, trajectory: Trajectory): + # """Set the trajectory for the MPC controller.""" + # # Assume the argument can only be trajectory + # assert isinstance(trajectory, Trajectory), "Invalid trajectory type." + # print("*"*10) + # print("set_path") + # self.trajectory = trajectory + + def set_path(self, path : Path): + print("Get new path!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!") + if path == self.path_arg: + return + self.path_arg = path + if len(path.points[0]) > 2: + path = path.get_dims([0,1]) + self.path = path.arc_length_parameterize() + self.trajectory = self.path + self.current_traj_parameter = self.trajectory.domain()[0] + self.current_path_parameter = 0.0 + + def compute(self, state: VehicleState, component: Component = None): + """Compute the control commands using MPC.""" + + if self.trajectory is not None: + if self.trajectory.frame != state.pose.frame: + print("Transforming trajectory from",self.trajectory.frame.name,"to",state.pose.frame.name) + self.trajectory = self.trajectory.to_frame(state.pose.frame, current_pose=state.pose) + + x0 = np.array([state.pose.x, state.pose.y, state.pose.yaw, state.v]) + + # Interpolate trajectory points to match MPC time horizon + times = self.trajectory.times + points = self.trajectory.points + traj_points = [] + j = 0 + for i in range(self.T + 1): + t_query = times[0] + i * self.dt + if t_query <= times[0]: + traj_points.append(points[0]) + elif t_query >= times[-1]: + traj_points.append(points[-1]) + else: + while j < len(times) - 2 and times[j+1] < t_query: + j += 1 + alpha = (t_query - times[j]) / (times[j+1] - times[j]) + pt = (1 - alpha) * np.array(points[j]) + alpha * np.array(points[j+1]) + traj_points.append(pt) + + # Optimization setup + opti = casadi.Opti() + x = opti.variable(self.T+1, 4) # [x, y, theta, v] + u = opti.variable(self.T, 2) # [a, delta] + + def model(x, u): + """Dynamic model of the vehicle using kinematic bicycle model""" + px, py, theta, v = x[0], x[1], x[2], x[3] + a, delta = u[0], u[1] + beta = casadi.atan(casadi.tan(delta) / 2.0) + dx = v * casadi.cos(theta + beta) + dy = v * casadi.sin(theta + beta) + dtheta = v * casadi.tan(delta) / self.L + dv = a + return casadi.vertcat(dx, dy, dtheta, dv) + + obj = 0 + for t in range(self.T): + # Vehicle dynamics + x_next = x[t,:] + self.dt * model(x[t,:], u[t,:]).T + opti.subject_to(x[t+1,:] == x_next) + target = traj_points[t+1] + # Cost function + obj += casadi.sumsqr(x[t+1,0:2] - casadi.reshape(target[0:2], 1, 2)) + obj += 0.1 * casadi.sumsqr(u[t,:]) + + # Initial condition + opti.subject_to(x[0, :] == casadi.reshape(x0[:4], 1, 4)) + + # Constraints + for t in range(self.T): + opti.subject_to(opti.bounded(self.a_bounds[0], u[t,0], self.a_bounds[1])) + opti.subject_to(opti.bounded(self.delta_bounds[0], u[t,1], self.delta_bounds[1])) + opti.subject_to(opti.bounded(self.v_bounds[0], x[t+1,3], self.v_bounds[1])) + + # Initial guess + if self.prev_x is not None and self.prev_u is not None: + if len(self.prev_x) == self.T+1 and len(self.prev_u) == self.T: + opti.set_initial(x, np.vstack((self.prev_x[1:], self.prev_x[-1]))) + opti.set_initial(u, np.vstack((self.prev_u[1:], self.prev_u[-1]))) + + # Solver settings + opti.minimize(obj) + opti.solver("ipopt", {"expand": True}, {"max_iter": 100}) + + try: + # Solve the optimization problem + sol = opti.solve() + acc = float(sol.value(u[0,0])) + delta = float(sol.value(u[0,1])) + self.prev_x = sol.value(x) + self.prev_u = sol.value(u) + print(acc, delta) + return acc, delta + except RuntimeError: + # Handle optimization failure + print("MPC optimization failed.") + return 0.0, 0.0 + + +class MPCTrajectoryTracker(Component): + def __init__(self, vehicle_interface=None, **args): + self.mpc = MPCController(**args) + self.vehicle_interface = vehicle_interface + print(type(vehicle_interface)) + + def rate(self): + return 5.0 + + def state_inputs(self): + return ['vehicle', 'trajectory'] + + def state_outputs(self): + return [] + + def update(self, vehicle: VehicleState, trajectory: Trajectory): + self.mpc.set_path(trajectory) + accel, delta = self.mpc.compute(vehicle, self) + + # Clip acceleration and steering angle to vehicle limits + accel = np.clip(accel, self.mpc.a_bounds[0], self.mpc.a_bounds[1]) + delta = np.clip(delta, self.mpc.delta_bounds[0], self.mpc.delta_bounds[1]) + + # Convert delta to steering angle + steering_angle = np.clip(front2steer(delta), + self.mpc.steering_angle_range[0], self.mpc.steering_angle_range[1]) + self.vehicle_interface.send_command(self.vehicle_interface.simple_command(accel, steering_angle, vehicle)) + + def healthy(self): + return self.mpc.path is not None \ No newline at end of file diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley.py index 8f871f488..19efbfbf4 100644 --- a/GEMstack/onboard/planning/stanley.py +++ b/GEMstack/onboard/planning/stanley.py @@ -1,312 +1,312 @@ -import numpy as np -from math import sin, cos, atan2 - -# These imports match your existing project structure -from ...mathutils.control import PID -from ...utils import settings -from ...mathutils import transforms -from ...knowledge.vehicle.geometry import front2steer -from ...knowledge.vehicle.dynamics import acceleration_to_pedal_positions -from ...state.vehicle import VehicleState, ObjectFrameEnum -from ...state.trajectory import Path, Trajectory -from ..interface.gem import GEMVehicleCommand -from ..component import Component - -##################################### -# 1. Angle normalization -##################################### -def normalise_angle(angle): - """Wraps an angle to the [-pi, pi] range.""" - while angle > np.pi: - angle -= 2.0 * np.pi - while angle < -np.pi: - angle += 2.0 * np.pi - return angle - -##################################### -# 2. Stanley-based controller with longitudinal PID -##################################### -class Stanley(object): - """ - A Stanley controller that handles lateral control (steering) - plus a basic longitudinal control (PID + optional feedforward). - It has been modified to reduce oscillations by: - 1) Lower gains - 2) Steering damping - 3) Low-pass filter on steering - 4) Gentler speed logic when cornering - """ - - def __init__( - self, - control_gain=None, - softening_gain=None, - desired_speed=None - ): - """ - :param control_gain: Stanley lateral control gain k (lowered from the default to reduce overshoot). - :param softening_gain: Softening gain k_soft. - :param yaw_rate_gain: Yaw-rate gain k_yaw_rate (helps reduce oscillations). - :param steering_damp_gain: Steering damping gain k_damp_steer. - :param desired_speed: Desired speed (float) or 'path'/'trajectory'. - """ - - # 1) Lower Gains - self.k = control_gain if control_gain is not None else settings.get('control.stanley.control_gain') - self.k_soft = softening_gain if softening_gain is not None else settings.get('control.stanley.softening_gain') - - # Typically, this is the max front-wheel steering angle in radians - self.max_steer = settings.get('vehicle.geometry.max_wheel_angle') - self.wheelbase = settings.get('vehicle.geometry.wheelbase') - - # Basic longitudinal constraints - self.speed_limit = settings.get('vehicle.limits.max_speed') - self.max_accel = settings.get('vehicle.limits.max_acceleration') - self.max_decel = settings.get('vehicle.limits.max_deceleration') - - # PID for longitudinal speed tracking - p = settings.get('control.longitudinal_control.pid_p') - d = settings.get('control.longitudinal_control.pid_d') - i = settings.get('control.longitudinal_control.pid_i') - self.pid_speed = PID(p, d, i, windup_limit=20) - - # Speed source: numeric or derived from path/trajectory - if desired_speed is not None: - self.desired_speed_source = desired_speed - else: - self.desired_speed_source = settings.get('control.stanley.desired_speed', 'path') - - if isinstance(self.desired_speed_source, (int, float)): - self.desired_speed = self.desired_speed_source - else: - self.desired_speed = None - - # For path/trajectory - self.path_arg = None - self.path = None - self.trajectory = None - self.current_path_parameter = 0.0 - self.current_traj_parameter = 0.0 - self.t_last = None - - def set_path(self, path: Path): - """Sets the path or trajectory to track.""" - if path == self.path_arg: - return - self.path_arg = path - - # If the path has more than 2D, reduce to (x,y) - if len(path.points[0]) > 2: - path = path.get_dims([0,1]) - - # If no timing info, we can't rely on 'path'/'trajectory' speed - if not isinstance(path, Trajectory): - self.path = path.arc_length_parameterize() - self.trajectory = None - self.current_traj_parameter = 0.0 - if self.desired_speed_source in ['path', 'trajectory']: - raise ValueError("Stanley: Provided path has no timing. Either provide a Trajectory or set a constant desired_speed.") - else: - self.path = path.arc_length_parameterize() - self.trajectory = path - self.current_traj_parameter = self.trajectory.domain()[0] - - self.current_path_parameter = 0.0 - - def _find_front_axle_position(self, x, y, yaw): - """Compute front-axle world position from the center/rear and yaw.""" - fx = x + self.wheelbase * cos(yaw) - fy = y + self.wheelbase * sin(yaw) - return fx, fy - - def compute(self, state: VehicleState, component: Component = None): - """Compute the control outputs: (longitudinal acceleration, front wheel angle).""" - t = state.pose.t - if self.t_last is None: - self.t_last = t - dt = t - self.t_last - - # Current vehicle states - curr_x = state.pose.x - curr_y = state.pose.y - curr_yaw = state.pose.yaw if state.pose.yaw is not None else 0.0 - speed = state.v - - if self.path is None: - if component: - component.debug_event("No path provided to Stanley controller. Doing nothing.") - return (0.0, 0.0) - - # Ensure same frame - if self.path.frame != state.pose.frame: - if component: - component.debug_event(f"Transforming path from {self.path.frame.name} to {state.pose.frame.name}") - self.path = self.path.to_frame(state.pose.frame, current_pose=state.pose) - - if self.trajectory and self.trajectory.frame != state.pose.frame: - if component: - component.debug_event(f"Transforming trajectory from {self.trajectory.frame.name} to {state.pose.frame.name}") - self.trajectory = self.trajectory.to_frame(state.pose.frame, current_pose=state.pose) - - # 1) Closest point - fx, fy = self._find_front_axle_position(curr_x, curr_y, curr_yaw) - search_start = self.current_path_parameter - 5.0 - search_end = self.current_path_parameter + 5.0 - closest_dist, closest_parameter = self.path.closest_point_local((fx, fy), [search_start, search_end]) - self.current_path_parameter = closest_parameter - - # 2) Path heading - target_x, target_y = self.path.eval(closest_parameter) - tangent = self.path.eval_tangent(closest_parameter) - path_yaw = atan2(tangent[1], tangent[0]) - desired_x = target_x - desired_y = target_y - # 3) Lateral error - dx = fx - target_x - dy = fy - target_y - left_vec = np.array([sin(curr_yaw), -cos(curr_yaw)]) - cross_track_error = np.sign(np.dot(np.array([dx, dy]), left_vec)) * closest_dist - - # 4) Heading error - yaw_error = normalise_angle(path_yaw - curr_yaw) - - # 5) Standard Stanley terms - cross_term = atan2(self.k * cross_track_error, self.k_soft + speed) - desired_steering_angle = yaw_error + cross_term - - desired_speed = self.desired_speed - feedforward_accel = 0.0 - - if self.trajectory and self.desired_speed_source in ['path', 'trajectory']: - if len(self.trajectory.points) < 2 or self.current_path_parameter >= self.path.domain()[1]: - # End of trajectory -> stop - if component: - component.debug_event("Stanley: Past the end of trajectory, stopping.") - desired_speed = 0.0 - feedforward_accel = -2.0 - else: - if self.desired_speed_source == 'path': - current_trajectory_time = self.trajectory.parameter_to_time(self.current_path_parameter) - else: - self.current_traj_parameter += dt - current_trajectory_time = self.current_traj_parameter - - deriv = self.trajectory.eval_derivative(current_trajectory_time) - desired_speed = min(np.linalg.norm(deriv), self.speed_limit) - - difference_dt = 0.1 - future_t = current_trajectory_time + difference_dt - if future_t > self.trajectory.domain()[1]: - future_t = self.trajectory.domain()[1] - future_deriv = self.trajectory.eval_derivative(future_t) - next_desired_speed = min(np.linalg.norm(future_deriv), self.speed_limit) - feedforward_accel = (next_desired_speed - desired_speed) / difference_dt - feedforward_accel = np.clip(feedforward_accel, -self.max_decel, self.max_accel) - else: - if desired_speed is None: - desired_speed = 4.0 - - # Cross-track-based slowdown (less aggressive than before). - desired_speed *= np.exp(-abs(cross_track_error) * 0.6) - - # Clip to speed limit - if desired_speed > self.speed_limit: - desired_speed = self.speed_limit - - # PID for longitudinal control - speed_error = desired_speed - speed - output_accel = self.pid_speed.advance(e=speed_error, t=t, feedforward_term=feedforward_accel) - - # Clip acceleration - if output_accel > self.max_accel: - output_accel = self.max_accel - elif output_accel < -self.max_decel: - output_accel = -self.max_decel - - # Avoid negative accel when fully stopped - if desired_speed == 0 and abs(speed) < 0.01 and output_accel < 0: - output_accel = 0.0 - - # Debug - if component is not None: - # component.debug("Stanley: fx, fy", (fx, fy)) - component.debug('curr pt',(curr_x,curr_y)) - component.debug("desired_x",desired_x) - component.debug("desired_y",desired_y) - component.debug("Stanley: path param", self.current_path_parameter) - component.debug("Stanley: crosstrack dist", closest_dist) - component.debug("crosstrack error", cross_track_error) - component.debug("Stanley: yaw_error", yaw_error) - component.debug('steering_angle', desired_steering_angle) - component.debug("Stanley: desired_speed (m/s)", desired_speed) - component.debug("Stanley: feedforward_accel (m/s^2)", feedforward_accel) - component.debug("Stanley: output_accel (m/s^2)", output_accel) - - if output_accel > self.max_accel: - output_accel = self.max_accel - - if output_accel < -self.max_decel: - output_accel = -self.max_decel - - self.t_last = t - return (output_accel, desired_steering_angle) - -##################################### -# 3. Tracker component -##################################### -class StanleyTrajectoryTracker(Component): - """ - A trajectory-tracking Component that uses the above Stanley controller - for lateral control plus PID-based longitudinal control. - It now includes measures to mitigate oscillations. - """ - - def __init__(self, vehicle_interface=None, **kwargs): - """ - :param vehicle_interface: The low-level interface to send commands to the vehicle. - :param kwargs: Optional parameters to pass into the Stanley(...) constructor. - """ - self.stanley = Stanley(**kwargs) - self.vehicle_interface = vehicle_interface - - def rate(self): - """Control frequency in Hz.""" - return 50.0 - - def state_inputs(self): - """ - Required state inputs: - - Vehicle state - - Trajectory - """ - return ["vehicle", "trajectory"] - - def state_outputs(self): - """No direct output state here.""" - return [] - - def update(self, vehicle: VehicleState, trajectory: Trajectory): - """ - Per control cycle: - 1) Set the path/trajectory - 2) Compute (acceleration, front wheel angle) - 3) Convert front wheel angle to steering wheel angle (if necessary) - 4) Send command to the vehicle - """ - self.stanley.set_path(trajectory) - accel, f_delta = self.stanley.compute(vehicle, self) - - # If your low-level interface expects steering wheel angle: - steering_angle = front2steer(f_delta) - steering_angle = np.clip( - steering_angle, - settings.get('vehicle.geometry.min_steering_angle', -0.5), - settings.get('vehicle.geometry.max_steering_angle', 0.5) - ) - - cmd = self.vehicle_interface.simple_command(accel, steering_angle, vehicle) - self.vehicle_interface.send_command(cmd) - - def healthy(self): - """Optional: check if the controller has a valid path.""" - return self.stanley.path is not None +import numpy as np +from math import sin, cos, atan2 + +# These imports match your existing project structure +from ...mathutils.control import PID +from ...utils import settings +from ...mathutils import transforms +from ...knowledge.vehicle.geometry import front2steer +from ...knowledge.vehicle.dynamics import acceleration_to_pedal_positions +from ...state.vehicle import VehicleState, ObjectFrameEnum +from ...state.trajectory import Path, Trajectory +from ..interface.gem import GEMVehicleCommand +from ..component import Component + +##################################### +# 1. Angle normalization +##################################### +def normalise_angle(angle): + """Wraps an angle to the [-pi, pi] range.""" + while angle > np.pi: + angle -= 2.0 * np.pi + while angle < -np.pi: + angle += 2.0 * np.pi + return angle + +##################################### +# 2. Stanley-based controller with longitudinal PID +##################################### +class Stanley(object): + """ + A Stanley controller that handles lateral control (steering) + plus a basic longitudinal control (PID + optional feedforward). + It has been modified to reduce oscillations by: + 1) Lower gains + 2) Steering damping + 3) Low-pass filter on steering + 4) Gentler speed logic when cornering + """ + + def __init__( + self, + control_gain=None, + softening_gain=None, + desired_speed=None + ): + """ + :param control_gain: Stanley lateral control gain k (lowered from the default to reduce overshoot). + :param softening_gain: Softening gain k_soft. + :param yaw_rate_gain: Yaw-rate gain k_yaw_rate (helps reduce oscillations). + :param steering_damp_gain: Steering damping gain k_damp_steer. + :param desired_speed: Desired speed (float) or 'path'/'trajectory'. + """ + + # 1) Lower Gains + self.k = control_gain if control_gain is not None else settings.get('control.stanley.control_gain') + self.k_soft = softening_gain if softening_gain is not None else settings.get('control.stanley.softening_gain') + + # Typically, this is the max front-wheel steering angle in radians + self.max_steer = settings.get('vehicle.geometry.max_wheel_angle') + self.wheelbase = settings.get('vehicle.geometry.wheelbase') + + # Basic longitudinal constraints + self.speed_limit = settings.get('vehicle.limits.max_speed') + self.max_accel = settings.get('vehicle.limits.max_acceleration') + self.max_decel = settings.get('vehicle.limits.max_deceleration') + + # PID for longitudinal speed tracking + p = settings.get('control.longitudinal_control.pid_p') + d = settings.get('control.longitudinal_control.pid_d') + i = settings.get('control.longitudinal_control.pid_i') + self.pid_speed = PID(p, d, i, windup_limit=20) + + # Speed source: numeric or derived from path/trajectory + if desired_speed is not None: + self.desired_speed_source = desired_speed + else: + self.desired_speed_source = settings.get('control.stanley.desired_speed', 'path') + + if isinstance(self.desired_speed_source, (int, float)): + self.desired_speed = self.desired_speed_source + else: + self.desired_speed = None + + # For path/trajectory + self.path_arg = None + self.path = None + self.trajectory = None + self.current_path_parameter = 0.0 + self.current_traj_parameter = 0.0 + self.t_last = None + + def set_path(self, path: Path): + """Sets the path or trajectory to track.""" + if path == self.path_arg: + return + self.path_arg = path + + # If the path has more than 2D, reduce to (x,y) + if len(path.points[0]) > 2: + path = path.get_dims([0,1]) + + # If no timing info, we can't rely on 'path'/'trajectory' speed + if not isinstance(path, Trajectory): + self.path = path.arc_length_parameterize() + self.trajectory = None + self.current_traj_parameter = 0.0 + if self.desired_speed_source in ['path', 'trajectory']: + raise ValueError("Stanley: Provided path has no timing. Either provide a Trajectory or set a constant desired_speed.") + else: + self.path = path.arc_length_parameterize() + self.trajectory = path + self.current_traj_parameter = self.trajectory.domain()[0] + + self.current_path_parameter = 0.0 + + def _find_front_axle_position(self, x, y, yaw): + """Compute front-axle world position from the center/rear and yaw.""" + fx = x + self.wheelbase * cos(yaw) + fy = y + self.wheelbase * sin(yaw) + return fx, fy + + def compute(self, state: VehicleState, component: Component = None): + """Compute the control outputs: (longitudinal acceleration, front wheel angle).""" + t = state.pose.t + if self.t_last is None: + self.t_last = t + dt = t - self.t_last + + # Current vehicle states + curr_x = state.pose.x + curr_y = state.pose.y + curr_yaw = state.pose.yaw if state.pose.yaw is not None else 0.0 + speed = state.v + + if self.path is None: + if component: + component.debug_event("No path provided to Stanley controller. Doing nothing.") + return (0.0, 0.0) + + # Ensure same frame + if self.path.frame != state.pose.frame: + if component: + component.debug_event(f"Transforming path from {self.path.frame.name} to {state.pose.frame.name}") + self.path = self.path.to_frame(state.pose.frame, current_pose=state.pose) + + if self.trajectory and self.trajectory.frame != state.pose.frame: + if component: + component.debug_event(f"Transforming trajectory from {self.trajectory.frame.name} to {state.pose.frame.name}") + self.trajectory = self.trajectory.to_frame(state.pose.frame, current_pose=state.pose) + + # 1) Closest point + fx, fy = self._find_front_axle_position(curr_x, curr_y, curr_yaw) + search_start = self.current_path_parameter - 5.0 + search_end = self.current_path_parameter + 5.0 + closest_dist, closest_parameter = self.path.closest_point_local((fx, fy), [search_start, search_end]) + self.current_path_parameter = closest_parameter + + # 2) Path heading + target_x, target_y = self.path.eval(closest_parameter) + tangent = self.path.eval_tangent(closest_parameter) + path_yaw = atan2(tangent[1], tangent[0]) + desired_x = target_x + desired_y = target_y + # 3) Lateral error + dx = fx - target_x + dy = fy - target_y + left_vec = np.array([sin(curr_yaw), -cos(curr_yaw)]) + cross_track_error = np.sign(np.dot(np.array([dx, dy]), left_vec)) * closest_dist + + # 4) Heading error + yaw_error = normalise_angle(path_yaw - curr_yaw) + + # 5) Standard Stanley terms + cross_term = atan2(self.k * cross_track_error, self.k_soft + speed) + desired_steering_angle = yaw_error + cross_term + + desired_speed = self.desired_speed + feedforward_accel = 0.0 + + if self.trajectory and self.desired_speed_source in ['path', 'trajectory']: + if len(self.trajectory.points) < 2 or self.current_path_parameter >= self.path.domain()[1]: + # End of trajectory -> stop + if component: + component.debug_event("Stanley: Past the end of trajectory, stopping.") + desired_speed = 0.0 + feedforward_accel = -2.0 + else: + if self.desired_speed_source == 'path': + current_trajectory_time = self.trajectory.parameter_to_time(self.current_path_parameter) + else: + self.current_traj_parameter += dt + current_trajectory_time = self.current_traj_parameter + + deriv = self.trajectory.eval_derivative(current_trajectory_time) + desired_speed = min(np.linalg.norm(deriv), self.speed_limit) + + difference_dt = 0.1 + future_t = current_trajectory_time + difference_dt + if future_t > self.trajectory.domain()[1]: + future_t = self.trajectory.domain()[1] + future_deriv = self.trajectory.eval_derivative(future_t) + next_desired_speed = min(np.linalg.norm(future_deriv), self.speed_limit) + feedforward_accel = (next_desired_speed - desired_speed) / difference_dt + feedforward_accel = np.clip(feedforward_accel, -self.max_decel, self.max_accel) + else: + if desired_speed is None: + desired_speed = 4.0 + + # Cross-track-based slowdown (less aggressive than before). + desired_speed *= np.exp(-abs(cross_track_error) * 0.6) + + # Clip to speed limit + if desired_speed > self.speed_limit: + desired_speed = self.speed_limit + + # PID for longitudinal control + speed_error = desired_speed - speed + output_accel = self.pid_speed.advance(e=speed_error, t=t, feedforward_term=feedforward_accel) + + # Clip acceleration + if output_accel > self.max_accel: + output_accel = self.max_accel + elif output_accel < -self.max_decel: + output_accel = -self.max_decel + + # Avoid negative accel when fully stopped + if desired_speed == 0 and abs(speed) < 0.01 and output_accel < 0: + output_accel = 0.0 + + # Debug + if component is not None: + # component.debug("Stanley: fx, fy", (fx, fy)) + component.debug('curr pt',(curr_x,curr_y)) + component.debug("desired_x",desired_x) + component.debug("desired_y",desired_y) + component.debug("Stanley: path param", self.current_path_parameter) + component.debug("Stanley: crosstrack dist", closest_dist) + component.debug("crosstrack error", cross_track_error) + component.debug("Stanley: yaw_error", yaw_error) + component.debug('steering_angle', desired_steering_angle) + component.debug("Stanley: desired_speed (m/s)", desired_speed) + component.debug("Stanley: feedforward_accel (m/s^2)", feedforward_accel) + component.debug("Stanley: output_accel (m/s^2)", output_accel) + + if output_accel > self.max_accel: + output_accel = self.max_accel + + if output_accel < -self.max_decel: + output_accel = -self.max_decel + + self.t_last = t + return (output_accel, desired_steering_angle) + +##################################### +# 3. Tracker component +##################################### +class StanleyTrajectoryTracker(Component): + """ + A trajectory-tracking Component that uses the above Stanley controller + for lateral control plus PID-based longitudinal control. + It now includes measures to mitigate oscillations. + """ + + def __init__(self, vehicle_interface=None, **kwargs): + """ + :param vehicle_interface: The low-level interface to send commands to the vehicle. + :param kwargs: Optional parameters to pass into the Stanley(...) constructor. + """ + self.stanley = Stanley(**kwargs) + self.vehicle_interface = vehicle_interface + + def rate(self): + """Control frequency in Hz.""" + return 50.0 + + def state_inputs(self): + """ + Required state inputs: + - Vehicle state + - Trajectory + """ + return ["vehicle", "trajectory"] + + def state_outputs(self): + """No direct output state here.""" + return [] + + def update(self, vehicle: VehicleState, trajectory: Trajectory): + """ + Per control cycle: + 1) Set the path/trajectory + 2) Compute (acceleration, front wheel angle) + 3) Convert front wheel angle to steering wheel angle (if necessary) + 4) Send command to the vehicle + """ + self.stanley.set_path(trajectory) + accel, f_delta = self.stanley.compute(vehicle, self) + + # If your low-level interface expects steering wheel angle: + steering_angle = front2steer(f_delta) + steering_angle = np.clip( + steering_angle, + settings.get('vehicle.geometry.min_steering_angle', -0.5), + settings.get('vehicle.geometry.max_steering_angle', 0.5) + ) + + cmd = self.vehicle_interface.simple_command(accel, steering_angle, vehicle) + self.vehicle_interface.send_command(cmd) + + def healthy(self): + """Optional: check if the controller has a valid path.""" + return self.stanley.path is not None diff --git a/GEMstack/utils/klampt_visualization.py b/GEMstack/utils/klampt_visualization.py index 063f690c7..188c8d4f9 100644 --- a/GEMstack/utils/klampt_visualization.py +++ b/GEMstack/utils/klampt_visualization.py @@ -1,299 +1,299 @@ -from klampt import vis -from klampt.math import vectorops,so3,se3 -from klampt.model.trajectory import Trajectory -from klampt import Geometry3D, GeometricPrimitive, TriangleMesh -import numpy as np -from . import settings -from ..state import ObjectFrameEnum,ObjectPose,PhysicalObject,VehicleState,VehicleGearEnum,Path,Obstacle,AgentState,AgentEnum,Roadgraph,RoadgraphLane,RoadgraphLaneEnum,RoadgraphCurve,RoadgraphCurveEnum,RoadgraphRegion,RoadgraphRegionEnum,RoadgraphSurfaceEnum,Trajectory,Route,SceneState,AllState - -#KH: there is a bug on some system where the visualization crashes with an OpenGL error when drawing curves -#this is a workaround. We really should find the source of the bug! -MAX_POINTS_IN_CURVE = 50 - -OBJECT_COLORS = { - AgentEnum.CAR : (1,1,0,1), - AgentEnum.PEDESTRIAN : (0,1,0,1), - AgentEnum.BICYCLIST : (0,0,1,1), - AgentEnum.MEDIUM_TRUCK : (1,0,1,1), - AgentEnum.LARGE_TRUCK : (0,1,1,1), - None: (0.7,0.7,0.7,1), -} - -AUX_BBOX_COLOR = (0,0,1,1) - -CURVE_TO_STYLE = { - RoadgraphCurveEnum.LANE_BOUNDARY : {'color':(1,1,1,1),'width':2,'pointSize':0}, - RoadgraphCurveEnum.CURB : {'color':(0.5,0.5,0.5,1),'width':2,'pointSize':0}, - RoadgraphCurveEnum.CLIFF : {'color':(1,0,0,1),'width':2,'pointSize':0}, - RoadgraphCurveEnum.CROSSING_BOUNDARY : {'color':(1,1,1,1),'width':1,'pointSize':0}, - RoadgraphCurveEnum.PARKING_SPOT_BOUNDARY : {'color':(1,1,1,1),'width':1,'pointSize':0}, - RoadgraphCurveEnum.STOP_LINE : {'color':(1,1,1,1),'width':2,'pointSize':0}, - RoadgraphCurveEnum.WALL : {'color':(0,0,1,1),'width':2,'pointSize':0}, - None : {'color':(1,1,1,1),'width':1,'pointSize':0}, -} - -SURFACE_TO_STYLE = { - RoadgraphSurfaceEnum.PAVEMENT : {'color':(0.5,0.5,0.5,0.2),'pointSize':0}, - RoadgraphSurfaceEnum.DIRT : {'color':(160/255.0,82/255.0,45/255.0,0.2),'pointSize':0}, - RoadgraphSurfaceEnum.GRASS : {'color':(50/255.0,255/255.0,50/255.0,0.2),'pointSize':0}, - RoadgraphSurfaceEnum.GRAVEL : {'color':(0.7,0.7,0.7,0.2),'pointSize':0}, - None: {'color':(1,0,0,0.2),'pointSize':0}, -} - -REGION_TO_STYLE = { - RoadgraphRegionEnum.INTERSECTION : {'color':(0,1,0,0.5),'width':1,'pointSize':0}, - RoadgraphRegionEnum.PARKING_LOT : {'color':(0,0,1,0.5),'width':1,'pointSize':0}, - RoadgraphRegionEnum.CLOSED_COURSE : {'color':(1,0,0,0.5),'width':1,'pointSize':0}, - RoadgraphRegionEnum.VIRTUAL : {'color':(0,0,0,0.5),'width':1,'pointSize':0}, -} - -def plot_pose(name : str, pose : ObjectPose, axis_len=0.1, label=True): - """Plots the pose in the given axes. The coordinates - of the pose are plotted in the pose's indicated frame.""" - R = pose.rotation() - t = pose.translation() - T = (so3.from_matrix(R),t) - vis.add(name, T, length=axis_len, hide_label=(not label)) - if pose.frame == ObjectFrameEnum.START: - vis.add(name+"_origin",t,color=(0,0,1,1),size=5,hide_label=True) - elif pose.frame == ObjectFrameEnum.CURRENT: - vis.add(name+"_origin",t,color=(1,0,0,1),size=5,hide_label=True) - elif pose.frame in [ObjectFrameEnum.GLOBAL,ObjectFrameEnum.ABSOLUTE_CARTESIAN]: - vis.add(name+"_origin",t,color=(0,1,0,1),size=5,hide_label=True) - else: - raise ValueError("Unknown frame %s" % pose.frame) - -def plot_object(name : str, obj : PhysicalObject, type=None, axis_len=None, outline=True, solid=True, bbox=True, label=True): - """Shows an object in the given axes. - - If axis_len is given, shows the object's pose with - a coordinate frame of the given length. - - If outline is True, shows the object's outline. - - If bbox is True, shows the object's bounding box. - """ - height = obj.dimensions[2] - core_color = OBJECT_COLORS[type] - bbox_color = AUX_BBOX_COLOR - if label: - #add a point at the object's origin - vis.add(name,obj.pose.translation(),size=5,color=(0,0,0,1)) - if axis_len: - plot_pose(name+"_pose", obj.pose, axis_len=0) - #plot bounding box - R = obj.pose.rotation() - t = obj.pose.translation() - if bbox or (outline and obj.outline is None): - bounds = obj.bounds() - (xmin,xmax),(ymin,ymax),(zmin,zmax) = bounds - if not solid: - corners = [[xmin,ymin,0.01],[xmin,ymax,0.01],[xmax,ymax,0.01],[xmax,ymin,0.01]] - corners = [list(R.dot(c)+t) for c in corners] - corners.append(corners[0]) - if not bbox: - vis.add(name+"_bbox1",corners,width=1,color=core_color,pointSize=0,hide_label=True) - else: - vis.add(name+"_bbox1",corners,width=1,color=AUX_BBOX_COLOR,pointSize=0,hide_label=True) - if height > 0: - corners = [[xmin,ymin,height],[xmin,ymax,height],[xmax,ymax,height],[xmax,ymin,height]] - corners = [list(R.dot(c)+t) for c in corners] - corners.append(corners[0]) - if not bbox: - vis.add(name+"_bbox2",corners,width=1,color=core_color,pointSize=0,hide_label=True) - else: - vis.add(name+"_bbox2",corners,width=1,color=AUX_BBOX_COLOR,pointSize=0,hide_label=True) - else: - prim = GeometricPrimitive() - prim.setAABB([xmin,ymin,0],[xmax,ymax,height]) - g = Geometry3D(prim) - g.setCurrentTransform(so3.from_matrix(R),t) - vis.add(name+"_bbox",g,color=core_color[:3]+(0.5,),hide_label=True) - - #plot outline - if outline and obj.outline: - outline = [R.dot((p[0],p[1],0))+t for p in obj.outline] - outline.append(outline[0]) - vis.add(name+"_outline1",outline,width=1,color=core_color,hide_label=True) - if height > 0: - outline = [R.dot((p[0],p[1],height))+t for p in obj.outline] - outline.append(outline[0]) - vis.add(name+"_outline2",outline,width=1,color=core_color,hide_label=True) - - -def plot_vehicle(vehicle : VehicleState, vehicle_model=None, axis_len=1.0): - """Plots the vehicle in the given axes. The coordinates - of the vehicle are plotted in the vehicle's indicated frame.""" - plot_object("vehicle",vehicle.to_object(), type=None, axis_len=0, solid=False, label=False) - R = vehicle.pose.rotation() - t = vehicle.pose.translation() - T = (so3.from_matrix(R),t) - vis.add("vehicle", T, length=axis_len, hide_label=True) - - xbounds,ybounds,zbounds = settings.get('vehicle.geometry.bounds') - #plot velocity arrow - R = vehicle.pose.rotation2d() - t = np.array([vehicle.pose.x,vehicle.pose.y]) - v = R.dot([vehicle.v,0]) - front_pt = vehicle.pose.apply((xbounds[1],0.0)) - h = (vehicle.pose.z if vehicle.pose.z is not None else 0) + zbounds[1] - vis.add("vehicle_velocity",[[front_pt[0],front_pt[1],h],[front_pt[0]+v[0],front_pt[1]+v[1],h]],width=2,color=(0,1,0,1),pointSize=0) - - #plot front wheel angles - wheelbase = settings.get('vehicle.geometry.wheelbase') - wheel_spacing = 0.8*settings.get('vehicle.geometry.width') / 2 - phi = vehicle.front_wheel_angle - if vehicle_model: - q = vehicle_model.getConfig() - lwhindex = vehicle_model.link("left_steering_hinge_link").index - rwhindex = vehicle_model.link("right_steering_hinge_link").index - lwindex = vehicle_model.link("front_left_wheel_link").index - rwindex = vehicle_model.link("front_right_wheel_link").index - rlwindex = vehicle_model.link("rear_left_wheel_link").index - rrwindex = vehicle_model.link("rear_right_wheel_link").index - q[lwhindex] = phi - q[rwhindex] = phi - q[lwindex] += vehicle.v * 0.2 - q[rwindex] += vehicle.v * 0.2 - q[rlwindex] += vehicle.v * 0.2 - q[rrwindex] += vehicle.v * 0.2 - vehicle_model.setConfig(q) - else: - left_wheel_origin = t + R.dot([wheelbase,wheel_spacing]) - right_wheel_origin = t + R.dot([wheelbase,-wheel_spacing]) - wheel_width = 0.5 #meters - wheel_offset = R.dot(np.array([np.cos(phi),np.sin(phi)]))*wheel_width*0.5 - vis.add("left_wheel",[vectorops.sub(left_wheel_origin,wheel_offset),vectorops.add(left_wheel_origin,wheel_offset)],color=(0,0,0,1),width=2) - vis.add("right_wheel",[vectorops.sub(right_wheel_origin,wheel_offset),vectorops.add(right_wheel_origin,wheel_offset)],color=(0,0,0,1),width=2) - - #plot gear - if vehicle.gear in [VehicleGearEnum.NEUTRAL,VehicleGearEnum.REVERSE,VehicleGearEnum.PARK]: - if vehicle.gear == VehicleGearEnum.NEUTRAL: - gear = 'N' - elif vehicle.gear == VehicleGearEnum.REVERSE: - gear = 'R' - else: - gear = 'P' - vis.addText("gear",gear,position=(t[0],t[1],1.5),color=(0,0,0,1)) - - #plot lights - light_point_size = 4 - light_size = 0.15 - light_color = (1,1,0,1) - turn_indicator_height = 0.7 - headlight_height = 0.6 - if vehicle.left_turn_indicator: - lp = vehicle.pose.apply([xbounds[0],ybounds[0]+light_size,turn_indicator_height]) - vis.add("left_turn_indicator",list(lp),size=light_point_size,color=light_color) - if vehicle.right_turn_indicator: - lp = vehicle.pose.apply([xbounds[0],ybounds[1]-light_size,turn_indicator_height]) - vis.add("right_turn_indicator",list(lp),size=light_point_size,color=light_color) - if vehicle.headlights_on: - lp = vehicle.pose.apply([xbounds[1],ybounds[0]+light_size*2,headlight_height]) - vis.add("left_headlight",list(lp),size=light_point_size,color=light_color) - lp = vehicle.pose.apply([xbounds[1],ybounds[1]-light_size*2,headlight_height]) - vis.add("right_headlight",list(lp),size=light_point_size,color=light_color) - if vehicle_model is not None: - if vehicle.brake_pedal_position > 0.1: - vehicle_model.link('rear_right_stop_light_link').appearance().setColor(1,0,0,1) - vehicle_model.link('rear_left_stop_light_link').appearance().setColor(1,0,0,1) - else: - vehicle_model.link('rear_right_stop_light_link').appearance().setColor(0.3,0,0,1) - vehicle_model.link('rear_left_stop_light_link').appearance().setColor(0.3,0,0,1) - -def plot_path(name : str, path : Path, color=(0,0,0), width=1): - if len(path.points) > MAX_POINTS_IN_CURVE: # downsample due to OpenGL error? - vis.add(name,[list(p) for p in path.points[::len(path.points)//MAX_POINTS_IN_CURVE]],color=color,width=width) - else: - vis.add(name,[list(p) for p in path.points],color=color,width=width) - -def plot_curve(name : str, curve : RoadgraphCurve, color=None, width=None): - style = CURVE_TO_STYLE.get(curve.type,CURVE_TO_STYLE[None]) - if curve.crossable and curve.type == RoadgraphCurveEnum.LANE_BOUNDARY: - #style['linestyle'] = '--' - #TODO: how to indicate crossable lines? - pass - if color is not None: - style['color'] = color - if width is not None: - style['width'] = width - for i,seg in enumerate(curve.segments): - if len(seg) > MAX_POINTS_IN_CURVE: # downsample due to OpenGL error? - vis.add(name+"_%d" % i,seg[::len(seg)//MAX_POINTS_IN_CURVE],**style) - else: - vis.add(name+"_%d" % i,seg,**style) - -def plot_lane(name : str, lane : RoadgraphLane, on_route=False): - if lane.surface != RoadgraphSurfaceEnum.PAVEMENT: - style = SURFACE_TO_STYLE.get(lane.surface,SURFACE_TO_STYLE[None]) - outline = lane.outline() - vis.add(name, outline, **style) - if lane.left is not None: - plot_curve(name+"_left", lane.left) - if lane.right is not None: - plot_curve(name+"_right", lane.right) - -def plot_region(name : str, region : RoadgraphRegion, color=None, width=None): - style = REGION_TO_STYLE.get(region.type,REGION_TO_STYLE[None]) - points = region.outline() - pts = points + points[0] - if color is not None: - style['color'] = color - if width is not None: - style['width'] = width - vis.add(name, [list(p) for p in pts], **style) - -def plot_roadgraph(roadgraph : Roadgraph, route : Route = None): - #plot lanes - for k,l in roadgraph.lanes.items(): - if route is not None and k in route.lanes: - plot_lane(k,l,on_route=True) - else: - plot_lane(k,l) - for k,c in roadgraph.curves.items(): - plot_curve(k,c,color=(0,0,0,1)) - #plot intersections - for k,r in roadgraph.regions.items(): - plot_region(k,r) - #plot - for k,o in roadgraph.static_obstacles.items(): - plot_object(k,o) - -def plot_scene(scene : SceneState, ground_truth_vehicle=None, vehicle_model = None, title = None, show=True): - for i in list(vis.scene().items.keys()): - if not i.startswith("vehicle"): - if not isinstance(vis.scene().items[i],vis.VisPlot): - vis.remove(i) - #set plot range - #TODO - if vehicle_model is not None: - vis.add("vehicle_model",vehicle_model) - if ground_truth_vehicle is not None: - xform = ground_truth_vehicle.to_object().pose.transform() - else: - xform = scene.vehicle.to_object().pose.transform() - vehicle_model.link(0).setParentTransform(*se3.from_ndarray(xform)) - vehicle_model.setConfig(vehicle_model.getConfig()) - - #plot roadgraph - plot_roadgraph(scene.roadgraph,scene.route) - #plot vehicle and objects - plot_vehicle(scene.vehicle, vehicle_model) - for k,a in scene.agents.items(): - plot_object(k,a,type=a.type) - for k,o in scene.obstacles.items(): - plot_object(k,o) - if title is None: - if show: - vis.add("title","Scene at t = %.2f" % scene.t, position=(10,10)) - else: - vis.add("title",title) - if show: - vis.show() - -def plot(state : AllState, ground_truth_vehicle = None, vehicle_model=None, title=None, show=True): - plot_scene(state, ground_truth_vehicle=ground_truth_vehicle, vehicle_model=vehicle_model, title=title, show=show) - if state.route is not None: - plot_path("route",state.route,color=(1,0.5,0,1),width=2) - if state.trajectory is not None: - plot_path("trajectory",state.trajectory,color=(1,0,0,1),width=3) +from klampt import vis +from klampt.math import vectorops,so3,se3 +from klampt.model.trajectory import Trajectory +from klampt import Geometry3D, GeometricPrimitive, TriangleMesh +import numpy as np +from . import settings +from ..state import ObjectFrameEnum,ObjectPose,PhysicalObject,VehicleState,VehicleGearEnum,Path,Obstacle,AgentState,AgentEnum,Roadgraph,RoadgraphLane,RoadgraphLaneEnum,RoadgraphCurve,RoadgraphCurveEnum,RoadgraphRegion,RoadgraphRegionEnum,RoadgraphSurfaceEnum,Trajectory,Route,SceneState,AllState + +#KH: there is a bug on some system where the visualization crashes with an OpenGL error when drawing curves +#this is a workaround. We really should find the source of the bug! +MAX_POINTS_IN_CURVE = 50 + +OBJECT_COLORS = { + AgentEnum.CAR : (1,1,0,1), + AgentEnum.PEDESTRIAN : (0,1,0,1), + AgentEnum.BICYCLIST : (0,0,1,1), + AgentEnum.MEDIUM_TRUCK : (1,0,1,1), + AgentEnum.LARGE_TRUCK : (0,1,1,1), + None: (0.7,0.7,0.7,1), +} + +AUX_BBOX_COLOR = (0,0,1,1) + +CURVE_TO_STYLE = { + RoadgraphCurveEnum.LANE_BOUNDARY : {'color':(1,1,1,1),'width':2,'pointSize':0}, + RoadgraphCurveEnum.CURB : {'color':(0.5,0.5,0.5,1),'width':2,'pointSize':0}, + RoadgraphCurveEnum.CLIFF : {'color':(1,0,0,1),'width':2,'pointSize':0}, + RoadgraphCurveEnum.CROSSING_BOUNDARY : {'color':(1,1,1,1),'width':1,'pointSize':0}, + RoadgraphCurveEnum.PARKING_SPOT_BOUNDARY : {'color':(1,1,1,1),'width':1,'pointSize':0}, + RoadgraphCurveEnum.STOP_LINE : {'color':(1,1,1,1),'width':2,'pointSize':0}, + RoadgraphCurveEnum.WALL : {'color':(0,0,1,1),'width':2,'pointSize':0}, + None : {'color':(1,1,1,1),'width':1,'pointSize':0}, +} + +SURFACE_TO_STYLE = { + RoadgraphSurfaceEnum.PAVEMENT : {'color':(0.5,0.5,0.5,0.2),'pointSize':0}, + RoadgraphSurfaceEnum.DIRT : {'color':(160/255.0,82/255.0,45/255.0,0.2),'pointSize':0}, + RoadgraphSurfaceEnum.GRASS : {'color':(50/255.0,255/255.0,50/255.0,0.2),'pointSize':0}, + RoadgraphSurfaceEnum.GRAVEL : {'color':(0.7,0.7,0.7,0.2),'pointSize':0}, + None: {'color':(1,0,0,0.2),'pointSize':0}, +} + +REGION_TO_STYLE = { + RoadgraphRegionEnum.INTERSECTION : {'color':(0,1,0,0.5),'width':1,'pointSize':0}, + RoadgraphRegionEnum.PARKING_LOT : {'color':(0,0,1,0.5),'width':1,'pointSize':0}, + RoadgraphRegionEnum.CLOSED_COURSE : {'color':(1,0,0,0.5),'width':1,'pointSize':0}, + RoadgraphRegionEnum.VIRTUAL : {'color':(0,0,0,0.5),'width':1,'pointSize':0}, +} + +def plot_pose(name : str, pose : ObjectPose, axis_len=0.1, label=True): + """Plots the pose in the given axes. The coordinates + of the pose are plotted in the pose's indicated frame.""" + R = pose.rotation() + t = pose.translation() + T = (so3.from_matrix(R),t) + vis.add(name, T, length=axis_len, hide_label=(not label)) + if pose.frame == ObjectFrameEnum.START: + vis.add(name+"_origin",t,color=(0,0,1,1),size=5,hide_label=True) + elif pose.frame == ObjectFrameEnum.CURRENT: + vis.add(name+"_origin",t,color=(1,0,0,1),size=5,hide_label=True) + elif pose.frame in [ObjectFrameEnum.GLOBAL,ObjectFrameEnum.ABSOLUTE_CARTESIAN]: + vis.add(name+"_origin",t,color=(0,1,0,1),size=5,hide_label=True) + else: + raise ValueError("Unknown frame %s" % pose.frame) + +def plot_object(name : str, obj : PhysicalObject, type=None, axis_len=None, outline=True, solid=True, bbox=True, label=True): + """Shows an object in the given axes. + + If axis_len is given, shows the object's pose with + a coordinate frame of the given length. + + If outline is True, shows the object's outline. + + If bbox is True, shows the object's bounding box. + """ + height = obj.dimensions[2] + core_color = OBJECT_COLORS[type] + bbox_color = AUX_BBOX_COLOR + if label: + #add a point at the object's origin + vis.add(name,obj.pose.translation(),size=5,color=(0,0,0,1)) + if axis_len: + plot_pose(name+"_pose", obj.pose, axis_len=0) + #plot bounding box + R = obj.pose.rotation() + t = obj.pose.translation() + if bbox or (outline and obj.outline is None): + bounds = obj.bounds() + (xmin,xmax),(ymin,ymax),(zmin,zmax) = bounds + if not solid: + corners = [[xmin,ymin,0.01],[xmin,ymax,0.01],[xmax,ymax,0.01],[xmax,ymin,0.01]] + corners = [list(R.dot(c)+t) for c in corners] + corners.append(corners[0]) + if not bbox: + vis.add(name+"_bbox1",corners,width=1,color=core_color,pointSize=0,hide_label=True) + else: + vis.add(name+"_bbox1",corners,width=1,color=AUX_BBOX_COLOR,pointSize=0,hide_label=True) + if height > 0: + corners = [[xmin,ymin,height],[xmin,ymax,height],[xmax,ymax,height],[xmax,ymin,height]] + corners = [list(R.dot(c)+t) for c in corners] + corners.append(corners[0]) + if not bbox: + vis.add(name+"_bbox2",corners,width=1,color=core_color,pointSize=0,hide_label=True) + else: + vis.add(name+"_bbox2",corners,width=1,color=AUX_BBOX_COLOR,pointSize=0,hide_label=True) + else: + prim = GeometricPrimitive() + prim.setAABB([xmin,ymin,0],[xmax,ymax,height]) + g = Geometry3D(prim) + g.setCurrentTransform(so3.from_matrix(R),t) + vis.add(name+"_bbox",g,color=core_color[:3]+(0.5,),hide_label=True) + + #plot outline + if outline and obj.outline: + outline = [R.dot((p[0],p[1],0))+t for p in obj.outline] + outline.append(outline[0]) + vis.add(name+"_outline1",outline,width=1,color=core_color,hide_label=True) + if height > 0: + outline = [R.dot((p[0],p[1],height))+t for p in obj.outline] + outline.append(outline[0]) + vis.add(name+"_outline2",outline,width=1,color=core_color,hide_label=True) + + +def plot_vehicle(vehicle : VehicleState, vehicle_model=None, axis_len=1.0): + """Plots the vehicle in the given axes. The coordinates + of the vehicle are plotted in the vehicle's indicated frame.""" + plot_object("vehicle",vehicle.to_object(), type=None, axis_len=0, solid=False, label=False) + R = vehicle.pose.rotation() + t = vehicle.pose.translation() + T = (so3.from_matrix(R),t) + vis.add("vehicle", T, length=axis_len, hide_label=True) + + xbounds,ybounds,zbounds = settings.get('vehicle.geometry.bounds') + #plot velocity arrow + R = vehicle.pose.rotation2d() + t = np.array([vehicle.pose.x,vehicle.pose.y]) + v = R.dot([vehicle.v,0]) + front_pt = vehicle.pose.apply((xbounds[1],0.0)) + h = (vehicle.pose.z if vehicle.pose.z is not None else 0) + zbounds[1] + vis.add("vehicle_velocity",[[front_pt[0],front_pt[1],h],[front_pt[0]+v[0],front_pt[1]+v[1],h]],width=2,color=(0,1,0,1),pointSize=0) + + #plot front wheel angles + wheelbase = settings.get('vehicle.geometry.wheelbase') + wheel_spacing = 0.8*settings.get('vehicle.geometry.width') / 2 + phi = vehicle.front_wheel_angle + if vehicle_model: + q = vehicle_model.getConfig() + lwhindex = vehicle_model.link("left_steering_hinge_link").index + rwhindex = vehicle_model.link("right_steering_hinge_link").index + lwindex = vehicle_model.link("front_left_wheel_link").index + rwindex = vehicle_model.link("front_right_wheel_link").index + rlwindex = vehicle_model.link("rear_left_wheel_link").index + rrwindex = vehicle_model.link("rear_right_wheel_link").index + q[lwhindex] = phi + q[rwhindex] = phi + q[lwindex] += vehicle.v * 0.2 + q[rwindex] += vehicle.v * 0.2 + q[rlwindex] += vehicle.v * 0.2 + q[rrwindex] += vehicle.v * 0.2 + vehicle_model.setConfig(q) + else: + left_wheel_origin = t + R.dot([wheelbase,wheel_spacing]) + right_wheel_origin = t + R.dot([wheelbase,-wheel_spacing]) + wheel_width = 0.5 #meters + wheel_offset = R.dot(np.array([np.cos(phi),np.sin(phi)]))*wheel_width*0.5 + vis.add("left_wheel",[vectorops.sub(left_wheel_origin,wheel_offset),vectorops.add(left_wheel_origin,wheel_offset)],color=(0,0,0,1),width=2) + vis.add("right_wheel",[vectorops.sub(right_wheel_origin,wheel_offset),vectorops.add(right_wheel_origin,wheel_offset)],color=(0,0,0,1),width=2) + + #plot gear + if vehicle.gear in [VehicleGearEnum.NEUTRAL,VehicleGearEnum.REVERSE,VehicleGearEnum.PARK]: + if vehicle.gear == VehicleGearEnum.NEUTRAL: + gear = 'N' + elif vehicle.gear == VehicleGearEnum.REVERSE: + gear = 'R' + else: + gear = 'P' + vis.addText("gear",gear,position=(t[0],t[1],1.5),color=(0,0,0,1)) + + #plot lights + light_point_size = 4 + light_size = 0.15 + light_color = (1,1,0,1) + turn_indicator_height = 0.7 + headlight_height = 0.6 + if vehicle.left_turn_indicator: + lp = vehicle.pose.apply([xbounds[0],ybounds[0]+light_size,turn_indicator_height]) + vis.add("left_turn_indicator",list(lp),size=light_point_size,color=light_color) + if vehicle.right_turn_indicator: + lp = vehicle.pose.apply([xbounds[0],ybounds[1]-light_size,turn_indicator_height]) + vis.add("right_turn_indicator",list(lp),size=light_point_size,color=light_color) + if vehicle.headlights_on: + lp = vehicle.pose.apply([xbounds[1],ybounds[0]+light_size*2,headlight_height]) + vis.add("left_headlight",list(lp),size=light_point_size,color=light_color) + lp = vehicle.pose.apply([xbounds[1],ybounds[1]-light_size*2,headlight_height]) + vis.add("right_headlight",list(lp),size=light_point_size,color=light_color) + if vehicle_model is not None: + if vehicle.brake_pedal_position > 0.1: + vehicle_model.link('rear_right_stop_light_link').appearance().setColor(1,0,0,1) + vehicle_model.link('rear_left_stop_light_link').appearance().setColor(1,0,0,1) + else: + vehicle_model.link('rear_right_stop_light_link').appearance().setColor(0.3,0,0,1) + vehicle_model.link('rear_left_stop_light_link').appearance().setColor(0.3,0,0,1) + +def plot_path(name : str, path : Path, color=(0,0,0), width=1): + if len(path.points) > MAX_POINTS_IN_CURVE: # downsample due to OpenGL error? + vis.add(name,[list(p) for p in path.points[::len(path.points)//MAX_POINTS_IN_CURVE]],color=color,width=width) + else: + vis.add(name,[list(p) for p in path.points],color=color,width=width) + +def plot_curve(name : str, curve : RoadgraphCurve, color=None, width=None): + style = CURVE_TO_STYLE.get(curve.type,CURVE_TO_STYLE[None]) + if curve.crossable and curve.type == RoadgraphCurveEnum.LANE_BOUNDARY: + #style['linestyle'] = '--' + #TODO: how to indicate crossable lines? + pass + if color is not None: + style['color'] = color + if width is not None: + style['width'] = width + for i,seg in enumerate(curve.segments): + if len(seg) > MAX_POINTS_IN_CURVE: # downsample due to OpenGL error? + vis.add(name+"_%d" % i,seg[::len(seg)//MAX_POINTS_IN_CURVE],**style) + else: + vis.add(name+"_%d" % i,seg,**style) + +def plot_lane(name : str, lane : RoadgraphLane, on_route=False): + if lane.surface != RoadgraphSurfaceEnum.PAVEMENT: + style = SURFACE_TO_STYLE.get(lane.surface,SURFACE_TO_STYLE[None]) + outline = lane.outline() + vis.add(name, outline, **style) + if lane.left is not None: + plot_curve(name+"_left", lane.left) + if lane.right is not None: + plot_curve(name+"_right", lane.right) + +def plot_region(name : str, region : RoadgraphRegion, color=None, width=None): + style = REGION_TO_STYLE.get(region.type,REGION_TO_STYLE[None]) + points = region.outline() + pts = points + points[0] + if color is not None: + style['color'] = color + if width is not None: + style['width'] = width + vis.add(name, [list(p) for p in pts], **style) + +def plot_roadgraph(roadgraph : Roadgraph, route : Route = None): + #plot lanes + for k,l in roadgraph.lanes.items(): + if route is not None and k in route.lanes: + plot_lane(k,l,on_route=True) + else: + plot_lane(k,l) + for k,c in roadgraph.curves.items(): + plot_curve(k,c,color=(0,0,0,1)) + #plot intersections + for k,r in roadgraph.regions.items(): + plot_region(k,r) + #plot + for k,o in roadgraph.static_obstacles.items(): + plot_object(k,o) + +def plot_scene(scene : SceneState, ground_truth_vehicle=None, vehicle_model = None, title = None, show=True): + for i in list(vis.scene().items.keys()): + if not i.startswith("vehicle"): + if not isinstance(vis.scene().items[i],vis.VisPlot): + vis.remove(i) + #set plot range + #TODO + if vehicle_model is not None: + vis.add("vehicle_model",vehicle_model) + if ground_truth_vehicle is not None: + xform = ground_truth_vehicle.to_object().pose.transform() + else: + xform = scene.vehicle.to_object().pose.transform() + vehicle_model.link(0).setParentTransform(*se3.from_ndarray(xform)) + vehicle_model.setConfig(vehicle_model.getConfig()) + + #plot roadgraph + plot_roadgraph(scene.roadgraph,scene.route) + #plot vehicle and objects + plot_vehicle(scene.vehicle, vehicle_model) + for k,a in scene.agents.items(): + plot_object(k,a,type=a.type) + for k,o in scene.obstacles.items(): + plot_object(k,o) + if title is None: + if show: + vis.add("title","Scene at t = %.2f" % scene.t, position=(10,10)) + else: + vis.add("title",title) + if show: + vis.show() + +def plot(state : AllState, ground_truth_vehicle = None, vehicle_model=None, title=None, show=True): + plot_scene(state, ground_truth_vehicle=ground_truth_vehicle, vehicle_model=vehicle_model, title=title, show=show) + if state.route is not None: + plot_path("route",state.route,color=(1,0.5,0,1),width=2) + if state.trajectory is not None: + plot_path("trajectory",state.trajectory,color=(1,0,0,1),width=3) diff --git a/GEMstack/utils/settings.py b/GEMstack/utils/settings.py index a3d193658..9486b3bf2 100644 --- a/GEMstack/utils/settings.py +++ b/GEMstack/utils/settings.py @@ -1,80 +1,80 @@ -import json -from ..knowledge import defaults -import copy -from typing import List,Union,Any - -SETTINGS = None - -def load_settings(): - """Loads the settings object for the first time. - - Order of operations is to look into defaults.SETTINGS, and then - look through the command line arguments to determine whether the user has - overridden any settings using --KEY=VALUE. - """ - global SETTINGS - if SETTINGS is not None: - return - import os - import sys - SETTINGS = copy.deepcopy(defaults.SETTINGS) - for arg in sys.argv: - if arg.startswith('--'): - k,v = arg.split('=',1) - k = k[2:] - v = v.strip('"') - try: - v = json.loads(v) - except json.decoder.JSONDecodeError: - pass - if v.startswith('{'): - set(k,v,leaf_only=False) - else: - set(k,v) - - return - - -def settings(): - """Returns all global settings, loading them if necessary.""" - #global SETTINGS - load_settings() - return SETTINGS - - -def get(path : Union[str,List[str]], defaultValue=KeyError) -> Any: - """Retrieves a setting by a list of keys or a '.'-separated string.""" - #global SETTINGS - load_settings() - if isinstance(path,str): - path = path.split('.') - try: - val = SETTINGS - for key in path: - val = val[key] - return val - except KeyError: - if defaultValue is KeyError: - print("settings.py: Unable to get",path,"available keys:",val.keys()) - raise - return defaultValue - -def set(path : Union[str,List[str]], value : Any, leaf_only=True) -> None: - """Sets a setting by a list of keys or a '.'-separated string. - - If leaf_only=True (default), we prevent inadvertently deleting parts of the - settings dictionary. - """ - #global SETTINGS - load_settings() - if isinstance(path,str): - path = path.split('.') - val = SETTINGS - if len(path) == 0: - raise KeyError("Cannot set top-level settings") - for key in path[:-1]: - val = val[key] - if leaf_only: - if path[-1] in val and isinstance(val[path[-1]],dict): - raise ValueError("Can only set leaves of the settings dictionary when leaf_only=True is given") - val[path[-1]] = value +import json +from ..knowledge import defaults +import copy +from typing import List,Union,Any + +SETTINGS = None + +def load_settings(): + """Loads the settings object for the first time. + + Order of operations is to look into defaults.SETTINGS, and then + look through the command line arguments to determine whether the user has + overridden any settings using --KEY=VALUE. + """ + global SETTINGS + if SETTINGS is not None: + return + import os + import sys + SETTINGS = copy.deepcopy(defaults.SETTINGS) + for arg in sys.argv: + if arg.startswith('--'): + k,v = arg.split('=',1) + k = k[2:] + v = v.strip('"') + try: + v = json.loads(v) + except json.decoder.JSONDecodeError: + pass + if v.startswith('{'): + set(k,v,leaf_only=False) + else: + set(k,v) + + return + + +def settings(): + """Returns all global settings, loading them if necessary.""" + #global SETTINGS + load_settings() + return SETTINGS + + +def get(path : Union[str,List[str]], defaultValue=KeyError) -> Any: + """Retrieves a setting by a list of keys or a '.'-separated string.""" + #global SETTINGS + load_settings() + if isinstance(path,str): + path = path.split('.') + try: + val = SETTINGS + for key in path: + val = val[key] + return val + except KeyError: + if defaultValue is KeyError: + print("settings.py: Unable to get",path,"available keys:",val.keys()) + raise + return defaultValue + +def set(path : Union[str,List[str]], value : Any, leaf_only=True) -> None: + """Sets a setting by a list of keys or a '.'-separated string. + + If leaf_only=True (default), we prevent inadvertently deleting parts of the + settings dictionary. + """ + #global SETTINGS + load_settings() + if isinstance(path,str): + path = path.split('.') + val = SETTINGS + if len(path) == 0: + raise KeyError("Cannot set top-level settings") + for key in path[:-1]: + val = val[key] + if leaf_only: + if path[-1] in val and isinstance(val[path[-1]],dict): + raise ValueError("Can only set leaves of the settings dictionary when leaf_only=True is given") + val[path[-1]] = value diff --git a/launch/fixed_route.yaml b/launch/fixed_route.yaml index abd8242e6..3e28ea668 100644 --- a/launch/fixed_route.yaml +++ b/launch/fixed_route.yaml @@ -1,80 +1,80 @@ -description: "Drive the GEM vehicle along a fixed route (currently xyhead_highbay_backlot_p.csv)" -mode: hardware -vehicle_interface: gem_hardware.GEMHardwareInterface -mission_execution: StandardExecutor -# Recovery behavior after a component failure -recovery: - planning: - trajectory_tracking: - type: recovery.StopTrajectoryTracker - print: False -# Driving behavior for the GEM vehicle following a fixed route -drive: - perception: - state_estimation : GNSSStateEstimator - perception_normalization : StandardPerceptionNormalizer - planning: - route_planning: - type: StaticRoutePlanner - args: [!relative_path '../GEMstack/knowledge/routes/xyhead_highbay_backlot_p.csv'] - motion_planning: - type: RouteToTrajectoryPlanner - args: [null] #desired speed in m/s. If null, this will keep the route untimed for the trajectory tracker - trajectory_tracking: - #type: pure_pursuit.PurePursuitTrajectoryTracker - type: stanley.StanleyTrajectoryTracker - args: {desired_speed: 2.5} #approximately 5mph - print: False -log: - # Specify the top-level folder to save the log files. Default is 'logs' - #folder : 'logs' - # If prefix is specified, then the log folder will be named with the prefix followed by the date and time. Default no prefix - #prefix : 'fixed_route_' - # If suffix is specified, then logs will output to folder/prefix+suffix. Default uses date and time as the suffix - #suffix : 'test3' - # Specify which ros topics to record to vehicle.bag. Default records nothing. This records the "standard" ROS topics. - ros_topics : [] - # Specify options to pass to rosbag record. Default is no options. - #rosbag_options : '--split --size=1024' - # If True, then record all readings / commands of the vehicle interface. Default False - vehicle_interface : True - # Specify which components to record to behavior.json. Default records nothing - components : ['state_estimation','trajectory_tracking'] - # Specify which components of state to record to state.json. Default records nothing - #state: ['all'] - # Specify the rate in Hz at which to record state to state.json. Default records at the pipeline's rate - #state_rate: 10 -replay: # Add items here to set certain topics / inputs to be replayed from logs - # Specify which log folder to replay from - log: - # For replaying sensor data, try !include "../knowledge/defaults/standard_sensor_ros_topics.yaml" - ros_topics : [] - components : [] - -#usually can keep this constant -computation_graph: !include "../GEMstack/knowledge/defaults/computation_graph.yaml" - -after: - show_log_folder: True #set to false to avoid showing the log folder - -#on load, variants will overload the settings structure -variants: - #sim variant doesn't execute on the real vehicle - #real variant executes on the real robot - sim: - run: - mode: simulation - vehicle_interface: - type: gem_simulator.GEMDoubleIntegratorSimulationInterface - args: - scene: !relative_path '../scenes/xyhead_demo.yaml' - - drive: - perception: - state_estimation : OmniscientStateEstimator - agent_detection : OmniscientAgentDetector - visualization: !include "klampt_visualization.yaml" - #visualization: !include "mpl_visualization.yaml" - log_ros: - log: - ros_topics : !include "../GEMstack/knowledge/defaults/standard_ros_topics.yaml" +description: "Drive the GEM vehicle along a fixed route (currently xyhead_highbay_backlot_p.csv)" +mode: hardware +vehicle_interface: gem_hardware.GEMHardwareInterface +mission_execution: StandardExecutor +# Recovery behavior after a component failure +recovery: + planning: + trajectory_tracking: + type: recovery.StopTrajectoryTracker + print: False +# Driving behavior for the GEM vehicle following a fixed route +drive: + perception: + state_estimation : GNSSStateEstimator + perception_normalization : StandardPerceptionNormalizer + planning: + route_planning: + type: StaticRoutePlanner + args: [!relative_path '../GEMstack/knowledge/routes/xyhead_highbay_backlot_p.csv'] + motion_planning: + type: RouteToTrajectoryPlanner + args: [null] #desired speed in m/s. If null, this will keep the route untimed for the trajectory tracker + trajectory_tracking: + #type: pure_pursuit.PurePursuitTrajectoryTracker + type: stanley.StanleyTrajectoryTracker + args: {desired_speed: 2.5} #approximately 5mph + print: False +log: + # Specify the top-level folder to save the log files. Default is 'logs' + #folder : 'logs' + # If prefix is specified, then the log folder will be named with the prefix followed by the date and time. Default no prefix + #prefix : 'fixed_route_' + # If suffix is specified, then logs will output to folder/prefix+suffix. Default uses date and time as the suffix + #suffix : 'test3' + # Specify which ros topics to record to vehicle.bag. Default records nothing. This records the "standard" ROS topics. + ros_topics : [] + # Specify options to pass to rosbag record. Default is no options. + #rosbag_options : '--split --size=1024' + # If True, then record all readings / commands of the vehicle interface. Default False + vehicle_interface : True + # Specify which components to record to behavior.json. Default records nothing + components : ['state_estimation','trajectory_tracking'] + # Specify which components of state to record to state.json. Default records nothing + #state: ['all'] + # Specify the rate in Hz at which to record state to state.json. Default records at the pipeline's rate + #state_rate: 10 +replay: # Add items here to set certain topics / inputs to be replayed from logs + # Specify which log folder to replay from + log: + # For replaying sensor data, try !include "../knowledge/defaults/standard_sensor_ros_topics.yaml" + ros_topics : [] + components : [] + +#usually can keep this constant +computation_graph: !include "../GEMstack/knowledge/defaults/computation_graph.yaml" + +after: + show_log_folder: True #set to false to avoid showing the log folder + +#on load, variants will overload the settings structure +variants: + #sim variant doesn't execute on the real vehicle + #real variant executes on the real robot + sim: + run: + mode: simulation + vehicle_interface: + type: gem_simulator.GEMDoubleIntegratorSimulationInterface + args: + scene: !relative_path '../scenes/xyhead_demo.yaml' + + drive: + perception: + state_estimation : OmniscientStateEstimator + agent_detection : OmniscientAgentDetector + visualization: !include "klampt_visualization.yaml" + #visualization: !include "mpl_visualization.yaml" + log_ros: + log: + ros_topics : !include "../GEMstack/knowledge/defaults/standard_ros_topics.yaml" diff --git a/logs/logplot_pp.py b/logs/logplot_pp.py index 74025c291..2a6533afc 100644 --- a/logs/logplot_pp.py +++ b/logs/logplot_pp.py @@ -1,120 +1,120 @@ -####Following code can be used to visualize performance of Pure Pursuit after using it.### -###How to use?- After running the simulation or on actual vehicle you get logs in log folder### -### You can run the command- python3 logplot_pp.py "logfilename".### - - -import json -import sys -import os -import matplotlib.pyplot as plt -import numpy as np - -# Safety thresholds (not used in the current plots) -ACC_SAFE = 0.8 # |acceleration| <= 0.8 m/s² is safe -ACC_UNSAFE = 2.0 # |acceleration| >= 2.0 m/s² is unsafe -HR_SAFE = 0.05 # |heading_rate| <= 0.05 rad/s is safe -HR_UNSAFE = 0.2 # |heading_rate| >= 0.2 rad/s is unsafe - -def parse_behavior_log(filename): - times = [] - accelerations = [] - heading_rates = [] - - with open(filename, 'r') as f: - first_time = None # Store the first timestamp for shifting - for line in f: - try: - entry = json.loads(line) - except json.JSONDecodeError: - print(f"Skipping invalid JSON line: {line.strip()}") - continue - if "vehicle" in entry: - t = entry.get("time") - vehicle_data = entry["vehicle"].get("data", {}) - acceleration = vehicle_data.get("acceleration") - heading_rate = vehicle_data.get("heading_rate") - if t is not None and acceleration is not None and heading_rate is not None: - if first_time is None: - first_time = t # Set the first timestamp - times.append(t - first_time) # Shift time to start from 0 - accelerations.append(acceleration) - heading_rates.append(heading_rate) - - return np.array(times), np.array(accelerations), np.array(heading_rates) - -def parse_tracker_csv(filename): - data = np.genfromtxt(filename, delimiter=',', skip_header=1) - first_time = data[0, 18] # Normalize time to start from 0 - cte_time = data[:, 18] - first_time - cte = data[:, 20] - x_actual, y_actual = data[:, 2], data[:, 5] - x_desired, y_desired = data[:, 11], data[:, 14] - return cte_time, cte, x_actual, y_actual, x_desired, y_desired - -def compute_derivative(times, values): - dt = np.diff(times) - dv = np.diff(values) - derivative = dv / dt - return times[1:], derivative - -def plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired): - global_min_time = min(time_jerk[0], time_heading_acc[0], cte_time[0]) - global_max_time = max(time_jerk[-1], time_heading_acc[-1], cte_time[-1]) - - fig, axs = plt.subplots(2, 2, figsize=(12, 8)) - fig.subplots_adjust(hspace=0.4, wspace=0.3) - - axs[0,0].plot(time_jerk, jerk, marker='', linestyle='-', color='blue') - axs[0,0].set_ylabel("Jerk (m/s³)") - axs[0,0].set_title("Vehicle Jerk Over Time") - axs[0,0].grid(True) - axs[0,0].set_xlim(global_min_time, global_max_time) - - axs[0,1].plot(time_heading_acc, heading_acc, marker='', linestyle='-', color='orange') - axs[0,1].set_ylabel("Heading Acceleration (rad/s²)") - axs[0,1].set_title("Vehicle Heading Acceleration Over Time") - axs[0,1].grid(True) - axs[0,1].set_xlim(global_min_time, global_max_time) - - axs[1,0].plot(cte_time, cte, marker='', linestyle='-', color='green') - axs[1,0].set_xlabel("Time (s)") - axs[1,0].set_ylabel("Cross Track Error") - axs[1,0].set_title("Vehicle Cross Track Error Over Time") - axs[1,0].grid(True) - axs[1,0].set_xlim(global_min_time, global_max_time) - - axs[1,1].plot(x_actual, y_actual, marker='', linestyle='-', color='blue', label='Actual') - axs[1,1].plot(x_desired, y_desired, marker='', linestyle='-', color='orange', label='Desired') - axs[1,1].set_xlabel("X Position (m)") - axs[1,1].set_ylabel("Y Position (m)") - axs[1,1].set_title("Vehicle Position") - axs[1,1].legend() - axs[1,1].grid(True) - - plt.show() - -if __name__=='__main__': - if len(sys.argv) != 2: - print("Usage: python test_comfort_metrics.py ") - sys.exit(1) - - log_dir = sys.argv[1] - behavior_file = os.path.join(log_dir, "behavior.json") - tracker_file = os.path.join(log_dir, "PurePursuitTrajectoryTracker_debug.csv") - #tracker_file = os.path.join(log_dir, "StanleyTrajectoryTracker_debug.csv") - - times, accelerations, heading_rates = parse_behavior_log(behavior_file) - time_jerk, jerk = compute_derivative(times, accelerations) - time_heading_acc, heading_acc = compute_derivative(times, heading_rates) - - cte_time, cte, x_actual, y_actual, x_desired, y_desired = parse_tracker_csv(tracker_file) - - plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired) - - print("Max (abs) jerk:", np.max(np.abs(jerk))) - print("Avg jerk:", np.mean(np.abs(jerk))) - print("Max (abs) heading acceleration:", np.max(np.abs(heading_acc))) - print("Avg heading acceleration:", np.mean(np.abs(heading_acc))) - print("Max (abs) cross track error:", np.max(np.abs(cte))) - print("Avg cross track error:", np.mean(np.abs(cte))) - +####Following code can be used to visualize performance of Pure Pursuit after using it.### +###How to use?- After running the simulation or on actual vehicle you get logs in log folder### +### You can run the command- python3 logplot_pp.py "logfilename".### + + +import json +import sys +import os +import matplotlib.pyplot as plt +import numpy as np + +# Safety thresholds (not used in the current plots) +ACC_SAFE = 0.8 # |acceleration| <= 0.8 m/s² is safe +ACC_UNSAFE = 2.0 # |acceleration| >= 2.0 m/s² is unsafe +HR_SAFE = 0.05 # |heading_rate| <= 0.05 rad/s is safe +HR_UNSAFE = 0.2 # |heading_rate| >= 0.2 rad/s is unsafe + +def parse_behavior_log(filename): + times = [] + accelerations = [] + heading_rates = [] + + with open(filename, 'r') as f: + first_time = None # Store the first timestamp for shifting + for line in f: + try: + entry = json.loads(line) + except json.JSONDecodeError: + print(f"Skipping invalid JSON line: {line.strip()}") + continue + if "vehicle" in entry: + t = entry.get("time") + vehicle_data = entry["vehicle"].get("data", {}) + acceleration = vehicle_data.get("acceleration") + heading_rate = vehicle_data.get("heading_rate") + if t is not None and acceleration is not None and heading_rate is not None: + if first_time is None: + first_time = t # Set the first timestamp + times.append(t - first_time) # Shift time to start from 0 + accelerations.append(acceleration) + heading_rates.append(heading_rate) + + return np.array(times), np.array(accelerations), np.array(heading_rates) + +def parse_tracker_csv(filename): + data = np.genfromtxt(filename, delimiter=',', skip_header=1) + first_time = data[0, 18] # Normalize time to start from 0 + cte_time = data[:, 18] - first_time + cte = data[:, 20] + x_actual, y_actual = data[:, 2], data[:, 5] + x_desired, y_desired = data[:, 11], data[:, 14] + return cte_time, cte, x_actual, y_actual, x_desired, y_desired + +def compute_derivative(times, values): + dt = np.diff(times) + dv = np.diff(values) + derivative = dv / dt + return times[1:], derivative + +def plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired): + global_min_time = min(time_jerk[0], time_heading_acc[0], cte_time[0]) + global_max_time = max(time_jerk[-1], time_heading_acc[-1], cte_time[-1]) + + fig, axs = plt.subplots(2, 2, figsize=(12, 8)) + fig.subplots_adjust(hspace=0.4, wspace=0.3) + + axs[0,0].plot(time_jerk, jerk, marker='', linestyle='-', color='blue') + axs[0,0].set_ylabel("Jerk (m/s³)") + axs[0,0].set_title("Vehicle Jerk Over Time") + axs[0,0].grid(True) + axs[0,0].set_xlim(global_min_time, global_max_time) + + axs[0,1].plot(time_heading_acc, heading_acc, marker='', linestyle='-', color='orange') + axs[0,1].set_ylabel("Heading Acceleration (rad/s²)") + axs[0,1].set_title("Vehicle Heading Acceleration Over Time") + axs[0,1].grid(True) + axs[0,1].set_xlim(global_min_time, global_max_time) + + axs[1,0].plot(cte_time, cte, marker='', linestyle='-', color='green') + axs[1,0].set_xlabel("Time (s)") + axs[1,0].set_ylabel("Cross Track Error") + axs[1,0].set_title("Vehicle Cross Track Error Over Time") + axs[1,0].grid(True) + axs[1,0].set_xlim(global_min_time, global_max_time) + + axs[1,1].plot(x_actual, y_actual, marker='', linestyle='-', color='blue', label='Actual') + axs[1,1].plot(x_desired, y_desired, marker='', linestyle='-', color='orange', label='Desired') + axs[1,1].set_xlabel("X Position (m)") + axs[1,1].set_ylabel("Y Position (m)") + axs[1,1].set_title("Vehicle Position") + axs[1,1].legend() + axs[1,1].grid(True) + + plt.show() + +if __name__=='__main__': + if len(sys.argv) != 2: + print("Usage: python test_comfort_metrics.py ") + sys.exit(1) + + log_dir = sys.argv[1] + behavior_file = os.path.join(log_dir, "behavior.json") + tracker_file = os.path.join(log_dir, "PurePursuitTrajectoryTracker_debug.csv") + #tracker_file = os.path.join(log_dir, "StanleyTrajectoryTracker_debug.csv") + + times, accelerations, heading_rates = parse_behavior_log(behavior_file) + time_jerk, jerk = compute_derivative(times, accelerations) + time_heading_acc, heading_acc = compute_derivative(times, heading_rates) + + cte_time, cte, x_actual, y_actual, x_desired, y_desired = parse_tracker_csv(tracker_file) + + plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired) + + print("Max (abs) jerk:", np.max(np.abs(jerk))) + print("Avg jerk:", np.mean(np.abs(jerk))) + print("Max (abs) heading acceleration:", np.max(np.abs(heading_acc))) + print("Avg heading acceleration:", np.mean(np.abs(heading_acc))) + print("Max (abs) cross track error:", np.max(np.abs(cte))) + print("Avg cross track error:", np.mean(np.abs(cte))) + diff --git a/logs/logplot_s.py b/logs/logplot_s.py index f534bbd27..ecff7f118 100644 --- a/logs/logplot_s.py +++ b/logs/logplot_s.py @@ -1,119 +1,119 @@ -####Following code can be used to visualize performance of Stanley after using it.### -###How to use?- After running the simulation or on actual vehicle you get logs in log folder### -### You can run the command- python3 logplot_s.py "logfilename".### - -import json -import sys -import os -import matplotlib.pyplot as plt -import numpy as np - -# Safety thresholds (not used in the current plots) -ACC_SAFE = 0.8 # |acceleration| <= 0.8 m/s² is safe -ACC_UNSAFE = 2.0 # |acceleration| >= 2.0 m/s² is unsafe -HR_SAFE = 0.05 # |heading_rate| <= 0.05 rad/s is safe -HR_UNSAFE = 0.2 # |heading_rate| >= 0.2 rad/s is unsafe - -def parse_behavior_log(filename): - times = [] - accelerations = [] - heading_rates = [] - - with open(filename, 'r') as f: - first_time = None # Store the first timestamp for shifting - for line in f: - try: - entry = json.loads(line) - except json.JSONDecodeError: - print(f"Skipping invalid JSON line: {line.strip()}") - continue - if "vehicle" in entry: - t = entry.get("time") - vehicle_data = entry["vehicle"].get("data", {}) - acceleration = vehicle_data.get("acceleration") - heading_rate = vehicle_data.get("heading_rate") - if t is not None and acceleration is not None and heading_rate is not None: - if first_time is None: - first_time = t # Set the first timestamp - times.append(t - first_time) # Shift time to start from 0 - accelerations.append(acceleration) - heading_rates.append(heading_rate) - - return np.array(times), np.array(accelerations), np.array(heading_rates) - -def parse_tracker_csv(filename): - data = np.genfromtxt(filename, delimiter=',', skip_header=1) - first_time = data[0, 18] # Normalize time to start from 0 - cte_time = data[:, 18] - first_time - cte = data[:, 20] - x_actual, y_actual = data[:, 2], data[:, 5] - x_desired, y_desired = data[:, 8], data[:, 11] - return cte_time, cte, x_actual, y_actual, x_desired, y_desired - -def compute_derivative(times, values): - dt = np.diff(times) - dv = np.diff(values) - derivative = dv / dt - return times[1:], derivative - -def plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired): - global_min_time = min(time_jerk[0], time_heading_acc[0], cte_time[0]) - global_max_time = max(time_jerk[-1], time_heading_acc[-1], cte_time[-1]) - - fig, axs = plt.subplots(2, 2, figsize=(12, 8)) - fig.subplots_adjust(hspace=0.4, wspace=0.3) - - axs[0,0].plot(time_jerk, jerk, marker='', linestyle='-', color='blue') - axs[0,0].set_ylabel("Jerk (m/s³)") - axs[0,0].set_title("Vehicle Jerk Over Time") - axs[0,0].grid(True) - axs[0,0].set_xlim(global_min_time, global_max_time) - - axs[0,1].plot(time_heading_acc, heading_acc, marker='', linestyle='-', color='orange') - axs[0,1].set_ylabel("Heading Acceleration (rad/s²)") - axs[0,1].set_title("Vehicle Heading Acceleration Over Time") - axs[0,1].grid(True) - axs[0,1].set_xlim(global_min_time, global_max_time) - - axs[1,0].plot(cte_time, cte, marker='', linestyle='-', color='green') - axs[1,0].set_xlabel("Time (s)") - axs[1,0].set_ylabel("Cross Track Error") - axs[1,0].set_title("Vehicle Cross Track Error Over Time") - axs[1,0].grid(True) - axs[1,0].set_xlim(global_min_time, global_max_time) - - axs[1,1].plot(x_actual, y_actual, marker='', linestyle='-', color='blue', label='Actual') - axs[1,1].plot(x_desired, y_desired, marker='', linestyle='-', color='orange', label='Desired') - axs[1,1].set_xlabel("X Position (m)") - axs[1,1].set_ylabel("Y Position (m)") - axs[1,1].set_title("Vehicle Position") - axs[1,1].legend() - axs[1,1].grid(True) - - plt.show() - -if __name__=='__main__': - if len(sys.argv) != 2: - print("Usage: python test_comfort_metrics.py ") - sys.exit(1) - - log_dir = sys.argv[1] - behavior_file = os.path.join(log_dir, "behavior.json") - #tracker_file = os.path.join(log_dir, "PurePursuitTrajectoryTracker_debug.csv") - tracker_file = os.path.join(log_dir, "StanleyTrajectoryTracker_debug.csv") - - times, accelerations, heading_rates = parse_behavior_log(behavior_file) - time_jerk, jerk = compute_derivative(times, accelerations) - time_heading_acc, heading_acc = compute_derivative(times, heading_rates) - - cte_time, cte, x_actual, y_actual, x_desired, y_desired = parse_tracker_csv(tracker_file) - - plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired) - - print("Max (abs) jerk:", np.max(np.abs(jerk))) - print("Avg jerk:", np.mean(np.abs(jerk))) - print("Max (abs) heading acceleration:", np.max(np.abs(heading_acc))) - print("Avg heading acceleration:", np.mean(np.abs(heading_acc))) - print("Max (abs) cross track error:", np.max(np.abs(cte))) - print("Avg cross track error:", np.mean(np.abs(cte))) - +####Following code can be used to visualize performance of Stanley after using it.### +###How to use?- After running the simulation or on actual vehicle you get logs in log folder### +### You can run the command- python3 logplot_s.py "logfilename".### + +import json +import sys +import os +import matplotlib.pyplot as plt +import numpy as np + +# Safety thresholds (not used in the current plots) +ACC_SAFE = 0.8 # |acceleration| <= 0.8 m/s² is safe +ACC_UNSAFE = 2.0 # |acceleration| >= 2.0 m/s² is unsafe +HR_SAFE = 0.05 # |heading_rate| <= 0.05 rad/s is safe +HR_UNSAFE = 0.2 # |heading_rate| >= 0.2 rad/s is unsafe + +def parse_behavior_log(filename): + times = [] + accelerations = [] + heading_rates = [] + + with open(filename, 'r') as f: + first_time = None # Store the first timestamp for shifting + for line in f: + try: + entry = json.loads(line) + except json.JSONDecodeError: + print(f"Skipping invalid JSON line: {line.strip()}") + continue + if "vehicle" in entry: + t = entry.get("time") + vehicle_data = entry["vehicle"].get("data", {}) + acceleration = vehicle_data.get("acceleration") + heading_rate = vehicle_data.get("heading_rate") + if t is not None and acceleration is not None and heading_rate is not None: + if first_time is None: + first_time = t # Set the first timestamp + times.append(t - first_time) # Shift time to start from 0 + accelerations.append(acceleration) + heading_rates.append(heading_rate) + + return np.array(times), np.array(accelerations), np.array(heading_rates) + +def parse_tracker_csv(filename): + data = np.genfromtxt(filename, delimiter=',', skip_header=1) + first_time = data[0, 18] # Normalize time to start from 0 + cte_time = data[:, 18] - first_time + cte = data[:, 20] + x_actual, y_actual = data[:, 2], data[:, 5] + x_desired, y_desired = data[:, 8], data[:, 11] + return cte_time, cte, x_actual, y_actual, x_desired, y_desired + +def compute_derivative(times, values): + dt = np.diff(times) + dv = np.diff(values) + derivative = dv / dt + return times[1:], derivative + +def plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired): + global_min_time = min(time_jerk[0], time_heading_acc[0], cte_time[0]) + global_max_time = max(time_jerk[-1], time_heading_acc[-1], cte_time[-1]) + + fig, axs = plt.subplots(2, 2, figsize=(12, 8)) + fig.subplots_adjust(hspace=0.4, wspace=0.3) + + axs[0,0].plot(time_jerk, jerk, marker='', linestyle='-', color='blue') + axs[0,0].set_ylabel("Jerk (m/s³)") + axs[0,0].set_title("Vehicle Jerk Over Time") + axs[0,0].grid(True) + axs[0,0].set_xlim(global_min_time, global_max_time) + + axs[0,1].plot(time_heading_acc, heading_acc, marker='', linestyle='-', color='orange') + axs[0,1].set_ylabel("Heading Acceleration (rad/s²)") + axs[0,1].set_title("Vehicle Heading Acceleration Over Time") + axs[0,1].grid(True) + axs[0,1].set_xlim(global_min_time, global_max_time) + + axs[1,0].plot(cte_time, cte, marker='', linestyle='-', color='green') + axs[1,0].set_xlabel("Time (s)") + axs[1,0].set_ylabel("Cross Track Error") + axs[1,0].set_title("Vehicle Cross Track Error Over Time") + axs[1,0].grid(True) + axs[1,0].set_xlim(global_min_time, global_max_time) + + axs[1,1].plot(x_actual, y_actual, marker='', linestyle='-', color='blue', label='Actual') + axs[1,1].plot(x_desired, y_desired, marker='', linestyle='-', color='orange', label='Desired') + axs[1,1].set_xlabel("X Position (m)") + axs[1,1].set_ylabel("Y Position (m)") + axs[1,1].set_title("Vehicle Position") + axs[1,1].legend() + axs[1,1].grid(True) + + plt.show() + +if __name__=='__main__': + if len(sys.argv) != 2: + print("Usage: python test_comfort_metrics.py ") + sys.exit(1) + + log_dir = sys.argv[1] + behavior_file = os.path.join(log_dir, "behavior.json") + #tracker_file = os.path.join(log_dir, "PurePursuitTrajectoryTracker_debug.csv") + tracker_file = os.path.join(log_dir, "StanleyTrajectoryTracker_debug.csv") + + times, accelerations, heading_rates = parse_behavior_log(behavior_file) + time_jerk, jerk = compute_derivative(times, accelerations) + time_heading_acc, heading_acc = compute_derivative(times, heading_rates) + + cte_time, cte, x_actual, y_actual, x_desired, y_desired = parse_tracker_csv(tracker_file) + + plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired) + + print("Max (abs) jerk:", np.max(np.abs(jerk))) + print("Avg jerk:", np.mean(np.abs(jerk))) + print("Max (abs) heading acceleration:", np.max(np.abs(heading_acc))) + print("Avg heading acceleration:", np.mean(np.abs(heading_acc))) + print("Max (abs) cross track error:", np.max(np.abs(cte))) + print("Avg cross track error:", np.mean(np.abs(cte))) + From a3f3ea653aa29e884369a965fb990d50df277058 Mon Sep 17 00:00:00 2001 From: hhxjqm Date: Wed, 23 Apr 2025 19:26:58 +0000 Subject: [PATCH 24/71] update by Bo-Hao Wu's idea --- GEMstack/onboard/interface/gem_simulator.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/onboard/interface/gem_simulator.py b/GEMstack/onboard/interface/gem_simulator.py index 2abfde71f..bb49054b5 100644 --- a/GEMstack/onboard/interface/gem_simulator.py +++ b/GEMstack/onboard/interface/gem_simulator.py @@ -172,7 +172,7 @@ def simulate(self, T : float, command : Optional[GEMVehicleCommand]): #simulate actuators accelerator_pedal_position = np.clip(self.last_command.accelerator_pedal_position,0.0,1.0) brake_pedal_position = np.clip(self.last_command.brake_pedal_position,0.0,1.0) - acceleration = pedal_positions_to_acceleration(accelerator_pedal_position,brake_pedal_position,v,0,1) + acceleration = pedal_positions_to_acceleration(accelerator_pedal_position,brake_pedal_position,v,0,self.last_command.gear) acceleration = np.clip(acceleration,*self.dubins.accelRange) phides = steer2front(self.last_command.steering_wheel_angle) phides = np.clip(phides,*self.dubins.wheelAngleRange) From b149ff39ad3afc44bcb78ec506a68a6a16f0f126 Mon Sep 17 00:00:00 2001 From: hhxjqm Date: Wed, 23 Apr 2025 19:29:20 +0000 Subject: [PATCH 25/71] implement reverse stanley in reverse_stanley.py (need to merge with stanley.py --- GEMstack/knowledge/routes/reverse_15m.csv | 20 + GEMstack/onboard/planning/reverse_stanley.py | 405 +++++++++++++++++++ launch/fixed_route.yaml | 7 +- 3 files changed, 429 insertions(+), 3 deletions(-) create mode 100644 GEMstack/knowledge/routes/reverse_15m.csv create mode 100644 GEMstack/onboard/planning/reverse_stanley.py diff --git a/GEMstack/knowledge/routes/reverse_15m.csv b/GEMstack/knowledge/routes/reverse_15m.csv new file mode 100644 index 000000000..2311d2fc5 --- /dev/null +++ b/GEMstack/knowledge/routes/reverse_15m.csv @@ -0,0 +1,20 @@ +0.0,0,0 +-1.0,0,0 +-2.0,0,0 +-3.0,1.0,0 +-4,1.0,0 +-5,1.0,0 +-6,1.0,0 +-7,1.0,0 +-8,1.0,0 +-9,1.0,0 +-10,0,0 +-11,-1,0 +-12,-1,0 +-13,-1,0 +-14,-1,0 +-15,-1,0 +-16,-1,0 +-17,-1,0 +-18,-1,0 +-19,-1,0 diff --git a/GEMstack/onboard/planning/reverse_stanley.py b/GEMstack/onboard/planning/reverse_stanley.py new file mode 100644 index 000000000..197cbc537 --- /dev/null +++ b/GEMstack/onboard/planning/reverse_stanley.py @@ -0,0 +1,405 @@ +import numpy as np +from math import sin, cos, atan2 + +from ...mathutils.control import PID +from ...utils import settings +from ...mathutils import transforms +from ...knowledge.vehicle.geometry import front2steer +from ...knowledge.vehicle.dynamics import acceleration_to_pedal_positions +from ...state.vehicle import VehicleState, ObjectFrameEnum +from ...state.trajectory import Path, Trajectory +from ..interface.gem import GEMVehicleCommand +from ..component import Component + +##################################### +# 1. Angle normalization +##################################### +def normalise_angle(angle): + """Wraps an angle to the [-pi, pi] range.""" + while angle > np.pi: + angle -= 2.0 * np.pi + while angle < -np.pi: + angle += 2.0 * np.pi + return angle + +##################################### +# 2. Reverse-only Stanley controller with longitudinal PID +##################################### +class Stanley(object): + """ + A Stanley controller modified to ONLY perform reverse path tracking. + It uses the rear axle as the control reference and calculates + commands for backward motion. + """ + + def __init__( + self, + control_gain=None, + softening_gain=None, + desired_speed=None # This will be interpreted as desired speed magnitude + ): + """ + :param control_gain: Stanley lateral control gain k. + :param softening_gain: Softening gain k_soft. + :param desired_speed: Desired speed magnitude (float) or 'path'/'trajectory'. + The controller will always target a negative speed. + """ + + self.k = control_gain if control_gain is not None else settings.get('control.stanley.control_gain') + self.k_soft = softening_gain if softening_gain is not None else settings.get('control.stanley.softening_gain') + + self.max_steer = settings.get('vehicle.geometry.max_wheel_angle') + self.wheelbase = settings.get('vehicle.geometry.wheelbase') + + self.speed_limit = settings.get('vehicle.limits.max_speed') + self.reverse_speed_limit = settings.get('vehicle.limits.max_reverse_speed') # add for reve + self.max_accel = settings.get('vehicle.limits.max_acceleration') # Max positive acceleration magnitude + self.max_decel = settings.get('vehicle.limits.max_deceleration') # Max deceleration magnitude (used for max n egative accel) + + # PID for longitudinal speed tracking + p = settings.get('control.longitudinal_control.pid_p') + d = settings.get('control.longitudinal_control.pid_d') + i = settings.get('control.longitudinal_control.pid_i') + self.pid_speed = PID(p, d, i, windup_limit=20) + + # Speed source: numeric or derived from path/trajectory + if desired_speed is not None: + self.desired_speed_source = -abs(desired_speed) + else: + self.desired_speed_source = settings.get('control.stanley.desired_speed', 'path') + + if isinstance(self.desired_speed_source, (int, float)): + self.desired_speed_magnitude = self.desired_speed_source + else: + self.desired_speed_magnitude = None + + + # For path/trajectory + self.path_arg = None + self.path = None + self.trajectory = None + self.current_path_parameter = 0.0 + self.current_traj_parameter = 0.0 + self.t_last = None + + def set_path(self, path: Path): + """Sets the path or trajectory to track.""" + if path == self.path_arg: + return + self.path_arg = path + + # If the path has more than 2D, reduce to (x,y) + if len(path.points[0]) > 2: + path = path.get_dims([0,1]) + + # If no timing info, cannot use 'path'/'trajectory' speed + if not isinstance(path, Trajectory): + self.path = path.arc_length_parameterize() + self.trajectory = None + self.current_traj_parameter = 0.0 + # Error if speed source requires trajectory but none provided + if self.desired_speed_source in ['path', 'trajectory']: + raise ValueError("Stanley: Provided path has no timing. Either provide a Trajectory or set a constant desired_speed.") + else: + self.path = path.arc_length_parameterize() # Parameterize by arc length for lateral tracking + self.trajectory = path # Store the original trajectory object + self.current_traj_parameter = self.trajectory.domain()[0] + + # Reset path parameter to the start of the parameterized path + self.current_path_parameter = self.path.domain()[0] + + + # Helper to find rear axle position (Front axle not needed in reverse-only) + def _find_rear_axle_position(self, x, y, yaw): + """Compute rear-axle world position from the vehicle's reference point and yaw.""" + rx = x - self.wheelbase * cos(yaw) + ry = y - self.wheelbase * sin(yaw) + return rx, ry + + + # compute method hardcoded for reverse + def compute(self, state: VehicleState, component: Component = None): + """Compute the control outputs for reverse tracking: + (longitudinal acceleration, front wheel angle). + + :param state: Current vehicle state. + :param component: Optional component for debugging. + # No 'reverse' parameter here, always reverse + """ + t = state.pose.t + if self.t_last is None: + self.t_last = t + dt = 0 # Initialize dt to 0 on first iteration + else: + dt = t - self.t_last + + # Current vehicle states + curr_x = state.pose.x + curr_y = state.pose.y + curr_yaw = state.pose.yaw if state.pose.yaw is not None else 0.0 + # Use signed longitudinal velocity. Assuming state.v is signed. + velocity = state.v + + + # Must have a trajectory to track in ReverseStanley if speed source is 'path'/'trajectory' + # or if we need path domain/evaluation. Let's assume Path is always needed. + if self.path is None: + if component: + component.debug_event("No path provided to ReverseStanley controller. Doing nothing.") + self.t_last = t # Update t_last even if doing nothing + self.pid_speed.reset() # Reset PID if path is lost + return (0.0, 0.0) + + # Ensure same frame + if self.path.frame != state.pose.frame: + if component: + component.debug_event(f"Transforming path from {self.path.frame.name} to {state.pose.frame.name}") + # Use transforms utility correctly + self.path = transforms.transform_path(self.path, self.path.frame, state.pose.frame, current_pose=state.pose) + + # Must transform trajectory if using trajectory speed source + if self.trajectory and self.trajectory.frame != state.pose.frame: + if component: + component.debug_event(f"Transforming trajectory from {self.trajectory.frame.name} to {state.pose.frame.name}") + # Use transforms utility correctly for trajectory + self.trajectory = transforms.transform_path(self.trajectory, self.trajectory.frame, state.pose.frame, current_pose=state.pose) + + + # Reverse always uses the rear axle position as the reference + ref_x, ref_y = self._find_rear_axle_position(curr_x, curr_y, curr_yaw) + + + # 1) Closest point on the arc-length parameterized path + # Find closest point relative to the rear axle + # Search range around the current path parameter. + search_start = self.current_path_parameter - 5.0 # Search range behind current parameter + search_end = self.current_path_parameter + 5.0 # Search range ahead of current parameter + + # Pass ref_x, ref_y to closest_point_local + closest_dist, closest_parameter = self.path.closest_point_local((ref_x, ref_y), [search_start, search_end]) + + # Update path parameter based on closest point. + # Note: This simple update might not be ideal for transitions between forward/reverse segments + # if using a trajectory with mixed gears, but it works for full-path reverse. + self.current_path_parameter = closest_parameter + + + # 2) Path heading + target_x, target_y = self.path.eval(self.current_path_parameter) # Use updated parameter + tangent = self.path.eval_tangent(self.current_path_parameter) # Use updated parameter + path_yaw = atan2(tangent[1], tangent[0]) + + # 3) Lateral error calculation + # Calculate cross-track error from the reference point (rear axle) to the path. + dx = ref_x - target_x + dy = ref_y - target_y + # Calculate signed cross-track error relative to path tangent + # This gives signed distance to the "left" of the path tangent + cross_track_error = dx * (-tangent[1]) + dy * tangent[0] + + + # 4) Heading error + # The heading error is the difference between the path tangent's orientation + # and the vehicle's current orientation. + yaw_error = normalise_angle(path_yaw - curr_yaw) + + # 5) Stanley lateral control terms (hardcoded for reverse) + # For reverse, heading error term is typically inverted + heading_term = -yaw_error + # For reverse, cross-track error term's effect is inverted. + # Negate the cross_track_error input to atan2. + # Use absolute velocity in the denominator. Avoid division by small velocity. + cross_term_input = self.k * (-cross_track_error) / (self.k_soft + abs(velocity)) + cross_term = atan2(cross_term_input, 1.0) # atan2(y, x) gives angle of vector (x, y) + + # The final steering angle is the sum of adjusted terms + desired_steering_angle = heading_term + cross_term + + # Longitudinal Control - Hardcoded for reverse (negative speed) + desired_speed_magnitude = self.desired_speed_magnitude # Start with base desired speed magnitude + feedforward_accel = 0.0 # Initialize feedforward + + # If speed source is trajectory, get magnitude from trajectory + if self.desired_speed_source in ['path', 'trajectory']: + # Get speed magnitude from trajectory at current parameter + # Assuming trajectory.eval_derivative(s) returns velocity vector at path parameter 's' + # or we map s to trajectory's native parameter (like time) if needed + current_trajectory_param_for_eval = self.current_path_parameter # Start with s + if hasattr(self.trajectory, 'parameter_to_time'): # Check if time mapping is possible + current_trajectory_param_for_eval = self.trajectory.parameter_to_time(self.current_path_parameter) + # Fallback to using path parameter 's' if time mapping fails is removed + + # Evaluate derivative (assumed to return velocity vector along path) + if self.trajectory is not None: # Ensure trajectory object exists + deriv = self.trajectory.eval_derivative(current_trajectory_param_for_eval) + path_speed_magnitude = np.linalg.norm(deriv) # Get speed magnitude + else: + path_speed_magnitude = 0.0 # Default if trajectory object somehow missing + + # Set desired speed magnitude, clipped by limit + desired_speed_magnitude = min(path_speed_magnitude, self.reverse_speed_limit) + + + # Apply direction sign (always negative for ReverseStanley) + desired_speed = desired_speed_magnitude * -1.0 + + + # Check for reaching the START of the path (for stopping in reverse) + is_at_start_backward = self.current_path_parameter <= self.path.domain()[0] + 1e-3 # Close to start parameter + + if is_at_start_backward: + # Reached the start of the path in reverse -> stop + # if component: + # component.debug_event(f"ReverseStanley: Reached start of path, stopping.") + desired_speed = 0.0 + # Apply braking force when moving negatively (positive acceleration) + feedforward_accel = -2.0 * -1.0 # = +2.0 + + else: + # Calculate feedforward acceleration based on speed change (still assuming magnitude from trajectory) + difference_dt = 0.1 + # Estimate future path parameter based on current speed magnitude and reverse direction + current_speed_abs = abs(velocity) # Use current vehicle speed magnitude + # Avoid division by near zero when estimating future parameter distance + if current_speed_abs < 0.1: estimated_path_step = 0.1 * difference_dt * -1.0 # Fixed step backwards + else: estimated_path_step = current_speed_abs * difference_dt * -1.0 # Step backwards based on speed + + future_parameter = self.current_path_parameter + estimated_path_step # Move along path towards start + + # Clip future parameter to path domain (ensure it doesn't go before start) + future_parameter = np.clip(future_parameter, self.path.domain()[0], self.path.domain()[-1]) + + # Need to map future_parameter (s) back to trajectory's native parameter if necessary + future_trajectory_param_for_eval = future_parameter # Start with s + if hasattr(self.trajectory, 'parameter_to_time'): # Check if time mapping is possible + future_trajectory_param_for_eval = self.trajectory.parameter_to_time(future_parameter) + # Fallback to using path parameter 's' if time mapping fails is removed + + # Evaluate derivative at future point (assumed to return velocity vector along path) + if self.trajectory is not None: # Ensure trajectory object exists + future_deriv = self.trajectory.eval_derivative(future_trajectory_param_for_eval) + next_path_speed_magnitude = min(np.linalg.norm(future_deriv), self.speed_limit) + else: + next_path_speed_magnitude = 0.0 # Default if trajectory object somehow missing + + # Apply direction sign (always negative) + next_desired_speed = next_path_speed_magnitude * -1.0 + + # Estimate acceleration = delta_v / delta_t + # Use the fixed difference_dt for acceleration calculation + feedforward_accel = (next_desired_speed - desired_speed) / difference_dt + feedforward_accel = np.clip(feedforward_accel, -self.max_decel, self.max_accel) + + + # Cross-track-based slowdown applied to speed magnitude (uses abs error) + desired_speed_magnitude_slowed = abs(desired_speed) * np.exp(-abs(cross_track_error) * 0.6) + + # Reapply direction sign (always negative) + desired_speed = desired_speed_magnitude_slowed * -1.0 if desired_speed_magnitude_slowed != 0 else 0.0 + + + # Clip to speed limit (magnitude) + if abs(desired_speed) > self.speed_limit: + desired_speed = self.speed_limit * -1.0 if desired_speed != 0 else 0.0 + + + # PID for longitudinal control + # Use signed velocity for error calculation + speed_error = desired_speed - velocity + output_accel = self.pid_speed.advance(e=speed_error, t=t, feedforward_term=feedforward_accel) + + # Clip acceleration + # output_accel can be positive (forward accel / reverse brake) or negative (forward brake / reverse accel) + if output_accel > self.max_accel: # Max positive accel + output_accel = self.max_accel + elif output_accel < -self.max_decel: # Max negative accel (corresponding to max decel magnitude) + output_accel = -self.max_decel + + + # Avoid acceleration from near zero if desired speed is zero + # This prevents "creep" when stopped. + if desired_speed == 0 and abs(velocity) < 0.05: # Use small epsilon for near zero velocity + # Check if output_accel would increase speed magnitude away from zero + # If current velocity > 0, positive accel increases speed. If velocity < 0, negative accel increases speed (makes it more negative). + # If velocity is 0, any non-zero accel would move away from zero. + if (velocity * output_accel > 0) or (abs(velocity) < 1e-3 and output_accel != 0): # Use smaller epsilon for the zero check + output_accel = 0.0 + + + + + # Debug + if component is not None: + component.debug('curr pt',(curr_x,curr_y)) + component.debug(" ", self.current_path_parameter) + component.debug("crosstrack error", cross_track_error) + component.debug("crosstrack dist", closest_dist) + component.debug("yaw_error", yaw_error) + component.debug('steering_angle', desired_steering_angle) + component.debug("desired_speed (m/s)", desired_speed) + component.debug("feedforward_accel (m/s^2)", feedforward_accel) + component.debug(" output_accel (m/s^2)", output_accel) + + self.t_last = t + # Return signed acceleration and desired front wheel steering angle + return (output_accel, desired_steering_angle) + +##################################### +# 3. Tracker component +##################################### +class StanleyTrajectoryTracker(Component): + """ + A trajectory-tracking Component that uses the above Stanley controller + for lateral control plus PID-based longitudinal control. + It now includes measures to mitigate oscillations. + """ + + def __init__(self, vehicle_interface=None, **kwargs): + """ + :param vehicle_interface: The low-level interface to send commands to the vehicle. + :param kwargs: Optional parameters to pass into the Stanley(...) constructor. + """ + self.stanley = Stanley(**kwargs) + self.vehicle_interface = vehicle_interface + + def rate(self): + """Control frequency in Hz.""" + return 50.0 + + def state_inputs(self): + """ + Required state inputs: + - Vehicle state + - Trajectory + """ + return ["vehicle", "trajectory"] + + def state_outputs(self): + """No direct output state here.""" + return [] + + def update(self, vehicle: VehicleState, trajectory: Trajectory): + """ + Per control cycle: + 1) Set the path/trajectory + 2) Compute (acceleration, front wheel angle) + 3) Convert front wheel angle to steering wheel angle (if necessary) + 4) Send command to the vehicle + """ + self.stanley.set_path(trajectory) + accel, f_delta = self.stanley.compute(vehicle, self) + + # If your low-level interface expects steering wheel angle: + steering_angle = front2steer(f_delta) + steering_angle = np.clip( + steering_angle, + settings.get('vehicle.geometry.min_steering_angle', -0.5), + settings.get('vehicle.geometry.max_steering_angle', 0.5) + ) + + cmd = self.vehicle_interface.simple_command(accel, steering_angle, vehicle) + self.vehicle_interface.send_command(cmd) + + def healthy(self): + """Optional: check if the controller has a valid path.""" + return self.stanley.path is not None diff --git a/launch/fixed_route.yaml b/launch/fixed_route.yaml index 3e28ea668..904e82f2e 100644 --- a/launch/fixed_route.yaml +++ b/launch/fixed_route.yaml @@ -16,13 +16,14 @@ drive: planning: route_planning: type: StaticRoutePlanner - args: [!relative_path '../GEMstack/knowledge/routes/xyhead_highbay_backlot_p.csv'] + args: [!relative_path '../GEMstack/knowledge/routes/forward_15m.csv'] + # args: [!relative_path '../GEMstack/knowledge/routes/reverse_15m.csv'] motion_planning: type: RouteToTrajectoryPlanner args: [null] #desired speed in m/s. If null, this will keep the route untimed for the trajectory tracker trajectory_tracking: - #type: pure_pursuit.PurePursuitTrajectoryTracker - type: stanley.StanleyTrajectoryTracker + type: pure_pursuit.PurePursuitTrajectoryTracker + # type: reverse_stanley.StanleyTrajectoryTracker args: {desired_speed: 2.5} #approximately 5mph print: False log: From 57d8c3be06420659072593d754490c3f3a9401e9 Mon Sep 17 00:00:00 2001 From: hhxjqm Date: Fri, 25 Apr 2025 05:48:52 +0000 Subject: [PATCH 26/71] Merge reverse_stanley.py and stanley.py --- GEMstack/onboard/planning/reverse_stanley.py | 405 ------------------- GEMstack/onboard/planning/stanley.py | 239 ++++++----- launch/fixed_route.yaml | 8 +- 3 files changed, 133 insertions(+), 519 deletions(-) delete mode 100644 GEMstack/onboard/planning/reverse_stanley.py diff --git a/GEMstack/onboard/planning/reverse_stanley.py b/GEMstack/onboard/planning/reverse_stanley.py deleted file mode 100644 index 197cbc537..000000000 --- a/GEMstack/onboard/planning/reverse_stanley.py +++ /dev/null @@ -1,405 +0,0 @@ -import numpy as np -from math import sin, cos, atan2 - -from ...mathutils.control import PID -from ...utils import settings -from ...mathutils import transforms -from ...knowledge.vehicle.geometry import front2steer -from ...knowledge.vehicle.dynamics import acceleration_to_pedal_positions -from ...state.vehicle import VehicleState, ObjectFrameEnum -from ...state.trajectory import Path, Trajectory -from ..interface.gem import GEMVehicleCommand -from ..component import Component - -##################################### -# 1. Angle normalization -##################################### -def normalise_angle(angle): - """Wraps an angle to the [-pi, pi] range.""" - while angle > np.pi: - angle -= 2.0 * np.pi - while angle < -np.pi: - angle += 2.0 * np.pi - return angle - -##################################### -# 2. Reverse-only Stanley controller with longitudinal PID -##################################### -class Stanley(object): - """ - A Stanley controller modified to ONLY perform reverse path tracking. - It uses the rear axle as the control reference and calculates - commands for backward motion. - """ - - def __init__( - self, - control_gain=None, - softening_gain=None, - desired_speed=None # This will be interpreted as desired speed magnitude - ): - """ - :param control_gain: Stanley lateral control gain k. - :param softening_gain: Softening gain k_soft. - :param desired_speed: Desired speed magnitude (float) or 'path'/'trajectory'. - The controller will always target a negative speed. - """ - - self.k = control_gain if control_gain is not None else settings.get('control.stanley.control_gain') - self.k_soft = softening_gain if softening_gain is not None else settings.get('control.stanley.softening_gain') - - self.max_steer = settings.get('vehicle.geometry.max_wheel_angle') - self.wheelbase = settings.get('vehicle.geometry.wheelbase') - - self.speed_limit = settings.get('vehicle.limits.max_speed') - self.reverse_speed_limit = settings.get('vehicle.limits.max_reverse_speed') # add for reve - self.max_accel = settings.get('vehicle.limits.max_acceleration') # Max positive acceleration magnitude - self.max_decel = settings.get('vehicle.limits.max_deceleration') # Max deceleration magnitude (used for max n egative accel) - - # PID for longitudinal speed tracking - p = settings.get('control.longitudinal_control.pid_p') - d = settings.get('control.longitudinal_control.pid_d') - i = settings.get('control.longitudinal_control.pid_i') - self.pid_speed = PID(p, d, i, windup_limit=20) - - # Speed source: numeric or derived from path/trajectory - if desired_speed is not None: - self.desired_speed_source = -abs(desired_speed) - else: - self.desired_speed_source = settings.get('control.stanley.desired_speed', 'path') - - if isinstance(self.desired_speed_source, (int, float)): - self.desired_speed_magnitude = self.desired_speed_source - else: - self.desired_speed_magnitude = None - - - # For path/trajectory - self.path_arg = None - self.path = None - self.trajectory = None - self.current_path_parameter = 0.0 - self.current_traj_parameter = 0.0 - self.t_last = None - - def set_path(self, path: Path): - """Sets the path or trajectory to track.""" - if path == self.path_arg: - return - self.path_arg = path - - # If the path has more than 2D, reduce to (x,y) - if len(path.points[0]) > 2: - path = path.get_dims([0,1]) - - # If no timing info, cannot use 'path'/'trajectory' speed - if not isinstance(path, Trajectory): - self.path = path.arc_length_parameterize() - self.trajectory = None - self.current_traj_parameter = 0.0 - # Error if speed source requires trajectory but none provided - if self.desired_speed_source in ['path', 'trajectory']: - raise ValueError("Stanley: Provided path has no timing. Either provide a Trajectory or set a constant desired_speed.") - else: - self.path = path.arc_length_parameterize() # Parameterize by arc length for lateral tracking - self.trajectory = path # Store the original trajectory object - self.current_traj_parameter = self.trajectory.domain()[0] - - # Reset path parameter to the start of the parameterized path - self.current_path_parameter = self.path.domain()[0] - - - # Helper to find rear axle position (Front axle not needed in reverse-only) - def _find_rear_axle_position(self, x, y, yaw): - """Compute rear-axle world position from the vehicle's reference point and yaw.""" - rx = x - self.wheelbase * cos(yaw) - ry = y - self.wheelbase * sin(yaw) - return rx, ry - - - # compute method hardcoded for reverse - def compute(self, state: VehicleState, component: Component = None): - """Compute the control outputs for reverse tracking: - (longitudinal acceleration, front wheel angle). - - :param state: Current vehicle state. - :param component: Optional component for debugging. - # No 'reverse' parameter here, always reverse - """ - t = state.pose.t - if self.t_last is None: - self.t_last = t - dt = 0 # Initialize dt to 0 on first iteration - else: - dt = t - self.t_last - - # Current vehicle states - curr_x = state.pose.x - curr_y = state.pose.y - curr_yaw = state.pose.yaw if state.pose.yaw is not None else 0.0 - # Use signed longitudinal velocity. Assuming state.v is signed. - velocity = state.v - - - # Must have a trajectory to track in ReverseStanley if speed source is 'path'/'trajectory' - # or if we need path domain/evaluation. Let's assume Path is always needed. - if self.path is None: - if component: - component.debug_event("No path provided to ReverseStanley controller. Doing nothing.") - self.t_last = t # Update t_last even if doing nothing - self.pid_speed.reset() # Reset PID if path is lost - return (0.0, 0.0) - - # Ensure same frame - if self.path.frame != state.pose.frame: - if component: - component.debug_event(f"Transforming path from {self.path.frame.name} to {state.pose.frame.name}") - # Use transforms utility correctly - self.path = transforms.transform_path(self.path, self.path.frame, state.pose.frame, current_pose=state.pose) - - # Must transform trajectory if using trajectory speed source - if self.trajectory and self.trajectory.frame != state.pose.frame: - if component: - component.debug_event(f"Transforming trajectory from {self.trajectory.frame.name} to {state.pose.frame.name}") - # Use transforms utility correctly for trajectory - self.trajectory = transforms.transform_path(self.trajectory, self.trajectory.frame, state.pose.frame, current_pose=state.pose) - - - # Reverse always uses the rear axle position as the reference - ref_x, ref_y = self._find_rear_axle_position(curr_x, curr_y, curr_yaw) - - - # 1) Closest point on the arc-length parameterized path - # Find closest point relative to the rear axle - # Search range around the current path parameter. - search_start = self.current_path_parameter - 5.0 # Search range behind current parameter - search_end = self.current_path_parameter + 5.0 # Search range ahead of current parameter - - # Pass ref_x, ref_y to closest_point_local - closest_dist, closest_parameter = self.path.closest_point_local((ref_x, ref_y), [search_start, search_end]) - - # Update path parameter based on closest point. - # Note: This simple update might not be ideal for transitions between forward/reverse segments - # if using a trajectory with mixed gears, but it works for full-path reverse. - self.current_path_parameter = closest_parameter - - - # 2) Path heading - target_x, target_y = self.path.eval(self.current_path_parameter) # Use updated parameter - tangent = self.path.eval_tangent(self.current_path_parameter) # Use updated parameter - path_yaw = atan2(tangent[1], tangent[0]) - - # 3) Lateral error calculation - # Calculate cross-track error from the reference point (rear axle) to the path. - dx = ref_x - target_x - dy = ref_y - target_y - # Calculate signed cross-track error relative to path tangent - # This gives signed distance to the "left" of the path tangent - cross_track_error = dx * (-tangent[1]) + dy * tangent[0] - - - # 4) Heading error - # The heading error is the difference between the path tangent's orientation - # and the vehicle's current orientation. - yaw_error = normalise_angle(path_yaw - curr_yaw) - - # 5) Stanley lateral control terms (hardcoded for reverse) - # For reverse, heading error term is typically inverted - heading_term = -yaw_error - # For reverse, cross-track error term's effect is inverted. - # Negate the cross_track_error input to atan2. - # Use absolute velocity in the denominator. Avoid division by small velocity. - cross_term_input = self.k * (-cross_track_error) / (self.k_soft + abs(velocity)) - cross_term = atan2(cross_term_input, 1.0) # atan2(y, x) gives angle of vector (x, y) - - # The final steering angle is the sum of adjusted terms - desired_steering_angle = heading_term + cross_term - - # Longitudinal Control - Hardcoded for reverse (negative speed) - desired_speed_magnitude = self.desired_speed_magnitude # Start with base desired speed magnitude - feedforward_accel = 0.0 # Initialize feedforward - - # If speed source is trajectory, get magnitude from trajectory - if self.desired_speed_source in ['path', 'trajectory']: - # Get speed magnitude from trajectory at current parameter - # Assuming trajectory.eval_derivative(s) returns velocity vector at path parameter 's' - # or we map s to trajectory's native parameter (like time) if needed - current_trajectory_param_for_eval = self.current_path_parameter # Start with s - if hasattr(self.trajectory, 'parameter_to_time'): # Check if time mapping is possible - current_trajectory_param_for_eval = self.trajectory.parameter_to_time(self.current_path_parameter) - # Fallback to using path parameter 's' if time mapping fails is removed - - # Evaluate derivative (assumed to return velocity vector along path) - if self.trajectory is not None: # Ensure trajectory object exists - deriv = self.trajectory.eval_derivative(current_trajectory_param_for_eval) - path_speed_magnitude = np.linalg.norm(deriv) # Get speed magnitude - else: - path_speed_magnitude = 0.0 # Default if trajectory object somehow missing - - # Set desired speed magnitude, clipped by limit - desired_speed_magnitude = min(path_speed_magnitude, self.reverse_speed_limit) - - - # Apply direction sign (always negative for ReverseStanley) - desired_speed = desired_speed_magnitude * -1.0 - - - # Check for reaching the START of the path (for stopping in reverse) - is_at_start_backward = self.current_path_parameter <= self.path.domain()[0] + 1e-3 # Close to start parameter - - if is_at_start_backward: - # Reached the start of the path in reverse -> stop - # if component: - # component.debug_event(f"ReverseStanley: Reached start of path, stopping.") - desired_speed = 0.0 - # Apply braking force when moving negatively (positive acceleration) - feedforward_accel = -2.0 * -1.0 # = +2.0 - - else: - # Calculate feedforward acceleration based on speed change (still assuming magnitude from trajectory) - difference_dt = 0.1 - # Estimate future path parameter based on current speed magnitude and reverse direction - current_speed_abs = abs(velocity) # Use current vehicle speed magnitude - # Avoid division by near zero when estimating future parameter distance - if current_speed_abs < 0.1: estimated_path_step = 0.1 * difference_dt * -1.0 # Fixed step backwards - else: estimated_path_step = current_speed_abs * difference_dt * -1.0 # Step backwards based on speed - - future_parameter = self.current_path_parameter + estimated_path_step # Move along path towards start - - # Clip future parameter to path domain (ensure it doesn't go before start) - future_parameter = np.clip(future_parameter, self.path.domain()[0], self.path.domain()[-1]) - - # Need to map future_parameter (s) back to trajectory's native parameter if necessary - future_trajectory_param_for_eval = future_parameter # Start with s - if hasattr(self.trajectory, 'parameter_to_time'): # Check if time mapping is possible - future_trajectory_param_for_eval = self.trajectory.parameter_to_time(future_parameter) - # Fallback to using path parameter 's' if time mapping fails is removed - - # Evaluate derivative at future point (assumed to return velocity vector along path) - if self.trajectory is not None: # Ensure trajectory object exists - future_deriv = self.trajectory.eval_derivative(future_trajectory_param_for_eval) - next_path_speed_magnitude = min(np.linalg.norm(future_deriv), self.speed_limit) - else: - next_path_speed_magnitude = 0.0 # Default if trajectory object somehow missing - - # Apply direction sign (always negative) - next_desired_speed = next_path_speed_magnitude * -1.0 - - # Estimate acceleration = delta_v / delta_t - # Use the fixed difference_dt for acceleration calculation - feedforward_accel = (next_desired_speed - desired_speed) / difference_dt - feedforward_accel = np.clip(feedforward_accel, -self.max_decel, self.max_accel) - - - # Cross-track-based slowdown applied to speed magnitude (uses abs error) - desired_speed_magnitude_slowed = abs(desired_speed) * np.exp(-abs(cross_track_error) * 0.6) - - # Reapply direction sign (always negative) - desired_speed = desired_speed_magnitude_slowed * -1.0 if desired_speed_magnitude_slowed != 0 else 0.0 - - - # Clip to speed limit (magnitude) - if abs(desired_speed) > self.speed_limit: - desired_speed = self.speed_limit * -1.0 if desired_speed != 0 else 0.0 - - - # PID for longitudinal control - # Use signed velocity for error calculation - speed_error = desired_speed - velocity - output_accel = self.pid_speed.advance(e=speed_error, t=t, feedforward_term=feedforward_accel) - - # Clip acceleration - # output_accel can be positive (forward accel / reverse brake) or negative (forward brake / reverse accel) - if output_accel > self.max_accel: # Max positive accel - output_accel = self.max_accel - elif output_accel < -self.max_decel: # Max negative accel (corresponding to max decel magnitude) - output_accel = -self.max_decel - - - # Avoid acceleration from near zero if desired speed is zero - # This prevents "creep" when stopped. - if desired_speed == 0 and abs(velocity) < 0.05: # Use small epsilon for near zero velocity - # Check if output_accel would increase speed magnitude away from zero - # If current velocity > 0, positive accel increases speed. If velocity < 0, negative accel increases speed (makes it more negative). - # If velocity is 0, any non-zero accel would move away from zero. - if (velocity * output_accel > 0) or (abs(velocity) < 1e-3 and output_accel != 0): # Use smaller epsilon for the zero check - output_accel = 0.0 - - - - - # Debug - if component is not None: - component.debug('curr pt',(curr_x,curr_y)) - component.debug(" ", self.current_path_parameter) - component.debug("crosstrack error", cross_track_error) - component.debug("crosstrack dist", closest_dist) - component.debug("yaw_error", yaw_error) - component.debug('steering_angle', desired_steering_angle) - component.debug("desired_speed (m/s)", desired_speed) - component.debug("feedforward_accel (m/s^2)", feedforward_accel) - component.debug(" output_accel (m/s^2)", output_accel) - - self.t_last = t - # Return signed acceleration and desired front wheel steering angle - return (output_accel, desired_steering_angle) - -##################################### -# 3. Tracker component -##################################### -class StanleyTrajectoryTracker(Component): - """ - A trajectory-tracking Component that uses the above Stanley controller - for lateral control plus PID-based longitudinal control. - It now includes measures to mitigate oscillations. - """ - - def __init__(self, vehicle_interface=None, **kwargs): - """ - :param vehicle_interface: The low-level interface to send commands to the vehicle. - :param kwargs: Optional parameters to pass into the Stanley(...) constructor. - """ - self.stanley = Stanley(**kwargs) - self.vehicle_interface = vehicle_interface - - def rate(self): - """Control frequency in Hz.""" - return 50.0 - - def state_inputs(self): - """ - Required state inputs: - - Vehicle state - - Trajectory - """ - return ["vehicle", "trajectory"] - - def state_outputs(self): - """No direct output state here.""" - return [] - - def update(self, vehicle: VehicleState, trajectory: Trajectory): - """ - Per control cycle: - 1) Set the path/trajectory - 2) Compute (acceleration, front wheel angle) - 3) Convert front wheel angle to steering wheel angle (if necessary) - 4) Send command to the vehicle - """ - self.stanley.set_path(trajectory) - accel, f_delta = self.stanley.compute(vehicle, self) - - # If your low-level interface expects steering wheel angle: - steering_angle = front2steer(f_delta) - steering_angle = np.clip( - steering_angle, - settings.get('vehicle.geometry.min_steering_angle', -0.5), - settings.get('vehicle.geometry.max_steering_angle', 0.5) - ) - - cmd = self.vehicle_interface.simple_command(accel, steering_angle, vehicle) - self.vehicle_interface.send_command(cmd) - - def healthy(self): - """Optional: check if the controller has a valid path.""" - return self.stanley.path is not None diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley.py index 19efbfbf4..f820ff9b9 100644 --- a/GEMstack/onboard/planning/stanley.py +++ b/GEMstack/onboard/planning/stanley.py @@ -1,7 +1,6 @@ import numpy as np from math import sin, cos, atan2 -# These imports match your existing project structure from ...mathutils.control import PID from ...utils import settings from ...mathutils import transforms @@ -27,15 +26,6 @@ def normalise_angle(angle): # 2. Stanley-based controller with longitudinal PID ##################################### class Stanley(object): - """ - A Stanley controller that handles lateral control (steering) - plus a basic longitudinal control (PID + optional feedforward). - It has been modified to reduce oscillations by: - 1) Lower gains - 2) Steering damping - 3) Low-pass filter on steering - 4) Gentler speed logic when cornering - """ def __init__( self, @@ -46,33 +36,32 @@ def __init__( """ :param control_gain: Stanley lateral control gain k (lowered from the default to reduce overshoot). :param softening_gain: Softening gain k_soft. - :param yaw_rate_gain: Yaw-rate gain k_yaw_rate (helps reduce oscillations). - :param steering_damp_gain: Steering damping gain k_damp_steer. :param desired_speed: Desired speed (float) or 'path'/'trajectory'. """ - - # 1) Lower Gains self.k = control_gain if control_gain is not None else settings.get('control.stanley.control_gain') self.k_soft = softening_gain if softening_gain is not None else settings.get('control.stanley.softening_gain') - # Typically, this is the max front-wheel steering angle in radians self.max_steer = settings.get('vehicle.geometry.max_wheel_angle') self.wheelbase = settings.get('vehicle.geometry.wheelbase') - # Basic longitudinal constraints self.speed_limit = settings.get('vehicle.limits.max_speed') - self.max_accel = settings.get('vehicle.limits.max_acceleration') - self.max_decel = settings.get('vehicle.limits.max_deceleration') + self.reverse_speed_limit = settings.get('vehicle.limits.max_reverse_speed') + self.max_accel = settings.get('vehicle.limits.max_acceleration') + self.max_decel = settings.get('vehicle.limits.max_deceleration') - # PID for longitudinal speed tracking p = settings.get('control.longitudinal_control.pid_p') d = settings.get('control.longitudinal_control.pid_d') i = settings.get('control.longitudinal_control.pid_i') self.pid_speed = PID(p, d, i, windup_limit=20) - # Speed source: numeric or derived from path/trajectory + reverse = True + # reverse = something_here() # TODO: Implement reverse logic + if desired_speed is not None: - self.desired_speed_source = desired_speed + if reverse: + self.desired_speed_source = -abs(desired_speed) + else: + self.desired_speed_source = abs(desired_speed) else: self.desired_speed_source = settings.get('control.stanley.desired_speed', 'path') @@ -81,7 +70,6 @@ def __init__( else: self.desired_speed = None - # For path/trajectory self.path_arg = None self.path = None self.trajectory = None @@ -90,16 +78,13 @@ def __init__( self.t_last = None def set_path(self, path: Path): - """Sets the path or trajectory to track.""" if path == self.path_arg: return self.path_arg = path - # If the path has more than 2D, reduce to (x,y) if len(path.points[0]) > 2: path = path.get_dims([0,1]) - # If no timing info, we can't rely on 'path'/'trajectory' speed if not isinstance(path, Trajectory): self.path = path.arc_length_parameterize() self.trajectory = None @@ -111,7 +96,7 @@ def set_path(self, path: Path): self.trajectory = path self.current_traj_parameter = self.trajectory.domain()[0] - self.current_path_parameter = 0.0 + self.current_path_parameter = self.path.domain()[0] def _find_front_axle_position(self, x, y, yaw): """Compute front-axle world position from the center/rear and yaw.""" @@ -119,14 +104,21 @@ def _find_front_axle_position(self, x, y, yaw): fy = y + self.wheelbase * sin(yaw) return fx, fy - def compute(self, state: VehicleState, component: Component = None): + def _find_rear_axle_position(self, x, y, yaw): + """Compute rear-axle world position from the vehicle's reference point and yaw.""" + rx = x - self.wheelbase * cos(yaw) + ry = y - self.wheelbase * sin(yaw) + return rx, ry + + def compute(self, state: VehicleState, component: Component = None, reverse = True): # TODO change only during debugging """Compute the control outputs: (longitudinal acceleration, front wheel angle).""" t = state.pose.t if self.t_last is None: self.t_last = t - dt = t - self.t_last + dt = 0 + else: + dt = t - self.t_last - # Current vehicle states curr_x = state.pose.x curr_y = state.pose.y curr_yaw = state.pose.yaw if state.pose.yaw is not None else 0.0 @@ -135,104 +127,157 @@ def compute(self, state: VehicleState, component: Component = None): if self.path is None: if component: component.debug_event("No path provided to Stanley controller. Doing nothing.") + self.t_last = t + self.pid_speed.reset() return (0.0, 0.0) - # Ensure same frame if self.path.frame != state.pose.frame: if component: component.debug_event(f"Transforming path from {self.path.frame.name} to {state.pose.frame.name}") - self.path = self.path.to_frame(state.pose.frame, current_pose=state.pose) + self.path = transforms.transform_path(self.path, self.path.frame, state.pose.frame, current_pose=state.pose) if self.trajectory and self.trajectory.frame != state.pose.frame: if component: component.debug_event(f"Transforming trajectory from {self.trajectory.frame.name} to {state.pose.frame.name}") - self.trajectory = self.trajectory.to_frame(state.pose.frame, current_pose=state.pose) + self.trajectory = transforms.transform_path(self.trajectory, self.trajectory.frame, state.pose.frame, current_pose=state.pose) - # 1) Closest point - fx, fy = self._find_front_axle_position(curr_x, curr_y, curr_yaw) + if reverse: + fx, fy = self._find_rear_axle_position(curr_x, curr_y, curr_yaw) + else: + fx, fy = self._find_front_axle_position(curr_x, curr_y, curr_yaw) search_start = self.current_path_parameter - 5.0 search_end = self.current_path_parameter + 5.0 closest_dist, closest_parameter = self.path.closest_point_local((fx, fy), [search_start, search_end]) self.current_path_parameter = closest_parameter - # 2) Path heading - target_x, target_y = self.path.eval(closest_parameter) - tangent = self.path.eval_tangent(closest_parameter) + target_x, target_y = self.path.eval(self.current_path_parameter) + tangent = self.path.eval_tangent(self.current_path_parameter) path_yaw = atan2(tangent[1], tangent[0]) - desired_x = target_x - desired_y = target_y - # 3) Lateral error + dx = fx - target_x dy = fy - target_y - left_vec = np.array([sin(curr_yaw), -cos(curr_yaw)]) - cross_track_error = np.sign(np.dot(np.array([dx, dy]), left_vec)) * closest_dist - # 4) Heading error - yaw_error = normalise_angle(path_yaw - curr_yaw) + if reverse: + cross_track_error = dx * (-tangent[1]) + dy * tangent[0] + else: + left_vec = np.array([sin(curr_yaw), -cos(curr_yaw)]) + cross_track_error = np.sign(np.dot(np.array([dx, dy]), left_vec)) * closest_dist - # 5) Standard Stanley terms - cross_term = atan2(self.k * cross_track_error, self.k_soft + speed) - desired_steering_angle = yaw_error + cross_term + yaw_error = normalise_angle(path_yaw - curr_yaw) desired_speed = self.desired_speed feedforward_accel = 0.0 - if self.trajectory and self.desired_speed_source in ['path', 'trajectory']: - if len(self.trajectory.points) < 2 or self.current_path_parameter >= self.path.domain()[1]: - # End of trajectory -> stop - if component: - component.debug_event("Stanley: Past the end of trajectory, stopping.") + if reverse: + heading_term = -yaw_error + cross_term_input = self.k * (-cross_track_error) / (self.k_soft + abs(speed)) + cross_term = atan2(cross_term_input, 1.0) + desired_steering_angle = heading_term + cross_term + + if self.trajectory and self.desired_speed_source in ['path', 'trajectory']: + current_trajectory_param_for_eval = self.current_path_parameter + if hasattr(self.trajectory, 'parameter_to_time'): + current_trajectory_param_for_eval = self.trajectory.parameter_to_time(self.current_path_parameter) + + if self.trajectory is not None: + deriv = self.trajectory.eval_derivative(current_trajectory_param_for_eval) + path_speed_magnitude = np.linalg.norm(deriv) + else: + path_speed_magnitude = 0.0 + + desired_speed = min(path_speed_magnitude, self.reverse_speed_limit) + + desired_speed = desired_speed * -1.0 + + is_at_start_backward = self.current_path_parameter <= self.path.domain()[0] + 1e-3 + + if is_at_start_backward: desired_speed = 0.0 - feedforward_accel = -2.0 + feedforward_accel = -2.0 * -1.0 # = +2.0 + else: - if self.desired_speed_source == 'path': - current_trajectory_time = self.trajectory.parameter_to_time(self.current_path_parameter) + difference_dt = 0.1 + current_speed_abs = abs(speed) + if current_speed_abs < 0.1: estimated_path_step = 0.1 * difference_dt * -1.0 + else: estimated_path_step = current_speed_abs * difference_dt * -1.0 + + future_parameter = self.current_path_parameter + estimated_path_step + + future_parameter = np.clip(future_parameter, self.path.domain()[0], self.path.domain()[-1]) + + future_trajectory_param_for_eval = future_parameter + if hasattr(self.trajectory, 'parameter_to_time'): + future_trajectory_param_for_eval = self.trajectory.parameter_to_time(future_parameter) + + if self.trajectory is not None: + future_deriv = self.trajectory.eval_derivative(future_trajectory_param_for_eval) + next_path_speed_magnitude = min(np.linalg.norm(future_deriv), self.speed_limit) else: - self.current_traj_parameter += dt - current_trajectory_time = self.current_traj_parameter + next_path_speed_magnitude = 0.0 - deriv = self.trajectory.eval_derivative(current_trajectory_time) - desired_speed = min(np.linalg.norm(deriv), self.speed_limit) + next_desired_speed = next_path_speed_magnitude * -1.0 - difference_dt = 0.1 - future_t = current_trajectory_time + difference_dt - if future_t > self.trajectory.domain()[1]: - future_t = self.trajectory.domain()[1] - future_deriv = self.trajectory.eval_derivative(future_t) - next_desired_speed = min(np.linalg.norm(future_deriv), self.speed_limit) feedforward_accel = (next_desired_speed - desired_speed) / difference_dt feedforward_accel = np.clip(feedforward_accel, -self.max_decel, self.max_accel) + + desired_speed_magnitude_slowed = abs(desired_speed) * np.exp(-abs(cross_track_error) * 0.6) + desired_speed = desired_speed_magnitude_slowed * -1.0 if desired_speed_magnitude_slowed != 0 else 0.0 + else: - if desired_speed is None: - desired_speed = 4.0 + cross_term = atan2(self.k * cross_track_error, self.k_soft + speed) + desired_steering_angle = yaw_error + cross_term + + if self.trajectory and self.desired_speed_source in ['path', 'trajectory']: + if len(self.trajectory.points) < 2 or self.current_path_parameter >= self.path.domain()[1]: + if component: + component.debug_event("Stanley: Past the end of trajectory, stopping.") + desired_speed = 0.0 + feedforward_accel = -2.0 + else: + if self.desired_speed_source == 'path': + current_trajectory_time = self.trajectory.parameter_to_time(self.current_path_parameter) + else: + self.current_traj_parameter += dt + current_trajectory_time = self.current_traj_parameter + + deriv = self.trajectory.eval_derivative(current_trajectory_time) + desired_speed = min(np.linalg.norm(deriv), self.speed_limit) + + difference_dt = 0.1 + future_t = current_trajectory_time + difference_dt + if future_t > self.trajectory.domain()[1]: + future_t = self.trajectory.domain()[1] + future_deriv = self.trajectory.eval_derivative(future_t) + next_desired_speed = min(np.linalg.norm(future_deriv), self.speed_limit) + feedforward_accel = (next_desired_speed - desired_speed) / difference_dt + feedforward_accel = np.clip(feedforward_accel, -self.max_decel, self.max_accel) + else: + if desired_speed is None: + desired_speed = 4.0 - # Cross-track-based slowdown (less aggressive than before). - desired_speed *= np.exp(-abs(cross_track_error) * 0.6) + desired_speed *= np.exp(-abs(cross_track_error) * 0.6) - # Clip to speed limit - if desired_speed > self.speed_limit: - desired_speed = self.speed_limit + if abs(desired_speed) > self.speed_limit: + if reverse: + desired_speed = self.speed_limit * -1.0 if desired_speed != 0 else 0.0 + else: + desired_speed = self.speed_limit - # PID for longitudinal control speed_error = desired_speed - speed output_accel = self.pid_speed.advance(e=speed_error, t=t, feedforward_term=feedforward_accel) - # Clip acceleration if output_accel > self.max_accel: output_accel = self.max_accel elif output_accel < -self.max_decel: output_accel = -self.max_decel - # Avoid negative accel when fully stopped if desired_speed == 0 and abs(speed) < 0.01 and output_accel < 0: output_accel = 0.0 - # Debug if component is not None: - # component.debug("Stanley: fx, fy", (fx, fy)) component.debug('curr pt',(curr_x,curr_y)) - component.debug("desired_x",desired_x) - component.debug("desired_y",desired_y) + component.debug("desired_x",target_x) + component.debug("desired_y",target_y) component.debug("Stanley: path param", self.current_path_parameter) component.debug("Stanley: crosstrack dist", closest_dist) component.debug("crosstrack error", cross_track_error) @@ -242,12 +287,6 @@ def compute(self, state: VehicleState, component: Component = None): component.debug("Stanley: feedforward_accel (m/s^2)", feedforward_accel) component.debug("Stanley: output_accel (m/s^2)", output_accel) - if output_accel > self.max_accel: - output_accel = self.max_accel - - if output_accel < -self.max_decel: - output_accel = -self.max_decel - self.t_last = t return (output_accel, desired_steering_angle) @@ -255,48 +294,28 @@ def compute(self, state: VehicleState, component: Component = None): # 3. Tracker component ##################################### class StanleyTrajectoryTracker(Component): - """ - A trajectory-tracking Component that uses the above Stanley controller - for lateral control plus PID-based longitudinal control. - It now includes measures to mitigate oscillations. - """ def __init__(self, vehicle_interface=None, **kwargs): - """ - :param vehicle_interface: The low-level interface to send commands to the vehicle. - :param kwargs: Optional parameters to pass into the Stanley(...) constructor. - """ self.stanley = Stanley(**kwargs) self.vehicle_interface = vehicle_interface def rate(self): - """Control frequency in Hz.""" return 50.0 def state_inputs(self): - """ - Required state inputs: - - Vehicle state - - Trajectory - """ return ["vehicle", "trajectory"] def state_outputs(self): - """No direct output state here.""" return [] def update(self, vehicle: VehicleState, trajectory: Trajectory): - """ - Per control cycle: - 1) Set the path/trajectory - 2) Compute (acceleration, front wheel angle) - 3) Convert front wheel angle to steering wheel angle (if necessary) - 4) Send command to the vehicle - """ self.stanley.set_path(trajectory) + + # TODO accel, f_delta = self.stanley.compute(vehicle, self) + # reverse = some_function_here() + # accel, f_delta = self.stanley.compute(vehicle, self, reverse) - # If your low-level interface expects steering wheel angle: steering_angle = front2steer(f_delta) steering_angle = np.clip( steering_angle, @@ -309,4 +328,4 @@ def update(self, vehicle: VehicleState, trajectory: Trajectory): def healthy(self): """Optional: check if the controller has a valid path.""" - return self.stanley.path is not None + return self.stanley.path is not None \ No newline at end of file diff --git a/launch/fixed_route.yaml b/launch/fixed_route.yaml index 904e82f2e..b773b20fd 100644 --- a/launch/fixed_route.yaml +++ b/launch/fixed_route.yaml @@ -16,15 +16,15 @@ drive: planning: route_planning: type: StaticRoutePlanner - args: [!relative_path '../GEMstack/knowledge/routes/forward_15m.csv'] + args: [!relative_path '../GEMstack/knowledge/routes/xyhead_highbay_backlot_p.csv'] # args: [!relative_path '../GEMstack/knowledge/routes/reverse_15m.csv'] motion_planning: type: RouteToTrajectoryPlanner args: [null] #desired speed in m/s. If null, this will keep the route untimed for the trajectory tracker trajectory_tracking: - type: pure_pursuit.PurePursuitTrajectoryTracker - # type: reverse_stanley.StanleyTrajectoryTracker - args: {desired_speed: 2.5} #approximately 5mph + # type: pure_pursuit.PurePursuitTrajectoryTracker + type: stanley.StanleyTrajectoryTracker + args: {desired_speed: 5.0} #approximately 5mph print: False log: # Specify the top-level folder to save the log files. Default is 'logs' From b67f1d21e36fb36399d85ce18e141be893ecfe2e Mon Sep 17 00:00:00 2001 From: hhxjqm Date: Fri, 25 Apr 2025 05:51:28 +0000 Subject: [PATCH 27/71] Set up stanley --- GEMstack/onboard/planning/stanley.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley.py index f820ff9b9..bb4f58749 100644 --- a/GEMstack/onboard/planning/stanley.py +++ b/GEMstack/onboard/planning/stanley.py @@ -54,7 +54,7 @@ def __init__( i = settings.get('control.longitudinal_control.pid_i') self.pid_speed = PID(p, d, i, windup_limit=20) - reverse = True + reverse = False # reverse = something_here() # TODO: Implement reverse logic if desired_speed is not None: @@ -110,7 +110,7 @@ def _find_rear_axle_position(self, x, y, yaw): ry = y - self.wheelbase * sin(yaw) return rx, ry - def compute(self, state: VehicleState, component: Component = None, reverse = True): # TODO change only during debugging + def compute(self, state: VehicleState, component: Component = None, reverse = False): # TODO change only during debugging """Compute the control outputs: (longitudinal acceleration, front wheel angle).""" t = state.pose.t if self.t_last is None: From bafc601d94814103aaa396ca549ca7bfd56758e9 Mon Sep 17 00:00:00 2001 From: hhxjqm Date: Fri, 25 Apr 2025 06:42:19 +0000 Subject: [PATCH 28/71] update stanley.py --- GEMstack/onboard/planning/stanley.py | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley.py index bb4f58749..132c29306 100644 --- a/GEMstack/onboard/planning/stanley.py +++ b/GEMstack/onboard/planning/stanley.py @@ -53,15 +53,9 @@ def __init__( d = settings.get('control.longitudinal_control.pid_d') i = settings.get('control.longitudinal_control.pid_i') self.pid_speed = PID(p, d, i, windup_limit=20) - - reverse = False - # reverse = something_here() # TODO: Implement reverse logic if desired_speed is not None: - if reverse: - self.desired_speed_source = -abs(desired_speed) - else: - self.desired_speed_source = abs(desired_speed) + self.desired_speed_source = desired_speed else: self.desired_speed_source = settings.get('control.stanley.desired_speed', 'path') @@ -165,7 +159,7 @@ def compute(self, state: VehicleState, component: Component = None, reverse = Fa yaw_error = normalise_angle(path_yaw - curr_yaw) - desired_speed = self.desired_speed + desired_speed = abs(self.desired_speed) feedforward_accel = 0.0 if reverse: @@ -193,7 +187,7 @@ def compute(self, state: VehicleState, component: Component = None, reverse = Fa if is_at_start_backward: desired_speed = 0.0 - feedforward_accel = -2.0 * -1.0 # = +2.0 + feedforward_accel = -2.0 * -1.0 else: difference_dt = 0.1 From 890469688ff51d46abbbcdca08508417d442deea Mon Sep 17 00:00:00 2001 From: hhxjqm Date: Fri, 25 Apr 2025 06:46:18 +0000 Subject: [PATCH 29/71] update --- GEMstack/onboard/planning/stanley.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley.py index 132c29306..72bf8b05f 100644 --- a/GEMstack/onboard/planning/stanley.py +++ b/GEMstack/onboard/planning/stanley.py @@ -104,7 +104,7 @@ def _find_rear_axle_position(self, x, y, yaw): ry = y - self.wheelbase * sin(yaw) return rx, ry - def compute(self, state: VehicleState, component: Component = None, reverse = False): # TODO change only during debugging + def compute(self, state: VehicleState, component: Component = None, reverse = False): """Compute the control outputs: (longitudinal acceleration, front wheel angle).""" t = state.pose.t if self.t_last is None: @@ -306,9 +306,9 @@ def update(self, vehicle: VehicleState, trajectory: Trajectory): self.stanley.set_path(trajectory) # TODO - accel, f_delta = self.stanley.compute(vehicle, self) + reverse = True # reverse = some_function_here() - # accel, f_delta = self.stanley.compute(vehicle, self, reverse) + accel, f_delta = self.stanley.compute(vehicle, self, reverse) steering_angle = front2steer(f_delta) steering_angle = np.clip( From 94d70eabc564581554844d5d1bc8e1aee26ba32c Mon Sep 17 00:00:00 2001 From: hhxjqm Date: Fri, 25 Apr 2025 06:51:33 +0000 Subject: [PATCH 30/71] update --- GEMstack/onboard/planning/stanley.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley.py index 72bf8b05f..5f7fc91fd 100644 --- a/GEMstack/onboard/planning/stanley.py +++ b/GEMstack/onboard/planning/stanley.py @@ -306,7 +306,7 @@ def update(self, vehicle: VehicleState, trajectory: Trajectory): self.stanley.set_path(trajectory) # TODO - reverse = True + reverse = False # reverse = some_function_here() accel, f_delta = self.stanley.compute(vehicle, self, reverse) From 4f26fc8cc56e7face2485783a40e7d0b7076bf98 Mon Sep 17 00:00:00 2001 From: hhxjqm Date: Wed, 30 Apr 2025 06:22:07 +0000 Subject: [PATCH 31/71] update current.yaml --- GEMstack/knowledge/defaults/current.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/GEMstack/knowledge/defaults/current.yaml b/GEMstack/knowledge/defaults/current.yaml index 1350e5204..61c1112af 100644 --- a/GEMstack/knowledge/defaults/current.yaml +++ b/GEMstack/knowledge/defaults/current.yaml @@ -22,13 +22,13 @@ control: # Stanley controller parameters (fine tune this) stanley: - control_gain: 1.0 - softening_gain: 0.01 + control_gain: 0.1 + softening_gain: 0.2 desired_speed: trajectory # Speed reference source: can be "trajectory", "path", or a constant value # Shared longitudinal control parameters longitudinal_control: - pid_p: 1.5 # Proportional gain for speed PID controller + pid_p: 1.0 # Proportional gain for speed PID controller pid_i: 0.1 # Integral gain for speed PID controller pid_d: 0.0 # Derivative gain for speed PID controller From f8873ebdfe2f3feb680ae1d05cd906c853b40020 Mon Sep 17 00:00:00 2001 From: hhxjqm Date: Wed, 30 Apr 2025 06:22:46 +0000 Subject: [PATCH 32/71] update stanley.py --- GEMstack/onboard/planning/stanley.py | 99 ++++++++++++++++++++++++++-- 1 file changed, 92 insertions(+), 7 deletions(-) diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley.py index 5f7fc91fd..6c1b5a445 100644 --- a/GEMstack/onboard/planning/stanley.py +++ b/GEMstack/onboard/planning/stanley.py @@ -153,6 +153,7 @@ def compute(self, state: VehicleState, component: Component = None, reverse = Fa if reverse: cross_track_error = dx * (-tangent[1]) + dy * tangent[0] + self.k += self.k else: left_vec = np.array([sin(curr_yaw), -cos(curr_yaw)]) cross_track_error = np.sign(np.dot(np.array([dx, dy]), left_vec)) * closest_dist @@ -292,7 +293,8 @@ class StanleyTrajectoryTracker(Component): def __init__(self, vehicle_interface=None, **kwargs): self.stanley = Stanley(**kwargs) self.vehicle_interface = vehicle_interface - + self.reverse = None + def rate(self): return 50.0 @@ -302,16 +304,99 @@ def state_inputs(self): def state_outputs(self): return [] + def _check_sharp_turn_ahead(self, lookahead_s=3.0, threshold_angle=np.pi/2.0, num_steps=4): + if not self.stanley.path: + return False + + path = self.stanley.path + current_s = self.stanley.current_path_parameter + domain_start, domain_end = path.domain() + + if current_s >= domain_end - 1e-3: + return False + + step_s = lookahead_s / num_steps + s_prev = current_s + + try: + tangent_prev = path.eval_tangent(s_prev) + if np.linalg.norm(tangent_prev) < 1e-6: + s_prev_adjusted = min(s_prev + step_s / 2, domain_end) + if s_prev_adjusted <= s_prev: + return False + tangent_prev = path.eval_tangent(s_prev_adjusted) + if np.linalg.norm(tangent_prev) < 1e-6: + return False + s_prev = s_prev_adjusted + + angle_prev = atan2(tangent_prev[1], tangent_prev[0]) + + except Exception as e: + return False + + for i in range(num_steps): + s_next = s_prev + step_s + s_next = min(s_next, domain_end) + + if s_next <= s_prev + 1e-6: + break + + try: + tangent_next = path.eval_tangent(s_next) + if np.linalg.norm(tangent_next) < 1e-6: + s_prev = s_next + continue + + angle_next = atan2(tangent_next[1], tangent_next[0]) + except Exception as e: + break + + angle_change = abs(normalise_angle(angle_next - angle_prev)) + + if angle_change > threshold_angle: + return True + + angle_prev = angle_next + s_prev = s_next + return False + def update(self, vehicle: VehicleState, trajectory: Trajectory): self.stanley.set_path(trajectory) - - # TODO - reverse = False - # reverse = some_function_here() - accel, f_delta = self.stanley.compute(vehicle, self, reverse) + + if self.reverse == None: + curr_x = vehicle.pose.x + curr_y = vehicle.pose.y + curr_yaw = vehicle.pose.yaw if vehicle.pose.yaw is not None else 0.0 + + fx, fy = self.stanley._find_front_axle_position(curr_x, curr_y, curr_yaw) + path_domain_start = self.stanley.path.domain()[0] + search_domain_start = [path_domain_start, min(self.stanley.path.domain()[1], path_domain_start + 5.0)] + + _, closest_param_at_start = self.stanley.path.closest_point_local((fx, fy), search_domain_start) + tangent_at_start = self.stanley.path.eval_tangent(closest_param_at_start) + + if np.linalg.norm(tangent_at_start) > 1e-6: + path_yaw_at_start = atan2(tangent_at_start[1], tangent_at_start[0]) + heading_diff = normalise_angle(path_yaw_at_start - curr_yaw) + self.reverse = abs(heading_diff) > (np.pi / 2.0) + else: + self.reverse = False + self.stanley.pid_speed.reset() + self.stanley.current_path_parameter = self.stanley.path.domain()[0] + + else: + is_sharp_turn_ahead = self._check_sharp_turn_ahead( + lookahead_s=4.0, + threshold_angle=np.pi/2.0 + ) + print(f"Sharp turn ahead: {is_sharp_turn_ahead}") + if is_sharp_turn_ahead: + self.reverse = not self.reverse + self.stanley.set_path(trajectory) + accel, f_delta = self.stanley.compute(vehicle, self, self.reverse) steering_angle = front2steer(f_delta) - steering_angle = np.clip( + steering_angle = np.clip( steering_angle, settings.get('vehicle.geometry.min_steering_angle', -0.5), settings.get('vehicle.geometry.max_steering_angle', 0.5) From 0daf53e74c859a1f1cfbaeb629b5befbdfe54053 Mon Sep 17 00:00:00 2001 From: Patrick Wu Date: Wed, 30 Apr 2025 14:30:50 -0500 Subject: [PATCH 33/71] add mpc, still need some adjustment to make it more fault tolerant --- GEMstack/knowledge/defaults/current.yaml | 5 + GEMstack/knowledge/routes/backward_15m.csv | 16 + GEMstack/knowledge/routes/backward_curve.csv | 6 + GEMstack/knowledge/routes/forward_curve.csv | 33 + GEMstack/knowledge/routes/offset.csv | 31 + GEMstack/onboard/interface/gem.py | 372 +++++------ GEMstack/onboard/interface/gem_simulator.py | 2 +- GEMstack/onboard/planning/mpc.py | 204 ++++-- GEMstack/state/trajectory.py | 618 +++++++++---------- launch/fixed_route.yaml | 12 +- scenes/xyhead_demo.yaml | 2 +- 11 files changed, 764 insertions(+), 537 deletions(-) create mode 100644 GEMstack/knowledge/routes/backward_15m.csv create mode 100644 GEMstack/knowledge/routes/backward_curve.csv create mode 100644 GEMstack/knowledge/routes/forward_curve.csv create mode 100644 GEMstack/knowledge/routes/offset.csv diff --git a/GEMstack/knowledge/defaults/current.yaml b/GEMstack/knowledge/defaults/current.yaml index 61c1112af..6e547a977 100644 --- a/GEMstack/knowledge/defaults/current.yaml +++ b/GEMstack/knowledge/defaults/current.yaml @@ -26,6 +26,11 @@ control: softening_gain: 0.2 desired_speed: trajectory # Speed reference source: can be "trajectory", "path", or a constant value + # MPC controller parameters + mpc: + dt: 0.2 # Time step for the MPC controller (seconds) + horizon: 30 # Prediction horizon for the MPC controller (number of time steps) + # Shared longitudinal control parameters longitudinal_control: pid_p: 1.0 # Proportional gain for speed PID controller diff --git a/GEMstack/knowledge/routes/backward_15m.csv b/GEMstack/knowledge/routes/backward_15m.csv new file mode 100644 index 000000000..27912b180 --- /dev/null +++ b/GEMstack/knowledge/routes/backward_15m.csv @@ -0,0 +1,16 @@ +0.0,0,0 +-1.0,0,0 +-2.0,0,0 +-3.0,0,0 +-4,0,0 +-5,0,0 +-6,0,0 +-7,0,0 +-8,0,0 +-9,0,0 +-10,0,0 +-11,0,0 +-12,0,0 +-13,0,0 +-14,0,0 +-15,0,0 diff --git a/GEMstack/knowledge/routes/backward_curve.csv b/GEMstack/knowledge/routes/backward_curve.csv new file mode 100644 index 000000000..87f25ee6e --- /dev/null +++ b/GEMstack/knowledge/routes/backward_curve.csv @@ -0,0 +1,6 @@ +0.0,0,0 +-1.0,0,0 +-2.0,0.2,0 +-3.0,0.4,0 +-4,0.6,0 +-5,0.8,0 diff --git a/GEMstack/knowledge/routes/forward_curve.csv b/GEMstack/knowledge/routes/forward_curve.csv new file mode 100644 index 000000000..9b776d8ec --- /dev/null +++ b/GEMstack/knowledge/routes/forward_curve.csv @@ -0,0 +1,33 @@ +0.0,0,0 +1.0,0,0 +2.0,0,0 +3.0,0,0 +4.0,0.4,0 +5.0,0.8,0 +6.0,1.2,0 +7.0,1.6,0 +8.0,2.0,0 +9.0,2.4,0 +10.0,2.8,0 +11.0,3.2,0 +12.0,3.6,0 +13.0,4.0,0 +14.0,4.4,0 +15.0,4.8,0 +16.0,5.2,0 +17.0,5.6,0 +18.0,6.0,0 +19.0,6.4,0 +20.0,6.8,0 +21.0,7.2,0 +22.0,7.6,0 +23.0,8.0,0 +24.0,8.4,0 +25.0,8.8,0 +26.0,9.2,0 +27.0,9.6,0 +28.0,10.0,0 +29.0,10.4,0 +30.0,10.8,0 +31.0,11.2,0 +32.0,11.6,0 \ No newline at end of file diff --git a/GEMstack/knowledge/routes/offset.csv b/GEMstack/knowledge/routes/offset.csv new file mode 100644 index 000000000..947697fbb --- /dev/null +++ b/GEMstack/knowledge/routes/offset.csv @@ -0,0 +1,31 @@ +0.0,0.5,0 +1.0,0.7,0 +2.0,0.9,0 +3.0,1.1,0 +4.0,1.3,0 +5.0,1.5,0 +6.0,1.7,0 +7.0,1.9,0 +8.0,2.1,0 +9.0,2.3,0 +10.0,2.5,0 +11.0,2.7,0 +12.0,2.9,0 +13.0,3.1,0 +14.0,3.3,0 +15.0,3.5,0 +16.0,3.7,0 +17.0,3.9,0 +18.0,4.1,0 +19.0,4.3,0 +20.0,4.5,0 +21.0,4.7,0 +22.0,4.9,0 +23.0,5.1,0 +24.0,5.3,0 +25.0,5.5,0 +26.0,5.7,0 +27.0,5.9,0 +28.0,6.1,0 +29.0,6.3,0 +30.0,6.5,0 \ No newline at end of file diff --git a/GEMstack/onboard/interface/gem.py b/GEMstack/onboard/interface/gem.py index 9abcd1b6b..4e44829d4 100644 --- a/GEMstack/onboard/interface/gem.py +++ b/GEMstack/onboard/interface/gem.py @@ -1,187 +1,187 @@ -from dataclasses import dataclass -from ...utils import settings,serialization -from ...state import VehicleState, ObjectPose, ObjectFrameEnum -from ...knowledge.vehicle.geometry import front2steer,steer2front,heading_rate -from ...knowledge.vehicle.dynamics import pedal_positions_to_acceleration, acceleration_to_pedal_positions -from typing import List,Optional,Callable - -@dataclass -@serialization.register -class GEMVehicleReading: - """All items that the vehicle reports directly from its internal sensors.""" - speed : float = 0 # in m/s - gear : int = 0 # 0 neutral, -1 reverse, -2 park, > 0 forward - accelerator_pedal_position : float = 0 # in range [0,1] - brake_pedal_position : float = 0 # in range [0,1] - steering_wheel_angle : float = 0 # in radians - left_turn_signal : bool = False - right_turn_signal : bool = False - headlights_on : bool = False - horn_on : bool = False - wiper_level : int = 0 - battery_level : Optional[float] = None # in range [0,1] - fuel_level : Optional[float] = None # in liters - driving_range : Optional[float] = None # remaining range left, in km - - def from_state(self, state: VehicleState) -> None: - """Sets the readings that would be approximately sensed at the given - VehicleState. - - Does not change the battery_level, fuel_level, or driving_range values. - """ - self.speed = state.v - self.steering_wheel_angle = state.steering_wheel_angle - pitch = state.pose.pitch if state.pose.pitch is not None else 0.0 - - #acc_pos,brake_pos,gear = acceleration_to_pedal_positions(state.acceleration, state.v, pitch, state.gear) - self.accelerator_pedal_position = state.accelerator_pedal_position - self.brake_pedal_position = state.brake_pedal_position - self.gear = state.gear - self.left_turn_signal = state.left_turn_indicator - self.right_turn_signal = state.right_turn_indicator - self.horn_on = state.horn_on - self.wiper_level = state.wiper_level - self.headlights_on = state.headlights_on - - def to_state(self, pose : ObjectPose = None) -> VehicleState: - """Returns a VehicleState representing the vehicle's current state given - these readings. - - Note: the acceleration attribute is totally bogus, and should be ignored - until the dynamics are calibrated better. - """ - if pose is None: - pose = ObjectPose(frame = ObjectFrameEnum.CURRENT,t=0,x=0,y=0,yaw=0) - pitch = pose.pitch if pose.pitch is not None else 0.0 - wheel_base = settings.get('vehicle.geometry.wheelbase') - front_wheel_angle=steer2front(self.steering_wheel_angle) - turn_rate=heading_rate(front_wheel_angle,self.speed,wheel_base) - acc = pedal_positions_to_acceleration(self.accelerator_pedal_position, self.brake_pedal_position, self.speed, pitch, self.gear) - return VehicleState(pose,v=self.speed,accelerator_pedal_position=self.accelerator_pedal_position,brake_pedal_position=self.brake_pedal_position, - acceleration=acc,gear=self.gear,steering_wheel_angle=self.steering_wheel_angle, - front_wheel_angle=front_wheel_angle,heading_rate=turn_rate, - left_turn_indicator=self.left_turn_signal,right_turn_indicator=self.right_turn_signal, - horn_on=self.horn_on,wiper_level=self.wiper_level,headlights_on=self.headlights_on) - - -@dataclass -@serialization.register -class GEMVehicleCommand: - """All items that can be directly commanded to the vehicle's actuators.""" - gear : int #follows convention in state.vehicle.VehicleState. -2: park, -1 reverse: 0: neutral, 1..n: forward - accelerator_pedal_position : float - accelerator_pedal_speed : float - brake_pedal_position : float - brake_pedal_speed : float - steering_wheel_angle : float - steering_wheel_speed : float - left_turn_signal : bool = False - right_turn_signal : bool = False - headlights_on : bool = False - horn_on : bool = False - wiper_level : int = 0 - - -@dataclass -class GNSSReading: - pose : ObjectPose - speed : float - status : str - - -class GEMInterface: - """Base class for simulated / physical GEM vehicle. - """ - def __init__(self): - self.last_command = None # type: GEMVehicleCommand - self.last_reading = None # type: GEMVehicleReading - - def start(self): - pass - - def stop(self): - pass - - def time(self) -> float: - """Returns the current time""" - raise NotImplementedError() - - def get_reading(self) -> GEMVehicleReading: - """Returns current read state of the vehicle""" - raise NotImplementedError() - - def send_command(self, cmd : GEMVehicleCommand): - """Sends a command to the vehicle""" - raise NotImplementedError() - - def sensors(self) -> List[str]: - """Returns all available sensors""" - return ['gnss','imu','top_lidar','front_camera','front_depth','front_radar'] - - def subscribe_sensor(self, name : str, callback : Callable, type = None) -> None: - """Subscribes to a sensor with a given callback. - - If type is not None, it should be the expected type of the message produced - by the sensor callback. - """ - raise NotImplementedError() - - def hardware_faults(self) -> List[str]: - """Returns a list of hardware faults, naming the failed component. - - Can be any sensor, actuator, or other component. - """ - raise NotImplementedError() - - def simple_command(self, acceleration_mps2 : float, steering_wheel_angle : float, state : VehicleState = None) -> GEMVehicleCommand: - """" - Returns a command according to a desired acceleration and steering angle - - Args: - acceleration_mps2: acceleration in m/s^2 - steering_wheel_angle: steering angle in radians - state: current vehicle state - """ - pitch = state.pose.pitch if state is not None and state.pose.pitch is not None else 0.0 - v = state.v if state is not None else 0.0 - gear = state.gear if state is not None else 1 - acc_pos,brake_pos,gear = acceleration_to_pedal_positions(acceleration_mps2, v, pitch, gear) - - cmd = GEMVehicleCommand(gear=gear, - accelerator_pedal_position=acc_pos, - brake_pedal_position=brake_pos, - steering_wheel_angle=steering_wheel_angle, - accelerator_pedal_speed=settings.get('vehicle.control_defaults.accelerator_pedal_speed'), - brake_pedal_speed = settings.get('vehicle.control_defaults.brake_pedal_speed'), - steering_wheel_speed = settings.get('vehicle.control_defaults.steering_wheel_speed')) - if state is not None: - #preserve indicators - cmd.left_turn_signal = state.left_turn_indicator - cmd.right_turn_signal = state.right_turn_indicator - cmd.headlights_on = state.headlights_on - cmd.horn_on = state.horn_on - cmd.wiper_level = state.wiper_level - - return cmd - - def command_from_reading(self, reading: GEMVehicleReading = None) -> GEMVehicleCommand: - """Returns a command that maintains all the current elements in the - provided vehicle reading. If reading=None, then the last reading - is used. - """ - if reading is None: - reading = self.last_reading - if reading is None: - raise RuntimeError("Can't get command from reading, no reading available") - return GEMVehicleCommand(gear=reading.gear, - accelerator_pedal_position=reading.accelerator_pedal_position, - brake_pedal_position=reading.brake_pedal_position, - steering_wheel_angle=reading.steering_wheel_angle, - accelerator_pedal_speed=settings.get('vehicle.control_defaults.accelerator_pedal_speed'), - brake_pedal_speed = settings.get('vehicle.control_defaults.brake_pedal_speed'), - steering_wheel_speed = settings.get('vehicle.control_defaults.steering_wheel_speed'), - left_turn_signal = reading.left_turn_signal, - right_turn_signal = reading.right_turn_signal, - headlights_on = reading.headlights_on, - horn_on = reading.horn_on, +from dataclasses import dataclass +from ...utils import settings,serialization +from ...state import VehicleState, ObjectPose, ObjectFrameEnum +from ...knowledge.vehicle.geometry import front2steer,steer2front,heading_rate +from ...knowledge.vehicle.dynamics import pedal_positions_to_acceleration, acceleration_to_pedal_positions +from typing import List,Optional,Callable + +@dataclass +@serialization.register +class GEMVehicleReading: + """All items that the vehicle reports directly from its internal sensors.""" + speed : float = 0 # in m/s + gear : int = 0 # 0 neutral, -1 reverse, -2 park, > 0 forward + accelerator_pedal_position : float = 0 # in range [0,1] + brake_pedal_position : float = 0 # in range [0,1] + steering_wheel_angle : float = 0 # in radians + left_turn_signal : bool = False + right_turn_signal : bool = False + headlights_on : bool = False + horn_on : bool = False + wiper_level : int = 0 + battery_level : Optional[float] = None # in range [0,1] + fuel_level : Optional[float] = None # in liters + driving_range : Optional[float] = None # remaining range left, in km + + def from_state(self, state: VehicleState) -> None: + """Sets the readings that would be approximately sensed at the given + VehicleState. + + Does not change the battery_level, fuel_level, or driving_range values. + """ + self.speed = state.v + self.steering_wheel_angle = state.steering_wheel_angle + pitch = state.pose.pitch if state.pose.pitch is not None else 0.0 + + #acc_pos,brake_pos,gear = acceleration_to_pedal_positions(state.acceleration, state.v, pitch, state.gear) + self.accelerator_pedal_position = state.accelerator_pedal_position + self.brake_pedal_position = state.brake_pedal_position + self.gear = state.gear + self.left_turn_signal = state.left_turn_indicator + self.right_turn_signal = state.right_turn_indicator + self.horn_on = state.horn_on + self.wiper_level = state.wiper_level + self.headlights_on = state.headlights_on + + def to_state(self, pose : ObjectPose = None) -> VehicleState: + """Returns a VehicleState representing the vehicle's current state given + these readings. + + Note: the acceleration attribute is totally bogus, and should be ignored + until the dynamics are calibrated better. + """ + if pose is None: + pose = ObjectPose(frame = ObjectFrameEnum.CURRENT,t=0,x=0,y=0,yaw=0) + pitch = pose.pitch if pose.pitch is not None else 0.0 + wheel_base = settings.get('vehicle.geometry.wheelbase') + front_wheel_angle=steer2front(self.steering_wheel_angle) + turn_rate=heading_rate(front_wheel_angle,self.speed,wheel_base) + acc = pedal_positions_to_acceleration(self.accelerator_pedal_position, self.brake_pedal_position, self.speed, pitch, self.gear) + return VehicleState(pose,v=self.speed,accelerator_pedal_position=self.accelerator_pedal_position,brake_pedal_position=self.brake_pedal_position, + acceleration=acc,gear=self.gear,steering_wheel_angle=self.steering_wheel_angle, + front_wheel_angle=front_wheel_angle,heading_rate=turn_rate, + left_turn_indicator=self.left_turn_signal,right_turn_indicator=self.right_turn_signal, + horn_on=self.horn_on,wiper_level=self.wiper_level,headlights_on=self.headlights_on) + + +@dataclass +@serialization.register +class GEMVehicleCommand: + """All items that can be directly commanded to the vehicle's actuators.""" + gear : int #follows convention in state.vehicle.VehicleState. -2: park, -1 reverse: 0: neutral, 1..n: forward + accelerator_pedal_position : float + accelerator_pedal_speed : float + brake_pedal_position : float + brake_pedal_speed : float + steering_wheel_angle : float + steering_wheel_speed : float + left_turn_signal : bool = False + right_turn_signal : bool = False + headlights_on : bool = False + horn_on : bool = False + wiper_level : int = 0 + + +@dataclass +class GNSSReading: + pose : ObjectPose + speed : float + status : str + + +class GEMInterface: + """Base class for simulated / physical GEM vehicle. + """ + def __init__(self): + self.last_command = None # type: GEMVehicleCommand + self.last_reading = None # type: GEMVehicleReading + + def start(self): + pass + + def stop(self): + pass + + def time(self) -> float: + """Returns the current time""" + raise NotImplementedError() + + def get_reading(self) -> GEMVehicleReading: + """Returns current read state of the vehicle""" + raise NotImplementedError() + + def send_command(self, cmd : GEMVehicleCommand): + """Sends a command to the vehicle""" + raise NotImplementedError() + + def sensors(self) -> List[str]: + """Returns all available sensors""" + return ['gnss','imu','top_lidar','front_camera','front_depth','front_radar'] + + def subscribe_sensor(self, name : str, callback : Callable, type = None) -> None: + """Subscribes to a sensor with a given callback. + + If type is not None, it should be the expected type of the message produced + by the sensor callback. + """ + raise NotImplementedError() + + def hardware_faults(self) -> List[str]: + """Returns a list of hardware faults, naming the failed component. + + Can be any sensor, actuator, or other component. + """ + raise NotImplementedError() + + def simple_command(self, acceleration_mps2 : float, steering_wheel_angle : float, state : VehicleState = None) -> GEMVehicleCommand: + """" + Returns a command according to a desired acceleration and steering angle + + Args: + acceleration_mps2: acceleration in m/s^2 + steering_wheel_angle: steering angle in radians + state: current vehicle state + """ + pitch = state.pose.pitch if state is not None and state.pose.pitch is not None else 0.0 + v = state.v if state is not None else 0.0 + gear = state.gear if state is not None else 1 + acc_pos,brake_pos,gear = acceleration_to_pedal_positions(acceleration_mps2, v, pitch, gear) + + cmd = GEMVehicleCommand(gear=gear, + accelerator_pedal_position=acc_pos, + brake_pedal_position=brake_pos, + steering_wheel_angle=steering_wheel_angle, + accelerator_pedal_speed=settings.get('vehicle.control_defaults.accelerator_pedal_speed'), + brake_pedal_speed = settings.get('vehicle.control_defaults.brake_pedal_speed'), + steering_wheel_speed = settings.get('vehicle.control_defaults.steering_wheel_speed')) + if state is not None: + #preserve indicators + cmd.left_turn_signal = state.left_turn_indicator + cmd.right_turn_signal = state.right_turn_indicator + cmd.headlights_on = state.headlights_on + cmd.horn_on = state.horn_on + cmd.wiper_level = state.wiper_level + + return cmd + + def command_from_reading(self, reading: GEMVehicleReading = None) -> GEMVehicleCommand: + """Returns a command that maintains all the current elements in the + provided vehicle reading. If reading=None, then the last reading + is used. + """ + if reading is None: + reading = self.last_reading + if reading is None: + raise RuntimeError("Can't get command from reading, no reading available") + return GEMVehicleCommand(gear=reading.gear, + accelerator_pedal_position=reading.accelerator_pedal_position, + brake_pedal_position=reading.brake_pedal_position, + steering_wheel_angle=reading.steering_wheel_angle, + accelerator_pedal_speed=settings.get('vehicle.control_defaults.accelerator_pedal_speed'), + brake_pedal_speed = settings.get('vehicle.control_defaults.brake_pedal_speed'), + steering_wheel_speed = settings.get('vehicle.control_defaults.steering_wheel_speed'), + left_turn_signal = reading.left_turn_signal, + right_turn_signal = reading.right_turn_signal, + headlights_on = reading.headlights_on, + horn_on = reading.horn_on, wiper_level = reading.wiper_level) \ No newline at end of file diff --git a/GEMstack/onboard/interface/gem_simulator.py b/GEMstack/onboard/interface/gem_simulator.py index bb49054b5..86cf2f683 100644 --- a/GEMstack/onboard/interface/gem_simulator.py +++ b/GEMstack/onboard/interface/gem_simulator.py @@ -3,7 +3,7 @@ from ...mathutils.dubins import SecondOrderDubinsCar from ...mathutils.dynamics import simulate from ...mathutils import transforms -from ...state import VehicleState,ObjectPose,ObjectFrameEnum,Roadgraph,AgentState,AgentEnum,AgentActivityEnum,Obstacle,Sign,AllState +from ...state import VehicleState,ObjectPose,ObjectFrameEnum,Roadgraph,AgentState,AgentEnum,AgentActivityEnum,Obstacle,Sign,AllState,VehicleGearEnum from ...knowledge.vehicle.geometry import front2steer,steer2front,heading_rate from ...knowledge.vehicle.dynamics import pedal_positions_to_acceleration, acceleration_to_pedal_positions from ...utils.loops import TimedLooper diff --git a/GEMstack/onboard/planning/mpc.py b/GEMstack/onboard/planning/mpc.py index fe511bbfa..131cf342f 100644 --- a/GEMstack/onboard/planning/mpc.py +++ b/GEMstack/onboard/planning/mpc.py @@ -1,19 +1,21 @@ from ...utils import settings -from ...state.vehicle import VehicleState,ObjectFrameEnum +from ...state.vehicle import VehicleState,VehicleGearEnum from ...state.trajectory import Trajectory, Path from ...knowledge.vehicle.geometry import front2steer from ..component import Component import numpy as np import casadi +import math class MPCController(object): """Model Predictive Controller for trajectory tracking.""" def __init__(self, T=None, dt=None): - self.T = T if T is not None else settings.get('control.mpc.horizon', 10) - self.dt = dt if dt is not None else settings.get('control.mpc.dt', 0.1) + self.T = T if T is not None else settings.get('control.mpc.horizon', 30) + self.dt = dt if dt is not None else settings.get('control.mpc.dt', 0.2) self.L = settings.get('vehicle.geometry.wheelbase') self.v_bounds = [-settings.get('vehicle.limits.max_reverse_speed'), settings.get('vehicle.limits.max_speed')] self.delta_bounds = [settings.get('vehicle.geometry.min_wheel_angle'),settings.get('vehicle.geometry.max_wheel_angle')] + self.delta_rate_bounds = [-0.4, 0.4] # Predefined front wheel rate limit to simplify computation self.steering_angle_range = [settings.get('vehicle.geometry.min_steering_angle'),settings.get('vehicle.geometry.max_steering_angle')] self.a_bounds = [-settings.get('vehicle.limits.max_deceleration'), settings.get('vehicle.limits.max_acceleration')] self.trajectory = None @@ -21,44 +23,67 @@ def __init__(self, T=None, dt=None): self.prev_u = None # Previous control inputs self.path = None self.path_arg = None - - # def set_path(self, trajectory: Trajectory): - # """Set the trajectory for the MPC controller.""" - # # Assume the argument can only be trajectory - # assert isinstance(trajectory, Trajectory), "Invalid trajectory type." - # print("*"*10) - # print("set_path") - # self.trajectory = trajectory + self.iter = 0 def set_path(self, path : Path): - print("Get new path!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!") if path == self.path_arg: return self.path_arg = path + self.iter = 0 if len(path.points[0]) > 2: path = path.get_dims([0,1]) - self.path = path.arc_length_parameterize() - self.trajectory = self.path - self.current_traj_parameter = self.trajectory.domain()[0] + self.path = path.arc_length_parameterize(1.0) + # self.path = path.arc_length_parameterize(0.5) self.current_path_parameter = 0.0 + # self.prev_u = None + # self.prev_x = None + if not isinstance(path,Trajectory): + self.trajectory = None + self.current_traj_parameter = 0.0 + else: + self.trajectory = path + self.current_traj_parameter = self.trajectory.domain()[0] def compute(self, state: VehicleState, component: Component = None): """Compute the control commands using MPC.""" + if self.iter < 10 and self.prev_x is not None: + self.iter += 1 + # return float(self.prev_u[self.iter, 0]), float(self.prev_x[self.iter + 1, 4]) + self.iter = 0 + + if self.path is not None: + if self.path.frame != state.pose.frame: + print("Transforming trajectory from",self.path.frame.name,"to",state.pose.frame.name) + self.path = self.path.to_frame(state.pose.frame, current_pose=state.pose) if self.trajectory is not None: if self.trajectory.frame != state.pose.frame: print("Transforming trajectory from",self.trajectory.frame.name,"to",state.pose.frame.name) self.trajectory = self.trajectory.to_frame(state.pose.frame, current_pose=state.pose) + + x0 = np.array([state.pose.x, state.pose.y, state.pose.yaw % (2 * np.pi), state.v, state.front_wheel_angle]) + + print(x0) - x0 = np.array([state.pose.x, state.pose.y, state.pose.yaw, state.v]) + closest_dist,closest_time = self.path.closest_point_local((x0[0], x0[1]),[self.current_traj_parameter-7.0,self.current_traj_parameter+7.0], True) + # Interpolate trajectory points to match MPC time horizon + self.current_traj_parameter = closest_time + + times = self.path.times + points = self.path.points + j = math.floor(self.current_path_parameter) + while j < len(times) - 1 and times[j+1] < closest_time: + j += 1 + self.current_path_parameter = j + # print("closest_index: ", self.current_path_parameter) + # print("vehicle position: ", x0[0], x0[1]) + # print("closest_point: ", points[int(self.current_path_parameter)]) + # print("closest_time: ", closest_time) + # print("local points: ", points[j-5:j+5]) - # Interpolate trajectory points to match MPC time horizon - times = self.trajectory.times - points = self.trajectory.points traj_points = [] - j = 0 for i in range(self.T + 1): - t_query = times[0] + i * self.dt + t_query = closest_time + i * self.dt if t_query <= times[0]: traj_points.append(points[0]) elif t_query >= times[-1]: @@ -69,22 +94,95 @@ def compute(self, state: VehicleState, component: Component = None): alpha = (t_query - times[j]) / (times[j+1] - times[j]) pt = (1 - alpha) * np.array(points[j]) + alpha * np.array(points[j+1]) traj_points.append(pt) + + # print("trajectory points: ", traj_points) + traj_str = ", ".join( + [f"np.array([{round(p[0], 8)}, {round(p[1], 8)}])" for p in traj_points] + ) + print(f"traj_points = [{traj_str}]") + + # Apply gradually decreasing offset correction + cur_offset = np.array([state.pose.x, state.pose.y]) - np.array(traj_points[0][0:2]) + for i in range(len(traj_points)): + s = i / (len(traj_points) + 5) # Normalized [0, 1] + decay_ratio = (1 - s) ** 1 # linear decay + + # Get direction of trajectory at this point + if i < len(traj_points) - 1: + tangent = np.array(traj_points[i+1]) - np.array(traj_points[i]) + else: + tangent = np.array(traj_points[i]) - np.array(traj_points[i-1]) + + tangent_norm = np.linalg.norm(tangent) + if tangent_norm == 0: + continue # Skip degenerate points + + tangent /= tangent_norm + normal = np.array([-tangent[1], tangent[0]]) # Rotate tangent by 90° to get normal + + # Project initial offset onto this normal + offset_proj = np.dot(cur_offset, normal) * normal + + # Apply decaying lateral offset + traj_points[i] = np.array(traj_points[i]) + decay_ratio * offset_proj + + # print("trajectory points after correction: ", traj_points) + traj_corrected_str = ", ".join( + [f"np.array([{round(p[0], 8)}, {round(p[1], 8)}])" for p in traj_points] + ) + print(f"traj_corrected = [{traj_corrected_str}]") + + + target_angles = [] + prev_angle = x0[2] + + for i in range(1, len(traj_points)): + dx = traj_points[i][0] - traj_points[i-1][0] + dy = traj_points[i][1] - traj_points[i-1][1] + raw_angle = np.arctan2(dy, dx) % (2 * np.pi) + + # Convert raw_angle to be close to prev_angle (modulo 2π), within ±0.5π + # Compute smallest angular difference + delta = ((raw_angle - prev_angle + np.pi) % (2 * np.pi)) - np.pi + + # Clip delta to ±0.5π + if delta > 0.5 * np.pi: + delta -= np.pi + elif delta < -0.5 * np.pi: + delta += np.pi + + corrected_angle = prev_angle + delta + assert abs(corrected_angle - prev_angle) < 0.5 * np.pi + target_angles.append(corrected_angle) + prev_angle = corrected_angle + + # print(target_angles) + + # delta_desired = [] + # for i in range(1, len(target_angles)): + # ds = np.linalg.norm(np.array(traj_points[i]) - np.array(traj_points[i-1])) + # dtheta = target_angles[i] - target_angles[i-1] + # dtheta = (dtheta + np.pi) % (2 * np.pi) - np.pi # Normalize to [-pi, pi] + + # curvature = dtheta / ds if ds > 1e-6 else 0.0 + # delta_ref = np.arctan(self.L * curvature) + # delta_desired.append(delta_ref) # Optimization setup opti = casadi.Opti() - x = opti.variable(self.T+1, 4) # [x, y, theta, v] - u = opti.variable(self.T, 2) # [a, delta] + x = opti.variable(self.T+1, 5) # [x, y, theta, v, delta] + u = opti.variable(self.T, 2) # [a, delta_dot] def model(x, u): """Dynamic model of the vehicle using kinematic bicycle model""" - px, py, theta, v = x[0], x[1], x[2], x[3] - a, delta = u[0], u[1] - beta = casadi.atan(casadi.tan(delta) / 2.0) - dx = v * casadi.cos(theta + beta) - dy = v * casadi.sin(theta + beta) + px, py, theta, v, delta = x[0], x[1], x[2], x[3], x[4] + a, delta_dot = u[0], u[1] + dx = v * casadi.cos(theta) + dy = v * casadi.sin(theta) dtheta = v * casadi.tan(delta) / self.L dv = a - return casadi.vertcat(dx, dy, dtheta, dv) + ddelta = delta_dot + return casadi.vertcat(dx, dy, dtheta, dv, ddelta) obj = 0 for t in range(self.T): @@ -93,17 +191,30 @@ def model(x, u): opti.subject_to(x[t+1,:] == x_next) target = traj_points[t+1] # Cost function - obj += casadi.sumsqr(x[t+1,0:2] - casadi.reshape(target[0:2], 1, 2)) - obj += 0.1 * casadi.sumsqr(u[t,:]) + # Apply larger cost for the first three points + weight = 10 if t < 3 else 1 # Larger weight for the first three points + obj += weight * casadi.sumsqr(x[t + 1, 0:2] - casadi.reshape(target[0:2], 1, 2)) + + # Control effort penalty + obj += 0.1 * casadi.sumsqr(u[t, :]) + + # Heading angle error + # theta_error = x[t + 1, 2] - target_angles[t] + # obj += 1 * weight * casadi.sumsqr(theta_error) + + # if t != self.T - 1: + # delta_error = x[t + 1, 4] - delta_desired[t] + # obj += 0.1 * weight * casadi.sumsqr(delta_error) # Initial condition - opti.subject_to(x[0, :] == casadi.reshape(x0[:4], 1, 4)) + opti.subject_to(x[0, :] == casadi.reshape(x0, 1, 5)) # Constraints for t in range(self.T): - opti.subject_to(opti.bounded(self.a_bounds[0], u[t,0], self.a_bounds[1])) - opti.subject_to(opti.bounded(self.delta_bounds[0], u[t,1], self.delta_bounds[1])) - opti.subject_to(opti.bounded(self.v_bounds[0], x[t+1,3], self.v_bounds[1])) + opti.subject_to(opti.bounded(self.a_bounds[0], u[t,0], self.a_bounds[1])) # a + opti.subject_to(opti.bounded(self.delta_rate_bounds[0], u[t,1], self.delta_rate_bounds[1])) # delta_dot + opti.subject_to(opti.bounded(self.delta_bounds[0], x[t+1,4], self.delta_bounds[1])) # delta as state + opti.subject_to(opti.bounded(self.v_bounds[0], x[t+1,3], self.v_bounds[1])) # v # Initial guess if self.prev_x is not None and self.prev_u is not None: @@ -113,16 +224,33 @@ def model(x, u): # Solver settings opti.minimize(obj) - opti.solver("ipopt", {"expand": True}, {"max_iter": 100}) + p_opts = {"expand": True, 'ipopt.print_level': 0, 'print_time': 0, 'ipopt.sb': 'yes'} + s_opts = {"max_iter": 150} + + opti.solver("ipopt", p_opts, s_opts) try: # Solve the optimization problem sol = opti.solve() acc = float(sol.value(u[0,0])) - delta = float(sol.value(u[0,1])) + # delta = float(sol.value(u[0,1])) + delta = float(sol.value(x[1,4])) self.prev_x = sol.value(x) self.prev_u = sol.value(u) + # if x0[3] < 1.3: + # for i in range(len(self.prev_x)): + # assert self.prev_x[i,3] <= 1.3 + + xy_array = [f"np.array([{round(self.prev_x[t,0],8)}, {round(self.prev_x[t,1],8)}])" for t in range(self.prev_x.shape[0])] + print("mpc = [", ", ".join(xy_array), "]") + print(acc, delta) + # print(self.prev_u[0]) + # print(self.prev_x[0]) + # print(self.prev_x[1]) + # print(self.prev_u) + # print(x0[4]) + # print(delta) return acc, delta except RuntimeError: # Handle optimization failure @@ -134,7 +262,7 @@ class MPCTrajectoryTracker(Component): def __init__(self, vehicle_interface=None, **args): self.mpc = MPCController(**args) self.vehicle_interface = vehicle_interface - print(type(vehicle_interface)) + self.counter = 0 def rate(self): return 5.0 diff --git a/GEMstack/state/trajectory.py b/GEMstack/state/trajectory.py index d7a57db00..b6ae31c0b 100644 --- a/GEMstack/state/trajectory.py +++ b/GEMstack/state/trajectory.py @@ -1,309 +1,309 @@ -from __future__ import annotations -from dataclasses import dataclass,replace -from ..utils.serialization import register -from ..mathutils import transforms,collisions -from .physical_object import ObjectFrameEnum, convert_point -import math -from typing import List,Tuple,Optional,Union - -@dataclass -@register -class Path: - """An untimed, piecewise linear path.""" - frame : ObjectFrameEnum - points : List[List[float]] - - def to_frame(self, frame : ObjectFrameEnum, current_pose = None, start_pose_abs = None) -> Path: - """Converts the route to a different frame.""" - new_points = [convert_point(p,self.frame,frame,current_pose,start_pose_abs) for p in self.points] - return replace(self,frame=frame,points=new_points) - - def domain(self) -> Tuple[float,float]: - """Returns the parameter domain""" - return (0.0,len(self.points)-1) - - def parameter_to_index(self, t : float) -> Tuple[int,float]: - """Converts a path parameter to an (edge index, edge parameter) tuple.""" - if len(self.points) < 2: - return 0,0.0 - ind = int(math.floor(t)) #truncate toward zero - if ind < 0: ind = 0 - if ind >= len(self.points)-1: ind = len(self.points)-2 - u = t - ind - if u > 1: u = 1 - if u < 0: u = 0 - return ind,u - - def eval(self, u : float) -> List[float]: - """Evaluates the path at a given parameter. The integer part of u - indicates the segment, and the fractional part indicates the progress - along the segment""" - if len(self.points) < 2: - return self.points[0] - ind,u = self.parameter_to_index(u) - p1 = self.points[ind] - p2 = self.points[ind+1] - return transforms.vector_madd(p1,transforms.vector_sub(p2,p1),u) - - def eval_derivative(self, u : float) -> List[float]: - """Evaluates the derivative at a given parameter. The integer part of - u indicates the segment, and the fractional part indicates the progress - along the segment.""" - ind = int(u) - if ind < 0: ind = 0 - if ind >= len(self.points)-1: ind = len(self.points)-2 - u = u - ind - if u > 1: u = 1 - if u < 0: u = 0 - p1 = self.points[ind] - p2 = self.points[ind+1] - return transforms.vector_sub(p2,p1) - - def length(self): - """Returns the length of the path.""" - l = 0.0 - for i in range(len(self.points)-1): - l += transforms.vector_dist(self.points[i],self.points[i+1]) - return l - - def arc_length_parameterize(self, speed = 1.0) -> Trajectory: - """Returns a new path that is parameterized by arc length.""" - times = [0.0] - points = [self.points[0]] - for i in range(len(self.points)-1): - p1 = self.points[i] - p2 = self.points[i+1] - d = transforms.vector_dist(p1,p2) - if d > 0: - points.append(p2) - times.append(times[-1] + d/speed) - return Trajectory(frame=self.frame,points=points,times=times) - - def closest_point(self, x : List[float], edges = True) -> Tuple[float,float]: - """Returns the closest point on the path to the given point. If - edges=False, only computes the distances to the vertices, not the - edges. This is slightly faster but less accurate. - - Returns (distance, closest_parameter) - """ - best_dist = float('inf') - best_point = None - for i,p in enumerate(self.points): - if edges and i > 0: - p1 = self.points[i-1] - p2 = p - dist,u = transforms.point_segment_distance(x,p1,p2) - if dist < best_dist: - best_dist = dist - best_point = i-1+u - else: - dist = transforms.vector_dist(p,x) - if dist < best_dist: - best_dist = dist - best_point = i - return best_dist,best_point - - def closest_point_local(self, x : List[float], param_range=Tuple[float,float], edges = True) -> Tuple[float,float]: - """Returns the closest point on the path to the given point within - the given parameter range. - - If edges=False, only computes the distances to the vertices, not the - edges. This is slightly faster but less accurate. - - Returns (distance, closest_parameter) - """ - best_dist = float('inf') - param_range = [max(param_range[0],0),min(param_range[1],len(self.points))] - imin = int(math.floor(param_range[0])) - imax = int(math.floor(param_range[1])) - if imax == len(self.points): - imax -= 1 - - umin = param_range[0] - imin - umax = param_range[1] - imax - best_point = None - for i in range(imin,imax+1): - p = self.points[i] - if edges and i > 0: - p1 = self.points[i-1] - p2 = p - dist,u = transforms.point_segment_distance(x,p1,p2) - if dist < best_dist: - best_dist = dist - best_point = i-1+u - else: - dist = transforms.vector_dist(p,x) - if dist < best_dist: - best_dist = dist - best_point = i - return best_dist,best_point - - def get_dims(self, dims : List[int]) -> Path: - """Returns a new path with only the given dimensions.""" - return replace(self,points=[[p[d] for d in dims] for p in self.points]) - - def append_dim(self, value : Union[float,List[float]] = 0.0) -> None: - """Appends a dimension to every point. If value is a list, then it - must be the same length as the number of points. Otherwise, the value - is appended to every point.""" - if isinstance(value,(int,float)): - for p in self.points: - p.append(value) - else: - if len(self.points) != len(value): - raise ValueError("Invalid length of values to append") - for p,v in zip(self.points,value): - p.append(v) - - def trim(self, start : float, end : float) -> Path: - """Returns a copy of this path but trimmed to the given parameter range.""" - sind,su = self.parameter_to_index(start) - eind,eu = self.parameter_to_index(end) - s = self.eval(start) - e = self.eval(end) - return replace(self,points=[s]+self.points[sind+1:eind]+[e]) - - -@dataclass -@register -class Trajectory(Path): - """A timed, piecewise linear path.""" - times : List[float] - - def domain(self) -> Tuple[float,float]: - """Returns the time parameter domain""" - return (self.times[0],self.times[-1]) - - def time_to_index(self, t : float) -> Tuple[int,float]: - """Converts a time to an (edge index, edge parameter) tuple.""" - if len(self.points) < 2: - return 0,0.0 - ind = 0 - while ind < len(self.times) and self.times[ind] < t: - ind += 1 - if ind == 0: return 0,0.0 - if ind >= len(self.times): return len(self.points)-2,1.0 - u = (t - self.times[ind-1])/(self.times[ind] - self.times[ind-1]) - return ind-1,u - - def time_to_parameter(self, t : float) -> float: - """Converts a time to a parameter.""" - ind,u = self.time_to_index(t) - return ind+u - - def parameter_to_time(self, u : float) -> float: - """Converts a parameter to a time""" - if len(self.points) < 2: - return self.times[0] - ind = int(math.floor(u)) - if ind < 0: ind = 0 - if ind >= len(self.times)-1: ind = len(self.times)-2 - u = u - ind - if u > 1: u = 1 - if u < 0: u = 0 - return self.times[ind] + u*(self.times[ind+1]-self.times[ind]) - - def eval(self, t : float) -> List[float]: - """Evaluates the trajectory at a given time.""" - if len(self.points) < 2: - return self.points[0] - ind,u = self.time_to_index(t) - return transforms.vector_madd(self.points[ind],transforms.vector_sub(self.points[ind+1],self.points[ind]),u) - - def eval_derivative(self, t : float) -> List[float]: - """Evaluates the derivative (velocity) at a given time.""" - if len(self.points) < 2: - return transforms.vector_mul(self.points[0],0.0) - ind,u = self.time_to_index(t) - return transforms.vector_mul(transforms.vector_sub(self.points[ind+1],self.points[ind]),1.0/(self.times[ind+1]-self.times[ind])) - - def eval_tangent(self, t : float) -> List[float]: - """Evaluates the tangent of the curve at a given time. This is related - to the velocity but normalized; at cusp points the previous tangent (or - next tangent, if the point is at the start) is returned. """ - if len(self.points) < 2: - raise ValueError("Trajectory has no tangent: only 1 point") - ind,u = self.time_to_index(t) - d = transforms.vector_sub(self.points[ind+1],self.points[ind]) - l = transforms.vector_norm(d) - pos = (ind == 0) - while l == 0: - if ind == 0: - if not pos: - raise ValueError("Trajectory has no tangent: all points are coincident") - ind+=1 - else: - if pos: - raise ValueError("Trajectory has no tangent: all points are coincident") - ind-=1 - d = transforms.vector_sub(self.points[ind+1],self.points[ind]) - l = transforms.vector_norm(d) - return transforms.vector_mul(d,1.0/l) - - def closest_point(self, x : List[float], edges = True) -> Tuple[float,float]: - """Returns the closest point on the path to the given point. If - edges=False, only computes the distances to the vertices, not the - edges. This is slightly faster but less accurate. - - Returns (distance, closest_time) - """ - distance, closest_index = Path.closest_point(self,x,edges) - closest_time = self.parameter_to_time(closest_index) - return distance, closest_time - - def closest_point_local(self, x : List[float], time_range=Tuple[float,float], edges = True) -> Tuple[float,float]: - """Returns the closest point on the path to the given point within - the given time range. - - If edges=False, only computes the distances to the vertices, not the - edges. This is slightly faster but less accurate. - - Returns (distance, closest_time) - """ - param_range = [self.time_to_parameter(time_range[0]),self.time_to_parameter(time_range[1])] - #print("Searching within time range",time_range,"= param range",param_range) - distance, closest_index = Path.closest_point_local(self,x,param_range,edges) - closest_time = self.parameter_to_time(closest_index) - return distance, closest_time - - def trim(self, start : float, end : float) -> Trajectory: - """Returns a copy of this trajectory but trimmed to the given time range.""" - sind,su = self.time_to_index(start) - eind,eu = self.time_to_index(end) - s = self.eval(start) - e = self.eval(end) - return replace(self,points=[s]+self.points[sind+1:eind]+[e],times=[start]+self.times[sind+1:eind]+[end]) - - - - -def compute_headings(path : Path, smoothed = False) -> Path: - """Converts a 2D (x,y) path into a 3D path (x,y,heading) or a 3D - (x,y,z) path into a 5D path (x,y,z,heading,pitch). - - If smoothed=True, then the path is smoothed using a spline to better - estimate good tangent vectors. - """ - if smoothed: - raise NotImplementedError("Smoothing not done yet") - if len(path.points) < 2: - raise ValueError("Path must have at least 2 points") - derivs = [] - derivs.append(transforms.vector_sub(path.points[1],path.points[0])) - for i in range(1,len(path.points)-1): - derivs.append(transforms.vector_sub(path.points[i+1],path.points[i-1])) - derivs[-1] = transforms.vector_sub(path.points[-1],path.points[-2]) - nd = len(path.points[0])-1 - coords = [] - for d in derivs: - if transforms.vector2_dist(d,[0,0]) < 1e-6: - coords.append([0]*nd) - else: - dunit = transforms.normalize_vector(d) - if nd == 1: - coords.append((transforms.vector2_angle(dunit,[1,0]),)) - elif nd == 2: - azimuth = transforms.vector2_angle(dunit[:2],[1,0]) - elevation = transforms.vector2_angle((dunit[2],transforms.vector_norm(dunit[:2])),[0,0,1]) - coords.append((azimuth,elevation)) - return replace(path,points=[tuple(p)+tuple(c) for p,c in zip(path.points,coords)]) +from __future__ import annotations +from dataclasses import dataclass,replace +from ..utils.serialization import register +from ..mathutils import transforms,collisions +from .physical_object import ObjectFrameEnum, convert_point +import math +from typing import List,Tuple,Optional,Union + +@dataclass +@register +class Path: + """An untimed, piecewise linear path.""" + frame : ObjectFrameEnum + points : List[List[float]] + + def to_frame(self, frame : ObjectFrameEnum, current_pose = None, start_pose_abs = None) -> Path: + """Converts the route to a different frame.""" + new_points = [convert_point(p,self.frame,frame,current_pose,start_pose_abs) for p in self.points] + return replace(self,frame=frame,points=new_points) + + def domain(self) -> Tuple[float,float]: + """Returns the parameter domain""" + return (0.0,len(self.points)-1) + + def parameter_to_index(self, t : float) -> Tuple[int,float]: + """Converts a path parameter to an (edge index, edge parameter) tuple.""" + if len(self.points) < 2: + return 0,0.0 + ind = int(math.floor(t)) #truncate toward zero + if ind < 0: ind = 0 + if ind >= len(self.points)-1: ind = len(self.points)-2 + u = t - ind + if u > 1: u = 1 + if u < 0: u = 0 + return ind,u + + def eval(self, u : float) -> List[float]: + """Evaluates the path at a given parameter. The integer part of u + indicates the segment, and the fractional part indicates the progress + along the segment""" + if len(self.points) < 2: + return self.points[0] + ind,u = self.parameter_to_index(u) + p1 = self.points[ind] + p2 = self.points[ind+1] + return transforms.vector_madd(p1,transforms.vector_sub(p2,p1),u) + + def eval_derivative(self, u : float) -> List[float]: + """Evaluates the derivative at a given parameter. The integer part of + u indicates the segment, and the fractional part indicates the progress + along the segment.""" + ind = int(u) + if ind < 0: ind = 0 + if ind >= len(self.points)-1: ind = len(self.points)-2 + u = u - ind + if u > 1: u = 1 + if u < 0: u = 0 + p1 = self.points[ind] + p2 = self.points[ind+1] + return transforms.vector_sub(p2,p1) + + def length(self): + """Returns the length of the path.""" + l = 0.0 + for i in range(len(self.points)-1): + l += transforms.vector_dist(self.points[i],self.points[i+1]) + return l + + def arc_length_parameterize(self, speed = 1.0) -> Trajectory: + """Returns a new path that is parameterized by arc length.""" + times = [0.0] + points = [self.points[0]] + for i in range(len(self.points)-1): + p1 = self.points[i] + p2 = self.points[i+1] + d = transforms.vector_dist(p1,p2) + if d > 0: + points.append(p2) + times.append(times[-1] + d/speed) + return Trajectory(frame=self.frame,points=points,times=times) + + def closest_point(self, x : List[float], edges = True) -> Tuple[float,float]: + """Returns the closest point on the path to the given point. If + edges=False, only computes the distances to the vertices, not the + edges. This is slightly faster but less accurate. + + Returns (distance, closest_parameter) + """ + best_dist = float('inf') + best_point = None + for i,p in enumerate(self.points): + if edges and i > 0: + p1 = self.points[i-1] + p2 = p + dist,u = transforms.point_segment_distance(x,p1,p2) + if dist < best_dist: + best_dist = dist + best_point = i-1+u + else: + dist = transforms.vector_dist(p,x) + if dist < best_dist: + best_dist = dist + best_point = i + return best_dist,best_point + + def closest_point_local(self, x : List[float], param_range=Tuple[float,float], edges = True) -> Tuple[float,float]: + """Returns the closest point on the path to the given point within + the given parameter range. + + If edges=False, only computes the distances to the vertices, not the + edges. This is slightly faster but less accurate. + + Returns (distance, closest_parameter) + """ + best_dist = float('inf') + param_range = [max(param_range[0],0),min(param_range[1],len(self.points))] + imin = int(math.floor(param_range[0])) + imax = int(math.floor(param_range[1])) + if imax == len(self.points): + imax -= 1 + + umin = param_range[0] - imin + umax = param_range[1] - imax + best_point = None + for i in range(imin,imax+1): + p = self.points[i] + if edges and i > 0: + p1 = self.points[i-1] + p2 = p + dist,u = transforms.point_segment_distance(x,p1,p2) + if dist < best_dist: + best_dist = dist + best_point = i-1+u + else: + dist = transforms.vector_dist(p,x) + if dist < best_dist: + best_dist = dist + best_point = i + return best_dist,best_point + + def get_dims(self, dims : List[int]) -> Path: + """Returns a new path with only the given dimensions.""" + return replace(self,points=[[p[d] for d in dims] for p in self.points]) + + def append_dim(self, value : Union[float,List[float]] = 0.0) -> None: + """Appends a dimension to every point. If value is a list, then it + must be the same length as the number of points. Otherwise, the value + is appended to every point.""" + if isinstance(value,(int,float)): + for p in self.points: + p.append(value) + else: + if len(self.points) != len(value): + raise ValueError("Invalid length of values to append") + for p,v in zip(self.points,value): + p.append(v) + + def trim(self, start : float, end : float) -> Path: + """Returns a copy of this path but trimmed to the given parameter range.""" + sind,su = self.parameter_to_index(start) + eind,eu = self.parameter_to_index(end) + s = self.eval(start) + e = self.eval(end) + return replace(self,points=[s]+self.points[sind+1:eind]+[e]) + + +@dataclass +@register +class Trajectory(Path): + """A timed, piecewise linear path.""" + times : List[float] + + def domain(self) -> Tuple[float,float]: + """Returns the time parameter domain""" + return (self.times[0],self.times[-1]) + + def time_to_index(self, t : float) -> Tuple[int,float]: + """Converts a time to an (edge index, edge parameter) tuple.""" + if len(self.points) < 2: + return 0,0.0 + ind = 0 + while ind < len(self.times) and self.times[ind] < t: + ind += 1 + if ind == 0: return 0,0.0 + if ind >= len(self.times): return len(self.points)-2,1.0 + u = (t - self.times[ind-1])/(self.times[ind] - self.times[ind-1]) + return ind-1,u + + def time_to_parameter(self, t : float) -> float: + """Converts a time to a parameter.""" + ind,u = self.time_to_index(t) + return ind+u + + def parameter_to_time(self, u : float) -> float: + """Converts a parameter to a time""" + if len(self.points) < 2: + return self.times[0] + ind = int(math.floor(u)) + if ind < 0: ind = 0 + if ind >= len(self.times)-1: ind = len(self.times)-2 + u = u - ind + if u > 1: u = 1 + if u < 0: u = 0 + return self.times[ind] + u*(self.times[ind+1]-self.times[ind]) + + def eval(self, t : float) -> List[float]: + """Evaluates the trajectory at a given time.""" + if len(self.points) < 2: + return self.points[0] + ind,u = self.time_to_index(t) + return transforms.vector_madd(self.points[ind],transforms.vector_sub(self.points[ind+1],self.points[ind]),u) + + def eval_derivative(self, t : float) -> List[float]: + """Evaluates the derivative (velocity) at a given time.""" + if len(self.points) < 2: + return transforms.vector_mul(self.points[0],0.0) + ind,u = self.time_to_index(t) + return transforms.vector_mul(transforms.vector_sub(self.points[ind+1],self.points[ind]),1.0/(self.times[ind+1]-self.times[ind])) + + def eval_tangent(self, t : float) -> List[float]: + """Evaluates the tangent of the curve at a given time. This is related + to the velocity but normalized; at cusp points the previous tangent (or + next tangent, if the point is at the start) is returned. """ + if len(self.points) < 2: + raise ValueError("Trajectory has no tangent: only 1 point") + ind,u = self.time_to_index(t) + d = transforms.vector_sub(self.points[ind+1],self.points[ind]) + l = transforms.vector_norm(d) + pos = (ind == 0) + while l == 0: + if ind == 0: + if not pos: + raise ValueError("Trajectory has no tangent: all points are coincident") + ind+=1 + else: + if pos: + raise ValueError("Trajectory has no tangent: all points are coincident") + ind-=1 + d = transforms.vector_sub(self.points[ind+1],self.points[ind]) + l = transforms.vector_norm(d) + return transforms.vector_mul(d,1.0/l) + + def closest_point(self, x : List[float], edges = True) -> Tuple[float,float]: + """Returns the closest point on the path to the given point. If + edges=False, only computes the distances to the vertices, not the + edges. This is slightly faster but less accurate. + + Returns (distance, closest_time) + """ + distance, closest_index = Path.closest_point(self,x,edges) + closest_time = self.parameter_to_time(closest_index) + return distance, closest_time + + def closest_point_local(self, x : List[float], time_range=Tuple[float,float], edges = True) -> Tuple[float,float]: + """Returns the closest point on the path to the given point within + the given time range. + + If edges=False, only computes the distances to the vertices, not the + edges. This is slightly faster but less accurate. + + Returns (distance, closest_time) + """ + param_range = [self.time_to_parameter(time_range[0]),self.time_to_parameter(time_range[1])] + #print("Searching within time range",time_range,"= param range",param_range) + distance, closest_index = Path.closest_point_local(self,x,param_range,edges) + closest_time = self.parameter_to_time(closest_index) + return distance, closest_time + + def trim(self, start : float, end : float) -> Trajectory: + """Returns a copy of this trajectory but trimmed to the given time range.""" + sind,su = self.time_to_index(start) + eind,eu = self.time_to_index(end) + s = self.eval(start) + e = self.eval(end) + return replace(self,points=[s]+self.points[sind+1:eind]+[e],times=[start]+self.times[sind+1:eind]+[end]) + + + + +def compute_headings(path : Path, smoothed = False) -> Path: + """Converts a 2D (x,y) path into a 3D path (x,y,heading) or a 3D + (x,y,z) path into a 5D path (x,y,z,heading,pitch). + + If smoothed=True, then the path is smoothed using a spline to better + estimate good tangent vectors. + """ + if smoothed: + raise NotImplementedError("Smoothing not done yet") + if len(path.points) < 2: + raise ValueError("Path must have at least 2 points") + derivs = [] + derivs.append(transforms.vector_sub(path.points[1],path.points[0])) + for i in range(1,len(path.points)-1): + derivs.append(transforms.vector_sub(path.points[i+1],path.points[i-1])) + derivs[-1] = transforms.vector_sub(path.points[-1],path.points[-2]) + nd = len(path.points[0])-1 + coords = [] + for d in derivs: + if transforms.vector2_dist(d,[0,0]) < 1e-6: + coords.append([0]*nd) + else: + dunit = transforms.normalize_vector(d) + if nd == 1: + coords.append((transforms.vector2_angle(dunit,[1,0]),)) + elif nd == 2: + azimuth = transforms.vector2_angle(dunit[:2],[1,0]) + elevation = transforms.vector2_angle((dunit[2],transforms.vector_norm(dunit[:2])),[0,0,1]) + coords.append((azimuth,elevation)) + return replace(path,points=[tuple(p)+tuple(c) for p,c in zip(path.points,coords)]) diff --git a/launch/fixed_route.yaml b/launch/fixed_route.yaml index b773b20fd..507fe48cc 100644 --- a/launch/fixed_route.yaml +++ b/launch/fixed_route.yaml @@ -18,14 +18,22 @@ drive: type: StaticRoutePlanner args: [!relative_path '../GEMstack/knowledge/routes/xyhead_highbay_backlot_p.csv'] # args: [!relative_path '../GEMstack/knowledge/routes/reverse_15m.csv'] + # args: [!relative_path '../GEMstack/knowledge/routes/offset.csv'] + # args: [!relative_path '../GEMstack/knowledge/routes/forward_15m.csv'] + # args: [!relative_path '../GEMstack/knowledge/routes/backward_15m.csv'] + # args: [!relative_path '../GEMstack/knowledge/routes/forward_curve.csv'] + # args: [!relative_path '../GEMstack/knowledge/routes/backward_curve.csv'] + # args: [!relative_path '../GEMstack/knowledge/routes/xyhead_highbay_backlot_oval.csv'] + motion_planning: type: RouteToTrajectoryPlanner args: [null] #desired speed in m/s. If null, this will keep the route untimed for the trajectory tracker trajectory_tracking: # type: pure_pursuit.PurePursuitTrajectoryTracker type: stanley.StanleyTrajectoryTracker - args: {desired_speed: 5.0} #approximately 5mph + args: {desired_speed: 2.5} #approximately 5mph print: False + # type: mpc.MPCTrajectoryTracker log: # Specify the top-level folder to save the log files. Default is 'logs' #folder : 'logs' @@ -40,7 +48,7 @@ log: # If True, then record all readings / commands of the vehicle interface. Default False vehicle_interface : True # Specify which components to record to behavior.json. Default records nothing - components : ['state_estimation','trajectory_tracking'] + components : ['state_estimation'] # Specify which components of state to record to state.json. Default records nothing #state: ['all'] # Specify the rate in Hz at which to record state to state.json. Default records at the pipeline's rate diff --git a/scenes/xyhead_demo.yaml b/scenes/xyhead_demo.yaml index c6d97477a..6e83a0a85 100644 --- a/scenes/xyhead_demo.yaml +++ b/scenes/xyhead_demo.yaml @@ -3,7 +3,7 @@ agents: ped1: type: pedestrian position: [15.0, 2.0] - nominal_velocity: 0.5 + nominal_velocity: -0.5 target: [15.0,10.0] behavior: loop \ No newline at end of file From 0d7b5e23d947086207087031bf7f0060413ab1cb Mon Sep 17 00:00:00 2001 From: Jugal Date: Wed, 30 Apr 2025 14:32:28 -0500 Subject: [PATCH 34/71] Add files via upload For reverse testing --- GEMstack/knowledge/routes/yamada.csv | 266 +++++++++++++++++++++++++++ 1 file changed, 266 insertions(+) create mode 100644 GEMstack/knowledge/routes/yamada.csv diff --git a/GEMstack/knowledge/routes/yamada.csv b/GEMstack/knowledge/routes/yamada.csv new file mode 100644 index 000000000..a2196cb60 --- /dev/null +++ b/GEMstack/knowledge/routes/yamada.csv @@ -0,0 +1,266 @@ +0.0,0.0,0.0 +0.1,5.5511151231257715e-18,0.0 +0.2,1.1102230246251555e-17,0.0 +0.30000000000000004,1.6653345369377338e-17,0.0 +0.4,2.220446049250312e-17,0.0 +0.5,2.77555756156289e-17,0.0 +0.6,3.330669073875468e-17,0.0 +0.7,3.885780586188047e-17,0.0 +0.7999999999999999,4.4408920985006246e-17,0.0 +0.8999999999999999,4.996003610813203e-17,0.0 +1.0,5.5511151231257815e-17,0.0 +1.0999999999999999,6.106226635438359e-17,0.0 +1.2,6.661338147750938e-17,0.0 +1.3,7.216449660063518e-17,0.0 +1.4000000000000001,7.771561172376095e-17,0.0 +1.5000000000000002,8.326672684688674e-17,0.0 +1.6000000000000003,8.881784197001253e-17,0.0 +1.7000000000000004,9.436895709313832e-17,0.0 +1.8000000000000005,9.99200722162641e-17,0.0 +1.9000000000000001,1.0547118733938988e-16,0.0 +2.0000000000000004,1.1102230246251568e-16,0.0 +2.1000000000000005,1.1657341758564147e-16,0.0 +2.2,1.2212453270876723e-16,0.0 +2.3000000000000003,1.2767564783189302e-16,0.0 +2.400000000000001,1.3322676295501883e-16,0.0 +2.500000000000001,1.3877787807814462e-16,0.0 +2.600000000000001,1.443289932012704e-16,0.0 +2.7000000000000006,1.4988010832439617e-16,0.0 +2.8000000000000007,1.5543122344752196e-16,0.0 +2.9000000000000012,1.6098233857064777e-16,0.0 +3.0000000000000013,1.6653345369377356e-16,0.0 +3.1000000000000014,1.7208456881689934e-16,0.0 +3.200000000000001,1.776356839400251e-16,0.0 +3.300000000000001,1.831867990631509e-16,0.0 +3.4000000000000017,1.887379141862767e-16,0.0 +3.5000000000000018,1.942890293094025e-16,0.0 +3.6000000000000014,1.9984014443252826e-16,0.0 +3.7000000000000024,2.053912595556541e-16,0.0 +3.8000000000000016,2.1094237467877983e-16,0.0 +3.900000000000002,2.1649348980190564e-16,0.0 +4.000000000000002,2.220446049250314e-16,0.0 +4.100000000000001,2.2759572004815717e-16,0.0 +4.200000000000001,2.3314683517128293e-16,0.0 +4.300000000000001,2.386979502944087e-16,0.0 +4.4,2.4424906541753446e-16,0.0 +4.5,2.498001805406602e-16,0.0 +4.6,2.55351295663786e-16,0.0 +4.699999999999999,2.6090241078691175e-16,0.0 +4.799999999999999,2.664535259100375e-16,0.0 +4.899999999999999,2.7200464103316327e-16,0.0 +4.999999999999998,2.7755575615628904e-16,0.0 +5.099999999999998,2.831068712794148e-16,0.0 +5.1999999999999975,2.8865798640254056e-16,0.0 +5.299999999999997,2.9420910152566633e-16,0.0 +5.399999999999997,2.997602166487921e-16,0.0 +5.4999999999999964,3.0531133177191785e-16,0.0 +5.599999999999996,3.108624468950436e-16,0.0 +5.699999999999996,3.164135620181694e-16,0.0 +5.799999999999995,3.2196467714129514e-16,0.0 +5.899999999999995,3.275157922644209e-16,0.0 +5.999999999999995,3.3306690738754667e-16,0.0 +6.099999999999994,3.3861802251067243e-16,0.0 +6.199999999999994,3.441691376337982e-16,0.0 +6.299999999999994,3.4972025275692396e-16,0.0 +6.399999999999993,3.552713678800497e-16,0.0 +6.499999999999993,3.608224830031755e-16,0.0 +6.5999999999999925,3.6637359812630124e-16,0.0 +6.699999999999992,3.71924713249427e-16,0.0 +6.799999999999992,3.7747582837255277e-16,0.0 +6.8999999999999915,3.8302694349567853e-16,0.0 +6.999999999999991,3.885780586188043e-16,0.0 +7.099999999999991,3.9412917374193006e-16,0.0 +7.19999999999999,3.996802888650558e-16,0.0 +7.29999999999999,4.052314039881816e-16,0.0 +7.39999999999999,4.1078251911130735e-16,0.0 +7.4999999999999885,4.1633363423443306e-16,0.0 +7.599999999999989,4.2188474935755887e-16,0.0 +7.6999999999999895,4.274358644806847e-16,0.0 +7.799999999999988,4.329869796038104e-16,0.0 +7.899999999999988,4.3853809472693616e-16,0.0 +7.999999999999987,4.440892098500619e-16,0.0 +8.099999999999987,4.496403249731877e-16,0.0 +8.199999999999987,4.551914400963135e-16,0.0 +8.299999999999986,4.607425552194392e-16,0.0 +8.399999999999986,4.66293670342565e-16,0.0 +8.499999999999986,4.718447854656907e-16,0.0 +8.599999999999985,4.773959005888165e-16,0.0 +8.699999999999985,4.829470157119423e-16,0.0 +8.799999999999985,4.88498130835068e-16,0.0 +8.899999999999984,4.940492459581938e-16,0.0 +8.999999999999984,4.996003610813196e-16,0.0 +9.099999999999984,5.051514762044453e-16,0.0 +9.199999999999983,5.107025913275711e-16,0.0 +9.299999999999983,5.162537064506968e-16,0.0 +9.399999999999983,5.218048215738226e-16,0.0 +9.499999999999982,5.273559366969484e-16,0.0 +9.599999999999982,5.329070518200741e-16,0.0 +9.699999999999982,5.384581669431999e-16,0.0 +9.799999999999981,5.440092820663257e-16,0.0 +9.89999999999998,5.495603971894514e-16,0.0 +9.99999999999998,5.551115123125772e-16,0.0 +10.09999999999998,5.606626274357029e-16,0.0 +10.19999999999998,5.662137425588287e-16,0.0 +10.29999999999998,5.717648576819545e-16,0.0 +10.399999999999979,5.773159728050802e-16,0.0 +10.499999999999979,5.82867087928206e-16,0.0 +10.599999999999978,5.884182030513318e-16,0.0 +10.699999999999978,5.939693181744575e-16,0.0 +10.799999999999978,5.995204332975833e-16,0.0 +10.899999999999977,6.050715484207091e-16,0.0 +10.999999999999977,6.106226635438348e-16,0.0 +11.099999999999977,6.161737786669606e-16,0.0 +11.199999999999976,6.217248937900863e-16,0.0 +11.299999999999976,6.272760089132121e-16,0.0 +11.399999999999975,6.328271240363379e-16,0.0 +11.499999999999975,6.383782391594636e-16,0.0 +11.599999999999975,6.439293542825894e-16,0.0 +11.699999999999974,6.494804694057152e-16,0.0 +11.799999999999974,6.550315845288409e-16,0.0 +11.899999999999974,6.605826996519667e-16,0.0 +11.999999999999973,6.661338147750924e-16,0.0 +12.099999999999973,6.716849298982182e-16,0.0 +12.199999999999973,6.77236045021344e-16,0.0 +12.299999999999972,6.827871601444697e-16,0.0 +12.399999999999972,6.883382752675955e-16,0.0 +12.499999999999972,6.938893903907213e-16,0.0 +12.599999999999971,6.99440505513847e-16,0.0 +12.69999999999997,7.049916206369728e-16,0.0 +12.79999999999997,7.105427357600985e-16,0.0 +12.89999999999997,7.160938508832243e-16,0.0 +12.99999999999997,7.216449660063501e-16,0.0 +13.09999999999997,7.271960811294758e-16,0.0 +13.199999999999969,7.327471962526016e-16,0.0 +13.299999999999969,7.382983113757274e-16,0.0 +13.399999999999968,7.438494264988531e-16,0.0 +13.499999999999968,7.494005416219789e-16,0.0 +13.599999999999968,7.549516567451047e-16,0.0 +13.699999999999967,7.605027718682304e-16,0.0 +13.799999999999967,7.660538869913562e-16,0.0 +13.899999999999967,7.716050021144819e-16,0.0 +13.999999999999966,7.771561172376077e-16,0.0 +14.099999999999966,7.827072323607335e-16,0.0 +14.199999999999966,7.882583474838592e-16,0.0 +14.299999999999965,7.93809462606985e-16,0.0 +14.399999999999965,7.993605777301108e-16,0.0 +14.499999999999964,8.049116928532365e-16,0.0 +14.599999999999964,8.104628079763623e-16,0.0 +14.699999999999964,8.16013923099488e-16,0.0 +14.799999999999963,8.215650382226138e-16,0.0 +14.899999999999965,8.271161533457397e-16,0.0 +14.999999999999961,8.326672684688652e-16,0.0 +15.099999999999962,8.382183835919911e-16,0.0 +15.199999999999962,8.437694987151169e-16,0.0 +15.299999999999962,8.493206138382426e-16,0.0 +15.399999999999963,8.548717289613685e-16,0.0 +15.499999999999963,8.604228440844942e-16,0.0 +15.599999999999959,8.659739592076198e-16,0.0 +15.69999999999996,8.715250743307457e-16,0.0 +15.79999999999996,8.770761894538714e-16,0.0 +15.89999999999996,8.826273045769972e-16,0.0 +15.999999999999961,8.881784197001231e-16,0.0 +16.09999999999996,8.937295348232487e-16,0.0 +16.19999999999996,8.992806499463746e-16,0.0 +16.29999999999996,9.048317650695005e-16,0.0 +16.399999999999963,9.103828801926263e-16,0.0 +16.499999999999964,9.159339953157522e-16,0.0 +16.599999999999966,9.21485110438878e-16,0.0 +16.699999999999967,9.270362255620039e-16,0.0 +16.79999999999997,9.325873406851298e-16,0.0 +16.89999999999997,9.381384558082556e-16,0.0 +16.99999999999997,9.436895709313815e-16,0.0 +17.099999999999973,9.492406860545073e-16,0.0 +17.199999999999974,9.547918011776332e-16,0.0 +17.299999999999976,9.60342916300759e-16,0.0 +17.399999999999977,9.65894031423885e-16,0.0 +17.49999999999998,9.714451465470108e-16,0.0 +17.59999999999998,9.769962616701367e-16,0.0 +17.69999999999998,9.825473767932625e-16,0.0 +17.799999999999983,9.880984919163884e-16,0.0 +17.899999999999984,9.936496070395142e-16,0.0 +17.999999999999986,9.9920072216264e-16,0.0 +18.099999999999987,1.004751837285766e-15,0.0 +18.19999999999999,1.0103029524088918e-15,0.0 +18.29999999999999,1.0158540675320177e-15,0.0 +18.39999999999999,1.0214051826551435e-15,0.0 +18.499999999999993,1.0269562977782694e-15,0.0 +18.599999999999994,1.0325074129013953e-15,0.0 +18.699999999999996,1.0380585280245211e-15,0.0 +18.799999999999997,1.043609643147647e-15,0.0 +18.9,1.0491607582707729e-15,0.0 +19.0,1.0547118733938987e-15,0.0 +19.1,1.0602629885170246e-15,0.0 +19.200000000000003,1.0658141036401504e-15,0.0 +19.300000000000004,1.0713652187632763e-15,0.0 +19.400000000000006,1.0769163338864022e-15,0.0 +19.500000000000007,1.082467449009528e-15,0.0 +19.60000000000001,1.0880185641326539e-15,0.0 +19.70000000000001,1.0935696792557797e-15,0.0 +19.80000000000001,1.0991207943789056e-15,0.0 +19.900000000000013,1.1046719095020315e-15,0.0 +20.000000000000014,1.1102230246251573e-15,0.0 +20.100000000000016,1.1157741397482832e-15,0.0 +20.200000000000017,1.121325254871409e-15,0.0 +20.30000000000002,0.0,0.0 +20.399987538147936,0.0013671557150166803,0.0 +20.499900316364652,0.0054676006480075985,0.0 +20.599663630616394,0.012298268927097164,0.0 +20.52616230766001,0.004930129155760037,0.0 +20.426921530655086,-0.007343626548595887,0.0 +20.32805343623484,-0.022326176576537883,0.0 +20.229631947330805,-0.04000631858795244,0.0 +20.13173065295093,-0.060370833273464486,0.0 +20.034422753157635,-0.08340449423841373,0.0 +19.93778100433665,-0.10909007938751655,0.0 +19.84187766479763,-0.1374083838016975,0.0 +19.746784440747163,-0.1683382340974619,0.0 +19.652572432674567,-0.20185650425807727,0.0 +19.559312082190594,-0.23793813292471971,0.0 +19.46707311935877,-0.2765561421346672,0.0 +19.375924510558733,-0.3176816574925169,0.0 +19.285934406920614,-0.36128392975935425,0.0 +19.197170093368932,-0.4073303578437292,0.0 +19.109697938314167,-0.4557865131772457,0.0 +19.023583344029596,-0.5066161654565449,0.0 +18.93889069775051,-0.5597813097324302,0.0 +18.855683323532343,-0.615242194825882,0.0 +18.774023434903786,-0.6729573530497173,0.0 +18.693972088350165,-0.7328836312136658,0.0 +18.615589137661985,-0.7949762228896875,0.0 +18.538933189182693,-0.8591887019134016,0.0 +18.464061557989158,-0.9254730570965805,0.0 +18.391030225037614,-0.9937797281247577,0.0 +18.31989379530713,-1.0640576426130972,0.0 +18.25070545697186,-1.1362542542928378,0.0 +18.183516941632668,-1.2103155822997387,0.0 +18.11681825596512,-1.2848189821117841,0.0 +18.048140850868222,-1.3575017925196606,0.0 +17.97750187000663,-1.4282796936079698,0.0 +17.90495412961572,-1.4970997652717235,0.0 +17.830551873096038,-1.5639105512602882,0.0 +17.754350730455897,-1.6286620976508384,0.0 +17.676407676717258,-1.6913059901985248,0.0 +17.596780989315917,-1.7517953905354469,0.0 +17.51553020452793,-1.8100850711913459,0.0 +17.432716072954754,-1.8661314494098449,0.0 +17.348400514100494,-1.9198926197349477,0.0 +17.262646570075166,-1.9713283853434362,0.0 +17.175518358458564,-2.0204002880997307,0.0 +17.08708102436004,-2.067071637310749,0.0 +16.997400691709995,-2.111307537159263,0.0 +16.906544413819503,-2.1530749127952387,0.0 +16.814580123245072,-2.1923425350656536,0.0 +16.72157658099596,-2.2290810438643,0.0 +16.6276033251221,-2.263262970084114,0.0 +16.532730618721022,-2.2948627561556223,0.0 +16.43702939740265,-2.323856775156145,0.0 +16.340571216251295,-2.350223348475467,0.0 +16.243428196324448,-2.3739427620247726,0.0 +16.14567297072841,-2.3949972809767224,0.0 +16.047378630311055,-2.4133711630256496,0.0 +15.948618669012365,-2.4290506701579635,0.0 +15.84946692891356,-2.4420240789239593,0.0 +15.74999754502592,-2.452281689203354,0.0 +15.824349913856949,-2.4469683510106868,0.0 +15.92423128589668,-2.442163310002374,0.0 +16.02420669425922,-2.440090962933454,0.0 From 9cd3d534e664a7d8c015deb3d3b08291b5920be2 Mon Sep 17 00:00:00 2001 From: hhxjqm <67122725+hhxjqm@users.noreply.github.com> Date: Sun, 4 May 2025 12:52:23 -0500 Subject: [PATCH 35/71] Update stanley.py --- GEMstack/onboard/planning/stanley.py | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley.py index 6c1b5a445..14fb4d132 100644 --- a/GEMstack/onboard/planning/stanley.py +++ b/GEMstack/onboard/planning/stanley.py @@ -303,7 +303,8 @@ def state_inputs(self): def state_outputs(self): return [] - + + """ def _check_sharp_turn_ahead(self, lookahead_s=3.0, threshold_angle=np.pi/2.0, num_steps=4): if not self.stanley.path: return False @@ -359,7 +360,7 @@ def _check_sharp_turn_ahead(self, lookahead_s=3.0, threshold_angle=np.pi/2.0, nu angle_prev = angle_next s_prev = s_next return False - + """ def update(self, vehicle: VehicleState, trajectory: Trajectory): self.stanley.set_path(trajectory) @@ -384,15 +385,19 @@ def update(self, vehicle: VehicleState, trajectory: Trajectory): self.stanley.pid_speed.reset() self.stanley.current_path_parameter = self.stanley.path.domain()[0] - else: + else: + """ is_sharp_turn_ahead = self._check_sharp_turn_ahead( lookahead_s=4.0, threshold_angle=np.pi/2.0 ) - print(f"Sharp turn ahead: {is_sharp_turn_ahead}") + print(f"Sharp turn ahead: {is_sharp_turn_ahead}") + if is_sharp_turn_ahead: self.reverse = not self.reverse - self.stanley.set_path(trajectory) + self.stanley.set_path(trajectory) + """ + pass accel, f_delta = self.stanley.compute(vehicle, self, self.reverse) steering_angle = front2steer(f_delta) From 9644454b43d0dcc3ac95513b60a117c4d15ef38f Mon Sep 17 00:00:00 2001 From: hhxjqm Date: Mon, 5 May 2025 19:14:03 +0000 Subject: [PATCH 36/71] fix reverse in stanley.py --- GEMstack/onboard/planning/stanley.py | 160 ++++++++++----------------- 1 file changed, 59 insertions(+), 101 deletions(-) diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley.py index 14fb4d132..f175e7706 100644 --- a/GEMstack/onboard/planning/stanley.py +++ b/GEMstack/onboard/planning/stanley.py @@ -70,6 +70,7 @@ def __init__( self.current_path_parameter = 0.0 self.current_traj_parameter = 0.0 self.t_last = None + self.reverse = None def set_path(self, path: Path): if path == self.path_arg: @@ -104,8 +105,47 @@ def _find_rear_axle_position(self, x, y, yaw): ry = y - self.wheelbase * sin(yaw) return rx, ry - def compute(self, state: VehicleState, component: Component = None, reverse = False): - """Compute the control outputs: (longitudinal acceleration, front wheel angle).""" + def initialize_state_and_direction(self, state: VehicleState): + if self.path is None: + raise ValueError("Stanley: Path must be set before initializing state and direction.") + curr_x = state.pose.x + curr_y = state.pose.y + curr_yaw = state.pose.yaw if state.pose.yaw is not None else 0.0 + fx, fy = self._find_front_axle_position(curr_x, curr_y, curr_yaw) + path_domain_start, path_domain_end = self.path.domain() + search_end = min(path_domain_end, path_domain_start + 5.0) + search_domain_start = [path_domain_start, search_end] + _, closest_param_at_start = self.path.closest_point_local((fx, fy), search_domain_start) + tangent_at_start = self.path.eval_tangent(closest_param_at_start) + initial_reverse = False + if np.linalg.norm(tangent_at_start) > 1e-6: + path_yaw_at_start = atan2(tangent_at_start[1], tangent_at_start[0]) + heading_diff = normalise_angle(path_yaw_at_start - curr_yaw) + initial_reverse = abs(heading_diff) > (np.pi / 2.0) + self.pid_speed.reset() + self.current_path_parameter = closest_param_at_start + if self.trajectory: + self.current_traj_parameter = self.trajectory.domain()[0] + return initial_reverse + + def is_target_behind_vehicle(self, vehicle_pose, target_point_coords): + curr_x = vehicle_pose.x + curr_y = vehicle_pose.y + curr_yaw = vehicle_pose.yaw + if curr_yaw is None: + return False + target_x = target_point_coords[0] + target_y = target_point_coords[1] + vec_x = target_x - curr_x + vec_y = target_y - curr_y + if abs(vec_x) < 1e-6 and abs(vec_y) < 1e-6: + return False + heading_x = cos(curr_yaw) + heading_y = sin(curr_yaw) + dot_product = vec_x * heading_x + vec_y * heading_y + return (dot_product < 0) + + def compute(self, state: VehicleState, component: Component = None): t = state.pose.t if self.t_last is None: self.t_last = t @@ -135,11 +175,15 @@ def compute(self, state: VehicleState, component: Component = None, reverse = Fa component.debug_event(f"Transforming trajectory from {self.trajectory.frame.name} to {state.pose.frame.name}") self.trajectory = transforms.transform_path(self.trajectory, self.trajectory.frame, state.pose.frame, current_pose=state.pose) - if reverse: + + if self.reverse is None: + self.reverse = self.initialize_state_and_direction(state) + + if self.reverse: fx, fy = self._find_rear_axle_position(curr_x, curr_y, curr_yaw) else: fx, fy = self._find_front_axle_position(curr_x, curr_y, curr_yaw) - search_start = self.current_path_parameter - 5.0 + search_start = self.current_path_parameter - 0.0 search_end = self.current_path_parameter + 5.0 closest_dist, closest_parameter = self.path.closest_point_local((fx, fy), [search_start, search_end]) self.current_path_parameter = closest_parameter @@ -151,7 +195,13 @@ def compute(self, state: VehicleState, component: Component = None, reverse = Fa dx = fx - target_x dy = fy - target_y - if reverse: + use_reverse = self.is_target_behind_vehicle(state.pose, (target_x, target_y)) + if use_reverse: + self.reverse = True + else: + self.reverse = False + + if self.reverse: cross_track_error = dx * (-tangent[1]) + dy * tangent[0] self.k += self.k else: @@ -163,7 +213,7 @@ def compute(self, state: VehicleState, component: Component = None, reverse = Fa desired_speed = abs(self.desired_speed) feedforward_accel = 0.0 - if reverse: + if self.reverse: heading_term = -yaw_error cross_term_input = self.k * (-cross_track_error) / (self.k_soft + abs(speed)) cross_term = atan2(cross_term_input, 1.0) @@ -253,7 +303,7 @@ def compute(self, state: VehicleState, component: Component = None, reverse = Fa desired_speed *= np.exp(-abs(cross_track_error) * 0.6) if abs(desired_speed) > self.speed_limit: - if reverse: + if self.reverse: desired_speed = self.speed_limit * -1.0 if desired_speed != 0 else 0.0 else: desired_speed = self.speed_limit @@ -293,7 +343,6 @@ class StanleyTrajectoryTracker(Component): def __init__(self, vehicle_interface=None, **kwargs): self.stanley = Stanley(**kwargs) self.vehicle_interface = vehicle_interface - self.reverse = None def rate(self): return 50.0 @@ -303,102 +352,11 @@ def state_inputs(self): def state_outputs(self): return [] - - """ - def _check_sharp_turn_ahead(self, lookahead_s=3.0, threshold_angle=np.pi/2.0, num_steps=4): - if not self.stanley.path: - return False - path = self.stanley.path - current_s = self.stanley.current_path_parameter - domain_start, domain_end = path.domain() - - if current_s >= domain_end - 1e-3: - return False - - step_s = lookahead_s / num_steps - s_prev = current_s - - try: - tangent_prev = path.eval_tangent(s_prev) - if np.linalg.norm(tangent_prev) < 1e-6: - s_prev_adjusted = min(s_prev + step_s / 2, domain_end) - if s_prev_adjusted <= s_prev: - return False - tangent_prev = path.eval_tangent(s_prev_adjusted) - if np.linalg.norm(tangent_prev) < 1e-6: - return False - s_prev = s_prev_adjusted - - angle_prev = atan2(tangent_prev[1], tangent_prev[0]) - - except Exception as e: - return False - - for i in range(num_steps): - s_next = s_prev + step_s - s_next = min(s_next, domain_end) - - if s_next <= s_prev + 1e-6: - break - - try: - tangent_next = path.eval_tangent(s_next) - if np.linalg.norm(tangent_next) < 1e-6: - s_prev = s_next - continue - - angle_next = atan2(tangent_next[1], tangent_next[0]) - except Exception as e: - break - - angle_change = abs(normalise_angle(angle_next - angle_prev)) - - if angle_change > threshold_angle: - return True - - angle_prev = angle_next - s_prev = s_next - return False - """ def update(self, vehicle: VehicleState, trajectory: Trajectory): self.stanley.set_path(trajectory) - - if self.reverse == None: - curr_x = vehicle.pose.x - curr_y = vehicle.pose.y - curr_yaw = vehicle.pose.yaw if vehicle.pose.yaw is not None else 0.0 - - fx, fy = self.stanley._find_front_axle_position(curr_x, curr_y, curr_yaw) - path_domain_start = self.stanley.path.domain()[0] - search_domain_start = [path_domain_start, min(self.stanley.path.domain()[1], path_domain_start + 5.0)] - - _, closest_param_at_start = self.stanley.path.closest_point_local((fx, fy), search_domain_start) - tangent_at_start = self.stanley.path.eval_tangent(closest_param_at_start) - - if np.linalg.norm(tangent_at_start) > 1e-6: - path_yaw_at_start = atan2(tangent_at_start[1], tangent_at_start[0]) - heading_diff = normalise_angle(path_yaw_at_start - curr_yaw) - self.reverse = abs(heading_diff) > (np.pi / 2.0) - else: - self.reverse = False - self.stanley.pid_speed.reset() - self.stanley.current_path_parameter = self.stanley.path.domain()[0] - - else: - """ - is_sharp_turn_ahead = self._check_sharp_turn_ahead( - lookahead_s=4.0, - threshold_angle=np.pi/2.0 - ) - print(f"Sharp turn ahead: {is_sharp_turn_ahead}") - - if is_sharp_turn_ahead: - self.reverse = not self.reverse - self.stanley.set_path(trajectory) - """ - pass - accel, f_delta = self.stanley.compute(vehicle, self, self.reverse) + + accel, f_delta = self.stanley.compute(vehicle, self) steering_angle = front2steer(f_delta) steering_angle = np.clip( From 8c3dde574401af0de9ca44acf8955aa4105f1484 Mon Sep 17 00:00:00 2001 From: hhxjqm Date: Sat, 10 May 2025 04:27:00 +0000 Subject: [PATCH 37/71] Fix the reverse issue in p_shape --- GEMstack/onboard/planning/stanley.py | 72 ++++++++++++++++++++++++++-- launch/fixed_route.yaml | 5 +- 2 files changed, 70 insertions(+), 7 deletions(-) diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley.py index f175e7706..532dc9d9c 100644 --- a/GEMstack/onboard/planning/stanley.py +++ b/GEMstack/onboard/planning/stanley.py @@ -71,6 +71,7 @@ def __init__( self.current_traj_parameter = 0.0 self.t_last = None self.reverse = None + self.sharp_turn = False def set_path(self, path: Path): if path == self.path_arg: @@ -145,6 +146,62 @@ def is_target_behind_vehicle(self, vehicle_pose, target_point_coords): dot_product = vec_x * heading_x + vec_y * heading_y return (dot_product < 0) + def _check_sharp_turn_ahead(self, lookahead_s=3.0, threshold_angle=np.pi/2.0, num_steps=4): + if not self.path: + return False + + path = self.path + current_s = self.current_path_parameter + domain_start, domain_end = path.domain() + + if current_s >= domain_end - 1e-3: + return False + + step_s = lookahead_s / num_steps + s_prev = current_s + + try: + tangent_prev = path.eval_tangent(s_prev) + if np.linalg.norm(tangent_prev) < 1e-6: + s_prev_adjusted = min(s_prev + step_s / 2, domain_end) + if s_prev_adjusted <= s_prev: + return False + tangent_prev = path.eval_tangent(s_prev_adjusted) + if np.linalg.norm(tangent_prev) < 1e-6: + return False + s_prev = s_prev_adjusted + + angle_prev = atan2(tangent_prev[1], tangent_prev[0]) + + except Exception as e: + return False + + for i in range(num_steps): + s_next = s_prev + step_s + s_next = min(s_next, domain_end) + + if s_next <= s_prev + 1e-6: + break + + try: + tangent_next = path.eval_tangent(s_next) + if np.linalg.norm(tangent_next) < 1e-6: + s_prev = s_next + continue + + angle_next = atan2(tangent_next[1], tangent_next[0]) + except Exception as e: + break + + angle_change = abs(normalise_angle(angle_next - angle_prev)) + + if angle_change > threshold_angle: + return True + + angle_prev = angle_next + s_prev = s_next + return False + def compute(self, state: VehicleState, component: Component = None): t = state.pose.t if self.t_last is None: @@ -183,7 +240,7 @@ def compute(self, state: VehicleState, component: Component = None): fx, fy = self._find_rear_axle_position(curr_x, curr_y, curr_yaw) else: fx, fy = self._find_front_axle_position(curr_x, curr_y, curr_yaw) - search_start = self.current_path_parameter - 0.0 + search_start = self.current_path_parameter search_end = self.current_path_parameter + 5.0 closest_dist, closest_parameter = self.path.closest_point_local((fx, fy), [search_start, search_end]) self.current_path_parameter = closest_parameter @@ -195,12 +252,17 @@ def compute(self, state: VehicleState, component: Component = None): dx = fx - target_x dy = fy - target_y + is_sharp_turn_ahead = self._check_sharp_turn_ahead( + lookahead_s=2.0, + threshold_angle=np.pi/2.0 + ) + if is_sharp_turn_ahead and not self.sharp_turn: + self.sharp_turn = True use_reverse = self.is_target_behind_vehicle(state.pose, (target_x, target_y)) - if use_reverse: + if use_reverse and self.sharp_turn and not self.reverse: self.reverse = True - else: - self.reverse = False - + self.sharp_turn = False + if self.reverse: cross_track_error = dx * (-tangent[1]) + dy * tangent[0] self.k += self.k diff --git a/launch/fixed_route.yaml b/launch/fixed_route.yaml index 507fe48cc..f218caa5a 100644 --- a/launch/fixed_route.yaml +++ b/launch/fixed_route.yaml @@ -23,6 +23,7 @@ drive: # args: [!relative_path '../GEMstack/knowledge/routes/backward_15m.csv'] # args: [!relative_path '../GEMstack/knowledge/routes/forward_curve.csv'] # args: [!relative_path '../GEMstack/knowledge/routes/backward_curve.csv'] + # args: [!relative_path '../GEMstack/knowledge/routes/yamada.csv'] # args: [!relative_path '../GEMstack/knowledge/routes/xyhead_highbay_backlot_oval.csv'] motion_planning: @@ -31,8 +32,8 @@ drive: trajectory_tracking: # type: pure_pursuit.PurePursuitTrajectoryTracker type: stanley.StanleyTrajectoryTracker - args: {desired_speed: 2.5} #approximately 5mph - print: False + args: {desired_speed: 5.0} #approximately 5mph + print: True # type: mpc.MPCTrajectoryTracker log: # Specify the top-level folder to save the log files. Default is 'logs' From 45bef181b8ccdc45c9d43c311fdb1238280420c8 Mon Sep 17 00:00:00 2001 From: Patrick Wu Date: Sat, 10 May 2025 12:42:07 -0500 Subject: [PATCH 38/71] finish mpc --- GEMstack/knowledge/routes/backward_curve.csv | 18 +- .../knowledge/vehicle/gem_e2_slow_limits.yaml | 16 +- GEMstack/onboard/interface/gem_hardware.py | 656 +++++++++--------- GEMstack/onboard/planning/mpc.py | 25 +- launch/fixed_route.yaml | 1 + scenes/xyhead_demo.yaml | 14 +- 6 files changed, 371 insertions(+), 359 deletions(-) diff --git a/GEMstack/knowledge/routes/backward_curve.csv b/GEMstack/knowledge/routes/backward_curve.csv index 87f25ee6e..9b9aacc13 100644 --- a/GEMstack/knowledge/routes/backward_curve.csv +++ b/GEMstack/knowledge/routes/backward_curve.csv @@ -2,5 +2,19 @@ -1.0,0,0 -2.0,0.2,0 -3.0,0.4,0 --4,0.6,0 --5,0.8,0 +-4.0,0.6,0 +-5.0,0.8,0 +-6.0,1.0,0 +-7.0,1.2,0 +-8.0,1.4,0 +-9.0,1.6,0 +-10.0,1.8,0 +-11.0,2.0,0 +-12.0,2.2,0 +-13.0,2.4,0 +-14.0,2.6,0 +-15.0,2.8,0 +-16.0,3.0,0 +-17.0,3.2,0 +-18.0,3.4,0 +-19.0,3.6,0 diff --git a/GEMstack/knowledge/vehicle/gem_e2_slow_limits.yaml b/GEMstack/knowledge/vehicle/gem_e2_slow_limits.yaml index 8720ae250..921ce234a 100644 --- a/GEMstack/knowledge/vehicle/gem_e2_slow_limits.yaml +++ b/GEMstack/knowledge/vehicle/gem_e2_slow_limits.yaml @@ -1,8 +1,8 @@ -max_steering_rate: 2.0 #radians/s -max_speed: 2.5 #m/s, approx 5pmh -max_reverse_speed: 0.25 #m/s, approx 1mph -max_acceleration: 1.0 #m/s^2 -max_deceleration: 2.0 #m/s^2 -max_accelerator_pedal: 0.5 #0-1 -max_brake_pedal: 0.5 #0-1 - +max_steering_rate: 2.0 #radians/s +max_speed: 2.5 #m/s, approx 5pmh +max_reverse_speed: 0.25 #m/s, approx 1mph +max_acceleration: 1.0 #m/s^2 +max_deceleration: 2.0 #m/s^2 +max_accelerator_pedal: 0.5 #0-1 +max_brake_pedal: 0.5 #0-1 + diff --git a/GEMstack/onboard/interface/gem_hardware.py b/GEMstack/onboard/interface/gem_hardware.py index 836d7ef71..681528d1d 100644 --- a/GEMstack/onboard/interface/gem_hardware.py +++ b/GEMstack/onboard/interface/gem_hardware.py @@ -1,328 +1,328 @@ -from .gem import * -from ...utils import settings -import math -import time - -# ROS Headers -import rospy -from std_msgs.msg import String, Bool, Float32, Float64 -from sensor_msgs.msg import Image,PointCloud2 -try: - from novatel_gps_msgs.msg import NovatelPosition, NovatelXYZ, Inspva -except ImportError: - pass -try: - from septentrio_gnss_driver.msg import INSNavGeod -except ImportError: - pass - -from radar_msgs.msg import RadarTracks -from tf.transformations import euler_from_quaternion, quaternion_from_euler - -# GEM PACMod Headers -from pacmod_msgs.msg import PositionWithSpeed, PacmodCmd, SystemRptFloat, VehicleSpeedRpt, GlobalRpt - -# OpenCV and cv2 bridge -import cv2 -import numpy as np -from ...utils import conversions - -class GEMHardwareInterface(GEMInterface): - """Interface for connnecting to the physical GEM e2 vehicle.""" - def __init__(self): - GEMInterface.__init__(self) - self.max_send_rate = settings.get('vehicle.max_command_rate',10.0) - self.ros_sensor_topics = settings.get('vehicle.sensors.ros_topics') - self.last_command_time = 0.0 - self.last_reading = GEMVehicleReading() - self.last_reading.speed = 0.0 - self.last_reading.steering_wheel_angle = 0.0 - self.last_reading.accelerator_pedal_position = 0.0 - self.last_reading.brake_pedal_position = 0.0 - self.last_reading.gear = 0 - self.last_reading.left_turn_signal = False - self.last_reading.right_turn_signal = False - self.last_reading.horn_on = False - self.last_reading.wiper_level = 0 - self.last_reading.headlights_on = False - - self.speed_sub = rospy.Subscriber("/pacmod/parsed_tx/vehicle_speed_rpt", VehicleSpeedRpt, self.speed_callback) - self.steer_sub = rospy.Subscriber("/pacmod/parsed_tx/steer_rpt", SystemRptFloat, self.steer_callback) - self.global_sub = rospy.Subscriber("/pacmod/parsed_tx/global_rpt", GlobalRpt, self.global_callback) - self.gnss_sub = None - self.imu_sub = None - self.front_radar_sub = None - self.front_camera_sub = None - self.front_depth_sub = None - self.top_lidar_sub = None - self.stereo_sub = None - self.faults = [] - - # -------------------- PACMod setup -------------------- - # GEM vehicle enable - self.enable_pub = rospy.Publisher('/pacmod/as_rx/enable', Bool, queue_size=1) - self.pacmod_enable = False - - # GEM vehicle gear control, neutral, forward and reverse, publish once - self.gear_pub = rospy.Publisher('/pacmod/as_rx/shift_cmd', PacmodCmd, queue_size=1) - self.gear_cmd = PacmodCmd() - self.gear_cmd.enable = True - self.gear_cmd.ui16_cmd = PacmodCmd.SHIFT_NEUTRAL - - # GEM vehicle brake control - self.brake_pub = rospy.Publisher('/pacmod/as_rx/brake_cmd', PacmodCmd, queue_size=1) - self.brake_cmd = PacmodCmd() - self.brake_cmd.enable = False - self.brake_cmd.clear = True - self.brake_cmd.ignore = True - - # GEM vehicle forward motion control - self.accel_pub = rospy.Publisher('/pacmod/as_rx/accel_cmd', PacmodCmd, queue_size=1) - self.accel_cmd = PacmodCmd() - self.accel_cmd.enable = False - self.accel_cmd.clear = True - self.accel_cmd.ignore = True - - # GEM vehicle turn signal control - self.turn_pub = rospy.Publisher('/pacmod/as_rx/turn_cmd', PacmodCmd, queue_size=1) - self.turn_cmd = PacmodCmd() - self.turn_cmd.ui16_cmd = 1 # None - - # GEM vechile steering wheel control - self.steer_pub = rospy.Publisher('/pacmod/as_rx/steer_cmd', PositionWithSpeed, queue_size=1) - self.steer_cmd = PositionWithSpeed() - self.steer_cmd.angular_position = 0.0 # radians, -: clockwise, +: counter-clockwise - self.steer_cmd.angular_velocity_limit = 2.0 # radians/second - - """TODO: other commands - /pacmod/as_rx/headlight_cmd - /pacmod/as_rx/horn_cmd - /pacmod/as_rx/wiper_cmd - """ - - #TODO: publish TwistStamped to /front_radar/front_radar/vehicle_motion to get better radar tracks - - #subscribers should go last because the callback might be called before the object is initialized - self.enable_sub = rospy.Subscriber('/pacmod/as_tx/enable', Bool, self.pacmod_enable_callback) - - - def start(self): - if settings.get('vehicle.enable_through_joystick',True): - pass - else: - print("ENABLING PACMOD") - enable_cmd = Bool() - enable_cmd.data = True - self.enable_pub.publish(enable_cmd) - #this doesn't seem to work super well, need to send enable command multiple times - - def time(self): - seconds = rospy.get_time() - return seconds - - def speed_callback(self,msg : VehicleSpeedRpt): - self.last_reading.speed = msg.vehicle_speed # forward velocity in m/s - - def steer_callback(self, msg): - self.last_reading.steering_wheel_angle = msg.output - - def global_callback(self, msg): - self.faults = [] - if msg.override_active: - self.faults.append("override_active") - if msg.config_fault_active: - self.faults.append("config_fault_active") - if msg.user_can_timeout: - self.faults.append("user_can_timeout") - if msg.user_can_read_errors: - self.faults.append("user_can_read_errors") - if msg.brake_can_timeout: - self.faults.append("brake_can_timeout") - if msg.steering_can_timeout: - self.faults.append("steering_can_timeout") - if msg.vehicle_can_timeout: - self.faults.append("vehicle_can_timeout") - if msg.subsystem_can_timeout: - self.faults.append("subsystem_can_timeout") - - def get_reading(self) -> GEMVehicleReading: - return self.last_reading - - def subscribe_sensor(self, name, callback, type = None): - if name == 'gnss': - topic = self.ros_sensor_topics[name] - if topic.endswith('inspva'): - #GEM e2 uses Novatel GNSS - if type is not None and (type is not Inspva and type is not GNSSReading): - raise ValueError("GEMHardwareInterface GEM e2 only supports Inspva/GNSSReading for GNSS") - if type is Inspva: - self.gnss_sub = rospy.Subscriber(topic, Inspva, callback) - else: - def callback_with_gnss_reading(inspva_msg: Inspva): - pose = ObjectPose(ObjectFrameEnum.GLOBAL, - x=inspva_msg.longitude, - y=inspva_msg.latitude, - z=inspva_msg.height, - yaw=math.radians(inspva_msg.azimuth), #heading from north in degrees - roll=math.radians(inspva_msg.roll), - pitch=math.radians(inspva_msg.pitch), - ) - speed = np.sqrt(inspva_msg.east_velocity**2 + inspva_msg.north_velocity**2) - callback(GNSSReading(pose,speed,inspva_msg.status)) - self.gnss_sub = rospy.Subscriber(topic, Inspva, callback_with_gnss_reading) - else: - #assume it's septentrio on GEM e4 - if type is not None and (type is not INSNavGeod and type is not GNSSReading): - raise ValueError("GEMHardwareInterface GEM e4 only supports INSNavGeod/GNSSReading for GNSS") - if type is INSNavGeod: - self.gnss_sub = rospy.Subscriber(topic, INSNavGeod, callback) - else: - def callback_with_gnss_reading(msg: INSNavGeod): - pose = ObjectPose(ObjectFrameEnum.GLOBAL, - t=self.time(), - x=math.degrees(msg.longitude), #Septentrio GNSS uses radians rather than degrees - y=math.degrees(msg.latitude), - z=msg.height, - yaw=math.radians(msg.heading), #heading from north in degrees (TODO: maybe?? check this) - roll=math.radians(msg.roll), - pitch=math.radians(msg.pitch), - ) - speed = np.sqrt(msg.ve**2 + msg.vn**2) - callback(GNSSReading(pose,speed,('error' if msg.error else 'ok'))) - self.gnss_sub = rospy.Subscriber(topic, INSNavGeod, callback_with_gnss_reading) - elif name == 'top_lidar': - topic = self.ros_sensor_topics[name] - if type is not None and (type is not PointCloud2 and type is not np.ndarray): - raise ValueError("GEMHardwareInterface only supports PointCloud2 or numpy array for top lidar") - if type is None or type is PointCloud2: - self.top_lidar_sub = rospy.Subscriber(topic, PointCloud2, callback) - else: - def callback_with_numpy(msg : Image): - #print("received image with size",msg.width,msg.height,"encoding",msg.encoding) - points = conversions.ros_PointCloud2_to_numpy(msg, want_rgb=False) - callback(points) - self.top_lidar_sub = rospy.Subscriber(topic, PointCloud2, callback_with_numpy) - elif name == 'front_radar': - if type is not None and type is not RadarTracks: - raise ValueError("GEMHardwareInterface only supports RadarTracks for front radar") - self.front_radar_sub = rospy.Subscriber("/front_radar/front_radar/radar_tracks", RadarTracks, callback) - elif name == 'front_camera': - topic = self.ros_sensor_topics[name] - if type is not None and (type is not Image and type is not cv2.Mat): - raise ValueError("GEMHardwareInterface only supports Image or OpenCV for front camera") - if type is None or type is Image: - self.front_camera_sub = rospy.Subscriber(topic, Image, callback) - else: - def callback_with_cv2(msg : Image): - #print("received image with size",msg.width,msg.height,"encoding",msg.encoding) - cv_image = conversions.ros_Image_to_cv2(msg, desired_encoding="bgr8") - callback(cv_image) - self.front_camera_sub = rospy.Subscriber(topic, Image, callback_with_cv2) - elif name == 'front_depth': - topic = self.ros_sensor_topics[name] - if type is not None and (type is not Image and type is not cv2.Mat): - raise ValueError("GEMHardwareInterface only supports Image or OpenCV for front depth") - if type is None or type is Image: - self.front_depth_sub = rospy.Subscriber(topic, Image, callback) - else: - def callback_with_cv2(msg : Image): - #print("received image with size",msg.width,msg.height,"encoding",msg.encoding) - cv_image = conversions.ros_Image_to_cv2(msg, desired_encoding="passthrough") - callback(cv_image) - self.front_depth_sub = rospy.Subscriber(topic, Image, callback_with_cv2) - - - # PACMod enable callback function - def pacmod_enable_callback(self, msg): - if self.pacmod_enable == False and msg.data == True: - print("PACMod enabled, enabling gear, brake, accel, steer, and turn") - self.send_first_command() - elif self.pacmod_enable == True and msg.data == False: - print("PACMod disabled") - self.pacmod_enable = msg.data - - def hardware_faults(self) -> List[str]: - if self.pacmod_enable == False: - return self.faults + ["disengaged"] - return self.faults - - def send_first_command(self): - # ---------- Enable PACMod ---------- - - # enable forward gear - self.gear_cmd.enable = True - self.gear_cmd.ui16_cmd = PacmodCmd.SHIFT_FORWARD - #helps debug whether gear command is being sent since you'll hear the backup beep - #self.gear_cmd.ui16_cmd = PacmodCmd.SHIFT_REVERSE - - # enable brake - self.brake_cmd.enable = True - self.brake_cmd.clear = False - self.brake_cmd.ignore = False - self.brake_cmd.f64_cmd = 0.0 - - # enable gas - self.accel_cmd.enable = True - self.accel_cmd.clear = False - self.accel_cmd.ignore = False - self.accel_cmd.f64_cmd = 0.0 - - self.gear_pub.publish(self.gear_cmd) - self.turn_pub.publish(self.turn_cmd) - self.brake_pub.publish(self.brake_cmd) - self.accel_pub.publish(self.accel_cmd) - self.last_command_time = self.time() - - def send_command(self, command : GEMVehicleCommand): - #throttle rate at which we send commands - t = self.time() - if t < self.last_command_time + 1.0/self.max_send_rate: - #skip command, PACMod can't handle commands this fast - return - self.last_command_time = t - - if command.left_turn_signal and command.right_turn_signal: - self.turn_cmd.ui16_cmd = PacmodCmd.TURN_HAZARDS - elif command.left_turn_signal: - self.turn_cmd.ui16_cmd = PacmodCmd.TURN_LEFT - elif command.right_turn_signal: - self.turn_cmd.ui16_cmd = PacmodCmd.TURN_RIGHT - else: - self.turn_cmd.ui16_cmd = PacmodCmd.TURN_NONE - - self.accel_cmd.f64_cmd = command.accelerator_pedal_position - if command.brake_pedal_position > 0.0: - self.accel_cmd.f64_cmd = 0.0 - self.brake_cmd.f64_cmd = command.brake_pedal_position - self.steer_cmd.angular_position = command.steering_wheel_angle - self.steer_cmd.angular_velocity_limit = command.steering_wheel_speed - print("**************************") - print("Steer cmd angular position {} velocity limit {}".format(self.steer_cmd.angular_position,self.steer_cmd.angular_velocity_limit)) - print("Accel pedal position {} brake position {}".format(self.accel_cmd.f64_cmd,self.brake_cmd.f64_cmd)) - maxacc = settings.get('vehicle.limits.max_accelerator_pedal') - maxbrake = settings.get('vehicle.limits.max_brake_pedal') - if self.accel_cmd.f64_cmd > maxacc: - print("Warning: commanded acceleration exceeded accel pedal limit") - self.accel_cmd.f64_cmd = maxacc - if self.brake_cmd.f64_cmd > maxbrake: - print("Warning: commanded braking exceeded brake pedal limit") - self.brake_cmd.f64_cmd = maxbrake - print("**************************") - - self.brake_cmd.enable = True - self.brake_cmd.clear = False - self.brake_cmd.ignore = False - - self.accel_cmd.enable = True - self.accel_cmd.clear = False - self.accel_cmd.ignore = False - - self.gear_cmd.ui16_cmd = PacmodCmd.SHIFT_FORWARD - self.gear_cmd.enable = True - self.gear_pub.publish(self.gear_cmd) - self.accel_pub.publish(self.accel_cmd) - self.brake_pub.publish(self.brake_cmd) - self.steer_pub.publish(self.steer_cmd) - self.turn_pub.publish(self.turn_cmd) - - self.last_command = command +from .gem import * +from ...utils import settings +import math +import time + +# ROS Headers +import rospy +from std_msgs.msg import String, Bool, Float32, Float64 +from sensor_msgs.msg import Image,PointCloud2 +try: + from novatel_gps_msgs.msg import NovatelPosition, NovatelXYZ, Inspva +except ImportError: + pass +try: + from septentrio_gnss_driver.msg import INSNavGeod +except ImportError: + pass + +from radar_msgs.msg import RadarTracks +from tf.transformations import euler_from_quaternion, quaternion_from_euler + +# GEM PACMod Headers +from pacmod_msgs.msg import PositionWithSpeed, PacmodCmd, SystemRptFloat, VehicleSpeedRpt, GlobalRpt + +# OpenCV and cv2 bridge +import cv2 +import numpy as np +from ...utils import conversions + +class GEMHardwareInterface(GEMInterface): + """Interface for connnecting to the physical GEM e2 vehicle.""" + def __init__(self): + GEMInterface.__init__(self) + self.max_send_rate = settings.get('vehicle.max_command_rate',10.0) + self.ros_sensor_topics = settings.get('vehicle.sensors.ros_topics') + self.last_command_time = 0.0 + self.last_reading = GEMVehicleReading() + self.last_reading.speed = 0.0 + self.last_reading.steering_wheel_angle = 0.0 + self.last_reading.accelerator_pedal_position = 0.0 + self.last_reading.brake_pedal_position = 0.0 + self.last_reading.gear = 0 + self.last_reading.left_turn_signal = False + self.last_reading.right_turn_signal = False + self.last_reading.horn_on = False + self.last_reading.wiper_level = 0 + self.last_reading.headlights_on = False + + self.speed_sub = rospy.Subscriber("/pacmod/parsed_tx/vehicle_speed_rpt", VehicleSpeedRpt, self.speed_callback) + self.steer_sub = rospy.Subscriber("/pacmod/parsed_tx/steer_rpt", SystemRptFloat, self.steer_callback) + self.global_sub = rospy.Subscriber("/pacmod/parsed_tx/global_rpt", GlobalRpt, self.global_callback) + self.gnss_sub = None + self.imu_sub = None + self.front_radar_sub = None + self.front_camera_sub = None + self.front_depth_sub = None + self.top_lidar_sub = None + self.stereo_sub = None + self.faults = [] + + # -------------------- PACMod setup -------------------- + # GEM vehicle enable + self.enable_pub = rospy.Publisher('/pacmod/as_rx/enable', Bool, queue_size=1) + self.pacmod_enable = False + + # GEM vehicle gear control, neutral, forward and reverse, publish once + self.gear_pub = rospy.Publisher('/pacmod/as_rx/shift_cmd', PacmodCmd, queue_size=1) + self.gear_cmd = PacmodCmd() + self.gear_cmd.enable = True + self.gear_cmd.ui16_cmd = PacmodCmd.SHIFT_NEUTRAL + + # GEM vehicle brake control + self.brake_pub = rospy.Publisher('/pacmod/as_rx/brake_cmd', PacmodCmd, queue_size=1) + self.brake_cmd = PacmodCmd() + self.brake_cmd.enable = False + self.brake_cmd.clear = True + self.brake_cmd.ignore = True + + # GEM vehicle forward motion control + self.accel_pub = rospy.Publisher('/pacmod/as_rx/accel_cmd', PacmodCmd, queue_size=1) + self.accel_cmd = PacmodCmd() + self.accel_cmd.enable = False + self.accel_cmd.clear = True + self.accel_cmd.ignore = True + + # GEM vehicle turn signal control + self.turn_pub = rospy.Publisher('/pacmod/as_rx/turn_cmd', PacmodCmd, queue_size=1) + self.turn_cmd = PacmodCmd() + self.turn_cmd.ui16_cmd = 1 # None + + # GEM vechile steering wheel control + self.steer_pub = rospy.Publisher('/pacmod/as_rx/steer_cmd', PositionWithSpeed, queue_size=1) + self.steer_cmd = PositionWithSpeed() + self.steer_cmd.angular_position = 0.0 # radians, -: clockwise, +: counter-clockwise + self.steer_cmd.angular_velocity_limit = 2.0 # radians/second + + """TODO: other commands + /pacmod/as_rx/headlight_cmd + /pacmod/as_rx/horn_cmd + /pacmod/as_rx/wiper_cmd + """ + + #TODO: publish TwistStamped to /front_radar/front_radar/vehicle_motion to get better radar tracks + + #subscribers should go last because the callback might be called before the object is initialized + self.enable_sub = rospy.Subscriber('/pacmod/as_tx/enable', Bool, self.pacmod_enable_callback) + + + def start(self): + if settings.get('vehicle.enable_through_joystick',True): + pass + else: + print("ENABLING PACMOD") + enable_cmd = Bool() + enable_cmd.data = True + self.enable_pub.publish(enable_cmd) + #this doesn't seem to work super well, need to send enable command multiple times + + def time(self): + seconds = rospy.get_time() + return seconds + + def speed_callback(self,msg : VehicleSpeedRpt): + self.last_reading.speed = msg.vehicle_speed # forward velocity in m/s + + def steer_callback(self, msg): + self.last_reading.steering_wheel_angle = msg.output + + def global_callback(self, msg): + self.faults = [] + if msg.override_active: + self.faults.append("override_active") + if msg.config_fault_active: + self.faults.append("config_fault_active") + if msg.user_can_timeout: + self.faults.append("user_can_timeout") + if msg.user_can_read_errors: + self.faults.append("user_can_read_errors") + if msg.brake_can_timeout: + self.faults.append("brake_can_timeout") + if msg.steering_can_timeout: + self.faults.append("steering_can_timeout") + if msg.vehicle_can_timeout: + self.faults.append("vehicle_can_timeout") + if msg.subsystem_can_timeout: + self.faults.append("subsystem_can_timeout") + + def get_reading(self) -> GEMVehicleReading: + return self.last_reading + + def subscribe_sensor(self, name, callback, type = None): + if name == 'gnss': + topic = self.ros_sensor_topics[name] + if topic.endswith('inspva'): + #GEM e2 uses Novatel GNSS + if type is not None and (type is not Inspva and type is not GNSSReading): + raise ValueError("GEMHardwareInterface GEM e2 only supports Inspva/GNSSReading for GNSS") + if type is Inspva: + self.gnss_sub = rospy.Subscriber(topic, Inspva, callback) + else: + def callback_with_gnss_reading(inspva_msg: Inspva): + pose = ObjectPose(ObjectFrameEnum.GLOBAL, + x=inspva_msg.longitude, + y=inspva_msg.latitude, + z=inspva_msg.height, + yaw=math.radians(inspva_msg.azimuth), #heading from north in degrees + roll=math.radians(inspva_msg.roll), + pitch=math.radians(inspva_msg.pitch), + ) + speed = np.sqrt(inspva_msg.east_velocity**2 + inspva_msg.north_velocity**2) + callback(GNSSReading(pose,speed,inspva_msg.status)) + self.gnss_sub = rospy.Subscriber(topic, Inspva, callback_with_gnss_reading) + else: + #assume it's septentrio on GEM e4 + if type is not None and (type is not INSNavGeod and type is not GNSSReading): + raise ValueError("GEMHardwareInterface GEM e4 only supports INSNavGeod/GNSSReading for GNSS") + if type is INSNavGeod: + self.gnss_sub = rospy.Subscriber(topic, INSNavGeod, callback) + else: + def callback_with_gnss_reading(msg: INSNavGeod): + pose = ObjectPose(ObjectFrameEnum.GLOBAL, + t=self.time(), + x=math.degrees(msg.longitude), #Septentrio GNSS uses radians rather than degrees + y=math.degrees(msg.latitude), + z=msg.height, + yaw=math.radians(msg.heading), #heading from north in degrees (TODO: maybe?? check this) + roll=math.radians(msg.roll), + pitch=math.radians(msg.pitch), + ) + speed = np.sqrt(msg.ve**2 + msg.vn**2) + callback(GNSSReading(pose,speed,('error' if msg.error else 'ok'))) + self.gnss_sub = rospy.Subscriber(topic, INSNavGeod, callback_with_gnss_reading) + elif name == 'top_lidar': + topic = self.ros_sensor_topics[name] + if type is not None and (type is not PointCloud2 and type is not np.ndarray): + raise ValueError("GEMHardwareInterface only supports PointCloud2 or numpy array for top lidar") + if type is None or type is PointCloud2: + self.top_lidar_sub = rospy.Subscriber(topic, PointCloud2, callback) + else: + def callback_with_numpy(msg : Image): + #print("received image with size",msg.width,msg.height,"encoding",msg.encoding) + points = conversions.ros_PointCloud2_to_numpy(msg, want_rgb=False) + callback(points) + self.top_lidar_sub = rospy.Subscriber(topic, PointCloud2, callback_with_numpy) + elif name == 'front_radar': + if type is not None and type is not RadarTracks: + raise ValueError("GEMHardwareInterface only supports RadarTracks for front radar") + self.front_radar_sub = rospy.Subscriber("/front_radar/front_radar/radar_tracks", RadarTracks, callback) + elif name == 'front_camera': + topic = self.ros_sensor_topics[name] + if type is not None and (type is not Image and type is not cv2.Mat): + raise ValueError("GEMHardwareInterface only supports Image or OpenCV for front camera") + if type is None or type is Image: + self.front_camera_sub = rospy.Subscriber(topic, Image, callback) + else: + def callback_with_cv2(msg : Image): + #print("received image with size",msg.width,msg.height,"encoding",msg.encoding) + cv_image = conversions.ros_Image_to_cv2(msg, desired_encoding="bgr8") + callback(cv_image) + self.front_camera_sub = rospy.Subscriber(topic, Image, callback_with_cv2) + elif name == 'front_depth': + topic = self.ros_sensor_topics[name] + if type is not None and (type is not Image and type is not cv2.Mat): + raise ValueError("GEMHardwareInterface only supports Image or OpenCV for front depth") + if type is None or type is Image: + self.front_depth_sub = rospy.Subscriber(topic, Image, callback) + else: + def callback_with_cv2(msg : Image): + #print("received image with size",msg.width,msg.height,"encoding",msg.encoding) + cv_image = conversions.ros_Image_to_cv2(msg, desired_encoding="passthrough") + callback(cv_image) + self.front_depth_sub = rospy.Subscriber(topic, Image, callback_with_cv2) + + + # PACMod enable callback function + def pacmod_enable_callback(self, msg): + if self.pacmod_enable == False and msg.data == True: + print("PACMod enabled, enabling gear, brake, accel, steer, and turn") + self.send_first_command() + elif self.pacmod_enable == True and msg.data == False: + print("PACMod disabled") + self.pacmod_enable = msg.data + + def hardware_faults(self) -> List[str]: + if self.pacmod_enable == False: + return self.faults + ["disengaged"] + return self.faults + + def send_first_command(self): + # ---------- Enable PACMod ---------- + + # enable forward gear + self.gear_cmd.enable = True + self.gear_cmd.ui16_cmd = PacmodCmd.SHIFT_FORWARD + #helps debug whether gear command is being sent since you'll hear the backup beep + #self.gear_cmd.ui16_cmd = PacmodCmd.SHIFT_REVERSE + + # enable brake + self.brake_cmd.enable = True + self.brake_cmd.clear = False + self.brake_cmd.ignore = False + self.brake_cmd.f64_cmd = 0.0 + + # enable gas + self.accel_cmd.enable = True + self.accel_cmd.clear = False + self.accel_cmd.ignore = False + self.accel_cmd.f64_cmd = 0.0 + + self.gear_pub.publish(self.gear_cmd) + self.turn_pub.publish(self.turn_cmd) + self.brake_pub.publish(self.brake_cmd) + self.accel_pub.publish(self.accel_cmd) + self.last_command_time = self.time() + + def send_command(self, command : GEMVehicleCommand): + #throttle rate at which we send commands + t = self.time() + if t < self.last_command_time + 1.0/self.max_send_rate: + #skip command, PACMod can't handle commands this fast + return + self.last_command_time = t + + if command.left_turn_signal and command.right_turn_signal: + self.turn_cmd.ui16_cmd = PacmodCmd.TURN_HAZARDS + elif command.left_turn_signal: + self.turn_cmd.ui16_cmd = PacmodCmd.TURN_LEFT + elif command.right_turn_signal: + self.turn_cmd.ui16_cmd = PacmodCmd.TURN_RIGHT + else: + self.turn_cmd.ui16_cmd = PacmodCmd.TURN_NONE + + self.accel_cmd.f64_cmd = command.accelerator_pedal_position + if command.brake_pedal_position > 0.0: + self.accel_cmd.f64_cmd = 0.0 + self.brake_cmd.f64_cmd = command.brake_pedal_position + self.steer_cmd.angular_position = command.steering_wheel_angle + self.steer_cmd.angular_velocity_limit = command.steering_wheel_speed + print("**************************") + print("Steer cmd angular position {} velocity limit {}".format(self.steer_cmd.angular_position,self.steer_cmd.angular_velocity_limit)) + print("Accel pedal position {} brake position {}".format(self.accel_cmd.f64_cmd,self.brake_cmd.f64_cmd)) + maxacc = settings.get('vehicle.limits.max_accelerator_pedal') + maxbrake = settings.get('vehicle.limits.max_brake_pedal') + if self.accel_cmd.f64_cmd > maxacc: + print("Warning: commanded acceleration exceeded accel pedal limit") + self.accel_cmd.f64_cmd = maxacc + if self.brake_cmd.f64_cmd > maxbrake: + print("Warning: commanded braking exceeded brake pedal limit") + self.brake_cmd.f64_cmd = maxbrake + print("**************************") + + self.brake_cmd.enable = True + self.brake_cmd.clear = False + self.brake_cmd.ignore = False + + self.accel_cmd.enable = True + self.accel_cmd.clear = False + self.accel_cmd.ignore = False + + self.gear_cmd.ui16_cmd = PacmodCmd.SHIFT_FORWARD + self.gear_cmd.enable = True + self.gear_pub.publish(self.gear_cmd) + self.accel_pub.publish(self.accel_cmd) + self.brake_pub.publish(self.brake_cmd) + self.steer_pub.publish(self.steer_cmd) + self.turn_pub.publish(self.turn_cmd) + + self.last_command = command diff --git a/GEMstack/onboard/planning/mpc.py b/GEMstack/onboard/planning/mpc.py index 131cf342f..b0eac5a19 100644 --- a/GEMstack/onboard/planning/mpc.py +++ b/GEMstack/onboard/planning/mpc.py @@ -63,7 +63,7 @@ def compute(self, state: VehicleState, component: Component = None): x0 = np.array([state.pose.x, state.pose.y, state.pose.yaw % (2 * np.pi), state.v, state.front_wheel_angle]) - print(x0) + # print(x0) closest_dist,closest_time = self.path.closest_point_local((x0[0], x0[1]),[self.current_traj_parameter-7.0,self.current_traj_parameter+7.0], True) # Interpolate trajectory points to match MPC time horizon @@ -96,10 +96,10 @@ def compute(self, state: VehicleState, component: Component = None): traj_points.append(pt) # print("trajectory points: ", traj_points) - traj_str = ", ".join( - [f"np.array([{round(p[0], 8)}, {round(p[1], 8)}])" for p in traj_points] - ) - print(f"traj_points = [{traj_str}]") + # traj_str = ", ".join( + # [f"np.array([{round(p[0], 8)}, {round(p[1], 8)}])" for p in traj_points] + # ) + # print(f"traj_points = [{traj_str}]") # Apply gradually decreasing offset correction cur_offset = np.array([state.pose.x, state.pose.y]) - np.array(traj_points[0][0:2]) @@ -127,10 +127,10 @@ def compute(self, state: VehicleState, component: Component = None): traj_points[i] = np.array(traj_points[i]) + decay_ratio * offset_proj # print("trajectory points after correction: ", traj_points) - traj_corrected_str = ", ".join( - [f"np.array([{round(p[0], 8)}, {round(p[1], 8)}])" for p in traj_points] - ) - print(f"traj_corrected = [{traj_corrected_str}]") + # traj_corrected_str = ", ".join( + # [f"np.array([{round(p[0], 8)}, {round(p[1], 8)}])" for p in traj_points] + # ) + # print(f"traj_corrected = [{traj_corrected_str}]") target_angles = [] @@ -237,12 +237,9 @@ def model(x, u): delta = float(sol.value(x[1,4])) self.prev_x = sol.value(x) self.prev_u = sol.value(u) - # if x0[3] < 1.3: - # for i in range(len(self.prev_x)): - # assert self.prev_x[i,3] <= 1.3 - xy_array = [f"np.array([{round(self.prev_x[t,0],8)}, {round(self.prev_x[t,1],8)}])" for t in range(self.prev_x.shape[0])] - print("mpc = [", ", ".join(xy_array), "]") + # xy_array = [f"np.array([{round(self.prev_x[t,0],8)}, {round(self.prev_x[t,1],8)}])" for t in range(self.prev_x.shape[0])] + # print("mpc = [", ", ".join(xy_array), "]") print(acc, delta) # print(self.prev_u[0]) diff --git a/launch/fixed_route.yaml b/launch/fixed_route.yaml index f218caa5a..9ae1b3286 100644 --- a/launch/fixed_route.yaml +++ b/launch/fixed_route.yaml @@ -16,6 +16,7 @@ drive: planning: route_planning: type: StaticRoutePlanner + # args: [!relative_path '../GEMstack/knowledge/routes/yamada.csv'] args: [!relative_path '../GEMstack/knowledge/routes/xyhead_highbay_backlot_p.csv'] # args: [!relative_path '../GEMstack/knowledge/routes/reverse_15m.csv'] # args: [!relative_path '../GEMstack/knowledge/routes/offset.csv'] diff --git a/scenes/xyhead_demo.yaml b/scenes/xyhead_demo.yaml index 6e83a0a85..f590b5675 100644 --- a/scenes/xyhead_demo.yaml +++ b/scenes/xyhead_demo.yaml @@ -1,9 +1,9 @@ vehicle_state: [4.0, 5.0, 0.0, 0.0, 0.0] -agents: - ped1: - type: pedestrian - position: [15.0, 2.0] - nominal_velocity: -0.5 - target: [15.0,10.0] - behavior: loop +# agents: + # ped1: + # type: pedestrian + # position: [15.0, 2.0] + # nominal_velocity: -0.5 + # target: [15.0,10.0] + # behavior: loop \ No newline at end of file From 184b49719b0819b60c8faa86f1f2b38fe765783b Mon Sep 17 00:00:00 2001 From: PatrickWu Date: Sun, 11 May 2025 17:24:14 -0500 Subject: [PATCH 39/71] support gear change on real vehicle and mpc controller / adjust pedal active range / unify mpc interface --- GEMstack/knowledge/defaults/current.yaml | 1 + GEMstack/knowledge/vehicle/dynamics.py | 325 +++++++++--------- .../knowledge/vehicle/gem_e4_dynamics.yaml | 2 +- GEMstack/onboard/interface/gem_hardware.py | 24 +- .../onboard/perception/state_estimation.py | 189 +++++----- GEMstack/onboard/planning/mpc.py | 96 ++++-- launch/fixed_route.yaml | 10 +- 7 files changed, 354 insertions(+), 293 deletions(-) diff --git a/GEMstack/knowledge/defaults/current.yaml b/GEMstack/knowledge/defaults/current.yaml index 6e547a977..1426d24a4 100644 --- a/GEMstack/knowledge/defaults/current.yaml +++ b/GEMstack/knowledge/defaults/current.yaml @@ -30,6 +30,7 @@ control: mpc: dt: 0.2 # Time step for the MPC controller (seconds) horizon: 30 # Prediction horizon for the MPC controller (number of time steps) + desired_speed: trajectory # Speed reference source: can be "trajectory", "path", or a constant value # Shared longitudinal control parameters longitudinal_control: diff --git a/GEMstack/knowledge/vehicle/dynamics.py b/GEMstack/knowledge/vehicle/dynamics.py index 9bd33a92e..0bb7831c0 100644 --- a/GEMstack/knowledge/vehicle/dynamics.py +++ b/GEMstack/knowledge/vehicle/dynamics.py @@ -1,160 +1,165 @@ -"""Functions to model the vehicle's steering / drivetrain dynamics. - -TODO: calibrate drivetrain dynamics. Power curves are not yet implemented. - -TODO: add functions defining steering friction limits at different speeds and on different surfaces. -""" - -from ...utils import settings -from typing import Tuple -import math - -def sign(x): - return 1 if x > 0 else -1 if x < 0 else 0 - -def acceleration_to_pedal_positions(acceleration : float, velocity : float, pitch : float, gear : int) -> Tuple[float,float,int]: - """Converts acceleration in m/s^2 to pedal positions in % of pedal travel. - - Returns tuple (accelerator_pedal_position, brake_pedal_position, desired_gear) - """ - model = settings.get('vehicle.dynamics.acceleration_model','hang_v1') - if model == 'hang_v1': - if gear != 1: - print("WARNING can't handle gears other than 1 yet") - max_accel = settings.get('vehicle.dynamics.max_accelerator_acceleration')[1] # m/s^2 - max_brake = settings.get('vehicle.dynamics.max_brake_deceleration') # m/s^2 - dry_decel = settings.get('vehicle.dynamics.internal_dry_deceleration') # m/s^2 - accel_active_range = settings.get('vehicle.dynamics.accelerator_active_range') # pedal position fraction - brake_active_range = settings.get('vehicle.dynamics.brake_active_range') # pedal position fraction - #linear curves - if acceleration > -dry_decel: - if acceleration < -dry_decel*0.5 or (acceleration <= 0 and velocity < 0.1): # a little deadband to avoid oscillation - throttle_percent = 0.0 #drift to stop - else: - throttle_percent = accel_active_range[0] + (acceleration+dry_decel)/max_accel * (accel_active_range[1]-accel_active_range[0]) - brake_percent = 0 - else: - brake_percent = brake_active_range[0] + -(acceleration+dry_decel)/max_brake * (brake_active_range[1]-brake_active_range[0]) - throttle_percent = 0 - return (max(throttle_percent,0.0),max(brake_percent,0.0),1) - - elif model == 'kris_v1': - brake_max = settings.get('vehicle.dynamics.max_brake_deceleration') - reverse_accel_max = settings.get('vehicle.dynamics.max_accelerator_acceleration_reverse') - accel_max = settings.get('vehicle.dynamics.max_accelerator_acceleration') - assert isinstance(brake_max,(int,float)) - assert isinstance(reverse_accel_max,(int,float)) - assert isinstance(accel_max,list) - assert isinstance(acceleration,(int,float)) - - #compute required acceleration - vsign = sign(velocity) - gravity = settings.get('vehicle.dynamics.gravity') - internal_dry_deceleration = settings.get('vehicle.dynamics.internal_dry_deceleration') - internal_viscous_deceleration = settings.get('vehicle.dynamics.internal_viscous_deceleration') - aerodynamic_drag_coefficient = settings.get('vehicle.dynamics.aerodynamic_drag_coefficient') - accel_active_range = settings.get('vehicle.dynamics.accelerator_active_range') # pedal position fraction - brake_active_range = settings.get('vehicle.dynamics.brake_active_range') # pedal position fraction - acceleration_deadband = settings.get('vehicle.dynamics.acceleration_deadband',0.0) - - drag = -(aerodynamic_drag_coefficient * velocity**2) * vsign - internal_dry_deceleration * vsign - internal_viscous_deceleration * velocity - sin_pitch = math.sin(pitch) - acceleration -= drag + gravity * sin_pitch - #this is the net acceleration that should be achieved by accelerator / brake pedal - - #TODO: power curves to select optimal gear - if abs(acceleration) < acceleration_deadband: - #deadband? - return (0,0,gear) - if velocity * acceleration < 0: - accel_pos = 0 - brake_pos = -acceleration / brake_max - if brake_pos > 1.0: - brake_pos = 1.0 - return (accel_pos,brake_active_range[0] + brake_pos*(brake_active_range[1]-brake_active_range[0]),gear) - else: - #may want to switch gear? - if velocity == 0: - if acceleration < 0: - gear = -1 - else: - #forward gear - gear = 1 - if gear < 0: - #reverse gear - accel_pos = -acceleration / reverse_accel_max - brake_pos = 0 - if accel_pos > 1.0: - accel_pos = 1.0 - return (accel_active_range[0] + accel_pos*(accel_active_range[1]-accel_active_range[0]),brake_pos,-1) - elif gear > 0: - accel_pos = acceleration / accel_max[gear] - brake_pos = 0 - if accel_pos > 1.0: - accel_pos = 1.0 - return (accel_active_range[0] + accel_pos*(accel_active_range[1]-accel_active_range[0]),brake_pos,gear) - else: - #stay in neutral gear, brake - return (0,1,0) - - -def pedal_positions_to_acceleration(accelerator_pedal_position : float, brake_pedal_position : float, velocity : float, pitch : float, gear : int) -> float: - """Converts pedal positions in % of pedal travel to acceleration in m/s^2. - - Simulates drag, gravity, and internal viscous deceleration. - - Does not yet simulate velocity-dependent power. - - Returns acceleration - """ - brake_max = settings.get('vehicle.dynamics.max_brake_deceleration') - reverse_accel_max = settings.get('vehicle.dynamics.max_accelerator_acceleration_reverse') - accel_max = settings.get('vehicle.dynamics.max_accelerator_acceleration') - accel_active_range = settings.get('vehicle.dynamics.accelerator_active_range') # pedal position fraction - brake_active_range = settings.get('vehicle.dynamics.brake_active_range') # pedal position fraction - #normalize to 0-1 depending on pedal range - accelerator_pedal_position = (accelerator_pedal_position - accel_active_range[0]) / (accel_active_range[1]-accel_active_range[0]) - accelerator_pedal_position = min(1.0,max(accelerator_pedal_position,0.0)) - brake_pedal_position = (brake_pedal_position - brake_active_range[0]) / (brake_active_range[1]-brake_active_range[0]) - brake_pedal_position = min(1.0,max(brake_pedal_position,0.0)) - assert isinstance(brake_max,(int,float)) - assert isinstance(reverse_accel_max,(int,float)) - assert isinstance(accel_max,list) - assert isinstance(accelerator_pedal_position,(int,float)) - assert isinstance(brake_pedal_position,(int,float)) - vsign = sign(velocity) - brake_accel = -vsign * brake_max * brake_pedal_position - if gear < 0: - accel = - reverse_accel_max * accelerator_pedal_position - else: - accel = accel_max[gear] * accelerator_pedal_position - gravity = settings.get('vehicle.dynamics.gravity') - internal_dry_deceleration = settings.get('vehicle.dynamics.internal_dry_deceleration') - internal_viscous_deceleration = settings.get('vehicle.dynamics.internal_viscous_deceleration') - aerodynamic_drag_coefficient = settings.get('vehicle.dynamics.aerodynamic_drag_coefficient') - drag = -(aerodynamic_drag_coefficient * velocity**2) * vsign - internal_dry_deceleration * vsign - internal_viscous_deceleration * velocity - sin_pitch = math.sin(pitch) - if velocity == 0: - #brake accel and drag will be 0 based on velocity sign, so instead, - #brake and dry friction may will counteract prevailing acceleration - drag = -sign(accel - gravity*sin_pitch)*internal_dry_deceleration - brake_accel = -sign(accel - gravity*sin_pitch) * brake_max * brake_pedal_position - - #does gravity overcome static friction from braking + dry friction? - if abs(accel - gravity * sin_pitch) < abs(brake_accel + drag): - return 0 - #does gravity push against the gear setting? - if accel - gravity * sin_pitch < 0 and gear > 0: - return 0 - if accel - gravity * sin_pitch > 0 and gear < 0: - return 0 - return accel + brake_accel + drag - gravity * sin_pitch - - -def acceleration_limits(velocity : float, pitch : float, gear : int) -> Tuple[float,float]: - """Returns the min and max achievable acceleration at the given velocity, pitch, and gear.""" - vals = [] - vals.append(pedal_positions_to_acceleration(0.0,0.0,velocity,pitch,gear)) - vals.append(pedal_positions_to_acceleration(1.0,0.0,velocity,pitch,gear)) - vals.append(pedal_positions_to_acceleration(0.0,1.0,velocity,pitch,gear)) - return min(vals),max(vals) +"""Functions to model the vehicle's steering / drivetrain dynamics. + +TODO: calibrate drivetrain dynamics. Power curves are not yet implemented. + +TODO: add functions defining steering friction limits at different speeds and on different surfaces. +""" + +from ...utils import settings +from typing import Tuple +import math + +def sign(x): + return 1 if x > 0 else -1 if x < 0 else 0 + +def acceleration_to_pedal_positions(acceleration : float, velocity : float, pitch : float, gear : int) -> Tuple[float,float,int]: + """Converts acceleration in m/s^2 to pedal positions in % of pedal travel. + + Returns tuple (accelerator_pedal_position, brake_pedal_position, desired_gear) + """ + model = settings.get('vehicle.dynamics.acceleration_model','hang_v1') + if model == 'hang_v1': + if gear != 1: + print("WARNING can't handle gears other than 1 yet") + max_accel = settings.get('vehicle.dynamics.max_accelerator_acceleration')[1] # m/s^2 + max_brake = settings.get('vehicle.dynamics.max_brake_deceleration') # m/s^2 + dry_decel = settings.get('vehicle.dynamics.internal_dry_deceleration') # m/s^2 + accel_active_range = settings.get('vehicle.dynamics.accelerator_active_range') # pedal position fraction + brake_active_range = settings.get('vehicle.dynamics.brake_active_range') # pedal position fraction + #linear curves + if acceleration > -dry_decel: + if acceleration < -dry_decel*0.5 or (acceleration <= 0 and velocity < 0.1): # a little deadband to avoid oscillation + throttle_percent = 0.0 #drift to stop + else: + throttle_percent = accel_active_range[0] + (acceleration+dry_decel)/max_accel * (accel_active_range[1]-accel_active_range[0]) + brake_percent = 0 + else: + brake_percent = brake_active_range[0] + -(acceleration+dry_decel)/max_brake * (brake_active_range[1]-brake_active_range[0]) + throttle_percent = 0 + return (max(throttle_percent,0.0),max(brake_percent,0.0),1) + + elif model == 'kris_v1': + brake_max = settings.get('vehicle.dynamics.max_brake_deceleration') + reverse_accel_max = settings.get('vehicle.dynamics.max_accelerator_acceleration_reverse') + accel_max = settings.get('vehicle.dynamics.max_accelerator_acceleration') + assert isinstance(brake_max,(int,float)) + assert isinstance(reverse_accel_max,(int,float)) + assert isinstance(accel_max,list) + assert isinstance(acceleration,(int,float)) + + #compute required acceleration + vsign = sign(velocity) + gravity = settings.get('vehicle.dynamics.gravity') + internal_dry_deceleration = settings.get('vehicle.dynamics.internal_dry_deceleration') + internal_viscous_deceleration = settings.get('vehicle.dynamics.internal_viscous_deceleration') + aerodynamic_drag_coefficient = settings.get('vehicle.dynamics.aerodynamic_drag_coefficient') + accel_active_range = settings.get('vehicle.dynamics.accelerator_active_range') # pedal position fraction + brake_active_range = settings.get('vehicle.dynamics.brake_active_range') # pedal position fraction + acceleration_deadband = settings.get('vehicle.dynamics.acceleration_deadband',0.0) + + drag = -(aerodynamic_drag_coefficient * velocity**2) * vsign - internal_dry_deceleration * vsign - internal_viscous_deceleration * velocity + sin_pitch = math.sin(pitch) + acceleration -= drag + gravity * sin_pitch + #this is the net acceleration that should be achieved by accelerator / brake pedal + + #TODO: power curves to select optimal gear + if abs(acceleration) < acceleration_deadband: + #deadband? + return (0,0,gear) + + #velocity threshold for switching gears + if abs(velocity) <= 0.01: + velocity = 0 + + if velocity * acceleration < 0: + accel_pos = 0 + brake_pos = -acceleration / brake_max + if brake_pos > 1.0: + brake_pos = 1.0 + return (accel_pos,brake_active_range[0] + brake_pos*(brake_active_range[1]-brake_active_range[0]),gear) + else: + #may want to switch gear? + if velocity == 0: + if acceleration < 0: + gear = -1 + else: + #forward gear + gear = 1 + if gear < 0: + #reverse gear + accel_pos = -acceleration / reverse_accel_max + brake_pos = 0 + if accel_pos > 1.0: + accel_pos = 1.0 + return (accel_active_range[0] + accel_pos*(accel_active_range[1]-accel_active_range[0]),brake_pos,-1) + elif gear > 0: + accel_pos = acceleration / accel_max[gear] + brake_pos = 0 + if accel_pos > 1.0: + accel_pos = 1.0 + return (accel_active_range[0] + accel_pos*(accel_active_range[1]-accel_active_range[0]),brake_pos,gear) + else: + #stay in neutral gear, brake + return (0,1,0) + + +def pedal_positions_to_acceleration(accelerator_pedal_position : float, brake_pedal_position : float, velocity : float, pitch : float, gear : int) -> float: + """Converts pedal positions in % of pedal travel to acceleration in m/s^2. + + Simulates drag, gravity, and internal viscous deceleration. + + Does not yet simulate velocity-dependent power. + + Returns acceleration + """ + brake_max = settings.get('vehicle.dynamics.max_brake_deceleration') + reverse_accel_max = settings.get('vehicle.dynamics.max_accelerator_acceleration_reverse') + accel_max = settings.get('vehicle.dynamics.max_accelerator_acceleration') + accel_active_range = settings.get('vehicle.dynamics.accelerator_active_range') # pedal position fraction + brake_active_range = settings.get('vehicle.dynamics.brake_active_range') # pedal position fraction + #normalize to 0-1 depending on pedal range + accelerator_pedal_position = (accelerator_pedal_position - accel_active_range[0]) / (accel_active_range[1]-accel_active_range[0]) + accelerator_pedal_position = min(1.0,max(accelerator_pedal_position,0.0)) + brake_pedal_position = (brake_pedal_position - brake_active_range[0]) / (brake_active_range[1]-brake_active_range[0]) + brake_pedal_position = min(1.0,max(brake_pedal_position,0.0)) + assert isinstance(brake_max,(int,float)) + assert isinstance(reverse_accel_max,(int,float)) + assert isinstance(accel_max,list) + assert isinstance(accelerator_pedal_position,(int,float)) + assert isinstance(brake_pedal_position,(int,float)) + vsign = sign(velocity) + brake_accel = -vsign * brake_max * brake_pedal_position + if gear < 0: + accel = - reverse_accel_max * accelerator_pedal_position + else: + accel = accel_max[gear] * accelerator_pedal_position + gravity = settings.get('vehicle.dynamics.gravity') + internal_dry_deceleration = settings.get('vehicle.dynamics.internal_dry_deceleration') + internal_viscous_deceleration = settings.get('vehicle.dynamics.internal_viscous_deceleration') + aerodynamic_drag_coefficient = settings.get('vehicle.dynamics.aerodynamic_drag_coefficient') + drag = -(aerodynamic_drag_coefficient * velocity**2) * vsign - internal_dry_deceleration * vsign - internal_viscous_deceleration * velocity + sin_pitch = math.sin(pitch) + if velocity == 0: + #brake accel and drag will be 0 based on velocity sign, so instead, + #brake and dry friction may will counteract prevailing acceleration + drag = -sign(accel - gravity*sin_pitch)*internal_dry_deceleration + brake_accel = -sign(accel - gravity*sin_pitch) * brake_max * brake_pedal_position + + #does gravity overcome static friction from braking + dry friction? + if abs(accel - gravity * sin_pitch) < abs(brake_accel + drag): + return 0 + #does gravity push against the gear setting? + if accel - gravity * sin_pitch < 0 and gear > 0: + return 0 + if accel - gravity * sin_pitch > 0 and gear < 0: + return 0 + return accel + brake_accel + drag - gravity * sin_pitch + + +def acceleration_limits(velocity : float, pitch : float, gear : int) -> Tuple[float,float]: + """Returns the min and max achievable acceleration at the given velocity, pitch, and gear.""" + vals = [] + vals.append(pedal_positions_to_acceleration(0.0,0.0,velocity,pitch,gear)) + vals.append(pedal_positions_to_acceleration(1.0,0.0,velocity,pitch,gear)) + vals.append(pedal_positions_to_acceleration(0.0,1.0,velocity,pitch,gear)) + return min(vals),max(vals) diff --git a/GEMstack/knowledge/vehicle/gem_e4_dynamics.yaml b/GEMstack/knowledge/vehicle/gem_e4_dynamics.yaml index d20d2c357..a2ac7c9de 100644 --- a/GEMstack/knowledge/vehicle/gem_e4_dynamics.yaml +++ b/GEMstack/knowledge/vehicle/gem_e4_dynamics.yaml @@ -13,7 +13,7 @@ max_accelerator_power: #Watts. Power at max accelerator pedal, by gear max_accelerator_power_reverse: 10000.0 #Watts. Power (backwards) in reverse gear acceleration_model : kris_v1 -accelerator_active_range : [0.2, 1.0] #range of accelerator pedal where output acceleration is not flat +accelerator_active_range : [0.32, 1.0] #range of accelerator pedal where output acceleration is not flat brake_active_range : [0,1] #range of brake pedal where output deceleration is not flat internal_dry_deceleration: 0.2 #m/s^2: deceleration due to internal dry friction (non-speed dependent) diff --git a/GEMstack/onboard/interface/gem_hardware.py b/GEMstack/onboard/interface/gem_hardware.py index 681528d1d..34089ca6c 100644 --- a/GEMstack/onboard/interface/gem_hardware.py +++ b/GEMstack/onboard/interface/gem_hardware.py @@ -20,7 +20,7 @@ from tf.transformations import euler_from_quaternion, quaternion_from_euler # GEM PACMod Headers -from pacmod_msgs.msg import PositionWithSpeed, PacmodCmd, SystemRptFloat, VehicleSpeedRpt, GlobalRpt +from pacmod_msgs.msg import PositionWithSpeed, PacmodCmd, SystemRptFloat, VehicleSpeedRpt, GlobalRpt, SystemRptInt # OpenCV and cv2 bridge import cv2 @@ -49,6 +49,7 @@ def __init__(self): self.speed_sub = rospy.Subscriber("/pacmod/parsed_tx/vehicle_speed_rpt", VehicleSpeedRpt, self.speed_callback) self.steer_sub = rospy.Subscriber("/pacmod/parsed_tx/steer_rpt", SystemRptFloat, self.steer_callback) self.global_sub = rospy.Subscriber("/pacmod/parsed_tx/global_rpt", GlobalRpt, self.global_callback) + self.gear_sub = rospy.Subscriber("/pacmod/parsed_tx/shift_rpt", SystemRptInt, self.geer_callback) self.gnss_sub = None self.imu_sub = None self.front_radar_sub = None @@ -125,6 +126,18 @@ def speed_callback(self,msg : VehicleSpeedRpt): def steer_callback(self, msg): self.last_reading.steering_wheel_angle = msg.output + + def geer_callback(self, msg): + # map pacmod gear to gear in vehicle state + if msg.output == 2: + # Neutral + self.last_reading.gear = 0 + elif msg.output == 1: + # Reverse + self.last_reading.gear = -1 + else: + #Forward + self.last_reading.gear = 1 def global_callback(self, msg): self.faults = [] @@ -317,7 +330,14 @@ def send_command(self, command : GEMVehicleCommand): self.accel_cmd.clear = False self.accel_cmd.ignore = False - self.gear_cmd.ui16_cmd = PacmodCmd.SHIFT_FORWARD + #switch gear + if command.gear == -1: + self.gear_cmd.ui16_cmd = PacmodCmd.SHIFT_REVERSE + elif command.gear == 1: + self.gear_cmd.ui16_cmd = PacmodCmd.SHIFT_FORWARD + else: + self.gear_cmd.ui16_cmd = PacmodCmd.SHIFT_NEUTRAL + self.gear_cmd.enable = True self.gear_pub.publish(self.gear_cmd) self.accel_pub.publish(self.accel_cmd) diff --git a/GEMstack/onboard/perception/state_estimation.py b/GEMstack/onboard/perception/state_estimation.py index 4aef25659..06c6f994e 100644 --- a/GEMstack/onboard/perception/state_estimation.py +++ b/GEMstack/onboard/perception/state_estimation.py @@ -1,93 +1,98 @@ -from dataclasses import replace -import math -from typing import List -from ...utils import settings -from ...mathutils import transforms -from ...state.vehicle import VehicleState,VehicleGearEnum -from ...state.physical_object import ObjectFrameEnum,ObjectPose,convert_xyhead -from ...knowledge.vehicle.geometry import front2steer,steer2front -from ...mathutils.signal import OnlineLowPassFilter -from ..interface.gem import GEMInterface -from ..component import Component -from ..interface.gem import GNSSReading - -class GNSSStateEstimator(Component): - """Just looks at the GNSS reading to estimate the vehicle state""" - def __init__(self, vehicle_interface : GEMInterface): - self.vehicle_interface = vehicle_interface - if 'gnss' not in vehicle_interface.sensors(): - raise RuntimeError("GNSS sensor not available") - vehicle_interface.subscribe_sensor('gnss',self.gnss_callback,GNSSReading) - self.gnss_pose = None - self.location = settings.get('vehicle.calibration.gnss_location')[:2] - self.yaw_offset = settings.get('vehicle.calibration.gnss_yaw') - self.speed_filter = OnlineLowPassFilter(1.2, 30, 4) - self.status = None - - # Get GNSS information - def gnss_callback(self, reading : GNSSReading): - self.gnss_pose = reading.pose - self.gnss_speed = reading.speed - self.status = reading.status - - def rate(self): - return 10.0 - - def state_outputs(self) -> List[str]: - return ['vehicle'] - - def healthy(self): - return self.gnss_pose is not None - - def update(self) -> VehicleState: - if self.gnss_pose is None: - return - #TODO: figure out what this status means - #print("INS status",self.status) - - # vehicle gnss heading (yaw) in radians - # vehicle x, y position in fixed local frame, in meters - # reference point is located at the center of GNSS antennas - localxy = transforms.rotate2d(self.location,-self.yaw_offset) - gnss_xyhead_inv = (-localxy[0],-localxy[1],-self.yaw_offset) - center_xyhead = self.gnss_pose.apply_xyhead(gnss_xyhead_inv) - vehicle_pose_global = replace(self.gnss_pose, - t=self.vehicle_interface.time(), - x=center_xyhead[0], - y=center_xyhead[1], - yaw=center_xyhead[2]) - - readings = self.vehicle_interface.get_reading() - raw = readings.to_state(vehicle_pose_global) - - #filtering speed - raw.v = self.gnss_speed - #filt_vel = self.speed_filter(raw.v) - #raw.v = filt_vel - return raw - - - -class OmniscientStateEstimator(Component): - def __init__(self, vehicle_interface : GEMInterface): - self.vehicle_interface = vehicle_interface - if 'gnss' not in vehicle_interface.sensors(): - raise RuntimeError("GNSS sensor not available") - vehicle_interface.subscribe_sensor('gnss',self.fake_gnss_callback) - self.vehicle_state = None - - # Get GNSS information - def fake_gnss_callback(self, vehicle_state): - self.vehicle_state = vehicle_state - - def rate(self): - return 50.0 - - def state_outputs(self) -> List[str]: - return ['vehicle'] - - def healthy(self): - return self.vehicle_state is not None - - def update(self) -> VehicleState: +from dataclasses import replace +import math +from typing import List +from ...utils import settings +from ...mathutils import transforms +from ...state.vehicle import VehicleState,VehicleGearEnum +from ...state.physical_object import ObjectFrameEnum,ObjectPose,convert_xyhead +from ...knowledge.vehicle.geometry import front2steer,steer2front +from ...mathutils.signal import OnlineLowPassFilter +from ..interface.gem import GEMInterface +from ..component import Component +from ..interface.gem import GNSSReading + +class GNSSStateEstimator(Component): + """Just looks at the GNSS reading to estimate the vehicle state""" + def __init__(self, vehicle_interface : GEMInterface): + self.vehicle_interface = vehicle_interface + if 'gnss' not in vehicle_interface.sensors(): + raise RuntimeError("GNSS sensor not available") + vehicle_interface.subscribe_sensor('gnss',self.gnss_callback,GNSSReading) + self.gnss_pose = None + self.location = settings.get('vehicle.calibration.gnss_location')[:2] + self.yaw_offset = settings.get('vehicle.calibration.gnss_yaw') + self.speed_filter = OnlineLowPassFilter(1.2, 30, 4) + self.status = None + + # Get GNSS information + def gnss_callback(self, reading : GNSSReading): + self.gnss_pose = reading.pose + self.gnss_speed = reading.speed + self.status = reading.status + + def rate(self): + return 10.0 + + def state_outputs(self) -> List[str]: + return ['vehicle'] + + def healthy(self): + return self.gnss_pose is not None + + def update(self) -> VehicleState: + if self.gnss_pose is None: + return + #TODO: figure out what this status means + #print("INS status",self.status) + + # vehicle gnss heading (yaw) in radians + # vehicle x, y position in fixed local frame, in meters + # reference point is located at the center of GNSS antennas + localxy = transforms.rotate2d(self.location,-self.yaw_offset) + gnss_xyhead_inv = (-localxy[0],-localxy[1],-self.yaw_offset) + center_xyhead = self.gnss_pose.apply_xyhead(gnss_xyhead_inv) + vehicle_pose_global = replace(self.gnss_pose, + t=self.vehicle_interface.time(), + x=center_xyhead[0], + y=center_xyhead[1], + yaw=center_xyhead[2]) + + readings = self.vehicle_interface.get_reading() + raw = readings.to_state(vehicle_pose_global) + + #filtering speed + raw.v = self.gnss_speed + + # Assume no backward slide, use gear to decide velocity sign + if raw.gear == -1: + raw.v *= -1 + + #filt_vel = self.speed_filter(raw.v) + #raw.v = filt_vel + return raw + + + +class OmniscientStateEstimator(Component): + def __init__(self, vehicle_interface : GEMInterface): + self.vehicle_interface = vehicle_interface + if 'gnss' not in vehicle_interface.sensors(): + raise RuntimeError("GNSS sensor not available") + vehicle_interface.subscribe_sensor('gnss',self.fake_gnss_callback) + self.vehicle_state = None + + # Get GNSS information + def fake_gnss_callback(self, vehicle_state): + self.vehicle_state = vehicle_state + + def rate(self): + return 50.0 + + def state_outputs(self) -> List[str]: + return ['vehicle'] + + def healthy(self): + return self.vehicle_state is not None + + def update(self) -> VehicleState: return self.vehicle_state \ No newline at end of file diff --git a/GEMstack/onboard/planning/mpc.py b/GEMstack/onboard/planning/mpc.py index b0eac5a19..5ba45b232 100644 --- a/GEMstack/onboard/planning/mpc.py +++ b/GEMstack/onboard/planning/mpc.py @@ -9,7 +9,7 @@ class MPCController(object): """Model Predictive Controller for trajectory tracking.""" - def __init__(self, T=None, dt=None): + def __init__(self, T=None, dt=None, desired_speed=None): self.T = T if T is not None else settings.get('control.mpc.horizon', 30) self.dt = dt if dt is not None else settings.get('control.mpc.dt', 0.2) self.L = settings.get('vehicle.geometry.wheelbase') @@ -18,7 +18,13 @@ def __init__(self, T=None, dt=None): self.delta_rate_bounds = [-0.4, 0.4] # Predefined front wheel rate limit to simplify computation self.steering_angle_range = [settings.get('vehicle.geometry.min_steering_angle'),settings.get('vehicle.geometry.max_steering_angle')] self.a_bounds = [-settings.get('vehicle.limits.max_deceleration'), settings.get('vehicle.limits.max_acceleration')] - self.trajectory = None + + if desired_speed is not None: + self.desired_speed_source = desired_speed + else: + self.desired_speed_source = settings.get('control.pure_pursuit.desired_speed',"path") + self.desired_speed = self.desired_speed_source if isinstance(self.desired_speed_source,(int,float)) else None + self.prev_x = None # Previous state trajectory self.prev_u = None # Previous control inputs self.path = None @@ -32,17 +38,43 @@ def set_path(self, path : Path): self.iter = 0 if len(path.points[0]) > 2: path = path.get_dims([0,1]) - self.path = path.arc_length_parameterize(1.0) - # self.path = path.arc_length_parameterize(0.5) self.current_path_parameter = 0.0 # self.prev_u = None # self.prev_x = None if not isinstance(path,Trajectory): - self.trajectory = None + if self.desired_speed_source in ['path', 'trajectory']: + print("rase") + raise ValueError("MPC: Provided path has no timing. Either provide a Trajectory or set a constant desired_speed.") + self.path = path.arc_length_parameterize(self.desired_speed) self.current_traj_parameter = 0.0 else: - self.trajectory = path - self.current_traj_parameter = self.trajectory.domain()[0] + self.path = path + self.current_traj_parameter = self.path.domain()[0] + + def clip_reverse_path_with_times(self, points, times): + def angle_between(p1, p2): + delta = p2 - p1 + return np.arctan2(delta[1], delta[0]) + + points = np.array(points) + times = np.array(times) + + if len(points) < 3: + return points, times + + ref_angle = angle_between(points[0], points[1]) + clipped_points = [points[0]] + clipped_times = [times[0]] + + for i in range(1, len(points)): + angle = angle_between(points[i-1], points[i]) + angle_diff = np.abs((angle - ref_angle + np.pi) % (2 * np.pi) - np.pi) + if angle_diff > np.pi / 2: + break + clipped_points.append(points[i]) + clipped_times.append(times[i]) + + return np.array(clipped_points), np.array(clipped_times) def compute(self, state: VehicleState, component: Component = None): """Compute the control commands using MPC.""" @@ -55,18 +87,11 @@ def compute(self, state: VehicleState, component: Component = None): if self.path.frame != state.pose.frame: print("Transforming trajectory from",self.path.frame.name,"to",state.pose.frame.name) self.path = self.path.to_frame(state.pose.frame, current_pose=state.pose) - - if self.trajectory is not None: - if self.trajectory.frame != state.pose.frame: - print("Transforming trajectory from",self.trajectory.frame.name,"to",state.pose.frame.name) - self.trajectory = self.trajectory.to_frame(state.pose.frame, current_pose=state.pose) x0 = np.array([state.pose.x, state.pose.y, state.pose.yaw % (2 * np.pi), state.v, state.front_wheel_angle]) - # print(x0) - closest_dist,closest_time = self.path.closest_point_local((x0[0], x0[1]),[self.current_traj_parameter-7.0,self.current_traj_parameter+7.0], True) - # Interpolate trajectory points to match MPC time horizon + closest_dist,closest_time = self.path.closest_point_local((x0[0], x0[1]),[self.current_traj_parameter-0.0,self.current_traj_parameter+1.0], True) self.current_traj_parameter = closest_time times = self.path.times @@ -75,24 +100,28 @@ def compute(self, state: VehicleState, component: Component = None): while j < len(times) - 1 and times[j+1] < closest_time: j += 1 self.current_path_parameter = j - # print("closest_index: ", self.current_path_parameter) - # print("vehicle position: ", x0[0], x0[1]) - # print("closest_point: ", points[int(self.current_path_parameter)]) - # print("closest_time: ", closest_time) - # print("local points: ", points[j-5:j+5]) + # Slice path from j + sliced_points = points[j:] + sliced_times = times[j:] + + # Clip reversed part + new_points, new_times = self.clip_reverse_path_with_times(sliced_points, sliced_times) + + # Interpolate trajectory points to match MPC time horizon traj_points = [] + j = 0 for i in range(self.T + 1): t_query = closest_time + i * self.dt - if t_query <= times[0]: - traj_points.append(points[0]) - elif t_query >= times[-1]: - traj_points.append(points[-1]) + if t_query <= new_times[0]: + traj_points.append(new_points[0]) + elif t_query >= new_times[-1]: + traj_points.append(new_points[-1]) else: - while j < len(times) - 2 and times[j+1] < t_query: + while j < len(new_times) - 2 and new_times[j+1] < t_query: j += 1 - alpha = (t_query - times[j]) / (times[j+1] - times[j]) - pt = (1 - alpha) * np.array(points[j]) + alpha * np.array(points[j+1]) + alpha = (t_query - new_times[j]) / (new_times[j+1] - new_times[j]) + pt = (1 - alpha) * np.array(new_points[j]) + alpha * np.array(new_points[j+1]) traj_points.append(pt) # print("trajectory points: ", traj_points) @@ -103,9 +132,10 @@ def compute(self, state: VehicleState, component: Component = None): # Apply gradually decreasing offset correction cur_offset = np.array([state.pose.x, state.pose.y]) - np.array(traj_points[0][0:2]) + overcorrection_guard = 5 for i in range(len(traj_points)): - s = i / (len(traj_points) + 5) # Normalized [0, 1] - decay_ratio = (1 - s) ** 1 # linear decay + s = i / (len(traj_points) + overcorrection_guard) # Normalized [0, 1] and add some offset to prevent overcorrection + decay_ratio = (1 - s) ** 1 # linear decay on each iteration to perform quadratic offset correction over time # Get direction of trajectory at this point if i < len(traj_points) - 1: @@ -132,7 +162,7 @@ def compute(self, state: VehicleState, component: Component = None): # ) # print(f"traj_corrected = [{traj_corrected_str}]") - + # Compute target angles target_angles = [] prev_angle = x0[2] @@ -191,7 +221,6 @@ def model(x, u): opti.subject_to(x[t+1,:] == x_next) target = traj_points[t+1] # Cost function - # Apply larger cost for the first three points weight = 10 if t < 3 else 1 # Larger weight for the first three points obj += weight * casadi.sumsqr(x[t + 1, 0:2] - casadi.reshape(target[0:2], 1, 2)) @@ -199,9 +228,10 @@ def model(x, u): obj += 0.1 * casadi.sumsqr(u[t, :]) # Heading angle error - # theta_error = x[t + 1, 2] - target_angles[t] - # obj += 1 * weight * casadi.sumsqr(theta_error) + theta_error = x[t + 1, 2] - target_angles[t] + obj += 1 * weight * casadi.sumsqr(theta_error) + # Front wheel angle error # if t != self.T - 1: # delta_error = x[t + 1, 4] - delta_desired[t] # obj += 0.1 * weight * casadi.sumsqr(delta_error) diff --git a/launch/fixed_route.yaml b/launch/fixed_route.yaml index 9ae1b3286..f083c003c 100644 --- a/launch/fixed_route.yaml +++ b/launch/fixed_route.yaml @@ -16,8 +16,8 @@ drive: planning: route_planning: type: StaticRoutePlanner - # args: [!relative_path '../GEMstack/knowledge/routes/yamada.csv'] - args: [!relative_path '../GEMstack/knowledge/routes/xyhead_highbay_backlot_p.csv'] + args: [!relative_path '../GEMstack/knowledge/routes/yamada.csv'] + # args: [!relative_path '../GEMstack/knowledge/routes/xyhead_highbay_backlot_p.csv'] # args: [!relative_path '../GEMstack/knowledge/routes/reverse_15m.csv'] # args: [!relative_path '../GEMstack/knowledge/routes/offset.csv'] # args: [!relative_path '../GEMstack/knowledge/routes/forward_15m.csv'] @@ -32,10 +32,10 @@ drive: args: [null] #desired speed in m/s. If null, this will keep the route untimed for the trajectory tracker trajectory_tracking: # type: pure_pursuit.PurePursuitTrajectoryTracker - type: stanley.StanleyTrajectoryTracker - args: {desired_speed: 5.0} #approximately 5mph + # type: stanley.StanleyTrajectoryTracker + type: mpc.MPCTrajectoryTracker + args: {desired_speed: 0.5} #approximately 5mph print: True - # type: mpc.MPCTrajectoryTracker log: # Specify the top-level folder to save the log files. Default is 'logs' #folder : 'logs' From b71e9fb117a0be35775ad5e8bbf1448bfa5cd934 Mon Sep 17 00:00:00 2001 From: Jugal Date: Mon, 12 May 2025 00:43:47 -0500 Subject: [PATCH 40/71] Update and rename stanley.py to stanley_reverse.py stanley_reverse is for paths focused more on reverse like parallel parking or s shape reverse. That does'nt mean it doesn't work on staright path like p shape or oval but just not that good like basic stanley.py which is developed purely for forward paths. --- GEMstack/onboard/planning/{stanley.py => stanley_reverse.py} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename GEMstack/onboard/planning/{stanley.py => stanley_reverse.py} (97%) diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley_reverse.py similarity index 97% rename from GEMstack/onboard/planning/stanley.py rename to GEMstack/onboard/planning/stanley_reverse.py index 532dc9d9c..4283e4479 100644 --- a/GEMstack/onboard/planning/stanley.py +++ b/GEMstack/onboard/planning/stanley_reverse.py @@ -432,4 +432,4 @@ def update(self, vehicle: VehicleState, trajectory: Trajectory): def healthy(self): """Optional: check if the controller has a valid path.""" - return self.stanley.path is not None \ No newline at end of file + return self.stanley.path is not None From 609f22a18b3ccacb402c5910c40c683fbee167eb Mon Sep 17 00:00:00 2001 From: Jugal Date: Mon, 12 May 2025 00:45:01 -0500 Subject: [PATCH 41/71] Create stanley.py stanley.py is purely for forward routes --- stanley.py | 312 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 312 insertions(+) create mode 100644 stanley.py diff --git a/stanley.py b/stanley.py new file mode 100644 index 000000000..8f871f488 --- /dev/null +++ b/stanley.py @@ -0,0 +1,312 @@ +import numpy as np +from math import sin, cos, atan2 + +# These imports match your existing project structure +from ...mathutils.control import PID +from ...utils import settings +from ...mathutils import transforms +from ...knowledge.vehicle.geometry import front2steer +from ...knowledge.vehicle.dynamics import acceleration_to_pedal_positions +from ...state.vehicle import VehicleState, ObjectFrameEnum +from ...state.trajectory import Path, Trajectory +from ..interface.gem import GEMVehicleCommand +from ..component import Component + +##################################### +# 1. Angle normalization +##################################### +def normalise_angle(angle): + """Wraps an angle to the [-pi, pi] range.""" + while angle > np.pi: + angle -= 2.0 * np.pi + while angle < -np.pi: + angle += 2.0 * np.pi + return angle + +##################################### +# 2. Stanley-based controller with longitudinal PID +##################################### +class Stanley(object): + """ + A Stanley controller that handles lateral control (steering) + plus a basic longitudinal control (PID + optional feedforward). + It has been modified to reduce oscillations by: + 1) Lower gains + 2) Steering damping + 3) Low-pass filter on steering + 4) Gentler speed logic when cornering + """ + + def __init__( + self, + control_gain=None, + softening_gain=None, + desired_speed=None + ): + """ + :param control_gain: Stanley lateral control gain k (lowered from the default to reduce overshoot). + :param softening_gain: Softening gain k_soft. + :param yaw_rate_gain: Yaw-rate gain k_yaw_rate (helps reduce oscillations). + :param steering_damp_gain: Steering damping gain k_damp_steer. + :param desired_speed: Desired speed (float) or 'path'/'trajectory'. + """ + + # 1) Lower Gains + self.k = control_gain if control_gain is not None else settings.get('control.stanley.control_gain') + self.k_soft = softening_gain if softening_gain is not None else settings.get('control.stanley.softening_gain') + + # Typically, this is the max front-wheel steering angle in radians + self.max_steer = settings.get('vehicle.geometry.max_wheel_angle') + self.wheelbase = settings.get('vehicle.geometry.wheelbase') + + # Basic longitudinal constraints + self.speed_limit = settings.get('vehicle.limits.max_speed') + self.max_accel = settings.get('vehicle.limits.max_acceleration') + self.max_decel = settings.get('vehicle.limits.max_deceleration') + + # PID for longitudinal speed tracking + p = settings.get('control.longitudinal_control.pid_p') + d = settings.get('control.longitudinal_control.pid_d') + i = settings.get('control.longitudinal_control.pid_i') + self.pid_speed = PID(p, d, i, windup_limit=20) + + # Speed source: numeric or derived from path/trajectory + if desired_speed is not None: + self.desired_speed_source = desired_speed + else: + self.desired_speed_source = settings.get('control.stanley.desired_speed', 'path') + + if isinstance(self.desired_speed_source, (int, float)): + self.desired_speed = self.desired_speed_source + else: + self.desired_speed = None + + # For path/trajectory + self.path_arg = None + self.path = None + self.trajectory = None + self.current_path_parameter = 0.0 + self.current_traj_parameter = 0.0 + self.t_last = None + + def set_path(self, path: Path): + """Sets the path or trajectory to track.""" + if path == self.path_arg: + return + self.path_arg = path + + # If the path has more than 2D, reduce to (x,y) + if len(path.points[0]) > 2: + path = path.get_dims([0,1]) + + # If no timing info, we can't rely on 'path'/'trajectory' speed + if not isinstance(path, Trajectory): + self.path = path.arc_length_parameterize() + self.trajectory = None + self.current_traj_parameter = 0.0 + if self.desired_speed_source in ['path', 'trajectory']: + raise ValueError("Stanley: Provided path has no timing. Either provide a Trajectory or set a constant desired_speed.") + else: + self.path = path.arc_length_parameterize() + self.trajectory = path + self.current_traj_parameter = self.trajectory.domain()[0] + + self.current_path_parameter = 0.0 + + def _find_front_axle_position(self, x, y, yaw): + """Compute front-axle world position from the center/rear and yaw.""" + fx = x + self.wheelbase * cos(yaw) + fy = y + self.wheelbase * sin(yaw) + return fx, fy + + def compute(self, state: VehicleState, component: Component = None): + """Compute the control outputs: (longitudinal acceleration, front wheel angle).""" + t = state.pose.t + if self.t_last is None: + self.t_last = t + dt = t - self.t_last + + # Current vehicle states + curr_x = state.pose.x + curr_y = state.pose.y + curr_yaw = state.pose.yaw if state.pose.yaw is not None else 0.0 + speed = state.v + + if self.path is None: + if component: + component.debug_event("No path provided to Stanley controller. Doing nothing.") + return (0.0, 0.0) + + # Ensure same frame + if self.path.frame != state.pose.frame: + if component: + component.debug_event(f"Transforming path from {self.path.frame.name} to {state.pose.frame.name}") + self.path = self.path.to_frame(state.pose.frame, current_pose=state.pose) + + if self.trajectory and self.trajectory.frame != state.pose.frame: + if component: + component.debug_event(f"Transforming trajectory from {self.trajectory.frame.name} to {state.pose.frame.name}") + self.trajectory = self.trajectory.to_frame(state.pose.frame, current_pose=state.pose) + + # 1) Closest point + fx, fy = self._find_front_axle_position(curr_x, curr_y, curr_yaw) + search_start = self.current_path_parameter - 5.0 + search_end = self.current_path_parameter + 5.0 + closest_dist, closest_parameter = self.path.closest_point_local((fx, fy), [search_start, search_end]) + self.current_path_parameter = closest_parameter + + # 2) Path heading + target_x, target_y = self.path.eval(closest_parameter) + tangent = self.path.eval_tangent(closest_parameter) + path_yaw = atan2(tangent[1], tangent[0]) + desired_x = target_x + desired_y = target_y + # 3) Lateral error + dx = fx - target_x + dy = fy - target_y + left_vec = np.array([sin(curr_yaw), -cos(curr_yaw)]) + cross_track_error = np.sign(np.dot(np.array([dx, dy]), left_vec)) * closest_dist + + # 4) Heading error + yaw_error = normalise_angle(path_yaw - curr_yaw) + + # 5) Standard Stanley terms + cross_term = atan2(self.k * cross_track_error, self.k_soft + speed) + desired_steering_angle = yaw_error + cross_term + + desired_speed = self.desired_speed + feedforward_accel = 0.0 + + if self.trajectory and self.desired_speed_source in ['path', 'trajectory']: + if len(self.trajectory.points) < 2 or self.current_path_parameter >= self.path.domain()[1]: + # End of trajectory -> stop + if component: + component.debug_event("Stanley: Past the end of trajectory, stopping.") + desired_speed = 0.0 + feedforward_accel = -2.0 + else: + if self.desired_speed_source == 'path': + current_trajectory_time = self.trajectory.parameter_to_time(self.current_path_parameter) + else: + self.current_traj_parameter += dt + current_trajectory_time = self.current_traj_parameter + + deriv = self.trajectory.eval_derivative(current_trajectory_time) + desired_speed = min(np.linalg.norm(deriv), self.speed_limit) + + difference_dt = 0.1 + future_t = current_trajectory_time + difference_dt + if future_t > self.trajectory.domain()[1]: + future_t = self.trajectory.domain()[1] + future_deriv = self.trajectory.eval_derivative(future_t) + next_desired_speed = min(np.linalg.norm(future_deriv), self.speed_limit) + feedforward_accel = (next_desired_speed - desired_speed) / difference_dt + feedforward_accel = np.clip(feedforward_accel, -self.max_decel, self.max_accel) + else: + if desired_speed is None: + desired_speed = 4.0 + + # Cross-track-based slowdown (less aggressive than before). + desired_speed *= np.exp(-abs(cross_track_error) * 0.6) + + # Clip to speed limit + if desired_speed > self.speed_limit: + desired_speed = self.speed_limit + + # PID for longitudinal control + speed_error = desired_speed - speed + output_accel = self.pid_speed.advance(e=speed_error, t=t, feedforward_term=feedforward_accel) + + # Clip acceleration + if output_accel > self.max_accel: + output_accel = self.max_accel + elif output_accel < -self.max_decel: + output_accel = -self.max_decel + + # Avoid negative accel when fully stopped + if desired_speed == 0 and abs(speed) < 0.01 and output_accel < 0: + output_accel = 0.0 + + # Debug + if component is not None: + # component.debug("Stanley: fx, fy", (fx, fy)) + component.debug('curr pt',(curr_x,curr_y)) + component.debug("desired_x",desired_x) + component.debug("desired_y",desired_y) + component.debug("Stanley: path param", self.current_path_parameter) + component.debug("Stanley: crosstrack dist", closest_dist) + component.debug("crosstrack error", cross_track_error) + component.debug("Stanley: yaw_error", yaw_error) + component.debug('steering_angle', desired_steering_angle) + component.debug("Stanley: desired_speed (m/s)", desired_speed) + component.debug("Stanley: feedforward_accel (m/s^2)", feedforward_accel) + component.debug("Stanley: output_accel (m/s^2)", output_accel) + + if output_accel > self.max_accel: + output_accel = self.max_accel + + if output_accel < -self.max_decel: + output_accel = -self.max_decel + + self.t_last = t + return (output_accel, desired_steering_angle) + +##################################### +# 3. Tracker component +##################################### +class StanleyTrajectoryTracker(Component): + """ + A trajectory-tracking Component that uses the above Stanley controller + for lateral control plus PID-based longitudinal control. + It now includes measures to mitigate oscillations. + """ + + def __init__(self, vehicle_interface=None, **kwargs): + """ + :param vehicle_interface: The low-level interface to send commands to the vehicle. + :param kwargs: Optional parameters to pass into the Stanley(...) constructor. + """ + self.stanley = Stanley(**kwargs) + self.vehicle_interface = vehicle_interface + + def rate(self): + """Control frequency in Hz.""" + return 50.0 + + def state_inputs(self): + """ + Required state inputs: + - Vehicle state + - Trajectory + """ + return ["vehicle", "trajectory"] + + def state_outputs(self): + """No direct output state here.""" + return [] + + def update(self, vehicle: VehicleState, trajectory: Trajectory): + """ + Per control cycle: + 1) Set the path/trajectory + 2) Compute (acceleration, front wheel angle) + 3) Convert front wheel angle to steering wheel angle (if necessary) + 4) Send command to the vehicle + """ + self.stanley.set_path(trajectory) + accel, f_delta = self.stanley.compute(vehicle, self) + + # If your low-level interface expects steering wheel angle: + steering_angle = front2steer(f_delta) + steering_angle = np.clip( + steering_angle, + settings.get('vehicle.geometry.min_steering_angle', -0.5), + settings.get('vehicle.geometry.max_steering_angle', 0.5) + ) + + cmd = self.vehicle_interface.simple_command(accel, steering_angle, vehicle) + self.vehicle_interface.send_command(cmd) + + def healthy(self): + """Optional: check if the controller has a valid path.""" + return self.stanley.path is not None From 9ec304e7d8fab68700cb67c12c55d753dd4f9cb1 Mon Sep 17 00:00:00 2001 From: Jugal Date: Mon, 12 May 2025 00:50:41 -0500 Subject: [PATCH 42/71] Delete stanley.py placed in wrong folder --- stanley.py | 312 ----------------------------------------------------- 1 file changed, 312 deletions(-) delete mode 100644 stanley.py diff --git a/stanley.py b/stanley.py deleted file mode 100644 index 8f871f488..000000000 --- a/stanley.py +++ /dev/null @@ -1,312 +0,0 @@ -import numpy as np -from math import sin, cos, atan2 - -# These imports match your existing project structure -from ...mathutils.control import PID -from ...utils import settings -from ...mathutils import transforms -from ...knowledge.vehicle.geometry import front2steer -from ...knowledge.vehicle.dynamics import acceleration_to_pedal_positions -from ...state.vehicle import VehicleState, ObjectFrameEnum -from ...state.trajectory import Path, Trajectory -from ..interface.gem import GEMVehicleCommand -from ..component import Component - -##################################### -# 1. Angle normalization -##################################### -def normalise_angle(angle): - """Wraps an angle to the [-pi, pi] range.""" - while angle > np.pi: - angle -= 2.0 * np.pi - while angle < -np.pi: - angle += 2.0 * np.pi - return angle - -##################################### -# 2. Stanley-based controller with longitudinal PID -##################################### -class Stanley(object): - """ - A Stanley controller that handles lateral control (steering) - plus a basic longitudinal control (PID + optional feedforward). - It has been modified to reduce oscillations by: - 1) Lower gains - 2) Steering damping - 3) Low-pass filter on steering - 4) Gentler speed logic when cornering - """ - - def __init__( - self, - control_gain=None, - softening_gain=None, - desired_speed=None - ): - """ - :param control_gain: Stanley lateral control gain k (lowered from the default to reduce overshoot). - :param softening_gain: Softening gain k_soft. - :param yaw_rate_gain: Yaw-rate gain k_yaw_rate (helps reduce oscillations). - :param steering_damp_gain: Steering damping gain k_damp_steer. - :param desired_speed: Desired speed (float) or 'path'/'trajectory'. - """ - - # 1) Lower Gains - self.k = control_gain if control_gain is not None else settings.get('control.stanley.control_gain') - self.k_soft = softening_gain if softening_gain is not None else settings.get('control.stanley.softening_gain') - - # Typically, this is the max front-wheel steering angle in radians - self.max_steer = settings.get('vehicle.geometry.max_wheel_angle') - self.wheelbase = settings.get('vehicle.geometry.wheelbase') - - # Basic longitudinal constraints - self.speed_limit = settings.get('vehicle.limits.max_speed') - self.max_accel = settings.get('vehicle.limits.max_acceleration') - self.max_decel = settings.get('vehicle.limits.max_deceleration') - - # PID for longitudinal speed tracking - p = settings.get('control.longitudinal_control.pid_p') - d = settings.get('control.longitudinal_control.pid_d') - i = settings.get('control.longitudinal_control.pid_i') - self.pid_speed = PID(p, d, i, windup_limit=20) - - # Speed source: numeric or derived from path/trajectory - if desired_speed is not None: - self.desired_speed_source = desired_speed - else: - self.desired_speed_source = settings.get('control.stanley.desired_speed', 'path') - - if isinstance(self.desired_speed_source, (int, float)): - self.desired_speed = self.desired_speed_source - else: - self.desired_speed = None - - # For path/trajectory - self.path_arg = None - self.path = None - self.trajectory = None - self.current_path_parameter = 0.0 - self.current_traj_parameter = 0.0 - self.t_last = None - - def set_path(self, path: Path): - """Sets the path or trajectory to track.""" - if path == self.path_arg: - return - self.path_arg = path - - # If the path has more than 2D, reduce to (x,y) - if len(path.points[0]) > 2: - path = path.get_dims([0,1]) - - # If no timing info, we can't rely on 'path'/'trajectory' speed - if not isinstance(path, Trajectory): - self.path = path.arc_length_parameterize() - self.trajectory = None - self.current_traj_parameter = 0.0 - if self.desired_speed_source in ['path', 'trajectory']: - raise ValueError("Stanley: Provided path has no timing. Either provide a Trajectory or set a constant desired_speed.") - else: - self.path = path.arc_length_parameterize() - self.trajectory = path - self.current_traj_parameter = self.trajectory.domain()[0] - - self.current_path_parameter = 0.0 - - def _find_front_axle_position(self, x, y, yaw): - """Compute front-axle world position from the center/rear and yaw.""" - fx = x + self.wheelbase * cos(yaw) - fy = y + self.wheelbase * sin(yaw) - return fx, fy - - def compute(self, state: VehicleState, component: Component = None): - """Compute the control outputs: (longitudinal acceleration, front wheel angle).""" - t = state.pose.t - if self.t_last is None: - self.t_last = t - dt = t - self.t_last - - # Current vehicle states - curr_x = state.pose.x - curr_y = state.pose.y - curr_yaw = state.pose.yaw if state.pose.yaw is not None else 0.0 - speed = state.v - - if self.path is None: - if component: - component.debug_event("No path provided to Stanley controller. Doing nothing.") - return (0.0, 0.0) - - # Ensure same frame - if self.path.frame != state.pose.frame: - if component: - component.debug_event(f"Transforming path from {self.path.frame.name} to {state.pose.frame.name}") - self.path = self.path.to_frame(state.pose.frame, current_pose=state.pose) - - if self.trajectory and self.trajectory.frame != state.pose.frame: - if component: - component.debug_event(f"Transforming trajectory from {self.trajectory.frame.name} to {state.pose.frame.name}") - self.trajectory = self.trajectory.to_frame(state.pose.frame, current_pose=state.pose) - - # 1) Closest point - fx, fy = self._find_front_axle_position(curr_x, curr_y, curr_yaw) - search_start = self.current_path_parameter - 5.0 - search_end = self.current_path_parameter + 5.0 - closest_dist, closest_parameter = self.path.closest_point_local((fx, fy), [search_start, search_end]) - self.current_path_parameter = closest_parameter - - # 2) Path heading - target_x, target_y = self.path.eval(closest_parameter) - tangent = self.path.eval_tangent(closest_parameter) - path_yaw = atan2(tangent[1], tangent[0]) - desired_x = target_x - desired_y = target_y - # 3) Lateral error - dx = fx - target_x - dy = fy - target_y - left_vec = np.array([sin(curr_yaw), -cos(curr_yaw)]) - cross_track_error = np.sign(np.dot(np.array([dx, dy]), left_vec)) * closest_dist - - # 4) Heading error - yaw_error = normalise_angle(path_yaw - curr_yaw) - - # 5) Standard Stanley terms - cross_term = atan2(self.k * cross_track_error, self.k_soft + speed) - desired_steering_angle = yaw_error + cross_term - - desired_speed = self.desired_speed - feedforward_accel = 0.0 - - if self.trajectory and self.desired_speed_source in ['path', 'trajectory']: - if len(self.trajectory.points) < 2 or self.current_path_parameter >= self.path.domain()[1]: - # End of trajectory -> stop - if component: - component.debug_event("Stanley: Past the end of trajectory, stopping.") - desired_speed = 0.0 - feedforward_accel = -2.0 - else: - if self.desired_speed_source == 'path': - current_trajectory_time = self.trajectory.parameter_to_time(self.current_path_parameter) - else: - self.current_traj_parameter += dt - current_trajectory_time = self.current_traj_parameter - - deriv = self.trajectory.eval_derivative(current_trajectory_time) - desired_speed = min(np.linalg.norm(deriv), self.speed_limit) - - difference_dt = 0.1 - future_t = current_trajectory_time + difference_dt - if future_t > self.trajectory.domain()[1]: - future_t = self.trajectory.domain()[1] - future_deriv = self.trajectory.eval_derivative(future_t) - next_desired_speed = min(np.linalg.norm(future_deriv), self.speed_limit) - feedforward_accel = (next_desired_speed - desired_speed) / difference_dt - feedforward_accel = np.clip(feedforward_accel, -self.max_decel, self.max_accel) - else: - if desired_speed is None: - desired_speed = 4.0 - - # Cross-track-based slowdown (less aggressive than before). - desired_speed *= np.exp(-abs(cross_track_error) * 0.6) - - # Clip to speed limit - if desired_speed > self.speed_limit: - desired_speed = self.speed_limit - - # PID for longitudinal control - speed_error = desired_speed - speed - output_accel = self.pid_speed.advance(e=speed_error, t=t, feedforward_term=feedforward_accel) - - # Clip acceleration - if output_accel > self.max_accel: - output_accel = self.max_accel - elif output_accel < -self.max_decel: - output_accel = -self.max_decel - - # Avoid negative accel when fully stopped - if desired_speed == 0 and abs(speed) < 0.01 and output_accel < 0: - output_accel = 0.0 - - # Debug - if component is not None: - # component.debug("Stanley: fx, fy", (fx, fy)) - component.debug('curr pt',(curr_x,curr_y)) - component.debug("desired_x",desired_x) - component.debug("desired_y",desired_y) - component.debug("Stanley: path param", self.current_path_parameter) - component.debug("Stanley: crosstrack dist", closest_dist) - component.debug("crosstrack error", cross_track_error) - component.debug("Stanley: yaw_error", yaw_error) - component.debug('steering_angle', desired_steering_angle) - component.debug("Stanley: desired_speed (m/s)", desired_speed) - component.debug("Stanley: feedforward_accel (m/s^2)", feedforward_accel) - component.debug("Stanley: output_accel (m/s^2)", output_accel) - - if output_accel > self.max_accel: - output_accel = self.max_accel - - if output_accel < -self.max_decel: - output_accel = -self.max_decel - - self.t_last = t - return (output_accel, desired_steering_angle) - -##################################### -# 3. Tracker component -##################################### -class StanleyTrajectoryTracker(Component): - """ - A trajectory-tracking Component that uses the above Stanley controller - for lateral control plus PID-based longitudinal control. - It now includes measures to mitigate oscillations. - """ - - def __init__(self, vehicle_interface=None, **kwargs): - """ - :param vehicle_interface: The low-level interface to send commands to the vehicle. - :param kwargs: Optional parameters to pass into the Stanley(...) constructor. - """ - self.stanley = Stanley(**kwargs) - self.vehicle_interface = vehicle_interface - - def rate(self): - """Control frequency in Hz.""" - return 50.0 - - def state_inputs(self): - """ - Required state inputs: - - Vehicle state - - Trajectory - """ - return ["vehicle", "trajectory"] - - def state_outputs(self): - """No direct output state here.""" - return [] - - def update(self, vehicle: VehicleState, trajectory: Trajectory): - """ - Per control cycle: - 1) Set the path/trajectory - 2) Compute (acceleration, front wheel angle) - 3) Convert front wheel angle to steering wheel angle (if necessary) - 4) Send command to the vehicle - """ - self.stanley.set_path(trajectory) - accel, f_delta = self.stanley.compute(vehicle, self) - - # If your low-level interface expects steering wheel angle: - steering_angle = front2steer(f_delta) - steering_angle = np.clip( - steering_angle, - settings.get('vehicle.geometry.min_steering_angle', -0.5), - settings.get('vehicle.geometry.max_steering_angle', 0.5) - ) - - cmd = self.vehicle_interface.simple_command(accel, steering_angle, vehicle) - self.vehicle_interface.send_command(cmd) - - def healthy(self): - """Optional: check if the controller has a valid path.""" - return self.stanley.path is not None From 884c122050b5f51c3d52c71cbf100ea345f91e6a Mon Sep 17 00:00:00 2001 From: Jugal Date: Mon, 12 May 2025 00:51:44 -0500 Subject: [PATCH 43/71] Create stanley.py stanley.py is purely for forward routes --- GEMstack/onboard/planning/stanley.py | 312 +++++++++++++++++++++++++++ 1 file changed, 312 insertions(+) create mode 100644 GEMstack/onboard/planning/stanley.py diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley.py new file mode 100644 index 000000000..8f871f488 --- /dev/null +++ b/GEMstack/onboard/planning/stanley.py @@ -0,0 +1,312 @@ +import numpy as np +from math import sin, cos, atan2 + +# These imports match your existing project structure +from ...mathutils.control import PID +from ...utils import settings +from ...mathutils import transforms +from ...knowledge.vehicle.geometry import front2steer +from ...knowledge.vehicle.dynamics import acceleration_to_pedal_positions +from ...state.vehicle import VehicleState, ObjectFrameEnum +from ...state.trajectory import Path, Trajectory +from ..interface.gem import GEMVehicleCommand +from ..component import Component + +##################################### +# 1. Angle normalization +##################################### +def normalise_angle(angle): + """Wraps an angle to the [-pi, pi] range.""" + while angle > np.pi: + angle -= 2.0 * np.pi + while angle < -np.pi: + angle += 2.0 * np.pi + return angle + +##################################### +# 2. Stanley-based controller with longitudinal PID +##################################### +class Stanley(object): + """ + A Stanley controller that handles lateral control (steering) + plus a basic longitudinal control (PID + optional feedforward). + It has been modified to reduce oscillations by: + 1) Lower gains + 2) Steering damping + 3) Low-pass filter on steering + 4) Gentler speed logic when cornering + """ + + def __init__( + self, + control_gain=None, + softening_gain=None, + desired_speed=None + ): + """ + :param control_gain: Stanley lateral control gain k (lowered from the default to reduce overshoot). + :param softening_gain: Softening gain k_soft. + :param yaw_rate_gain: Yaw-rate gain k_yaw_rate (helps reduce oscillations). + :param steering_damp_gain: Steering damping gain k_damp_steer. + :param desired_speed: Desired speed (float) or 'path'/'trajectory'. + """ + + # 1) Lower Gains + self.k = control_gain if control_gain is not None else settings.get('control.stanley.control_gain') + self.k_soft = softening_gain if softening_gain is not None else settings.get('control.stanley.softening_gain') + + # Typically, this is the max front-wheel steering angle in radians + self.max_steer = settings.get('vehicle.geometry.max_wheel_angle') + self.wheelbase = settings.get('vehicle.geometry.wheelbase') + + # Basic longitudinal constraints + self.speed_limit = settings.get('vehicle.limits.max_speed') + self.max_accel = settings.get('vehicle.limits.max_acceleration') + self.max_decel = settings.get('vehicle.limits.max_deceleration') + + # PID for longitudinal speed tracking + p = settings.get('control.longitudinal_control.pid_p') + d = settings.get('control.longitudinal_control.pid_d') + i = settings.get('control.longitudinal_control.pid_i') + self.pid_speed = PID(p, d, i, windup_limit=20) + + # Speed source: numeric or derived from path/trajectory + if desired_speed is not None: + self.desired_speed_source = desired_speed + else: + self.desired_speed_source = settings.get('control.stanley.desired_speed', 'path') + + if isinstance(self.desired_speed_source, (int, float)): + self.desired_speed = self.desired_speed_source + else: + self.desired_speed = None + + # For path/trajectory + self.path_arg = None + self.path = None + self.trajectory = None + self.current_path_parameter = 0.0 + self.current_traj_parameter = 0.0 + self.t_last = None + + def set_path(self, path: Path): + """Sets the path or trajectory to track.""" + if path == self.path_arg: + return + self.path_arg = path + + # If the path has more than 2D, reduce to (x,y) + if len(path.points[0]) > 2: + path = path.get_dims([0,1]) + + # If no timing info, we can't rely on 'path'/'trajectory' speed + if not isinstance(path, Trajectory): + self.path = path.arc_length_parameterize() + self.trajectory = None + self.current_traj_parameter = 0.0 + if self.desired_speed_source in ['path', 'trajectory']: + raise ValueError("Stanley: Provided path has no timing. Either provide a Trajectory or set a constant desired_speed.") + else: + self.path = path.arc_length_parameterize() + self.trajectory = path + self.current_traj_parameter = self.trajectory.domain()[0] + + self.current_path_parameter = 0.0 + + def _find_front_axle_position(self, x, y, yaw): + """Compute front-axle world position from the center/rear and yaw.""" + fx = x + self.wheelbase * cos(yaw) + fy = y + self.wheelbase * sin(yaw) + return fx, fy + + def compute(self, state: VehicleState, component: Component = None): + """Compute the control outputs: (longitudinal acceleration, front wheel angle).""" + t = state.pose.t + if self.t_last is None: + self.t_last = t + dt = t - self.t_last + + # Current vehicle states + curr_x = state.pose.x + curr_y = state.pose.y + curr_yaw = state.pose.yaw if state.pose.yaw is not None else 0.0 + speed = state.v + + if self.path is None: + if component: + component.debug_event("No path provided to Stanley controller. Doing nothing.") + return (0.0, 0.0) + + # Ensure same frame + if self.path.frame != state.pose.frame: + if component: + component.debug_event(f"Transforming path from {self.path.frame.name} to {state.pose.frame.name}") + self.path = self.path.to_frame(state.pose.frame, current_pose=state.pose) + + if self.trajectory and self.trajectory.frame != state.pose.frame: + if component: + component.debug_event(f"Transforming trajectory from {self.trajectory.frame.name} to {state.pose.frame.name}") + self.trajectory = self.trajectory.to_frame(state.pose.frame, current_pose=state.pose) + + # 1) Closest point + fx, fy = self._find_front_axle_position(curr_x, curr_y, curr_yaw) + search_start = self.current_path_parameter - 5.0 + search_end = self.current_path_parameter + 5.0 + closest_dist, closest_parameter = self.path.closest_point_local((fx, fy), [search_start, search_end]) + self.current_path_parameter = closest_parameter + + # 2) Path heading + target_x, target_y = self.path.eval(closest_parameter) + tangent = self.path.eval_tangent(closest_parameter) + path_yaw = atan2(tangent[1], tangent[0]) + desired_x = target_x + desired_y = target_y + # 3) Lateral error + dx = fx - target_x + dy = fy - target_y + left_vec = np.array([sin(curr_yaw), -cos(curr_yaw)]) + cross_track_error = np.sign(np.dot(np.array([dx, dy]), left_vec)) * closest_dist + + # 4) Heading error + yaw_error = normalise_angle(path_yaw - curr_yaw) + + # 5) Standard Stanley terms + cross_term = atan2(self.k * cross_track_error, self.k_soft + speed) + desired_steering_angle = yaw_error + cross_term + + desired_speed = self.desired_speed + feedforward_accel = 0.0 + + if self.trajectory and self.desired_speed_source in ['path', 'trajectory']: + if len(self.trajectory.points) < 2 or self.current_path_parameter >= self.path.domain()[1]: + # End of trajectory -> stop + if component: + component.debug_event("Stanley: Past the end of trajectory, stopping.") + desired_speed = 0.0 + feedforward_accel = -2.0 + else: + if self.desired_speed_source == 'path': + current_trajectory_time = self.trajectory.parameter_to_time(self.current_path_parameter) + else: + self.current_traj_parameter += dt + current_trajectory_time = self.current_traj_parameter + + deriv = self.trajectory.eval_derivative(current_trajectory_time) + desired_speed = min(np.linalg.norm(deriv), self.speed_limit) + + difference_dt = 0.1 + future_t = current_trajectory_time + difference_dt + if future_t > self.trajectory.domain()[1]: + future_t = self.trajectory.domain()[1] + future_deriv = self.trajectory.eval_derivative(future_t) + next_desired_speed = min(np.linalg.norm(future_deriv), self.speed_limit) + feedforward_accel = (next_desired_speed - desired_speed) / difference_dt + feedforward_accel = np.clip(feedforward_accel, -self.max_decel, self.max_accel) + else: + if desired_speed is None: + desired_speed = 4.0 + + # Cross-track-based slowdown (less aggressive than before). + desired_speed *= np.exp(-abs(cross_track_error) * 0.6) + + # Clip to speed limit + if desired_speed > self.speed_limit: + desired_speed = self.speed_limit + + # PID for longitudinal control + speed_error = desired_speed - speed + output_accel = self.pid_speed.advance(e=speed_error, t=t, feedforward_term=feedforward_accel) + + # Clip acceleration + if output_accel > self.max_accel: + output_accel = self.max_accel + elif output_accel < -self.max_decel: + output_accel = -self.max_decel + + # Avoid negative accel when fully stopped + if desired_speed == 0 and abs(speed) < 0.01 and output_accel < 0: + output_accel = 0.0 + + # Debug + if component is not None: + # component.debug("Stanley: fx, fy", (fx, fy)) + component.debug('curr pt',(curr_x,curr_y)) + component.debug("desired_x",desired_x) + component.debug("desired_y",desired_y) + component.debug("Stanley: path param", self.current_path_parameter) + component.debug("Stanley: crosstrack dist", closest_dist) + component.debug("crosstrack error", cross_track_error) + component.debug("Stanley: yaw_error", yaw_error) + component.debug('steering_angle', desired_steering_angle) + component.debug("Stanley: desired_speed (m/s)", desired_speed) + component.debug("Stanley: feedforward_accel (m/s^2)", feedforward_accel) + component.debug("Stanley: output_accel (m/s^2)", output_accel) + + if output_accel > self.max_accel: + output_accel = self.max_accel + + if output_accel < -self.max_decel: + output_accel = -self.max_decel + + self.t_last = t + return (output_accel, desired_steering_angle) + +##################################### +# 3. Tracker component +##################################### +class StanleyTrajectoryTracker(Component): + """ + A trajectory-tracking Component that uses the above Stanley controller + for lateral control plus PID-based longitudinal control. + It now includes measures to mitigate oscillations. + """ + + def __init__(self, vehicle_interface=None, **kwargs): + """ + :param vehicle_interface: The low-level interface to send commands to the vehicle. + :param kwargs: Optional parameters to pass into the Stanley(...) constructor. + """ + self.stanley = Stanley(**kwargs) + self.vehicle_interface = vehicle_interface + + def rate(self): + """Control frequency in Hz.""" + return 50.0 + + def state_inputs(self): + """ + Required state inputs: + - Vehicle state + - Trajectory + """ + return ["vehicle", "trajectory"] + + def state_outputs(self): + """No direct output state here.""" + return [] + + def update(self, vehicle: VehicleState, trajectory: Trajectory): + """ + Per control cycle: + 1) Set the path/trajectory + 2) Compute (acceleration, front wheel angle) + 3) Convert front wheel angle to steering wheel angle (if necessary) + 4) Send command to the vehicle + """ + self.stanley.set_path(trajectory) + accel, f_delta = self.stanley.compute(vehicle, self) + + # If your low-level interface expects steering wheel angle: + steering_angle = front2steer(f_delta) + steering_angle = np.clip( + steering_angle, + settings.get('vehicle.geometry.min_steering_angle', -0.5), + settings.get('vehicle.geometry.max_steering_angle', 0.5) + ) + + cmd = self.vehicle_interface.simple_command(accel, steering_angle, vehicle) + self.vehicle_interface.send_command(cmd) + + def healthy(self): + """Optional: check if the controller has a valid path.""" + return self.stanley.path is not None From 21b4fb2a7917125a40e18d4c89f0249e95041f1a Mon Sep 17 00:00:00 2001 From: Jugal Date: Mon, 12 May 2025 00:57:12 -0500 Subject: [PATCH 44/71] Update serialization.py Commented out global registry --- GEMstack/utils/serialization.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/GEMstack/utils/serialization.py b/GEMstack/utils/serialization.py index c16228a9d..d825645fd 100644 --- a/GEMstack/utils/serialization.py +++ b/GEMstack/utils/serialization.py @@ -97,7 +97,7 @@ def deserialize(data): `data` can be a `str`, `bytes`, `dict`, or ROS std_msgs/String object. """ - global REGISTRY + #global REGISTRY ##causing error while pushing changes name,version,data = deserialize_raw(data) if name not in REGISTRY: raise IOError("Class of type {} not found in registry".format(name)) @@ -170,4 +170,4 @@ def load(file): def save(obj,file): """Saves a JSON file containing serialized data into a registered class.""" - file.write(serialize(obj)) \ No newline at end of file + file.write(serialize(obj)) From 958209e9f024877b172828bfffec12acba34b419 Mon Sep 17 00:00:00 2001 From: Jugal Date: Mon, 12 May 2025 01:01:01 -0500 Subject: [PATCH 45/71] Update stanley.py pushing with comment at the heading and solving global registry error --- GEMstack/onboard/planning/stanley.py | 1 + 1 file changed, 1 insertion(+) diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley.py index 8f871f488..ab0e750d3 100644 --- a/GEMstack/onboard/planning/stanley.py +++ b/GEMstack/onboard/planning/stanley.py @@ -1,3 +1,4 @@ +###supports purely forward routes### import numpy as np from math import sin, cos, atan2 From 19521f9d554a5ca811170d5944a1de7eb97396dd Mon Sep 17 00:00:00 2001 From: Jugal Date: Mon, 12 May 2025 01:05:40 -0500 Subject: [PATCH 46/71] Update stanley_reverse.py --- GEMstack/onboard/planning/stanley_reverse.py | 1 + 1 file changed, 1 insertion(+) diff --git a/GEMstack/onboard/planning/stanley_reverse.py b/GEMstack/onboard/planning/stanley_reverse.py index 4283e4479..958de8892 100644 --- a/GEMstack/onboard/planning/stanley_reverse.py +++ b/GEMstack/onboard/planning/stanley_reverse.py @@ -1,3 +1,4 @@ +### supports both forward and reverse but works better with more reverse focused routes like parallel parking and reverse in S shape### import numpy as np from math import sin, cos, atan2 From b50598b3990e7c331cd84ecdcd3fa38cc1fb4d23 Mon Sep 17 00:00:00 2001 From: hhxjqm Date: Mon, 12 May 2025 17:19:54 +0000 Subject: [PATCH 47/71] Fixed the issue where the vehicle could not reverse when the speed was too slow. Fixed the oscillation issue when reversing. --- GEMstack/knowledge/defaults/current.yaml | 6 +- GEMstack/onboard/planning/stanley.py | 343 ++++++++++----- GEMstack/onboard/planning/stanley_forward.py | 315 ++++++++++++++ GEMstack/onboard/planning/stanley_reverse.py | 436 ------------------- launch/fixed_route.yaml | 11 +- 5 files changed, 557 insertions(+), 554 deletions(-) create mode 100644 GEMstack/onboard/planning/stanley_forward.py delete mode 100644 GEMstack/onboard/planning/stanley_reverse.py diff --git a/GEMstack/knowledge/defaults/current.yaml b/GEMstack/knowledge/defaults/current.yaml index 1426d24a4..a03e88cd8 100644 --- a/GEMstack/knowledge/defaults/current.yaml +++ b/GEMstack/knowledge/defaults/current.yaml @@ -22,7 +22,7 @@ control: # Stanley controller parameters (fine tune this) stanley: - control_gain: 0.1 + control_gain: 1.5 softening_gain: 0.2 desired_speed: trajectory # Speed reference source: can be "trajectory", "path", or a constant value @@ -34,9 +34,9 @@ control: # Shared longitudinal control parameters longitudinal_control: - pid_p: 1.0 # Proportional gain for speed PID controller + pid_p: 20.0 # Proportional gain for speed PID controller pid_i: 0.1 # Integral gain for speed PID controller - pid_d: 0.0 # Derivative gain for speed PID controller + pid_d: 1.0 # Derivative gain for speed PID controller #configure the simulator, if using simulator: diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley.py index ab0e750d3..cc875f1d7 100644 --- a/GEMstack/onboard/planning/stanley.py +++ b/GEMstack/onboard/planning/stanley.py @@ -1,8 +1,6 @@ -###supports purely forward routes### import numpy as np from math import sin, cos, atan2 -# These imports match your existing project structure from ...mathutils.control import PID from ...utils import settings from ...mathutils import transforms @@ -28,15 +26,6 @@ def normalise_angle(angle): # 2. Stanley-based controller with longitudinal PID ##################################### class Stanley(object): - """ - A Stanley controller that handles lateral control (steering) - plus a basic longitudinal control (PID + optional feedforward). - It has been modified to reduce oscillations by: - 1) Lower gains - 2) Steering damping - 3) Low-pass filter on steering - 4) Gentler speed logic when cornering - """ def __init__( self, @@ -47,31 +36,24 @@ def __init__( """ :param control_gain: Stanley lateral control gain k (lowered from the default to reduce overshoot). :param softening_gain: Softening gain k_soft. - :param yaw_rate_gain: Yaw-rate gain k_yaw_rate (helps reduce oscillations). - :param steering_damp_gain: Steering damping gain k_damp_steer. :param desired_speed: Desired speed (float) or 'path'/'trajectory'. """ - - # 1) Lower Gains self.k = control_gain if control_gain is not None else settings.get('control.stanley.control_gain') self.k_soft = softening_gain if softening_gain is not None else settings.get('control.stanley.softening_gain') - # Typically, this is the max front-wheel steering angle in radians self.max_steer = settings.get('vehicle.geometry.max_wheel_angle') self.wheelbase = settings.get('vehicle.geometry.wheelbase') - # Basic longitudinal constraints self.speed_limit = settings.get('vehicle.limits.max_speed') - self.max_accel = settings.get('vehicle.limits.max_acceleration') - self.max_decel = settings.get('vehicle.limits.max_deceleration') + self.reverse_speed_limit = settings.get('vehicle.limits.max_reverse_speed') + self.max_accel = settings.get('vehicle.limits.max_acceleration') + self.max_decel = settings.get('vehicle.limits.max_deceleration') - # PID for longitudinal speed tracking p = settings.get('control.longitudinal_control.pid_p') d = settings.get('control.longitudinal_control.pid_d') i = settings.get('control.longitudinal_control.pid_i') self.pid_speed = PID(p, d, i, windup_limit=20) - - # Speed source: numeric or derived from path/trajectory + if desired_speed is not None: self.desired_speed_source = desired_speed else: @@ -82,25 +64,23 @@ def __init__( else: self.desired_speed = None - # For path/trajectory self.path_arg = None self.path = None self.trajectory = None self.current_path_parameter = 0.0 self.current_traj_parameter = 0.0 self.t_last = None + self.reverse = None + self.sharp_turn = False def set_path(self, path: Path): - """Sets the path or trajectory to track.""" if path == self.path_arg: return self.path_arg = path - # If the path has more than 2D, reduce to (x,y) if len(path.points[0]) > 2: path = path.get_dims([0,1]) - # If no timing info, we can't rely on 'path'/'trajectory' speed if not isinstance(path, Trajectory): self.path = path.arc_length_parameterize() self.trajectory = None @@ -120,14 +100,116 @@ def _find_front_axle_position(self, x, y, yaw): fy = y + self.wheelbase * sin(yaw) return fx, fy + def _find_rear_axle_position(self, x, y, yaw): + """Compute rear-axle world position from the vehicle's reference point and yaw.""" + rx = x - self.wheelbase * cos(yaw) + ry = y - self.wheelbase * sin(yaw) + return rx, ry + + def initialize_state_and_direction(self, state: VehicleState): + if self.path is None: + raise ValueError("Stanley: Path must be set before initializing state and direction.") + curr_x = state.pose.x + curr_y = state.pose.y + curr_yaw = state.pose.yaw if state.pose.yaw is not None else 0.0 + fx, fy = self._find_front_axle_position(curr_x, curr_y, curr_yaw) + path_domain_start, path_domain_end = self.path.domain() + search_end = min(path_domain_end, path_domain_start + 5.0) + search_domain_start = [path_domain_start, search_end] + _, closest_param_at_start = self.path.closest_point_local((fx, fy), search_domain_start) + tangent_at_start = self.path.eval_tangent(closest_param_at_start) + initial_reverse = False + if np.linalg.norm(tangent_at_start) > 1e-6: + path_yaw_at_start = atan2(tangent_at_start[1], tangent_at_start[0]) + heading_diff = normalise_angle(path_yaw_at_start - curr_yaw) + initial_reverse = abs(heading_diff) > (np.pi / 2.0) + self.pid_speed.reset() + self.current_path_parameter = closest_param_at_start + if self.trajectory: + self.current_traj_parameter = self.trajectory.domain()[0] + return initial_reverse + + def is_target_behind_vehicle(self, vehicle_pose, target_point_coords): + curr_x = vehicle_pose.x + curr_y = vehicle_pose.y + curr_yaw = vehicle_pose.yaw + if curr_yaw is None: + return False + target_x = target_point_coords[0] + target_y = target_point_coords[1] + vec_x = target_x - curr_x + vec_y = target_y - curr_y + if abs(vec_x) < 1e-6 and abs(vec_y) < 1e-6: + return False + heading_x = cos(curr_yaw) + heading_y = sin(curr_yaw) + dot_product = vec_x * heading_x + vec_y * heading_y + return (dot_product < 0) + + def _check_sharp_turn_ahead(self, lookahead_s=3.0, threshold_angle=np.pi/2.0, num_steps=4): + if not self.path: + return False + + path = self.path + current_s = self.current_path_parameter + domain_start, domain_end = path.domain() + + if current_s >= domain_end - 1e-3: + return False + + step_s = lookahead_s / num_steps + s_prev = current_s + + try: + tangent_prev = path.eval_tangent(s_prev) + if np.linalg.norm(tangent_prev) < 1e-6: + s_prev_adjusted = min(s_prev + step_s / 2, domain_end) + if s_prev_adjusted <= s_prev: + return False + tangent_prev = path.eval_tangent(s_prev_adjusted) + if np.linalg.norm(tangent_prev) < 1e-6: + return False + s_prev = s_prev_adjusted + + angle_prev = atan2(tangent_prev[1], tangent_prev[0]) + + except Exception as e: + return False + + for i in range(num_steps): + s_next = s_prev + step_s + s_next = min(s_next, domain_end) + + if s_next <= s_prev + 1e-6: + break + + try: + tangent_next = path.eval_tangent(s_next) + if np.linalg.norm(tangent_next) < 1e-6: + s_prev = s_next + continue + + angle_next = atan2(tangent_next[1], tangent_next[0]) + except Exception as e: + break + + angle_change = abs(normalise_angle(angle_next - angle_prev)) + + if angle_change > threshold_angle: + return True + + angle_prev = angle_next + s_prev = s_next + return False + def compute(self, state: VehicleState, component: Component = None): - """Compute the control outputs: (longitudinal acceleration, front wheel angle).""" t = state.pose.t if self.t_last is None: self.t_last = t - dt = t - self.t_last + dt = 0 + else: + dt = t - self.t_last - # Current vehicle states curr_x = state.pose.x curr_y = state.pose.y curr_yaw = state.pose.yaw if state.pose.yaw is not None else 0.0 @@ -136,9 +218,10 @@ def compute(self, state: VehicleState, component: Component = None): if self.path is None: if component: component.debug_event("No path provided to Stanley controller. Doing nothing.") + self.t_last = t + self.pid_speed.reset() return (0.0, 0.0) - # Ensure same frame if self.path.frame != state.pose.frame: if component: component.debug_event(f"Transforming path from {self.path.frame.name} to {state.pose.frame.name}") @@ -149,91 +232,160 @@ def compute(self, state: VehicleState, component: Component = None): component.debug_event(f"Transforming trajectory from {self.trajectory.frame.name} to {state.pose.frame.name}") self.trajectory = self.trajectory.to_frame(state.pose.frame, current_pose=state.pose) - # 1) Closest point - fx, fy = self._find_front_axle_position(curr_x, curr_y, curr_yaw) - search_start = self.current_path_parameter - 5.0 + + if self.reverse is None: + self.reverse = self.initialize_state_and_direction(state) + + if self.reverse: + fx, fy = self._find_rear_axle_position(curr_x, curr_y, curr_yaw) + else: + fx, fy = self._find_front_axle_position(curr_x, curr_y, curr_yaw) + search_start = self.current_path_parameter search_end = self.current_path_parameter + 5.0 closest_dist, closest_parameter = self.path.closest_point_local((fx, fy), [search_start, search_end]) self.current_path_parameter = closest_parameter - # 2) Path heading - target_x, target_y = self.path.eval(closest_parameter) - tangent = self.path.eval_tangent(closest_parameter) + target_x, target_y = self.path.eval(self.current_path_parameter) + tangent = self.path.eval_tangent(self.current_path_parameter) path_yaw = atan2(tangent[1], tangent[0]) - desired_x = target_x - desired_y = target_y - # 3) Lateral error + dx = fx - target_x dy = fy - target_y - left_vec = np.array([sin(curr_yaw), -cos(curr_yaw)]) - cross_track_error = np.sign(np.dot(np.array([dx, dy]), left_vec)) * closest_dist - # 4) Heading error - yaw_error = normalise_angle(path_yaw - curr_yaw) + if not self.reverse: + try: + is_sharp_turn_ahead = self._check_sharp_turn_ahead( + lookahead_s=2.0, + threshold_angle=np.pi/2.0 + ) + except Exception as e: + is_sharp_turn_ahead = False + if is_sharp_turn_ahead and not self.sharp_turn: + self.sharp_turn = True + use_reverse = self.is_target_behind_vehicle(state.pose, (target_x, target_y)) + if use_reverse and self.sharp_turn: + self.reverse = True + self.sharp_turn = False + + if self.reverse: + cross_track_error = dx * (-tangent[1]) + dy * tangent[0] + else: + left_vec = np.array([sin(curr_yaw), -cos(curr_yaw)]) + cross_track_error = np.sign(np.dot(np.array([dx, dy]), left_vec)) * closest_dist - # 5) Standard Stanley terms - cross_term = atan2(self.k * cross_track_error, self.k_soft + speed) - desired_steering_angle = yaw_error + cross_term + yaw_error = normalise_angle(path_yaw - curr_yaw) - desired_speed = self.desired_speed + desired_speed = abs(self.desired_speed) feedforward_accel = 0.0 - if self.trajectory and self.desired_speed_source in ['path', 'trajectory']: - if len(self.trajectory.points) < 2 or self.current_path_parameter >= self.path.domain()[1]: - # End of trajectory -> stop - if component: - component.debug_event("Stanley: Past the end of trajectory, stopping.") + if self.reverse: + heading_term = -yaw_error + cross_term_input = self.k * (-cross_track_error) / (self.k_soft + abs(speed)) + cross_term = atan2(cross_term_input, 1.0) + desired_steering_angle = heading_term + cross_term + + if self.trajectory and self.desired_speed_source in ['path', 'trajectory']: + current_trajectory_param_for_eval = self.current_path_parameter + if hasattr(self.trajectory, 'parameter_to_time'): + current_trajectory_param_for_eval = self.trajectory.parameter_to_time(self.current_path_parameter) + + if self.trajectory is not None: + deriv = self.trajectory.eval_derivative(current_trajectory_param_for_eval) + path_speed_magnitude = np.linalg.norm(deriv) + else: + path_speed_magnitude = 0.0 + + desired_speed = min(path_speed_magnitude, self.reverse_speed_limit) + + desired_speed = desired_speed * -1.0 + + is_at_start_backward = self.current_path_parameter <= self.path.domain()[0] + 1e-3 + + if is_at_start_backward: desired_speed = 0.0 - feedforward_accel = -2.0 + feedforward_accel = -2.0 * -1.0 + else: - if self.desired_speed_source == 'path': - current_trajectory_time = self.trajectory.parameter_to_time(self.current_path_parameter) + difference_dt = 0.1 + current_speed_abs = abs(speed) + if current_speed_abs < 0.1: estimated_path_step = 0.1 * difference_dt * -1.0 + else: estimated_path_step = current_speed_abs * difference_dt * -1.0 + + future_parameter = self.current_path_parameter + estimated_path_step + + future_parameter = np.clip(future_parameter, self.path.domain()[0], self.path.domain()[-1]) + + future_trajectory_param_for_eval = future_parameter + if hasattr(self.trajectory, 'parameter_to_time'): + future_trajectory_param_for_eval = self.trajectory.parameter_to_time(future_parameter) + + if self.trajectory is not None: + future_deriv = self.trajectory.eval_derivative(future_trajectory_param_for_eval) + next_path_speed_magnitude = min(np.linalg.norm(future_deriv), self.speed_limit) else: - self.current_traj_parameter += dt - current_trajectory_time = self.current_traj_parameter + next_path_speed_magnitude = 0.0 - deriv = self.trajectory.eval_derivative(current_trajectory_time) - desired_speed = min(np.linalg.norm(deriv), self.speed_limit) + next_desired_speed = next_path_speed_magnitude * -1.0 - difference_dt = 0.1 - future_t = current_trajectory_time + difference_dt - if future_t > self.trajectory.domain()[1]: - future_t = self.trajectory.domain()[1] - future_deriv = self.trajectory.eval_derivative(future_t) - next_desired_speed = min(np.linalg.norm(future_deriv), self.speed_limit) feedforward_accel = (next_desired_speed - desired_speed) / difference_dt feedforward_accel = np.clip(feedforward_accel, -self.max_decel, self.max_accel) + + desired_speed_magnitude_slowed = abs(desired_speed) * np.exp(-abs(cross_track_error) * 0.6) + desired_speed = desired_speed_magnitude_slowed * -1.0 if desired_speed_magnitude_slowed != 0 else 0.0 + else: - if desired_speed is None: - desired_speed = 4.0 + cross_term = atan2(self.k * cross_track_error, self.k_soft + speed) + desired_steering_angle = yaw_error + cross_term + + if self.trajectory and self.desired_speed_source in ['path', 'trajectory']: + if len(self.trajectory.points) < 2 or self.current_path_parameter >= self.path.domain()[1]: + if component: + component.debug_event("Stanley: Past the end of trajectory, stopping.") + desired_speed = 0.0 + feedforward_accel = -2.0 + else: + if self.desired_speed_source == 'path': + current_trajectory_time = self.trajectory.parameter_to_time(self.current_path_parameter) + else: + self.current_traj_parameter += dt + current_trajectory_time = self.current_traj_parameter + + deriv = self.trajectory.eval_derivative(current_trajectory_time) + desired_speed = min(np.linalg.norm(deriv), self.speed_limit) + + difference_dt = 0.1 + future_t = current_trajectory_time + difference_dt + if future_t > self.trajectory.domain()[1]: + future_t = self.trajectory.domain()[1] + future_deriv = self.trajectory.eval_derivative(future_t) + next_desired_speed = min(np.linalg.norm(future_deriv), self.speed_limit) + feedforward_accel = (next_desired_speed - desired_speed) / difference_dt + feedforward_accel = np.clip(feedforward_accel, -self.max_decel, self.max_accel) + else: + if desired_speed is None: + desired_speed = 4.0 - # Cross-track-based slowdown (less aggressive than before). - desired_speed *= np.exp(-abs(cross_track_error) * 0.6) + desired_speed *= np.exp(-abs(cross_track_error) * 0.6) - # Clip to speed limit - if desired_speed > self.speed_limit: + if self.reverse and abs(desired_speed) > self.reverse_speed_limit: + desired_speed = self.reverse_speed_limit * -1.0 if desired_speed != 0 else 0.0 + elif not self.reverse and desired_speed > self.speed_limit: desired_speed = self.speed_limit - # PID for longitudinal control speed_error = desired_speed - speed output_accel = self.pid_speed.advance(e=speed_error, t=t, feedforward_term=feedforward_accel) - - # Clip acceleration if output_accel > self.max_accel: output_accel = self.max_accel elif output_accel < -self.max_decel: output_accel = -self.max_decel - # Avoid negative accel when fully stopped if desired_speed == 0 and abs(speed) < 0.01 and output_accel < 0: output_accel = 0.0 - # Debug if component is not None: - # component.debug("Stanley: fx, fy", (fx, fy)) component.debug('curr pt',(curr_x,curr_y)) - component.debug("desired_x",desired_x) - component.debug("desired_y",desired_y) + component.debug("desired_x",target_x) + component.debug("desired_y",target_y) component.debug("Stanley: path param", self.current_path_parameter) component.debug("Stanley: crosstrack dist", closest_dist) component.debug("crosstrack error", cross_track_error) @@ -243,12 +395,6 @@ def compute(self, state: VehicleState, component: Component = None): component.debug("Stanley: feedforward_accel (m/s^2)", feedforward_accel) component.debug("Stanley: output_accel (m/s^2)", output_accel) - if output_accel > self.max_accel: - output_accel = self.max_accel - - if output_accel < -self.max_decel: - output_accel = -self.max_decel - self.t_last = t return (output_accel, desired_steering_angle) @@ -256,48 +402,25 @@ def compute(self, state: VehicleState, component: Component = None): # 3. Tracker component ##################################### class StanleyTrajectoryTracker(Component): - """ - A trajectory-tracking Component that uses the above Stanley controller - for lateral control plus PID-based longitudinal control. - It now includes measures to mitigate oscillations. - """ def __init__(self, vehicle_interface=None, **kwargs): - """ - :param vehicle_interface: The low-level interface to send commands to the vehicle. - :param kwargs: Optional parameters to pass into the Stanley(...) constructor. - """ self.stanley = Stanley(**kwargs) self.vehicle_interface = vehicle_interface - + def rate(self): - """Control frequency in Hz.""" return 50.0 def state_inputs(self): - """ - Required state inputs: - - Vehicle state - - Trajectory - """ return ["vehicle", "trajectory"] def state_outputs(self): - """No direct output state here.""" return [] def update(self, vehicle: VehicleState, trajectory: Trajectory): - """ - Per control cycle: - 1) Set the path/trajectory - 2) Compute (acceleration, front wheel angle) - 3) Convert front wheel angle to steering wheel angle (if necessary) - 4) Send command to the vehicle - """ self.stanley.set_path(trajectory) + accel, f_delta = self.stanley.compute(vehicle, self) - # If your low-level interface expects steering wheel angle: steering_angle = front2steer(f_delta) steering_angle = np.clip( steering_angle, diff --git a/GEMstack/onboard/planning/stanley_forward.py b/GEMstack/onboard/planning/stanley_forward.py new file mode 100644 index 000000000..20d2c7ec3 --- /dev/null +++ b/GEMstack/onboard/planning/stanley_forward.py @@ -0,0 +1,315 @@ +###supports purely forward routes### +import numpy as np +from math import sin, cos, atan2 + +# These imports match your existing project structure +from ...mathutils.control import PID +from ...utils import settings +from ...mathutils import transforms +from ...knowledge.vehicle.geometry import front2steer +from ...knowledge.vehicle.dynamics import acceleration_to_pedal_positions +from ...state.vehicle import VehicleState, ObjectFrameEnum +from ...state.trajectory import Path, Trajectory +from ..interface.gem import GEMVehicleCommand +from ..component import Component + +##################################### +# 1. Angle normalization +##################################### +def normalise_angle(angle): + """Wraps an angle to the [-pi, pi] range.""" + while angle > np.pi: + angle -= 2.0 * np.pi + while angle < -np.pi: + angle += 2.0 * np.pi + return angle + +##################################### +# 2. Stanley-based controller with longitudinal PID +##################################### +class Stanley(object): + """ + A Stanley controller that handles lateral control (steering) + plus a basic longitudinal control (PID + optional feedforward). + It has been modified to reduce oscillations by: + 1) Lower gains + 2) Steering damping + 3) Low-pass filter on steering + 4) Gentler speed logic when cornering + """ + + def __init__( + self, + control_gain=None, + softening_gain=None, + desired_speed=None + ): + """ + :param control_gain: Stanley lateral control gain k (lowered from the default to reduce overshoot). + :param softening_gain: Softening gain k_soft. + :param yaw_rate_gain: Yaw-rate gain k_yaw_rate (helps reduce oscillations). + :param steering_damp_gain: Steering damping gain k_damp_steer. + :param desired_speed: Desired speed (float) or 'path'/'trajectory'. + """ + + # 1) Lower Gains + self.k = control_gain if control_gain is not None else settings.get('control.stanley.control_gain') + self.k_soft = softening_gain if softening_gain is not None else settings.get('control.stanley.softening_gain') + + # Typically, this is the max front-wheel steering angle in radians + self.max_steer = settings.get('vehicle.geometry.max_wheel_angle') + self.wheelbase = settings.get('vehicle.geometry.wheelbase') + + # Basic longitudinal constraints + self.speed_limit = settings.get('vehicle.limits.max_speed') + self.max_accel = settings.get('vehicle.limits.max_acceleration') + self.max_decel = settings.get('vehicle.limits.max_deceleration') + + # PID for longitudinal speed tracking + p = settings.get('control.longitudinal_control.pid_p') + d = settings.get('control.longitudinal_control.pid_d') + i = settings.get('control.longitudinal_control.pid_i') + self.pid_speed = PID(p, d, i, windup_limit=20) + + # Speed source: numeric or derived from path/trajectory + if desired_speed is not None: + self.desired_speed_source = desired_speed + else: + self.desired_speed_source = settings.get('control.stanley.desired_speed', 'path') + + if isinstance(self.desired_speed_source, (int, float)): + self.desired_speed = self.desired_speed_source + else: + self.desired_speed = None + + # For path/trajectory + self.path_arg = None + self.path = None + self.trajectory = None + self.current_path_parameter = 0.0 + self.current_traj_parameter = 0.0 + self.t_last = None + + def set_path(self, path: Path): + """Sets the path or trajectory to track.""" + if path == self.path_arg: + return + self.path_arg = path + + # If the path has more than 2D, reduce to (x,y) + if len(path.points[0]) > 2: + path = path.get_dims([0,1]) + + # If no timing info, we can't rely on 'path'/'trajectory' speed + if not isinstance(path, Trajectory): + self.path = path.arc_length_parameterize() + self.trajectory = None + self.current_traj_parameter = 0.0 + if self.desired_speed_source in ['path', 'trajectory']: + raise ValueError("Stanley: Provided path has no timing. Either provide a Trajectory or set a constant desired_speed.") + else: + self.path = path.arc_length_parameterize() + self.trajectory = path + self.current_traj_parameter = self.trajectory.domain()[0] + + self.current_path_parameter = 0.0 + + def _find_front_axle_position(self, x, y, yaw): + """Compute front-axle world position from the center/rear and yaw.""" + fx = x + self.wheelbase * cos(yaw) + fy = y + self.wheelbase * sin(yaw) + return fx, fy + + def compute(self, state: VehicleState, component: Component = None): + """Compute the control outputs: (longitudinal acceleration, front wheel angle).""" + t = state.pose.t + if self.t_last is None: + self.t_last = t + dt = 0 + else: + dt = t - self.t_last + + # Current vehicle states + curr_x = state.pose.x + curr_y = state.pose.y + curr_yaw = state.pose.yaw if state.pose.yaw is not None else 0.0 + speed = state.v + + if self.path is None: + if component: + component.debug_event("No path provided to Stanley controller. Doing nothing.") + return (0.0, 0.0) + + # Ensure same frame + if self.path.frame != state.pose.frame: + if component: + component.debug_event(f"Transforming path from {self.path.frame.name} to {state.pose.frame.name}") + self.path = self.path.to_frame(state.pose.frame, current_pose=state.pose) + + if self.trajectory and self.trajectory.frame != state.pose.frame: + if component: + component.debug_event(f"Transforming trajectory from {self.trajectory.frame.name} to {state.pose.frame.name}") + self.trajectory = self.trajectory.to_frame(state.pose.frame, current_pose=state.pose) + + # 1) Closest point + fx, fy = self._find_front_axle_position(curr_x, curr_y, curr_yaw) + search_start = self.current_path_parameter - 5.0 + search_end = self.current_path_parameter + 5.0 + closest_dist, closest_parameter = self.path.closest_point_local((fx, fy), [search_start, search_end]) + self.current_path_parameter = closest_parameter + + # 2) Path heading + target_x, target_y = self.path.eval(closest_parameter) + tangent = self.path.eval_tangent(closest_parameter) + path_yaw = atan2(tangent[1], tangent[0]) + desired_x = target_x + desired_y = target_y + # 3) Lateral error + dx = fx - target_x + dy = fy - target_y + left_vec = np.array([sin(curr_yaw), -cos(curr_yaw)]) + cross_track_error = np.sign(np.dot(np.array([dx, dy]), left_vec)) * closest_dist + + # 4) Heading error + yaw_error = normalise_angle(path_yaw - curr_yaw) + + # 5) Standard Stanley terms + cross_term = atan2(self.k * cross_track_error, self.k_soft + speed) + desired_steering_angle = yaw_error + cross_term + + desired_speed = self.desired_speed + feedforward_accel = 0.0 + + if self.trajectory and self.desired_speed_source in ['path', 'trajectory']: + if len(self.trajectory.points) < 2 or self.current_path_parameter >= self.path.domain()[1]: + # End of trajectory -> stop + if component: + component.debug_event("Stanley: Past the end of trajectory, stopping.") + desired_speed = 0.0 + feedforward_accel = -2.0 + else: + if self.desired_speed_source == 'path': + current_trajectory_time = self.trajectory.parameter_to_time(self.current_path_parameter) + else: + self.current_traj_parameter += dt + current_trajectory_time = self.current_traj_parameter + + deriv = self.trajectory.eval_derivative(current_trajectory_time) + desired_speed = min(np.linalg.norm(deriv), self.speed_limit) + + difference_dt = 0.1 + future_t = current_trajectory_time + difference_dt + if future_t > self.trajectory.domain()[1]: + future_t = self.trajectory.domain()[1] + future_deriv = self.trajectory.eval_derivative(future_t) + next_desired_speed = min(np.linalg.norm(future_deriv), self.speed_limit) + feedforward_accel = (next_desired_speed - desired_speed) / difference_dt + feedforward_accel = np.clip(feedforward_accel, -self.max_decel, self.max_accel) + else: + if desired_speed is None: + desired_speed = 4.0 + + # Cross-track-based slowdown (less aggressive than before). + desired_speed *= np.exp(-abs(cross_track_error) * 0.6) + + # Clip to speed limit + if desired_speed > self.speed_limit: + desired_speed = self.speed_limit + + # PID for longitudinal control + speed_error = desired_speed - speed + output_accel = self.pid_speed.advance(e=speed_error, t=t, feedforward_term=feedforward_accel) + + # Clip acceleration + if output_accel > self.max_accel: + output_accel = self.max_accel + elif output_accel < -self.max_decel: + output_accel = -self.max_decel + + # Avoid negative accel when fully stopped + if desired_speed == 0 and abs(speed) < 0.01 and output_accel < 0: + output_accel = 0.0 + + # Debug + if component is not None: + # component.debug("Stanley: fx, fy", (fx, fy)) + component.debug('curr pt',(curr_x,curr_y)) + component.debug("desired_x",desired_x) + component.debug("desired_y",desired_y) + component.debug("Stanley: path param", self.current_path_parameter) + component.debug("Stanley: crosstrack dist", closest_dist) + component.debug("crosstrack error", cross_track_error) + component.debug("Stanley: yaw_error", yaw_error) + component.debug('steering_angle', desired_steering_angle) + component.debug("Stanley: desired_speed (m/s)", desired_speed) + component.debug("Stanley: feedforward_accel (m/s^2)", feedforward_accel) + component.debug("Stanley: output_accel (m/s^2)", output_accel) + + if output_accel > self.max_accel: + output_accel = self.max_accel + + if output_accel < -self.max_decel: + output_accel = -self.max_decel + + self.t_last = t + return (output_accel, desired_steering_angle) + +##################################### +# 3. Tracker component +##################################### +class StanleyTrajectoryTracker(Component): + """ + A trajectory-tracking Component that uses the above Stanley controller + for lateral control plus PID-based longitudinal control. + It now includes measures to mitigate oscillations. + """ + + def __init__(self, vehicle_interface=None, **kwargs): + """ + :param vehicle_interface: The low-level interface to send commands to the vehicle. + :param kwargs: Optional parameters to pass into the Stanley(...) constructor. + """ + self.stanley = Stanley(**kwargs) + self.vehicle_interface = vehicle_interface + + def rate(self): + """Control frequency in Hz.""" + return 50.0 + + def state_inputs(self): + """ + Required state inputs: + - Vehicle state + - Trajectory + """ + return ["vehicle", "trajectory"] + + def state_outputs(self): + """No direct output state here.""" + return [] + + def update(self, vehicle: VehicleState, trajectory: Trajectory): + """ + Per control cycle: + 1) Set the path/trajectory + 2) Compute (acceleration, front wheel angle) + 3) Convert front wheel angle to steering wheel angle (if necessary) + 4) Send command to the vehicle + """ + self.stanley.set_path(trajectory) + accel, f_delta = self.stanley.compute(vehicle, self) + + # If your low-level interface expects steering wheel angle: + steering_angle = front2steer(f_delta) + steering_angle = np.clip( + steering_angle, + settings.get('vehicle.geometry.min_steering_angle', -0.5), + settings.get('vehicle.geometry.max_steering_angle', 0.5) + ) + + cmd = self.vehicle_interface.simple_command(accel, steering_angle, vehicle) + self.vehicle_interface.send_command(cmd) + + def healthy(self): + """Optional: check if the controller has a valid path.""" + return self.stanley.path is not None diff --git a/GEMstack/onboard/planning/stanley_reverse.py b/GEMstack/onboard/planning/stanley_reverse.py deleted file mode 100644 index 958de8892..000000000 --- a/GEMstack/onboard/planning/stanley_reverse.py +++ /dev/null @@ -1,436 +0,0 @@ -### supports both forward and reverse but works better with more reverse focused routes like parallel parking and reverse in S shape### -import numpy as np -from math import sin, cos, atan2 - -from ...mathutils.control import PID -from ...utils import settings -from ...mathutils import transforms -from ...knowledge.vehicle.geometry import front2steer -from ...knowledge.vehicle.dynamics import acceleration_to_pedal_positions -from ...state.vehicle import VehicleState, ObjectFrameEnum -from ...state.trajectory import Path, Trajectory -from ..interface.gem import GEMVehicleCommand -from ..component import Component - -##################################### -# 1. Angle normalization -##################################### -def normalise_angle(angle): - """Wraps an angle to the [-pi, pi] range.""" - while angle > np.pi: - angle -= 2.0 * np.pi - while angle < -np.pi: - angle += 2.0 * np.pi - return angle - -##################################### -# 2. Stanley-based controller with longitudinal PID -##################################### -class Stanley(object): - - def __init__( - self, - control_gain=None, - softening_gain=None, - desired_speed=None - ): - """ - :param control_gain: Stanley lateral control gain k (lowered from the default to reduce overshoot). - :param softening_gain: Softening gain k_soft. - :param desired_speed: Desired speed (float) or 'path'/'trajectory'. - """ - self.k = control_gain if control_gain is not None else settings.get('control.stanley.control_gain') - self.k_soft = softening_gain if softening_gain is not None else settings.get('control.stanley.softening_gain') - - self.max_steer = settings.get('vehicle.geometry.max_wheel_angle') - self.wheelbase = settings.get('vehicle.geometry.wheelbase') - - self.speed_limit = settings.get('vehicle.limits.max_speed') - self.reverse_speed_limit = settings.get('vehicle.limits.max_reverse_speed') - self.max_accel = settings.get('vehicle.limits.max_acceleration') - self.max_decel = settings.get('vehicle.limits.max_deceleration') - - p = settings.get('control.longitudinal_control.pid_p') - d = settings.get('control.longitudinal_control.pid_d') - i = settings.get('control.longitudinal_control.pid_i') - self.pid_speed = PID(p, d, i, windup_limit=20) - - if desired_speed is not None: - self.desired_speed_source = desired_speed - else: - self.desired_speed_source = settings.get('control.stanley.desired_speed', 'path') - - if isinstance(self.desired_speed_source, (int, float)): - self.desired_speed = self.desired_speed_source - else: - self.desired_speed = None - - self.path_arg = None - self.path = None - self.trajectory = None - self.current_path_parameter = 0.0 - self.current_traj_parameter = 0.0 - self.t_last = None - self.reverse = None - self.sharp_turn = False - - def set_path(self, path: Path): - if path == self.path_arg: - return - self.path_arg = path - - if len(path.points[0]) > 2: - path = path.get_dims([0,1]) - - if not isinstance(path, Trajectory): - self.path = path.arc_length_parameterize() - self.trajectory = None - self.current_traj_parameter = 0.0 - if self.desired_speed_source in ['path', 'trajectory']: - raise ValueError("Stanley: Provided path has no timing. Either provide a Trajectory or set a constant desired_speed.") - else: - self.path = path.arc_length_parameterize() - self.trajectory = path - self.current_traj_parameter = self.trajectory.domain()[0] - - self.current_path_parameter = self.path.domain()[0] - - def _find_front_axle_position(self, x, y, yaw): - """Compute front-axle world position from the center/rear and yaw.""" - fx = x + self.wheelbase * cos(yaw) - fy = y + self.wheelbase * sin(yaw) - return fx, fy - - def _find_rear_axle_position(self, x, y, yaw): - """Compute rear-axle world position from the vehicle's reference point and yaw.""" - rx = x - self.wheelbase * cos(yaw) - ry = y - self.wheelbase * sin(yaw) - return rx, ry - - def initialize_state_and_direction(self, state: VehicleState): - if self.path is None: - raise ValueError("Stanley: Path must be set before initializing state and direction.") - curr_x = state.pose.x - curr_y = state.pose.y - curr_yaw = state.pose.yaw if state.pose.yaw is not None else 0.0 - fx, fy = self._find_front_axle_position(curr_x, curr_y, curr_yaw) - path_domain_start, path_domain_end = self.path.domain() - search_end = min(path_domain_end, path_domain_start + 5.0) - search_domain_start = [path_domain_start, search_end] - _, closest_param_at_start = self.path.closest_point_local((fx, fy), search_domain_start) - tangent_at_start = self.path.eval_tangent(closest_param_at_start) - initial_reverse = False - if np.linalg.norm(tangent_at_start) > 1e-6: - path_yaw_at_start = atan2(tangent_at_start[1], tangent_at_start[0]) - heading_diff = normalise_angle(path_yaw_at_start - curr_yaw) - initial_reverse = abs(heading_diff) > (np.pi / 2.0) - self.pid_speed.reset() - self.current_path_parameter = closest_param_at_start - if self.trajectory: - self.current_traj_parameter = self.trajectory.domain()[0] - return initial_reverse - - def is_target_behind_vehicle(self, vehicle_pose, target_point_coords): - curr_x = vehicle_pose.x - curr_y = vehicle_pose.y - curr_yaw = vehicle_pose.yaw - if curr_yaw is None: - return False - target_x = target_point_coords[0] - target_y = target_point_coords[1] - vec_x = target_x - curr_x - vec_y = target_y - curr_y - if abs(vec_x) < 1e-6 and abs(vec_y) < 1e-6: - return False - heading_x = cos(curr_yaw) - heading_y = sin(curr_yaw) - dot_product = vec_x * heading_x + vec_y * heading_y - return (dot_product < 0) - - def _check_sharp_turn_ahead(self, lookahead_s=3.0, threshold_angle=np.pi/2.0, num_steps=4): - if not self.path: - return False - - path = self.path - current_s = self.current_path_parameter - domain_start, domain_end = path.domain() - - if current_s >= domain_end - 1e-3: - return False - - step_s = lookahead_s / num_steps - s_prev = current_s - - try: - tangent_prev = path.eval_tangent(s_prev) - if np.linalg.norm(tangent_prev) < 1e-6: - s_prev_adjusted = min(s_prev + step_s / 2, domain_end) - if s_prev_adjusted <= s_prev: - return False - tangent_prev = path.eval_tangent(s_prev_adjusted) - if np.linalg.norm(tangent_prev) < 1e-6: - return False - s_prev = s_prev_adjusted - - angle_prev = atan2(tangent_prev[1], tangent_prev[0]) - - except Exception as e: - return False - - for i in range(num_steps): - s_next = s_prev + step_s - s_next = min(s_next, domain_end) - - if s_next <= s_prev + 1e-6: - break - - try: - tangent_next = path.eval_tangent(s_next) - if np.linalg.norm(tangent_next) < 1e-6: - s_prev = s_next - continue - - angle_next = atan2(tangent_next[1], tangent_next[0]) - except Exception as e: - break - - angle_change = abs(normalise_angle(angle_next - angle_prev)) - - if angle_change > threshold_angle: - return True - - angle_prev = angle_next - s_prev = s_next - return False - - def compute(self, state: VehicleState, component: Component = None): - t = state.pose.t - if self.t_last is None: - self.t_last = t - dt = 0 - else: - dt = t - self.t_last - - curr_x = state.pose.x - curr_y = state.pose.y - curr_yaw = state.pose.yaw if state.pose.yaw is not None else 0.0 - speed = state.v - - if self.path is None: - if component: - component.debug_event("No path provided to Stanley controller. Doing nothing.") - self.t_last = t - self.pid_speed.reset() - return (0.0, 0.0) - - if self.path.frame != state.pose.frame: - if component: - component.debug_event(f"Transforming path from {self.path.frame.name} to {state.pose.frame.name}") - self.path = transforms.transform_path(self.path, self.path.frame, state.pose.frame, current_pose=state.pose) - - if self.trajectory and self.trajectory.frame != state.pose.frame: - if component: - component.debug_event(f"Transforming trajectory from {self.trajectory.frame.name} to {state.pose.frame.name}") - self.trajectory = transforms.transform_path(self.trajectory, self.trajectory.frame, state.pose.frame, current_pose=state.pose) - - - if self.reverse is None: - self.reverse = self.initialize_state_and_direction(state) - - if self.reverse: - fx, fy = self._find_rear_axle_position(curr_x, curr_y, curr_yaw) - else: - fx, fy = self._find_front_axle_position(curr_x, curr_y, curr_yaw) - search_start = self.current_path_parameter - search_end = self.current_path_parameter + 5.0 - closest_dist, closest_parameter = self.path.closest_point_local((fx, fy), [search_start, search_end]) - self.current_path_parameter = closest_parameter - - target_x, target_y = self.path.eval(self.current_path_parameter) - tangent = self.path.eval_tangent(self.current_path_parameter) - path_yaw = atan2(tangent[1], tangent[0]) - - dx = fx - target_x - dy = fy - target_y - - is_sharp_turn_ahead = self._check_sharp_turn_ahead( - lookahead_s=2.0, - threshold_angle=np.pi/2.0 - ) - if is_sharp_turn_ahead and not self.sharp_turn: - self.sharp_turn = True - use_reverse = self.is_target_behind_vehicle(state.pose, (target_x, target_y)) - if use_reverse and self.sharp_turn and not self.reverse: - self.reverse = True - self.sharp_turn = False - - if self.reverse: - cross_track_error = dx * (-tangent[1]) + dy * tangent[0] - self.k += self.k - else: - left_vec = np.array([sin(curr_yaw), -cos(curr_yaw)]) - cross_track_error = np.sign(np.dot(np.array([dx, dy]), left_vec)) * closest_dist - - yaw_error = normalise_angle(path_yaw - curr_yaw) - - desired_speed = abs(self.desired_speed) - feedforward_accel = 0.0 - - if self.reverse: - heading_term = -yaw_error - cross_term_input = self.k * (-cross_track_error) / (self.k_soft + abs(speed)) - cross_term = atan2(cross_term_input, 1.0) - desired_steering_angle = heading_term + cross_term - - if self.trajectory and self.desired_speed_source in ['path', 'trajectory']: - current_trajectory_param_for_eval = self.current_path_parameter - if hasattr(self.trajectory, 'parameter_to_time'): - current_trajectory_param_for_eval = self.trajectory.parameter_to_time(self.current_path_parameter) - - if self.trajectory is not None: - deriv = self.trajectory.eval_derivative(current_trajectory_param_for_eval) - path_speed_magnitude = np.linalg.norm(deriv) - else: - path_speed_magnitude = 0.0 - - desired_speed = min(path_speed_magnitude, self.reverse_speed_limit) - - desired_speed = desired_speed * -1.0 - - is_at_start_backward = self.current_path_parameter <= self.path.domain()[0] + 1e-3 - - if is_at_start_backward: - desired_speed = 0.0 - feedforward_accel = -2.0 * -1.0 - - else: - difference_dt = 0.1 - current_speed_abs = abs(speed) - if current_speed_abs < 0.1: estimated_path_step = 0.1 * difference_dt * -1.0 - else: estimated_path_step = current_speed_abs * difference_dt * -1.0 - - future_parameter = self.current_path_parameter + estimated_path_step - - future_parameter = np.clip(future_parameter, self.path.domain()[0], self.path.domain()[-1]) - - future_trajectory_param_for_eval = future_parameter - if hasattr(self.trajectory, 'parameter_to_time'): - future_trajectory_param_for_eval = self.trajectory.parameter_to_time(future_parameter) - - if self.trajectory is not None: - future_deriv = self.trajectory.eval_derivative(future_trajectory_param_for_eval) - next_path_speed_magnitude = min(np.linalg.norm(future_deriv), self.speed_limit) - else: - next_path_speed_magnitude = 0.0 - - next_desired_speed = next_path_speed_magnitude * -1.0 - - feedforward_accel = (next_desired_speed - desired_speed) / difference_dt - feedforward_accel = np.clip(feedforward_accel, -self.max_decel, self.max_accel) - - desired_speed_magnitude_slowed = abs(desired_speed) * np.exp(-abs(cross_track_error) * 0.6) - desired_speed = desired_speed_magnitude_slowed * -1.0 if desired_speed_magnitude_slowed != 0 else 0.0 - - else: - cross_term = atan2(self.k * cross_track_error, self.k_soft + speed) - desired_steering_angle = yaw_error + cross_term - - if self.trajectory and self.desired_speed_source in ['path', 'trajectory']: - if len(self.trajectory.points) < 2 or self.current_path_parameter >= self.path.domain()[1]: - if component: - component.debug_event("Stanley: Past the end of trajectory, stopping.") - desired_speed = 0.0 - feedforward_accel = -2.0 - else: - if self.desired_speed_source == 'path': - current_trajectory_time = self.trajectory.parameter_to_time(self.current_path_parameter) - else: - self.current_traj_parameter += dt - current_trajectory_time = self.current_traj_parameter - - deriv = self.trajectory.eval_derivative(current_trajectory_time) - desired_speed = min(np.linalg.norm(deriv), self.speed_limit) - - difference_dt = 0.1 - future_t = current_trajectory_time + difference_dt - if future_t > self.trajectory.domain()[1]: - future_t = self.trajectory.domain()[1] - future_deriv = self.trajectory.eval_derivative(future_t) - next_desired_speed = min(np.linalg.norm(future_deriv), self.speed_limit) - feedforward_accel = (next_desired_speed - desired_speed) / difference_dt - feedforward_accel = np.clip(feedforward_accel, -self.max_decel, self.max_accel) - else: - if desired_speed is None: - desired_speed = 4.0 - - desired_speed *= np.exp(-abs(cross_track_error) * 0.6) - - if abs(desired_speed) > self.speed_limit: - if self.reverse: - desired_speed = self.speed_limit * -1.0 if desired_speed != 0 else 0.0 - else: - desired_speed = self.speed_limit - - speed_error = desired_speed - speed - output_accel = self.pid_speed.advance(e=speed_error, t=t, feedforward_term=feedforward_accel) - - if output_accel > self.max_accel: - output_accel = self.max_accel - elif output_accel < -self.max_decel: - output_accel = -self.max_decel - - if desired_speed == 0 and abs(speed) < 0.01 and output_accel < 0: - output_accel = 0.0 - - if component is not None: - component.debug('curr pt',(curr_x,curr_y)) - component.debug("desired_x",target_x) - component.debug("desired_y",target_y) - component.debug("Stanley: path param", self.current_path_parameter) - component.debug("Stanley: crosstrack dist", closest_dist) - component.debug("crosstrack error", cross_track_error) - component.debug("Stanley: yaw_error", yaw_error) - component.debug('steering_angle', desired_steering_angle) - component.debug("Stanley: desired_speed (m/s)", desired_speed) - component.debug("Stanley: feedforward_accel (m/s^2)", feedforward_accel) - component.debug("Stanley: output_accel (m/s^2)", output_accel) - - self.t_last = t - return (output_accel, desired_steering_angle) - -##################################### -# 3. Tracker component -##################################### -class StanleyTrajectoryTracker(Component): - - def __init__(self, vehicle_interface=None, **kwargs): - self.stanley = Stanley(**kwargs) - self.vehicle_interface = vehicle_interface - - def rate(self): - return 50.0 - - def state_inputs(self): - return ["vehicle", "trajectory"] - - def state_outputs(self): - return [] - - def update(self, vehicle: VehicleState, trajectory: Trajectory): - self.stanley.set_path(trajectory) - - accel, f_delta = self.stanley.compute(vehicle, self) - - steering_angle = front2steer(f_delta) - steering_angle = np.clip( - steering_angle, - settings.get('vehicle.geometry.min_steering_angle', -0.5), - settings.get('vehicle.geometry.max_steering_angle', 0.5) - ) - - cmd = self.vehicle_interface.simple_command(accel, steering_angle, vehicle) - self.vehicle_interface.send_command(cmd) - - def healthy(self): - """Optional: check if the controller has a valid path.""" - return self.stanley.path is not None diff --git a/launch/fixed_route.yaml b/launch/fixed_route.yaml index f083c003c..be36221e6 100644 --- a/launch/fixed_route.yaml +++ b/launch/fixed_route.yaml @@ -16,12 +16,12 @@ drive: planning: route_planning: type: StaticRoutePlanner - args: [!relative_path '../GEMstack/knowledge/routes/yamada.csv'] + # args: [!relative_path '../GEMstack/knowledge/routes/yamada.csv'] # args: [!relative_path '../GEMstack/knowledge/routes/xyhead_highbay_backlot_p.csv'] # args: [!relative_path '../GEMstack/knowledge/routes/reverse_15m.csv'] # args: [!relative_path '../GEMstack/knowledge/routes/offset.csv'] # args: [!relative_path '../GEMstack/knowledge/routes/forward_15m.csv'] - # args: [!relative_path '../GEMstack/knowledge/routes/backward_15m.csv'] + args: [!relative_path '../GEMstack/knowledge/routes/backward_15m.csv'] # args: [!relative_path '../GEMstack/knowledge/routes/forward_curve.csv'] # args: [!relative_path '../GEMstack/knowledge/routes/backward_curve.csv'] # args: [!relative_path '../GEMstack/knowledge/routes/yamada.csv'] @@ -32,9 +32,10 @@ drive: args: [null] #desired speed in m/s. If null, this will keep the route untimed for the trajectory tracker trajectory_tracking: # type: pure_pursuit.PurePursuitTrajectoryTracker - # type: stanley.StanleyTrajectoryTracker - type: mpc.MPCTrajectoryTracker - args: {desired_speed: 0.5} #approximately 5mph + type: stanley.StanleyTrajectoryTracker + # type: stanley_forward.StanleyTrajectoryTracker + # type: mpc.MPCTrajectoryTracker + args: {desired_speed: 2.5} #approximately 5mph print: True log: # Specify the top-level folder to save the log files. Default is 'logs' From 5a5e79017000c3412e412bcfb1e29c3d9a0b2a0b Mon Sep 17 00:00:00 2001 From: Patrick Wu Date: Mon, 12 May 2025 19:39:29 -0500 Subject: [PATCH 48/71] update requirements.txt --- requirements.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/requirements.txt b/requirements.txt index 94db8ba43..f199eb053 100644 --- a/requirements.txt +++ b/requirements.txt @@ -7,3 +7,4 @@ shapely klampt pyyaml dacite +casadi From e4a2d7b6c2240807797afca73494e74748e5cc9a Mon Sep 17 00:00:00 2001 From: Jugal Date: Tue, 13 May 2025 15:17:48 -0500 Subject: [PATCH 49/71] Update mpc.py added logging for mpc --- GEMstack/onboard/planning/mpc.py | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/GEMstack/onboard/planning/mpc.py b/GEMstack/onboard/planning/mpc.py index 5ba45b232..fba789e32 100644 --- a/GEMstack/onboard/planning/mpc.py +++ b/GEMstack/onboard/planning/mpc.py @@ -267,7 +267,16 @@ def model(x, u): delta = float(sol.value(x[1,4])) self.prev_x = sol.value(x) self.prev_u = sol.value(u) - + if component is not None: + component.debug("mpc/accel", acc) + component.debug("mpc/delta", delta) + component.debug("mpc/closest_time", closest_time) + component.debug("mpc/state_x", state.pose.x) + component.debug("mpc/state_y", state.pose.y) + component.debug("mpc/state_yaw", state.pose.yaw) + component.debug("mpc/target_x", traj_points[1][0]) + component.debug("mpc/target_y", traj_points[1][1]) + component.debug("mpc/target_theta", target_angles[0]) # xy_array = [f"np.array([{round(self.prev_x[t,0],8)}, {round(self.prev_x[t,1],8)}])" for t in range(self.prev_x.shape[0])] # print("mpc = [", ", ".join(xy_array), "]") @@ -314,4 +323,4 @@ def update(self, vehicle: VehicleState, trajectory: Trajectory): self.vehicle_interface.send_command(self.vehicle_interface.simple_command(accel, steering_angle, vehicle)) def healthy(self): - return self.mpc.path is not None \ No newline at end of file + return self.mpc.path is not None From d895167436009dcc8aa899e94d37d4223d6430da Mon Sep 17 00:00:00 2001 From: Patrick8894 <45711086+Patrick8894@users.noreply.github.com> Date: Tue, 13 May 2025 15:49:05 -0500 Subject: [PATCH 50/71] Add log plot for mpc --- logs/logplot_mpc.py | 80 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 80 insertions(+) create mode 100644 logs/logplot_mpc.py diff --git a/logs/logplot_mpc.py b/logs/logplot_mpc.py new file mode 100644 index 000000000..10611558c --- /dev/null +++ b/logs/logplot_mpc.py @@ -0,0 +1,80 @@ +import pandas as pd +import matplotlib.pyplot as plt +import numpy as np +import sys +import os + +def find_mpc_csv(directory): + for filename in os.listdir(directory): + if filename.startswith("MPCTrajectoryTracker_debug") and filename.endswith(".csv"): + return os.path.join(directory, filename) + raise FileNotFoundError("No MPCTrajectoryTracker_debug.csv found in the directory.") + +def plot_mpc_debug(csv_path): + df = pd.read_csv(csv_path) + + time = df['mpc/accel time'] - df['mpc/accel time'].iloc[0] + accel = df['mpc/accel'] + delta = df['mpc/delta'] + state_x = df['mpc/state_x'] + state_y = df['mpc/state_y'] + target_x = df['mpc/target_x'] + target_y = df['mpc/target_y'] + yaw = df['mpc/state_yaw'] + target_theta = df['mpc/target_theta'] + + jerk_time = time[1:] + jerk = np.diff(accel) / np.diff(time) + heading_acc = np.diff(yaw) / np.diff(time) + + fig, axs = plt.subplots(2, 2, figsize=(12, 8)) + fig.subplots_adjust(hspace=0.4, wspace=0.3) + + axs[0,0].plot(jerk_time, jerk, color='blue') + axs[0,0].set_title("Vehicle Jerk Over Time") + axs[0,0].set_xlabel("Time (s)") + axs[0,0].set_ylabel("Jerk (m/s³)") + axs[0,0].grid(True) + + axs[0,1].plot(jerk_time, heading_acc, color='orange') + axs[0,1].set_title("Heading Acceleration Over Time") + axs[0,1].set_xlabel("Time (s)") + axs[0,1].set_ylabel("Heading Acceleration (rad/s²)") + axs[0,1].grid(True) + + axs[1,0].plot(time, state_x - target_x, label="CTE X", color='green') + axs[1,0].plot(time, state_y - target_y, label="CTE Y", color='purple') + axs[1,0].set_title("Cross Track Error Over Time") + axs[1,0].set_xlabel("Time (s)") + axs[1,0].set_ylabel("CTE (m)") + axs[1,0].legend() + axs[1,0].grid(True) + + axs[1,1].plot(state_x, state_y, label='Actual Trajectory', color='blue') + axs[1,1].plot(target_x, target_y, label='Target Trajectory', color='orange') + axs[1,1].set_title("Trajectory Comparison") + axs[1,1].set_xlabel("X (m)") + axs[1,1].set_ylabel("Y (m)") + axs[1,1].legend() + axs[1,1].grid(True) + + plt.show() + +if __name__ == '__main__': + if len(sys.argv) != 2: + print("Usage: python3 logplot_mpc.py ") + sys.exit(1) + + log_dir = sys.argv[1] + if not os.path.isdir(log_dir): + print(f"Error: '{log_dir}' is not a valid directory.") + sys.exit(1) + + try: + csv_file = find_mpc_csv(log_dir) + print(f"Found MPC CSV: {csv_file}") + plot_mpc_debug(csv_file) + except Exception as e: + print(f"Error: {e}") + sys.exit(1) + From 5c8995414004dd7fee36cbbf647b08c3ed61903d Mon Sep 17 00:00:00 2001 From: Rohit-R-Rao <104187252+Rohit-R-Rao@users.noreply.github.com> Date: Tue, 13 May 2025 14:02:55 -0700 Subject: [PATCH 51/71] Add files via upload adding creep control --- GEMstack/onboard/planning/creep_planning.py | 586 ++++++++++++++++++++ 1 file changed, 586 insertions(+) create mode 100644 GEMstack/onboard/planning/creep_planning.py diff --git a/GEMstack/onboard/planning/creep_planning.py b/GEMstack/onboard/planning/creep_planning.py new file mode 100644 index 000000000..2009a12ac --- /dev/null +++ b/GEMstack/onboard/planning/creep_planning.py @@ -0,0 +1,586 @@ +# File: GEMstack/onboard/planning/longitudinal_planning.py +from typing import List, Tuple +import math +from ..component import Component +from ...state import AllState, VehicleState, EntityRelation, EntityRelationEnum, Path, Trajectory, Route, \ + ObjectFrameEnum +from ...utils import serialization +from ...mathutils import transforms +import numpy as np + +DEBUG = False # Set to False to disable debug output + + +def generate_dense_points(points: List[Tuple[float, float]], density: int = 10) -> List[Tuple[float, float]]: + if not points: + return [] + if len(points) == 1: + return points.copy() + + dense_points = [points[0]] + for i in range(len(points) - 1): + p0 = points[i] + p1 = points[i + 1] + dx = p1[0] - p0[0] + dy = p1[1] - p0[1] + seg_length = math.hypot(dx, dy) + + n_interp = int(round(seg_length * density)) + + for j in range(1, n_interp + 1): + fraction = j / (n_interp + 1) + x_interp = p0[0] + fraction * dx + y_interp = p0[1] + fraction * dy + dense_points.append((x_interp, y_interp)) + + dense_points.append(p1) + + return dense_points + + +def compute_cumulative_distances(points: List[List[float]]) -> List[float]: + s_vals = [0.0] + for i in range(1, len(points)): + dx = points[i][0] - points[i - 1][0] + dy = points[i][1] - points[i - 1][1] + ds = math.hypot(dx, dy) + s_vals.append(s_vals[-1] + ds) + + if DEBUG: + print("[DEBUG] compute_cumulative_distances: s_vals =", s_vals) + + return s_vals + + +def longitudinal_plan(path, acceleration, deceleration, max_speed, current_speed): + path_normalized = path.arc_length_parameterize() + points = list(path_normalized.points) + dense_points = generate_dense_points(points) + s_vals = compute_cumulative_distances(dense_points) + L = s_vals[-1] # Total path length + stopping_distance = (current_speed ** 2) / (2 * deceleration) + + if DEBUG: + print("[DEBUG] compute_cumulative_distances: s_vals =", s_vals) + print("[DEBUG] longitudinal_plan: Total path length L =", L) + print("[DEBUG] longitudinal_plan: Braking distance needed =", stopping_distance) + + if stopping_distance > L: # Case where there is not enough stopping distance to stop before path ends (calls emergency brake) + return longitudinal_brake(path, deceleration, current_speed) + + if current_speed > max_speed: # Case where car is exceeding the max speed so we need to slow down (do initial slowdown) + if DEBUG: + print(f"[DEBUG] Handling case where current_speed ({current_speed:.2f}) > max_speed ({max_speed:.2f})") + + # Initial deceleration phase to reach max_speed + initial_decel_distance = (current_speed ** 2 - max_speed ** 2) / (2 * deceleration) + initial_decel_time = (current_speed - max_speed) / deceleration + remaining_distance = L - initial_decel_distance + + if DEBUG: + print( + f"[DEBUG] Phase 1 - Initial Decel: distance = {initial_decel_distance:.2f}, time = {initial_decel_time:.2f}") + print(f"[DEBUG] Remaining distance after reaching max_speed: {remaining_distance:.2f}") + + # Calculate final deceleration distance needed to stop from max_speed + final_decel_distance = (max_speed ** 2) / (2 * deceleration) + cruise_distance = remaining_distance - final_decel_distance + + if DEBUG: + print(f"[DEBUG] Phase 2 - Cruise: distance = {cruise_distance:.2f}") + print(f"[DEBUG] Phase 3 - Final Decel: distance = {final_decel_distance:.2f}") + + times = [] + for s in s_vals: + if s <= initial_decel_distance: # Phase 1: Initial deceleration to max_speed + v = math.sqrt(current_speed ** 2 - 2 * deceleration * s) + t = (current_speed - v) / deceleration + if DEBUG: # Print every 10m + print(f"[DEBUG] Initial Decel: s = {s:.2f}, v = {v:.2f}, t = {t:.2f}") + + elif s <= initial_decel_distance + cruise_distance: # Phase 2: Cruise at max_speed + s_in_cruise = s - initial_decel_distance + t = initial_decel_time + s_in_cruise / max_speed + if DEBUG: # Print every 10m + print(f"[DEBUG] Cruise: s = {s:.2f}, v = {max_speed:.2f}, t = {t:.2f}") + + else: # Phase 3: Final deceleration to stop + s_in_final_decel = s - (initial_decel_distance + cruise_distance) + v = math.sqrt(max(max_speed ** 2 - 2 * deceleration * s_in_final_decel, 0.0)) + t = initial_decel_time + cruise_distance / max_speed + (max_speed - v) / deceleration + if DEBUG: # Print every 10m + print(f"[DEBUG] Final Decel: s = {s:.2f}, v = {v:.2f}, t = {t:.2f}") + + times.append(t) + + if DEBUG: + print("[DEBUG] Trajectory complete: Three phases executed") + print(f"[DEBUG] Total time: {times[-1]:.2f}") + + return Trajectory(frame=path.frame, points=dense_points, times=times) + + if acceleration <= 0: + if DEBUG: + print(f"[DEBUG] No acceleration allowed. Current speed: {current_speed:.2f}") + + # Pure deceleration phase + s_decel = (current_speed ** 2) / (2 * deceleration) + T_decel = current_speed / deceleration + + if DEBUG: + print(f"[DEBUG] Will maintain speed until s_decel: {s_decel:.2f}") + print(f"[DEBUG] Total deceleration time will be: {T_decel:.2f}") + + times = [] + for s in s_vals: + if s <= L - s_decel: # Maintain current speed until deceleration point + t_point = s / current_speed + if DEBUG: + print(f"[DEBUG] Constant Speed Phase: s = {s:.2f}, v = {current_speed:.2f}, t = {t_point:.2f}") + else: # Deceleration phase + s_in_decel = s - (L - s_decel) + v = math.sqrt(max(current_speed ** 2 - 2 * deceleration * s_in_decel, 0.0)) + t_point = (L - s_decel) / current_speed + (current_speed - v) / deceleration + if DEBUG: + print(f"[DEBUG] Deceleration Phase: s = {s:.2f}, v = {v:.2f}, t = {t_point:.2f}") + + times.append(t_point) + + return Trajectory(frame=path.frame, points=dense_points, times=times) + + # Determine max possible peak speed given distance + v_peak_possible = math.sqrt( + (2 * acceleration * deceleration * L + deceleration * current_speed ** 2) / (acceleration + deceleration)) + v_target = min(max_speed, v_peak_possible) + + if DEBUG: + print("[DEBUG] longitudinal_plan: v_peak_possible =", v_peak_possible, "v_target =", v_target) + + # Compute acceleration phase + s_accel = max(0.0, (v_target ** 2 - current_speed ** 2) / (2 * acceleration)) + t_accel = max(0.0, (v_target - current_speed) / acceleration) + + # Compute deceleration phase + s_decel = max(0.0, (v_target ** 2) / (2 * deceleration)) + t_decel = max(0.0, v_target / deceleration) + + # Compute cruise phase + s_cruise = max(0.0, L - s_accel - s_decel) + t_cruise = s_cruise / v_target if v_target > 0 else 0.0 + + if DEBUG: + print("[DEBUG] longitudinal_plan: s_accel =", s_accel, "t_accel =", t_accel) + print("[DEBUG] longitudinal_plan: s_decel =", s_decel, "t_decel =", t_decel) + print("[DEBUG] longitudinal_plan: s_cruise =", s_cruise, "t_cruise =", t_cruise) + + times = [] + for s in s_vals: + if s <= s_accel: # Acceleration phase + v = math.sqrt(current_speed ** 2 + 2 * acceleration * s) + t_point = (v - current_speed) / acceleration + + if DEBUG: + print(f"[DEBUG] Acceleration Phase: s = {s:.2f}, v = {v:.2f}, t = {t_point:.2f}") + + elif s <= s_accel + s_cruise: # Cruise phase + t_point = t_accel + (s - s_accel) / v_target + + if DEBUG: + print(f"[DEBUG] Cruise Phase: s = {s:.2f}, t = {t_point:.2f}") + + else: # Deceleration phase + s_decel_phase = s - s_accel - s_cruise + v_decel = math.sqrt(max(v_target ** 2 - 2 * deceleration * s_decel_phase, 0.0)) + t_point = t_accel + t_cruise + (v_target - v_decel) / deceleration + + if t_point < times[-1]: # Ensure time always increases + t_point = times[-1] + 0.01 # Small time correction step + + if DEBUG: + print(f"[DEBUG] Deceleration Phase: s = {s:.2f}, v = {v_decel:.2f}, t = {t_point:.2f}") + + times.append(t_point) + + if DEBUG: + print("[DEBUG] longitudinal_plan: Final times =", times) + + return Trajectory(frame=path.frame, points=dense_points, times=times) + + +def longitudinal_brake(path: Path, deceleration: float, current_speed: float, + emergency_decel: float = 8.0) -> Trajectory: + # Vehicle already stopped - maintain position + if current_speed <= 0: + print("[DEBUG] longitudinal_brake: Zero velocity case! ", [path.points[0]] * len(path.points)) + return Trajectory( + frame=path.frame, + points=[path.points[0]] * len(path.points), + times=[float(i) for i in range(len(path.points))] + ) + + # Get total path length + path_length = sum( + np.linalg.norm(np.array(path.points[i + 1]) - np.array(path.points[i])) + for i in range(len(path.points) - 1) + ) + + # Calculate stopping distance with normal deceleration + T_stop_normal = current_speed / deceleration + s_stop_normal = current_speed * T_stop_normal - 0.5 * deceleration * (T_stop_normal ** 2) + + # Check if emergency braking is needed + if s_stop_normal > path_length: + if DEBUG: + print("[DEBUG] longitudinal_brake: Emergency braking needed!") + print(f"[DEBUG] longitudinal_brake: Normal stopping distance: {s_stop_normal:.2f}m") + print(f"[DEBUG] longitudinal_brake: Available distance: {path_length:.2f}m") + + # Calculate emergency braking parameters + T_stop = current_speed / emergency_decel + s_stop = current_speed * T_stop - 0.5 * emergency_decel * (T_stop ** 2) + + if DEBUG: + print(f"[DEBUG] longitudinal_brake: Emergency stopping distance: {s_stop:.2f}m") + print(f"[DEBUG] longitudinal_brake: Emergency stopping time: {T_stop:.2f}s") + + decel_to_use = emergency_decel + + else: + if DEBUG: + print("[DEBUG] longitudinal_brake: Normal braking sufficient") + T_stop = T_stop_normal + decel_to_use = deceleration + + # Generate time points (use more points for smoother trajectory) + num_points = max(len(path.points), 50) + times = np.linspace(0, T_stop, num_points) + + # Calculate distances at each time point using physics equation + distances = current_speed * times - 0.5 * decel_to_use * (times ** 2) + + # Generate points along the path + points = [] + for d in distances: + if d <= path_length: + points.append(path.eval(d)) + else: + points.append(path.eval(d)) + + if DEBUG: + print(f"[DEBUG] longitudinal_brake: Using deceleration of {decel_to_use:.2f} m/s²") + print(f"[DEBUG] longitudinal_brake: Final stopping time: {T_stop:.2f}s") + + return Trajectory(frame=path.frame, points=points, times=times.tolist()) + + times = [] + for s in s_vals: + if s <= s_accel: # Acceleration phase + v = math.sqrt(current_speed ** 2 + 2 * acceleration * s) + t_point = (v - current_speed) / acceleration + + if DEBUG: + print(f"[DEBUG] Acceleration Phase: s = {s:.2f}, v = {v:.2f}, t = {t_point:.2f}") + + elif s <= s_accel + s_cruise: # Cruise phase + t_point = t_accel + (s - s_accel) / v_target + + if DEBUG: + print(f"[DEBUG] Cruise Phase: s = {s:.2f}, t = {t_point:.2f}") + + else: # Deceleration phase + s_decel_phase = s - s_accel - s_cruise + v_decel = math.sqrt(max(v_target ** 2 - 2 * deceleration * s_decel_phase, 0.0)) + t_point = t_accel + t_cruise + (v_target - v_decel) / deceleration + + if t_point < times[-1]: # Ensure time always increases + t_point = times[-1] + 0.01 # Small time correction step + + if DEBUG: + print(f"[DEBUG] Deceleration Phase: s = {s:.2f}, v = {v_decel:.2f}, t = {t_point:.2f}") + + times.append(t_point) + + if DEBUG: + print("[DEBUG] longitudinal_plan: Final times =", times) + + return Trajectory(frame=path.frame, points=dense_points, times=times) + +def longitudinal_brake(path: Path, deceleration: float, current_speed: float, emergency_decel: float = 8.0) -> Trajectory: + # Vehicle already stopped - maintain position + if current_speed <= 0: + print("[DEBUG] longitudinal_brake: Zero velocity case! ", [path.points[0]] * len(path.points)) + return Trajectory( + frame=path.frame, + points=[path.points[0]] * len(path.points), + times=[float(i) for i in range(len(path.points))] + ) + + # Get total path length + path_length = sum( + np.linalg.norm(np.array(path.points[i+1]) - np.array(path.points[i])) + for i in range(len(path.points)-1) + ) + + # Calculate stopping distance with normal deceleration + T_stop_normal = current_speed / deceleration + s_stop_normal = current_speed * T_stop_normal - 0.5 * deceleration * (T_stop_normal ** 2) + + # Check if emergency braking is needed + if s_stop_normal > path_length: + if DEBUG: + print("[DEBUG] longitudinal_brake: Emergency braking needed!") + print(f"[DEBUG] longitudinal_brake: Normal stopping distance: {s_stop_normal:.2f}m") + print(f"[DEBUG] longitudinal_brake: Available distance: {path_length:.2f}m") + + # Calculate emergency braking parameters + T_stop = current_speed / emergency_decel + s_stop = current_speed * T_stop - 0.5 * emergency_decel * (T_stop ** 2) + + if DEBUG: + print(f"[DEBUG] longitudinal_brake: Emergency stopping distance: {s_stop:.2f}m") + print(f"[DEBUG] longitudinal_brake: Emergency stopping time: {T_stop:.2f}s") + + decel_to_use = emergency_decel + + else: + if DEBUG: + print("[DEBUG] longitudinal_brake: Normal braking sufficient") + T_stop = T_stop_normal + decel_to_use = deceleration + + # Generate time points (use more points for smoother trajectory) + num_points = max(len(path.points), 50) + times = np.linspace(0, T_stop, num_points) + + # Calculate distances at each time point using physics equation + distances = current_speed * times - 0.5 * decel_to_use * (times ** 2) + + # Generate points along the path + points = [] + for d in distances: + if d <= path_length: + points.append(path.eval(d)) + else: + points.append(path.eval(d)) + + if DEBUG: + print(f"[DEBUG] longitudinal_brake: Using deceleration of {decel_to_use:.2f} m/s²") + print(f"[DEBUG] longitudinal_brake: Final stopping time: {T_stop:.2f}s") + + return Trajectory(frame=path.frame, points=points, times=times.tolist()) + + +''' +# OTHER CREEP CONTROL CODE + +def inverse_speed_function(distance_to_object): + +# max_creep_speed = 1.0 # Maximum speed when creeping (m/s) +# min_creep_speed = 0.2 # Minimum speed to maintain (m/s) +# safe_distance = 5.0 # Distance at which to start slowing down (m) + +# # Almost stop when very close (≤0.25m) +# if distance_to_object <= 0.25: +# return 0.0 # Practically zero speed + +# # Slow creep when close (≤1.5m) +# if distance_to_object <= 3.0: +# return min_creep_speed +# speed = min_creep_speed + (max_creep_speed - min_creep_speed) * (distance_to_object / safe_distance) +# return min(speed, max_creep_speed) + +def pid_speed_control(distance_to_object, target_distance, current_speed, prev_error, integral, dt, + kp=0.5, ki=0.1, kd=0.05, min_speed=0.2, max_speed=1.0): + if distance_to_object <= 0.25: + return 0.0, 0.0, 0.0 + error = distance_to_object - target_distance + integral += error * dt + integral = max(-5.0, min(integral, 5.0)) # Prevent integral windup + derivative = (error - prev_error) / dt if dt > 0 else 0 + p_term = kp * error + i_term = ki * integral + d_term = kd * derivative + speed_adjustment = p_term + i_term + d_term + base_speed = 0.5 # Base creep speed + target_speed = base_speed + speed_adjustment + target_speed = max(min_speed, min(target_speed, max_speed)) + return target_speed, error, integral + + +''' + + + +class CreepTrajectoryPlanner(Component): + """Follows the given route. Brakes if the ego–vehicle must yield + (e.g. to a pedestrian) or if the end of the route is near; otherwise, + it accelerates (or cruises) toward a desired speed. + """ + + def __init__(self): + self.route_progress = None + self.t_last = None + self.acceleration = 5 + self.desired_speed = 2.0 + self.deceleration = 2.0 + self.emergency_brake = 8.0 + # Parameters for end-of-route linear deceleration + self.end_stop_distance = 12.5 # Distance in meters to start linear deceleration + # Did 15, 10, 7.5, 17.5, 20, 12.5 + def state_inputs(self): + return ['all'] + + def state_outputs(self) -> List[str]: + return ['trajectory'] + + def rate(self): + return 10.0 + + def check_end_of_route(self, route, current_parameter): + """Check if vehicle is approaching the end of the route and calculate + appropriate linear deceleration parameters. + + Args: + route: The complete route + current_parameter: Current position along the route + + Returns: + (bool, float): Tuple containing: + - Whether the vehicle should start decelerating + - Adjusted speed if deceleration needed, otherwise desired_speed + """ + route_length = route.length() + + distance_remaining = route_length - current_parameter + + if DEBUG: + print(f"[DEBUG] check_end_of_route: Route length = {route_length}, " + f"Current position = {current_parameter}, " + f"Distance remaining = {distance_remaining}") + + if distance_remaining <= self.end_stop_distance: + if distance_remaining > 0.1: # Avoid division by very small numbers + required_decel = (self.desired_speed ** 2) / (2 * distance_remaining) + + linear_factor = distance_remaining / self.end_stop_distance + adjusted_speed = self.desired_speed * linear_factor + if DEBUG: + print(f"[DEBUG] End deceleration active: {distance_remaining:.2f}m remaining, " + f"required deceleration = {required_decel:.2f} m/s², " + f"speed adjusted to {adjusted_speed:.2f} m/s") + + return True, adjusted_speed + else: + return True, 0.0 + + return False, self.desired_speed + + def update(self, state: AllState): + vehicle = state.vehicle # type: VehicleState + route = state.route # type: Route + t = state.t + + if DEBUG: + print("[DEBUG] CreepTrajectoryPlanner.update: t =", t) + + if self.t_last is None: + self.t_last = t + dt = t - self.t_last + if DEBUG: + print("[DEBUG] CreepTrajectoryPlanner.update: dt =", dt) + + curr_x = vehicle.pose.x + curr_y = vehicle.pose.y + curr_v = vehicle.v + if DEBUG: + print(f"[DEBUG] CreepTrajectoryPlanner.update: Vehicle position = ({curr_x}, {curr_y}), speed = {curr_v}, ") + + # Determine progress along the route. + if self.route_progress is None: + self.route_progress = 0.0 + _, closest_parameter = route.closest_point_local( + [curr_x, curr_y], + (self.route_progress - 5.0, self.route_progress + 5.0) + ) + if DEBUG: + print("[DEBUG] CreepTrajectoryPlanner.update: Closest parameter on route =", closest_parameter) + self.route_progress = closest_parameter + + # Check if approaching end of route and get adjusted speed + approaching_end, target_speed = self.check_end_of_route(route, closest_parameter) + if DEBUG and approaching_end: + print("[DEBUG] CreepTrajectoryPlanner.update: Vehicle is approaching end of route") + print(f"[DEBUG] CreepTrajectoryPlanner.update: Target speed = {target_speed}") + + # Extract a 10 m segment of the route for planning lookahead. + route_with_lookahead = route.trim(closest_parameter, closest_parameter + 10.0) + if DEBUG: + print("[DEBUG] CreepTrajectoryPlanner.update: Route Lookahead =", route_with_lookahead) + + print("[DEBUG] state", state.relations) + # Check whether any yield relations (e.g. due to pedestrians) require braking. + stay_braking = False + pointSet = set() + for i in range(len(route_with_lookahead.points)): + if tuple(route_with_lookahead.points[i]) in pointSet: + stay_braking = True + break + pointSet.add(tuple(route_with_lookahead.points[i])) + + should_brake = any( + r.type == EntityRelationEnum.STOPPING_AT and r.obj1 == '' + for r in state.relations + ) + should_decelerate = any( + r.type == EntityRelationEnum.YIELDING and r.obj1 == '' + for r in state.relations + ) if should_brake == False else False + + # If approaching end of route, override the target speed + if approaching_end: + should_accelerate = (not should_brake and not should_decelerate and curr_v < target_speed) + else: + should_accelerate = (not should_brake and not should_decelerate and curr_v < self.desired_speed) + + if DEBUG: + print("[DEBUG] CreepTrajectoryPlanner.update: stay_braking =", stay_braking) + print("[DEBUG] CreepTrajectoryPlanner.update: should_brake =", should_brake) + print("[DEBUG] CreepTrajectoryPlanner.update: should_accelerate =", should_accelerate) + print("[DEBUG] CreepTrajectoryPlanner.update: should_decelerate =", should_decelerate) + print("[DEBUG] CreepTrajectoryPlanner.update: target_speed =", target_speed if approaching_end else self.desired_speed) + + if stay_braking: + traj = longitudinal_brake(route_with_lookahead, 0.0, 0.0, 0.0) + if DEBUG: + print("[DEBUG] CreepTrajectoryPlanner.update: Using longitudinal_brake (stay braking).") + elif should_brake: + traj = longitudinal_brake(route_with_lookahead, self.emergency_brake, curr_v) + if DEBUG: + print("[DEBUG] CreepTrajectoryPlanner.update: Using longitudinal_brake.") + elif should_decelerate: + traj = longitudinal_brake(route_with_lookahead, self.deceleration, curr_v) + if DEBUG: + print("[DEBUG] CreepTrajectoryPlanner.update: Using longitudinal_brake.") + elif approaching_end: + # Use linear deceleration to stop at end of route + traj = longitudinal_plan(route_with_lookahead, self.acceleration, + self.deceleration, target_speed, curr_v) + if DEBUG: + print("[DEBUG] CreepTrajectoryPlanner.update: Using longitudinal_plan with end-of-route deceleration.") + elif should_accelerate: + traj = longitudinal_plan(route_with_lookahead, self.acceleration, + self.deceleration, self.desired_speed, curr_v) + if DEBUG: + print("[DEBUG] CreepTrajectoryPlanner.update: Using longitudinal_plan (accelerate).") + else: + # Maintain current speed if not accelerating or braking. + traj = longitudinal_plan(route_with_lookahead, 0.0, self.deceleration, self.desired_speed, curr_v) + if DEBUG: + print( + "[DEBUG] CreepTrajectoryPlanner.update: Maintaining current speed with longitudinal_plan (0 accel).") + + self.t_last = t + if DEBUG: + print(f'[DEBUG] Current Velocity: {curr_v}, Target Speed: {target_speed if approaching_end else self.desired_speed}') + print("[DEBUG] CreepTrajectoryPlanner.update: Returning trajectory with", len(traj.points), "points.") + return traj From 230d8066c59b820b4b47ff29824e33376ab474cc Mon Sep 17 00:00:00 2001 From: Patrick8894 <45711086+Patrick8894@users.noreply.github.com> Date: Tue, 13 May 2025 17:25:34 -0500 Subject: [PATCH 52/71] Update mpc.py --- GEMstack/onboard/planning/mpc.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/GEMstack/onboard/planning/mpc.py b/GEMstack/onboard/planning/mpc.py index fba789e32..5580e2834 100644 --- a/GEMstack/onboard/planning/mpc.py +++ b/GEMstack/onboard/planning/mpc.py @@ -108,6 +108,10 @@ def compute(self, state: VehicleState, component: Component = None): # Clip reversed part new_points, new_times = self.clip_reverse_path_with_times(sliced_points, sliced_times) + #New points and times is needed only when running on trajectory that includes both forward and reverse driving that requires gear shifting + new_points = points + new_times = times + # Interpolate trajectory points to match MPC time horizon traj_points = [] j = 0 From 7f51c43ccb071e12a118e7a556ad89cfedfe5a86 Mon Sep 17 00:00:00 2001 From: Patrick8894 <45711086+Patrick8894@users.noreply.github.com> Date: Tue, 13 May 2025 17:48:40 -0500 Subject: [PATCH 53/71] Update mpc.py --- GEMstack/onboard/planning/mpc.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/GEMstack/onboard/planning/mpc.py b/GEMstack/onboard/planning/mpc.py index 5580e2834..464e6838e 100644 --- a/GEMstack/onboard/planning/mpc.py +++ b/GEMstack/onboard/planning/mpc.py @@ -278,8 +278,8 @@ def model(x, u): component.debug("mpc/state_x", state.pose.x) component.debug("mpc/state_y", state.pose.y) component.debug("mpc/state_yaw", state.pose.yaw) - component.debug("mpc/target_x", traj_points[1][0]) - component.debug("mpc/target_y", traj_points[1][1]) + component.debug("mpc/target_x", self.path.points[self.current_path_parameter][0]) + component.debug("mpc/target_y", self.path.points[self.current_path_parameter][1]) component.debug("mpc/target_theta", target_angles[0]) # xy_array = [f"np.array([{round(self.prev_x[t,0],8)}, {round(self.prev_x[t,1],8)}])" for t in range(self.prev_x.shape[0])] # print("mpc = [", ", ".join(xy_array), "]") From 719eaea43aed426cccefd70b848d75bb9986aa08 Mon Sep 17 00:00:00 2001 From: Patrick8894 <45711086+Patrick8894@users.noreply.github.com> Date: Tue, 13 May 2025 17:51:26 -0500 Subject: [PATCH 54/71] Delete GEMstack/onboard/planning/stanley_forward.py --- GEMstack/onboard/planning/stanley_forward.py | 315 ------------------- 1 file changed, 315 deletions(-) delete mode 100644 GEMstack/onboard/planning/stanley_forward.py diff --git a/GEMstack/onboard/planning/stanley_forward.py b/GEMstack/onboard/planning/stanley_forward.py deleted file mode 100644 index 20d2c7ec3..000000000 --- a/GEMstack/onboard/planning/stanley_forward.py +++ /dev/null @@ -1,315 +0,0 @@ -###supports purely forward routes### -import numpy as np -from math import sin, cos, atan2 - -# These imports match your existing project structure -from ...mathutils.control import PID -from ...utils import settings -from ...mathutils import transforms -from ...knowledge.vehicle.geometry import front2steer -from ...knowledge.vehicle.dynamics import acceleration_to_pedal_positions -from ...state.vehicle import VehicleState, ObjectFrameEnum -from ...state.trajectory import Path, Trajectory -from ..interface.gem import GEMVehicleCommand -from ..component import Component - -##################################### -# 1. Angle normalization -##################################### -def normalise_angle(angle): - """Wraps an angle to the [-pi, pi] range.""" - while angle > np.pi: - angle -= 2.0 * np.pi - while angle < -np.pi: - angle += 2.0 * np.pi - return angle - -##################################### -# 2. Stanley-based controller with longitudinal PID -##################################### -class Stanley(object): - """ - A Stanley controller that handles lateral control (steering) - plus a basic longitudinal control (PID + optional feedforward). - It has been modified to reduce oscillations by: - 1) Lower gains - 2) Steering damping - 3) Low-pass filter on steering - 4) Gentler speed logic when cornering - """ - - def __init__( - self, - control_gain=None, - softening_gain=None, - desired_speed=None - ): - """ - :param control_gain: Stanley lateral control gain k (lowered from the default to reduce overshoot). - :param softening_gain: Softening gain k_soft. - :param yaw_rate_gain: Yaw-rate gain k_yaw_rate (helps reduce oscillations). - :param steering_damp_gain: Steering damping gain k_damp_steer. - :param desired_speed: Desired speed (float) or 'path'/'trajectory'. - """ - - # 1) Lower Gains - self.k = control_gain if control_gain is not None else settings.get('control.stanley.control_gain') - self.k_soft = softening_gain if softening_gain is not None else settings.get('control.stanley.softening_gain') - - # Typically, this is the max front-wheel steering angle in radians - self.max_steer = settings.get('vehicle.geometry.max_wheel_angle') - self.wheelbase = settings.get('vehicle.geometry.wheelbase') - - # Basic longitudinal constraints - self.speed_limit = settings.get('vehicle.limits.max_speed') - self.max_accel = settings.get('vehicle.limits.max_acceleration') - self.max_decel = settings.get('vehicle.limits.max_deceleration') - - # PID for longitudinal speed tracking - p = settings.get('control.longitudinal_control.pid_p') - d = settings.get('control.longitudinal_control.pid_d') - i = settings.get('control.longitudinal_control.pid_i') - self.pid_speed = PID(p, d, i, windup_limit=20) - - # Speed source: numeric or derived from path/trajectory - if desired_speed is not None: - self.desired_speed_source = desired_speed - else: - self.desired_speed_source = settings.get('control.stanley.desired_speed', 'path') - - if isinstance(self.desired_speed_source, (int, float)): - self.desired_speed = self.desired_speed_source - else: - self.desired_speed = None - - # For path/trajectory - self.path_arg = None - self.path = None - self.trajectory = None - self.current_path_parameter = 0.0 - self.current_traj_parameter = 0.0 - self.t_last = None - - def set_path(self, path: Path): - """Sets the path or trajectory to track.""" - if path == self.path_arg: - return - self.path_arg = path - - # If the path has more than 2D, reduce to (x,y) - if len(path.points[0]) > 2: - path = path.get_dims([0,1]) - - # If no timing info, we can't rely on 'path'/'trajectory' speed - if not isinstance(path, Trajectory): - self.path = path.arc_length_parameterize() - self.trajectory = None - self.current_traj_parameter = 0.0 - if self.desired_speed_source in ['path', 'trajectory']: - raise ValueError("Stanley: Provided path has no timing. Either provide a Trajectory or set a constant desired_speed.") - else: - self.path = path.arc_length_parameterize() - self.trajectory = path - self.current_traj_parameter = self.trajectory.domain()[0] - - self.current_path_parameter = 0.0 - - def _find_front_axle_position(self, x, y, yaw): - """Compute front-axle world position from the center/rear and yaw.""" - fx = x + self.wheelbase * cos(yaw) - fy = y + self.wheelbase * sin(yaw) - return fx, fy - - def compute(self, state: VehicleState, component: Component = None): - """Compute the control outputs: (longitudinal acceleration, front wheel angle).""" - t = state.pose.t - if self.t_last is None: - self.t_last = t - dt = 0 - else: - dt = t - self.t_last - - # Current vehicle states - curr_x = state.pose.x - curr_y = state.pose.y - curr_yaw = state.pose.yaw if state.pose.yaw is not None else 0.0 - speed = state.v - - if self.path is None: - if component: - component.debug_event("No path provided to Stanley controller. Doing nothing.") - return (0.0, 0.0) - - # Ensure same frame - if self.path.frame != state.pose.frame: - if component: - component.debug_event(f"Transforming path from {self.path.frame.name} to {state.pose.frame.name}") - self.path = self.path.to_frame(state.pose.frame, current_pose=state.pose) - - if self.trajectory and self.trajectory.frame != state.pose.frame: - if component: - component.debug_event(f"Transforming trajectory from {self.trajectory.frame.name} to {state.pose.frame.name}") - self.trajectory = self.trajectory.to_frame(state.pose.frame, current_pose=state.pose) - - # 1) Closest point - fx, fy = self._find_front_axle_position(curr_x, curr_y, curr_yaw) - search_start = self.current_path_parameter - 5.0 - search_end = self.current_path_parameter + 5.0 - closest_dist, closest_parameter = self.path.closest_point_local((fx, fy), [search_start, search_end]) - self.current_path_parameter = closest_parameter - - # 2) Path heading - target_x, target_y = self.path.eval(closest_parameter) - tangent = self.path.eval_tangent(closest_parameter) - path_yaw = atan2(tangent[1], tangent[0]) - desired_x = target_x - desired_y = target_y - # 3) Lateral error - dx = fx - target_x - dy = fy - target_y - left_vec = np.array([sin(curr_yaw), -cos(curr_yaw)]) - cross_track_error = np.sign(np.dot(np.array([dx, dy]), left_vec)) * closest_dist - - # 4) Heading error - yaw_error = normalise_angle(path_yaw - curr_yaw) - - # 5) Standard Stanley terms - cross_term = atan2(self.k * cross_track_error, self.k_soft + speed) - desired_steering_angle = yaw_error + cross_term - - desired_speed = self.desired_speed - feedforward_accel = 0.0 - - if self.trajectory and self.desired_speed_source in ['path', 'trajectory']: - if len(self.trajectory.points) < 2 or self.current_path_parameter >= self.path.domain()[1]: - # End of trajectory -> stop - if component: - component.debug_event("Stanley: Past the end of trajectory, stopping.") - desired_speed = 0.0 - feedforward_accel = -2.0 - else: - if self.desired_speed_source == 'path': - current_trajectory_time = self.trajectory.parameter_to_time(self.current_path_parameter) - else: - self.current_traj_parameter += dt - current_trajectory_time = self.current_traj_parameter - - deriv = self.trajectory.eval_derivative(current_trajectory_time) - desired_speed = min(np.linalg.norm(deriv), self.speed_limit) - - difference_dt = 0.1 - future_t = current_trajectory_time + difference_dt - if future_t > self.trajectory.domain()[1]: - future_t = self.trajectory.domain()[1] - future_deriv = self.trajectory.eval_derivative(future_t) - next_desired_speed = min(np.linalg.norm(future_deriv), self.speed_limit) - feedforward_accel = (next_desired_speed - desired_speed) / difference_dt - feedforward_accel = np.clip(feedforward_accel, -self.max_decel, self.max_accel) - else: - if desired_speed is None: - desired_speed = 4.0 - - # Cross-track-based slowdown (less aggressive than before). - desired_speed *= np.exp(-abs(cross_track_error) * 0.6) - - # Clip to speed limit - if desired_speed > self.speed_limit: - desired_speed = self.speed_limit - - # PID for longitudinal control - speed_error = desired_speed - speed - output_accel = self.pid_speed.advance(e=speed_error, t=t, feedforward_term=feedforward_accel) - - # Clip acceleration - if output_accel > self.max_accel: - output_accel = self.max_accel - elif output_accel < -self.max_decel: - output_accel = -self.max_decel - - # Avoid negative accel when fully stopped - if desired_speed == 0 and abs(speed) < 0.01 and output_accel < 0: - output_accel = 0.0 - - # Debug - if component is not None: - # component.debug("Stanley: fx, fy", (fx, fy)) - component.debug('curr pt',(curr_x,curr_y)) - component.debug("desired_x",desired_x) - component.debug("desired_y",desired_y) - component.debug("Stanley: path param", self.current_path_parameter) - component.debug("Stanley: crosstrack dist", closest_dist) - component.debug("crosstrack error", cross_track_error) - component.debug("Stanley: yaw_error", yaw_error) - component.debug('steering_angle', desired_steering_angle) - component.debug("Stanley: desired_speed (m/s)", desired_speed) - component.debug("Stanley: feedforward_accel (m/s^2)", feedforward_accel) - component.debug("Stanley: output_accel (m/s^2)", output_accel) - - if output_accel > self.max_accel: - output_accel = self.max_accel - - if output_accel < -self.max_decel: - output_accel = -self.max_decel - - self.t_last = t - return (output_accel, desired_steering_angle) - -##################################### -# 3. Tracker component -##################################### -class StanleyTrajectoryTracker(Component): - """ - A trajectory-tracking Component that uses the above Stanley controller - for lateral control plus PID-based longitudinal control. - It now includes measures to mitigate oscillations. - """ - - def __init__(self, vehicle_interface=None, **kwargs): - """ - :param vehicle_interface: The low-level interface to send commands to the vehicle. - :param kwargs: Optional parameters to pass into the Stanley(...) constructor. - """ - self.stanley = Stanley(**kwargs) - self.vehicle_interface = vehicle_interface - - def rate(self): - """Control frequency in Hz.""" - return 50.0 - - def state_inputs(self): - """ - Required state inputs: - - Vehicle state - - Trajectory - """ - return ["vehicle", "trajectory"] - - def state_outputs(self): - """No direct output state here.""" - return [] - - def update(self, vehicle: VehicleState, trajectory: Trajectory): - """ - Per control cycle: - 1) Set the path/trajectory - 2) Compute (acceleration, front wheel angle) - 3) Convert front wheel angle to steering wheel angle (if necessary) - 4) Send command to the vehicle - """ - self.stanley.set_path(trajectory) - accel, f_delta = self.stanley.compute(vehicle, self) - - # If your low-level interface expects steering wheel angle: - steering_angle = front2steer(f_delta) - steering_angle = np.clip( - steering_angle, - settings.get('vehicle.geometry.min_steering_angle', -0.5), - settings.get('vehicle.geometry.max_steering_angle', 0.5) - ) - - cmd = self.vehicle_interface.simple_command(accel, steering_angle, vehicle) - self.vehicle_interface.send_command(cmd) - - def healthy(self): - """Optional: check if the controller has a valid path.""" - return self.stanley.path is not None From 5513f8155ab967d36ba31525b90ab2dd7e24b286 Mon Sep 17 00:00:00 2001 From: Patrick Wu Date: Tue, 13 May 2025 20:50:01 -0500 Subject: [PATCH 55/71] modify mpc for better args --- GEMstack/knowledge/defaults/current.yaml | 3 +- GEMstack/knowledge/routes/reverse_15m.csv | 51 ++++++++++++++++------- GEMstack/onboard/planning/mpc.py | 28 ++++++++----- 3 files changed, 54 insertions(+), 28 deletions(-) diff --git a/GEMstack/knowledge/defaults/current.yaml b/GEMstack/knowledge/defaults/current.yaml index a03e88cd8..fc351eb61 100644 --- a/GEMstack/knowledge/defaults/current.yaml +++ b/GEMstack/knowledge/defaults/current.yaml @@ -30,6 +30,7 @@ control: mpc: dt: 0.2 # Time step for the MPC controller (seconds) horizon: 30 # Prediction horizon for the MPC controller (number of time steps) + switch_gear: False # Whether to switch gears during runtime desired_speed: trajectory # Speed reference source: can be "trajectory", "path", or a constant value # Shared longitudinal control parameters @@ -48,4 +49,4 @@ simulator: #orientation_noise: 0.04 # 2.3 degrees noise #velocity_noise: # constant: 0.04 # 4cm/s noise - # linear: 0.02 # 2% noise + # linear: 0.02 # 2% noise \ No newline at end of file diff --git a/GEMstack/knowledge/routes/reverse_15m.csv b/GEMstack/knowledge/routes/reverse_15m.csv index 2311d2fc5..9568aef2e 100644 --- a/GEMstack/knowledge/routes/reverse_15m.csv +++ b/GEMstack/knowledge/routes/reverse_15m.csv @@ -1,20 +1,39 @@ 0.0,0,0 +-0.5,0,0 -1.0,0,0 +-1.5,0,0 -2.0,0,0 +-2.5,0.5,0 -3.0,1.0,0 --4,1.0,0 --5,1.0,0 --6,1.0,0 --7,1.0,0 --8,1.0,0 --9,1.0,0 --10,0,0 --11,-1,0 --12,-1,0 --13,-1,0 --14,-1,0 --15,-1,0 --16,-1,0 --17,-1,0 --18,-1,0 --19,-1,0 +-3.5,1.0,0 +-4.0,1.0,0 +-4.5,1.0,0 +-5.0,1.0,0 +-5.5,1.0,0 +-6.0,1.0,0 +-6.5,1.0,0 +-7.0,1.0,0 +-7.5,1.0,0 +-8.0,1.0,0 +-8.5,0.5,0 +-9.0,0,0 +-9.5,-0.5,0 +-10.0,-1.0,0 +-10.5,-1.0,0 +-11.0,-1.0,0 +-11.5,-1.0,0 +-12.0,-1.0,0 +-12.5,-1.0,0 +-13.0,-1.0,0 +-13.5,-1.0,0 +-14.0,-1.0,0 +-14.5,-1.0,0 +-15.0,-1.0,0 +-15.5,-1.0,0 +-16.0,-1.0,0 +-16.5,-1.0,0 +-17.0,-1.0,0 +-17.5,-1.0,0 +-18.0,-1.0,0 +-18.5,-1.0,0 +-19.0,-1.0,0 \ No newline at end of file diff --git a/GEMstack/onboard/planning/mpc.py b/GEMstack/onboard/planning/mpc.py index 464e6838e..252e5a6b3 100644 --- a/GEMstack/onboard/planning/mpc.py +++ b/GEMstack/onboard/planning/mpc.py @@ -18,7 +18,8 @@ def __init__(self, T=None, dt=None, desired_speed=None): self.delta_rate_bounds = [-0.4, 0.4] # Predefined front wheel rate limit to simplify computation self.steering_angle_range = [settings.get('vehicle.geometry.min_steering_angle'),settings.get('vehicle.geometry.max_steering_angle')] self.a_bounds = [-settings.get('vehicle.limits.max_deceleration'), settings.get('vehicle.limits.max_acceleration')] - + self.switch_gear = settings.get('control.mpc.switch_gear', False) + if desired_speed is not None: self.desired_speed_source = desired_speed else: @@ -91,7 +92,10 @@ def compute(self, state: VehicleState, component: Component = None): x0 = np.array([state.pose.x, state.pose.y, state.pose.yaw % (2 * np.pi), state.v, state.front_wheel_angle]) # print(x0) - closest_dist,closest_time = self.path.closest_point_local((x0[0], x0[1]),[self.current_traj_parameter-0.0,self.current_traj_parameter+1.0], True) + if self.switch_gear: + closest_dist,closest_time = self.path.closest_point_local((x0[0], x0[1]),[self.current_traj_parameter-0.0,self.current_traj_parameter+1.0], True) + else: + closest_dist,closest_time = self.path.closest_point_local((x0[0], x0[1]),[self.current_traj_parameter-0.0,self.current_traj_parameter+5.0], True) self.current_traj_parameter = closest_time times = self.path.times @@ -101,16 +105,16 @@ def compute(self, state: VehicleState, component: Component = None): j += 1 self.current_path_parameter = j - # Slice path from j - sliced_points = points[j:] - sliced_times = times[j:] + if self.switch_gear: + # Slice path from j + sliced_points = points[j:] + sliced_times = times[j:] - # Clip reversed part - new_points, new_times = self.clip_reverse_path_with_times(sliced_points, sliced_times) - - #New points and times is needed only when running on trajectory that includes both forward and reverse driving that requires gear shifting - new_points = points - new_times = times + # Clip reversed part + new_points, new_times = self.clip_reverse_path_with_times(sliced_points, sliced_times) + else: + new_points = points + new_times = times # Interpolate trajectory points to match MPC time horizon traj_points = [] @@ -281,9 +285,11 @@ def model(x, u): component.debug("mpc/target_x", self.path.points[self.current_path_parameter][0]) component.debug("mpc/target_y", self.path.points[self.current_path_parameter][1]) component.debug("mpc/target_theta", target_angles[0]) + # xy_array = [f"np.array([{round(self.prev_x[t,0],8)}, {round(self.prev_x[t,1],8)}])" for t in range(self.prev_x.shape[0])] # print("mpc = [", ", ".join(xy_array), "]") + print(self.current_path_parameter) print(acc, delta) # print(self.prev_u[0]) # print(self.prev_x[0]) From 4ef6f549fbaada5b78a1bacd46712270a529c759 Mon Sep 17 00:00:00 2001 From: Patrick Wu Date: Tue, 13 May 2025 20:51:30 -0500 Subject: [PATCH 56/71] remove unused controller --- launch/fixed_route.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/launch/fixed_route.yaml b/launch/fixed_route.yaml index f5b49a9b3..fbd644797 100644 --- a/launch/fixed_route.yaml +++ b/launch/fixed_route.yaml @@ -32,7 +32,6 @@ drive: trajectory_tracking: # type: pure_pursuit.PurePursuitTrajectoryTracker type: stanley.StanleyTrajectoryTracker - # type: stanley_forward.StanleyTrajectoryTracker # type: mpc.MPCTrajectoryTracker args: {desired_speed: 2.5} #approximately 5mph print: False From a961e737ac3963ab7504e267aa52cb7bf185b56b Mon Sep 17 00:00:00 2001 From: Patrick Wu Date: Tue, 13 May 2025 20:53:05 -0500 Subject: [PATCH 57/71] add comments --- GEMstack/onboard/planning/mpc.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/GEMstack/onboard/planning/mpc.py b/GEMstack/onboard/planning/mpc.py index 252e5a6b3..2b42f9509 100644 --- a/GEMstack/onboard/planning/mpc.py +++ b/GEMstack/onboard/planning/mpc.py @@ -7,6 +7,10 @@ import casadi import math +########################### +# Bo-Hao Wu's code # +########################### + class MPCController(object): """Model Predictive Controller for trajectory tracking.""" def __init__(self, T=None, dt=None, desired_speed=None): From ede9caeeb6d5a71e79967d39eec1da2e3ee335b9 Mon Sep 17 00:00:00 2001 From: Jugal Date: Wed, 14 May 2025 02:41:27 -0500 Subject: [PATCH 58/71] Update logplot_s.py --- logs/logplot_s.py | 1 + 1 file changed, 1 insertion(+) diff --git a/logs/logplot_s.py b/logs/logplot_s.py index ecff7f118..a7d671e7b 100644 --- a/logs/logplot_s.py +++ b/logs/logplot_s.py @@ -1,3 +1,4 @@ +###Jugal's Code### ####Following code can be used to visualize performance of Stanley after using it.### ###How to use?- After running the simulation or on actual vehicle you get logs in log folder### ### You can run the command- python3 logplot_s.py "logfilename".### From 975bc67553a95c5e2d8f58d56d8e49d2ec6c38f2 Mon Sep 17 00:00:00 2001 From: Jugal Date: Wed, 14 May 2025 02:41:46 -0500 Subject: [PATCH 59/71] Update logplot_pp.py --- logs/logplot_pp.py | 1 + 1 file changed, 1 insertion(+) diff --git a/logs/logplot_pp.py b/logs/logplot_pp.py index 2a6533afc..3c67f6790 100644 --- a/logs/logplot_pp.py +++ b/logs/logplot_pp.py @@ -1,3 +1,4 @@ +###Jugal's Code### ####Following code can be used to visualize performance of Pure Pursuit after using it.### ###How to use?- After running the simulation or on actual vehicle you get logs in log folder### ### You can run the command- python3 logplot_pp.py "logfilename".### From 71fa98769b63512aa6ff96c5e204a57cb19031bb Mon Sep 17 00:00:00 2001 From: Jugal Date: Wed, 14 May 2025 02:43:01 -0500 Subject: [PATCH 60/71] Update logplot_mpc.py --- logs/logplot_mpc.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/logs/logplot_mpc.py b/logs/logplot_mpc.py index 10611558c..53bcabc3d 100644 --- a/logs/logplot_mpc.py +++ b/logs/logplot_mpc.py @@ -1,3 +1,5 @@ +###Jugal's Code### +###Following file gives the plots for MPC logs. Go inside logs folder where the logs are saved and run- python3 logplot_mpc.py "filename" to get the plots. import pandas as pd import matplotlib.pyplot as plt import numpy as np From 1d9834a8df5e4e0fc645dca5f7a0d537f0006aca Mon Sep 17 00:00:00 2001 From: hhxjqm <67122725+hhxjqm@users.noreply.github.com> Date: Thu, 15 May 2025 01:55:42 -0500 Subject: [PATCH 61/71] Update current.yaml --- GEMstack/knowledge/defaults/current.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/knowledge/defaults/current.yaml b/GEMstack/knowledge/defaults/current.yaml index fc351eb61..999be1845 100644 --- a/GEMstack/knowledge/defaults/current.yaml +++ b/GEMstack/knowledge/defaults/current.yaml @@ -35,7 +35,7 @@ control: # Shared longitudinal control parameters longitudinal_control: - pid_p: 20.0 # Proportional gain for speed PID controller + pid_p: 1.0 # Proportional gain for speed PID controller pid_i: 0.1 # Integral gain for speed PID controller pid_d: 1.0 # Derivative gain for speed PID controller From 5d08ec7f7fc8bb6c47ec4de27b95914fb17de99e Mon Sep 17 00:00:00 2001 From: Jugal Date: Thu, 15 May 2025 03:37:32 -0500 Subject: [PATCH 62/71] Update mpc.py Added logic for position error and heading error calculation and adding it to debug for plotting and further evaluation --- GEMstack/onboard/planning/mpc.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/GEMstack/onboard/planning/mpc.py b/GEMstack/onboard/planning/mpc.py index 2b42f9509..4fe192229 100644 --- a/GEMstack/onboard/planning/mpc.py +++ b/GEMstack/onboard/planning/mpc.py @@ -279,9 +279,25 @@ def model(x, u): delta = float(sol.value(x[1,4])) self.prev_x = sol.value(x) self.prev_u = sol.value(u) + ###Jugal's Code start here### + ###adding heading error and position error(similar to cross track error) for evaluation metric### + # NEW: Position and Heading Error + vehicle_x = state.pose.x + vehicle_y = state.pose.y + vehicle_yaw = state.pose.yaw + + path_x = self.path.points[self.current_path_parameter][0] + path_y = self.path.points[self.current_path_parameter][1] + path_yaw = target_angles[0] + + position_error = np.linalg.norm([vehicle_x - path_x, vehicle_y - path_y]) + heading_error = ((vehicle_yaw - path_yaw + np.pi) % (2 * np.pi)) - np.pi + ###Jugal's code end here### if component is not None: component.debug("mpc/accel", acc) component.debug("mpc/delta", delta) + component.debug("mpc/position_error", position_error) + component.debug("mpc/heading_error", heading_error) component.debug("mpc/closest_time", closest_time) component.debug("mpc/state_x", state.pose.x) component.debug("mpc/state_y", state.pose.y) From b7062da784ef9d47e140f51d20290c8fb5a73f29 Mon Sep 17 00:00:00 2001 From: Jugal Date: Thu, 15 May 2025 03:39:42 -0500 Subject: [PATCH 63/71] Update logplot_mpc.py added evaluation metric --- logs/logplot_mpc.py | 28 ++++++++++++++++++---------- 1 file changed, 18 insertions(+), 10 deletions(-) diff --git a/logs/logplot_mpc.py b/logs/logplot_mpc.py index 53bcabc3d..3c6fbb79f 100644 --- a/logs/logplot_mpc.py +++ b/logs/logplot_mpc.py @@ -25,12 +25,14 @@ def plot_mpc_debug(csv_path): yaw = df['mpc/state_yaw'] target_theta = df['mpc/target_theta'] + position_error = df['mpc/position_error'] + heading_error = df['mpc/heading_error'] + jerk_time = time[1:] jerk = np.diff(accel) / np.diff(time) - heading_acc = np.diff(yaw) / np.diff(time) fig, axs = plt.subplots(2, 2, figsize=(12, 8)) - fig.subplots_adjust(hspace=0.4, wspace=0.3) + fig.subplots_adjust(hspace=0.5, wspace=0.3) axs[0,0].plot(jerk_time, jerk, color='blue') axs[0,0].set_title("Vehicle Jerk Over Time") @@ -38,17 +40,17 @@ def plot_mpc_debug(csv_path): axs[0,0].set_ylabel("Jerk (m/s³)") axs[0,0].grid(True) - axs[0,1].plot(jerk_time, heading_acc, color='orange') - axs[0,1].set_title("Heading Acceleration Over Time") + axs[0,1].plot(time, position_error, label="Position Error", color='green') + axs[0,1].set_title("Position Error Over Time") axs[0,1].set_xlabel("Time (s)") - axs[0,1].set_ylabel("Heading Acceleration (rad/s²)") + axs[0,1].set_ylabel("Error (m)") + axs[0,1].legend() axs[0,1].grid(True) - axs[1,0].plot(time, state_x - target_x, label="CTE X", color='green') - axs[1,0].plot(time, state_y - target_y, label="CTE Y", color='purple') - axs[1,0].set_title("Cross Track Error Over Time") + axs[1,0].plot(time, heading_error, label="Heading Error", color='red') + axs[1,0].set_title("Heading Error Over Time") axs[1,0].set_xlabel("Time (s)") - axs[1,0].set_ylabel("CTE (m)") + axs[1,0].set_ylabel("Error (rad)") axs[1,0].legend() axs[1,0].grid(True) @@ -62,6 +64,13 @@ def plot_mpc_debug(csv_path): plt.show() + print("Max (abs) position error:", np.max(np.abs(position_error))) + print("Avg position error:", np.mean(np.abs(position_error))) + print("Max (abs) heading error:", np.max(np.abs(heading_error))) + print("Avg heading error:", np.mean(np.abs(heading_error))) + print("Max (abs) jerk:", np.max(np.abs(jerk))) + print("Avg jerk:", np.mean(np.abs(jerk))) + if __name__ == '__main__': if len(sys.argv) != 2: print("Usage: python3 logplot_mpc.py ") @@ -79,4 +88,3 @@ def plot_mpc_debug(csv_path): except Exception as e: print(f"Error: {e}") sys.exit(1) - From 55bceaaf292c75cdd8a4f0d33a18f2240f541e22 Mon Sep 17 00:00:00 2001 From: Patrick8894 <45711086+Patrick8894@users.noreply.github.com> Date: Thu, 15 May 2025 11:23:09 -0500 Subject: [PATCH 64/71] Update mpc.py comments --- GEMstack/onboard/planning/mpc.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/onboard/planning/mpc.py b/GEMstack/onboard/planning/mpc.py index 4fe192229..ab29f5402 100644 --- a/GEMstack/onboard/planning/mpc.py +++ b/GEMstack/onboard/planning/mpc.py @@ -147,7 +147,7 @@ def compute(self, state: VehicleState, component: Component = None): overcorrection_guard = 5 for i in range(len(traj_points)): s = i / (len(traj_points) + overcorrection_guard) # Normalized [0, 1] and add some offset to prevent overcorrection - decay_ratio = (1 - s) ** 1 # linear decay on each iteration to perform quadratic offset correction over time + decay_ratio = (1 - s) ** 1 # linear decay on each iteration to perform smooth offset correction over time # Get direction of trajectory at this point if i < len(traj_points) - 1: From 669ba96b019d180ae0056775a1b87daa7dfa4373 Mon Sep 17 00:00:00 2001 From: Rohit-R-Rao <104187252+Rohit-R-Rao@users.noreply.github.com> Date: Thu, 15 May 2025 10:01:28 -0700 Subject: [PATCH 65/71] Update creep_planning.py --- GEMstack/onboard/planning/creep_planning.py | 363 +------------------- 1 file changed, 3 insertions(+), 360 deletions(-) diff --git a/GEMstack/onboard/planning/creep_planning.py b/GEMstack/onboard/planning/creep_planning.py index 2009a12ac..9f69190db 100644 --- a/GEMstack/onboard/planning/creep_planning.py +++ b/GEMstack/onboard/planning/creep_planning.py @@ -7,367 +7,10 @@ from ...utils import serialization from ...mathutils import transforms import numpy as np +from .longitudinal_planning import longitudinal_plan, longitudinal_brake -DEBUG = False # Set to False to disable debug output - - -def generate_dense_points(points: List[Tuple[float, float]], density: int = 10) -> List[Tuple[float, float]]: - if not points: - return [] - if len(points) == 1: - return points.copy() - - dense_points = [points[0]] - for i in range(len(points) - 1): - p0 = points[i] - p1 = points[i + 1] - dx = p1[0] - p0[0] - dy = p1[1] - p0[1] - seg_length = math.hypot(dx, dy) - - n_interp = int(round(seg_length * density)) - - for j in range(1, n_interp + 1): - fraction = j / (n_interp + 1) - x_interp = p0[0] + fraction * dx - y_interp = p0[1] + fraction * dy - dense_points.append((x_interp, y_interp)) - - dense_points.append(p1) - - return dense_points - - -def compute_cumulative_distances(points: List[List[float]]) -> List[float]: - s_vals = [0.0] - for i in range(1, len(points)): - dx = points[i][0] - points[i - 1][0] - dy = points[i][1] - points[i - 1][1] - ds = math.hypot(dx, dy) - s_vals.append(s_vals[-1] + ds) - - if DEBUG: - print("[DEBUG] compute_cumulative_distances: s_vals =", s_vals) - - return s_vals - - -def longitudinal_plan(path, acceleration, deceleration, max_speed, current_speed): - path_normalized = path.arc_length_parameterize() - points = list(path_normalized.points) - dense_points = generate_dense_points(points) - s_vals = compute_cumulative_distances(dense_points) - L = s_vals[-1] # Total path length - stopping_distance = (current_speed ** 2) / (2 * deceleration) - - if DEBUG: - print("[DEBUG] compute_cumulative_distances: s_vals =", s_vals) - print("[DEBUG] longitudinal_plan: Total path length L =", L) - print("[DEBUG] longitudinal_plan: Braking distance needed =", stopping_distance) - - if stopping_distance > L: # Case where there is not enough stopping distance to stop before path ends (calls emergency brake) - return longitudinal_brake(path, deceleration, current_speed) - - if current_speed > max_speed: # Case where car is exceeding the max speed so we need to slow down (do initial slowdown) - if DEBUG: - print(f"[DEBUG] Handling case where current_speed ({current_speed:.2f}) > max_speed ({max_speed:.2f})") - - # Initial deceleration phase to reach max_speed - initial_decel_distance = (current_speed ** 2 - max_speed ** 2) / (2 * deceleration) - initial_decel_time = (current_speed - max_speed) / deceleration - remaining_distance = L - initial_decel_distance - - if DEBUG: - print( - f"[DEBUG] Phase 1 - Initial Decel: distance = {initial_decel_distance:.2f}, time = {initial_decel_time:.2f}") - print(f"[DEBUG] Remaining distance after reaching max_speed: {remaining_distance:.2f}") - - # Calculate final deceleration distance needed to stop from max_speed - final_decel_distance = (max_speed ** 2) / (2 * deceleration) - cruise_distance = remaining_distance - final_decel_distance - - if DEBUG: - print(f"[DEBUG] Phase 2 - Cruise: distance = {cruise_distance:.2f}") - print(f"[DEBUG] Phase 3 - Final Decel: distance = {final_decel_distance:.2f}") - - times = [] - for s in s_vals: - if s <= initial_decel_distance: # Phase 1: Initial deceleration to max_speed - v = math.sqrt(current_speed ** 2 - 2 * deceleration * s) - t = (current_speed - v) / deceleration - if DEBUG: # Print every 10m - print(f"[DEBUG] Initial Decel: s = {s:.2f}, v = {v:.2f}, t = {t:.2f}") - - elif s <= initial_decel_distance + cruise_distance: # Phase 2: Cruise at max_speed - s_in_cruise = s - initial_decel_distance - t = initial_decel_time + s_in_cruise / max_speed - if DEBUG: # Print every 10m - print(f"[DEBUG] Cruise: s = {s:.2f}, v = {max_speed:.2f}, t = {t:.2f}") - - else: # Phase 3: Final deceleration to stop - s_in_final_decel = s - (initial_decel_distance + cruise_distance) - v = math.sqrt(max(max_speed ** 2 - 2 * deceleration * s_in_final_decel, 0.0)) - t = initial_decel_time + cruise_distance / max_speed + (max_speed - v) / deceleration - if DEBUG: # Print every 10m - print(f"[DEBUG] Final Decel: s = {s:.2f}, v = {v:.2f}, t = {t:.2f}") - - times.append(t) - - if DEBUG: - print("[DEBUG] Trajectory complete: Three phases executed") - print(f"[DEBUG] Total time: {times[-1]:.2f}") - - return Trajectory(frame=path.frame, points=dense_points, times=times) - - if acceleration <= 0: - if DEBUG: - print(f"[DEBUG] No acceleration allowed. Current speed: {current_speed:.2f}") - - # Pure deceleration phase - s_decel = (current_speed ** 2) / (2 * deceleration) - T_decel = current_speed / deceleration - - if DEBUG: - print(f"[DEBUG] Will maintain speed until s_decel: {s_decel:.2f}") - print(f"[DEBUG] Total deceleration time will be: {T_decel:.2f}") - - times = [] - for s in s_vals: - if s <= L - s_decel: # Maintain current speed until deceleration point - t_point = s / current_speed - if DEBUG: - print(f"[DEBUG] Constant Speed Phase: s = {s:.2f}, v = {current_speed:.2f}, t = {t_point:.2f}") - else: # Deceleration phase - s_in_decel = s - (L - s_decel) - v = math.sqrt(max(current_speed ** 2 - 2 * deceleration * s_in_decel, 0.0)) - t_point = (L - s_decel) / current_speed + (current_speed - v) / deceleration - if DEBUG: - print(f"[DEBUG] Deceleration Phase: s = {s:.2f}, v = {v:.2f}, t = {t_point:.2f}") - - times.append(t_point) - - return Trajectory(frame=path.frame, points=dense_points, times=times) - - # Determine max possible peak speed given distance - v_peak_possible = math.sqrt( - (2 * acceleration * deceleration * L + deceleration * current_speed ** 2) / (acceleration + deceleration)) - v_target = min(max_speed, v_peak_possible) - - if DEBUG: - print("[DEBUG] longitudinal_plan: v_peak_possible =", v_peak_possible, "v_target =", v_target) - - # Compute acceleration phase - s_accel = max(0.0, (v_target ** 2 - current_speed ** 2) / (2 * acceleration)) - t_accel = max(0.0, (v_target - current_speed) / acceleration) - - # Compute deceleration phase - s_decel = max(0.0, (v_target ** 2) / (2 * deceleration)) - t_decel = max(0.0, v_target / deceleration) - - # Compute cruise phase - s_cruise = max(0.0, L - s_accel - s_decel) - t_cruise = s_cruise / v_target if v_target > 0 else 0.0 - - if DEBUG: - print("[DEBUG] longitudinal_plan: s_accel =", s_accel, "t_accel =", t_accel) - print("[DEBUG] longitudinal_plan: s_decel =", s_decel, "t_decel =", t_decel) - print("[DEBUG] longitudinal_plan: s_cruise =", s_cruise, "t_cruise =", t_cruise) - - times = [] - for s in s_vals: - if s <= s_accel: # Acceleration phase - v = math.sqrt(current_speed ** 2 + 2 * acceleration * s) - t_point = (v - current_speed) / acceleration - - if DEBUG: - print(f"[DEBUG] Acceleration Phase: s = {s:.2f}, v = {v:.2f}, t = {t_point:.2f}") - - elif s <= s_accel + s_cruise: # Cruise phase - t_point = t_accel + (s - s_accel) / v_target - - if DEBUG: - print(f"[DEBUG] Cruise Phase: s = {s:.2f}, t = {t_point:.2f}") - - else: # Deceleration phase - s_decel_phase = s - s_accel - s_cruise - v_decel = math.sqrt(max(v_target ** 2 - 2 * deceleration * s_decel_phase, 0.0)) - t_point = t_accel + t_cruise + (v_target - v_decel) / deceleration - - if t_point < times[-1]: # Ensure time always increases - t_point = times[-1] + 0.01 # Small time correction step - if DEBUG: - print(f"[DEBUG] Deceleration Phase: s = {s:.2f}, v = {v_decel:.2f}, t = {t_point:.2f}") - - times.append(t_point) - - if DEBUG: - print("[DEBUG] longitudinal_plan: Final times =", times) - - return Trajectory(frame=path.frame, points=dense_points, times=times) - - -def longitudinal_brake(path: Path, deceleration: float, current_speed: float, - emergency_decel: float = 8.0) -> Trajectory: - # Vehicle already stopped - maintain position - if current_speed <= 0: - print("[DEBUG] longitudinal_brake: Zero velocity case! ", [path.points[0]] * len(path.points)) - return Trajectory( - frame=path.frame, - points=[path.points[0]] * len(path.points), - times=[float(i) for i in range(len(path.points))] - ) - - # Get total path length - path_length = sum( - np.linalg.norm(np.array(path.points[i + 1]) - np.array(path.points[i])) - for i in range(len(path.points) - 1) - ) - - # Calculate stopping distance with normal deceleration - T_stop_normal = current_speed / deceleration - s_stop_normal = current_speed * T_stop_normal - 0.5 * deceleration * (T_stop_normal ** 2) - - # Check if emergency braking is needed - if s_stop_normal > path_length: - if DEBUG: - print("[DEBUG] longitudinal_brake: Emergency braking needed!") - print(f"[DEBUG] longitudinal_brake: Normal stopping distance: {s_stop_normal:.2f}m") - print(f"[DEBUG] longitudinal_brake: Available distance: {path_length:.2f}m") - - # Calculate emergency braking parameters - T_stop = current_speed / emergency_decel - s_stop = current_speed * T_stop - 0.5 * emergency_decel * (T_stop ** 2) - - if DEBUG: - print(f"[DEBUG] longitudinal_brake: Emergency stopping distance: {s_stop:.2f}m") - print(f"[DEBUG] longitudinal_brake: Emergency stopping time: {T_stop:.2f}s") - - decel_to_use = emergency_decel - - else: - if DEBUG: - print("[DEBUG] longitudinal_brake: Normal braking sufficient") - T_stop = T_stop_normal - decel_to_use = deceleration - - # Generate time points (use more points for smoother trajectory) - num_points = max(len(path.points), 50) - times = np.linspace(0, T_stop, num_points) - - # Calculate distances at each time point using physics equation - distances = current_speed * times - 0.5 * decel_to_use * (times ** 2) - - # Generate points along the path - points = [] - for d in distances: - if d <= path_length: - points.append(path.eval(d)) - else: - points.append(path.eval(d)) - - if DEBUG: - print(f"[DEBUG] longitudinal_brake: Using deceleration of {decel_to_use:.2f} m/s²") - print(f"[DEBUG] longitudinal_brake: Final stopping time: {T_stop:.2f}s") - - return Trajectory(frame=path.frame, points=points, times=times.tolist()) - - times = [] - for s in s_vals: - if s <= s_accel: # Acceleration phase - v = math.sqrt(current_speed ** 2 + 2 * acceleration * s) - t_point = (v - current_speed) / acceleration - - if DEBUG: - print(f"[DEBUG] Acceleration Phase: s = {s:.2f}, v = {v:.2f}, t = {t_point:.2f}") - - elif s <= s_accel + s_cruise: # Cruise phase - t_point = t_accel + (s - s_accel) / v_target - - if DEBUG: - print(f"[DEBUG] Cruise Phase: s = {s:.2f}, t = {t_point:.2f}") - - else: # Deceleration phase - s_decel_phase = s - s_accel - s_cruise - v_decel = math.sqrt(max(v_target ** 2 - 2 * deceleration * s_decel_phase, 0.0)) - t_point = t_accel + t_cruise + (v_target - v_decel) / deceleration - - if t_point < times[-1]: # Ensure time always increases - t_point = times[-1] + 0.01 # Small time correction step - - if DEBUG: - print(f"[DEBUG] Deceleration Phase: s = {s:.2f}, v = {v_decel:.2f}, t = {t_point:.2f}") - - times.append(t_point) - - if DEBUG: - print("[DEBUG] longitudinal_plan: Final times =", times) - - return Trajectory(frame=path.frame, points=dense_points, times=times) - -def longitudinal_brake(path: Path, deceleration: float, current_speed: float, emergency_decel: float = 8.0) -> Trajectory: - # Vehicle already stopped - maintain position - if current_speed <= 0: - print("[DEBUG] longitudinal_brake: Zero velocity case! ", [path.points[0]] * len(path.points)) - return Trajectory( - frame=path.frame, - points=[path.points[0]] * len(path.points), - times=[float(i) for i in range(len(path.points))] - ) - - # Get total path length - path_length = sum( - np.linalg.norm(np.array(path.points[i+1]) - np.array(path.points[i])) - for i in range(len(path.points)-1) - ) - - # Calculate stopping distance with normal deceleration - T_stop_normal = current_speed / deceleration - s_stop_normal = current_speed * T_stop_normal - 0.5 * deceleration * (T_stop_normal ** 2) - - # Check if emergency braking is needed - if s_stop_normal > path_length: - if DEBUG: - print("[DEBUG] longitudinal_brake: Emergency braking needed!") - print(f"[DEBUG] longitudinal_brake: Normal stopping distance: {s_stop_normal:.2f}m") - print(f"[DEBUG] longitudinal_brake: Available distance: {path_length:.2f}m") - - # Calculate emergency braking parameters - T_stop = current_speed / emergency_decel - s_stop = current_speed * T_stop - 0.5 * emergency_decel * (T_stop ** 2) - - if DEBUG: - print(f"[DEBUG] longitudinal_brake: Emergency stopping distance: {s_stop:.2f}m") - print(f"[DEBUG] longitudinal_brake: Emergency stopping time: {T_stop:.2f}s") - - decel_to_use = emergency_decel - - else: - if DEBUG: - print("[DEBUG] longitudinal_brake: Normal braking sufficient") - T_stop = T_stop_normal - decel_to_use = deceleration - - # Generate time points (use more points for smoother trajectory) - num_points = max(len(path.points), 50) - times = np.linspace(0, T_stop, num_points) - - # Calculate distances at each time point using physics equation - distances = current_speed * times - 0.5 * decel_to_use * (times ** 2) - - # Generate points along the path - points = [] - for d in distances: - if d <= path_length: - points.append(path.eval(d)) - else: - points.append(path.eval(d)) - - if DEBUG: - print(f"[DEBUG] longitudinal_brake: Using deceleration of {decel_to_use:.2f} m/s²") - print(f"[DEBUG] longitudinal_brake: Final stopping time: {T_stop:.2f}s") - - return Trajectory(frame=path.frame, points=points, times=times.tolist()) +DEBUG = False # Set to False to disable debug output ''' @@ -425,7 +68,7 @@ def __init__(self): self.deceleration = 2.0 self.emergency_brake = 8.0 # Parameters for end-of-route linear deceleration - self.end_stop_distance = 12.5 # Distance in meters to start linear deceleration + self.end_stop_distance = 15 # Distance in meters to start linear deceleration # Did 15, 10, 7.5, 17.5, 20, 12.5 def state_inputs(self): return ['all'] From eb9eda182acb13aadfa645b3ab9773ce451f7093 Mon Sep 17 00:00:00 2001 From: Rohit-R-Rao Date: Thu, 15 May 2025 13:29:42 -0500 Subject: [PATCH 66/71] revised creep_planning --- GEMstack/onboard/planning/creep_planning.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/onboard/planning/creep_planning.py b/GEMstack/onboard/planning/creep_planning.py index 9f69190db..57e4805ca 100644 --- a/GEMstack/onboard/planning/creep_planning.py +++ b/GEMstack/onboard/planning/creep_planning.py @@ -1,4 +1,4 @@ -# File: GEMstack/onboard/planning/longitudinal_planning.py +# File: GEMstack/onboard/planning/creep_planning.py from typing import List, Tuple import math from ..component import Component From 9e691c646918833a35b0fc7644e0c2a319be449b Mon Sep 17 00:00:00 2001 From: Jugal Date: Thu, 15 May 2025 15:55:14 -0500 Subject: [PATCH 67/71] Update xyhead_demo.yaml --- scenes/xyhead_demo.yaml | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/scenes/xyhead_demo.yaml b/scenes/xyhead_demo.yaml index f590b5675..52d943b34 100644 --- a/scenes/xyhead_demo.yaml +++ b/scenes/xyhead_demo.yaml @@ -1,9 +1,9 @@ vehicle_state: [4.0, 5.0, 0.0, 0.0, 0.0] -# agents: - # ped1: - # type: pedestrian - # position: [15.0, 2.0] - # nominal_velocity: -0.5 - # target: [15.0,10.0] - # behavior: loop - \ No newline at end of file +agents: + ped1: + type: pedestrian + position: [15.0, 2.0] + nominal_velocity: -0.5 + target: [15.0,10.0] + behavior: loop + From 0369cca3a56a4c58534daa78438f560dbb706a30 Mon Sep 17 00:00:00 2001 From: Jugal Date: Thu, 15 May 2025 15:56:23 -0500 Subject: [PATCH 68/71] Update serialization.py --- GEMstack/utils/serialization.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/utils/serialization.py b/GEMstack/utils/serialization.py index d825645fd..9ee161143 100644 --- a/GEMstack/utils/serialization.py +++ b/GEMstack/utils/serialization.py @@ -97,7 +97,7 @@ def deserialize(data): `data` can be a `str`, `bytes`, `dict`, or ROS std_msgs/String object. """ - #global REGISTRY ##causing error while pushing changes + global REGISTRY ##causing error while pushing changes name,version,data = deserialize_raw(data) if name not in REGISTRY: raise IOError("Class of type {} not found in registry".format(name)) From bc41621e945be42cb9be07cda84f31c87982df77 Mon Sep 17 00:00:00 2001 From: Patrick Wu Date: Thu, 15 May 2025 18:04:22 -0500 Subject: [PATCH 69/71] remove and change some files for PR --- .../knowledge/vehicle/gem_e2_slow_limits.yaml | 16 +- GEMstack/onboard/interface/gem.py | 372 +++++------ GEMstack/onboard/planning/creep_planning.py | 229 ------- GEMstack/utils/klampt_visualization.py | 598 +++++++++--------- GEMstack/utils/serialization.py | 4 +- scenes/xyhead_demo.yaml | 4 +- 6 files changed, 497 insertions(+), 726 deletions(-) delete mode 100644 GEMstack/onboard/planning/creep_planning.py diff --git a/GEMstack/knowledge/vehicle/gem_e2_slow_limits.yaml b/GEMstack/knowledge/vehicle/gem_e2_slow_limits.yaml index 921ce234a..8720ae250 100644 --- a/GEMstack/knowledge/vehicle/gem_e2_slow_limits.yaml +++ b/GEMstack/knowledge/vehicle/gem_e2_slow_limits.yaml @@ -1,8 +1,8 @@ -max_steering_rate: 2.0 #radians/s -max_speed: 2.5 #m/s, approx 5pmh -max_reverse_speed: 0.25 #m/s, approx 1mph -max_acceleration: 1.0 #m/s^2 -max_deceleration: 2.0 #m/s^2 -max_accelerator_pedal: 0.5 #0-1 -max_brake_pedal: 0.5 #0-1 - +max_steering_rate: 2.0 #radians/s +max_speed: 2.5 #m/s, approx 5pmh +max_reverse_speed: 0.25 #m/s, approx 1mph +max_acceleration: 1.0 #m/s^2 +max_deceleration: 2.0 #m/s^2 +max_accelerator_pedal: 0.5 #0-1 +max_brake_pedal: 0.5 #0-1 + diff --git a/GEMstack/onboard/interface/gem.py b/GEMstack/onboard/interface/gem.py index 4e44829d4..9abcd1b6b 100644 --- a/GEMstack/onboard/interface/gem.py +++ b/GEMstack/onboard/interface/gem.py @@ -1,187 +1,187 @@ -from dataclasses import dataclass -from ...utils import settings,serialization -from ...state import VehicleState, ObjectPose, ObjectFrameEnum -from ...knowledge.vehicle.geometry import front2steer,steer2front,heading_rate -from ...knowledge.vehicle.dynamics import pedal_positions_to_acceleration, acceleration_to_pedal_positions -from typing import List,Optional,Callable - -@dataclass -@serialization.register -class GEMVehicleReading: - """All items that the vehicle reports directly from its internal sensors.""" - speed : float = 0 # in m/s - gear : int = 0 # 0 neutral, -1 reverse, -2 park, > 0 forward - accelerator_pedal_position : float = 0 # in range [0,1] - brake_pedal_position : float = 0 # in range [0,1] - steering_wheel_angle : float = 0 # in radians - left_turn_signal : bool = False - right_turn_signal : bool = False - headlights_on : bool = False - horn_on : bool = False - wiper_level : int = 0 - battery_level : Optional[float] = None # in range [0,1] - fuel_level : Optional[float] = None # in liters - driving_range : Optional[float] = None # remaining range left, in km - - def from_state(self, state: VehicleState) -> None: - """Sets the readings that would be approximately sensed at the given - VehicleState. - - Does not change the battery_level, fuel_level, or driving_range values. - """ - self.speed = state.v - self.steering_wheel_angle = state.steering_wheel_angle - pitch = state.pose.pitch if state.pose.pitch is not None else 0.0 - - #acc_pos,brake_pos,gear = acceleration_to_pedal_positions(state.acceleration, state.v, pitch, state.gear) - self.accelerator_pedal_position = state.accelerator_pedal_position - self.brake_pedal_position = state.brake_pedal_position - self.gear = state.gear - self.left_turn_signal = state.left_turn_indicator - self.right_turn_signal = state.right_turn_indicator - self.horn_on = state.horn_on - self.wiper_level = state.wiper_level - self.headlights_on = state.headlights_on - - def to_state(self, pose : ObjectPose = None) -> VehicleState: - """Returns a VehicleState representing the vehicle's current state given - these readings. - - Note: the acceleration attribute is totally bogus, and should be ignored - until the dynamics are calibrated better. - """ - if pose is None: - pose = ObjectPose(frame = ObjectFrameEnum.CURRENT,t=0,x=0,y=0,yaw=0) - pitch = pose.pitch if pose.pitch is not None else 0.0 - wheel_base = settings.get('vehicle.geometry.wheelbase') - front_wheel_angle=steer2front(self.steering_wheel_angle) - turn_rate=heading_rate(front_wheel_angle,self.speed,wheel_base) - acc = pedal_positions_to_acceleration(self.accelerator_pedal_position, self.brake_pedal_position, self.speed, pitch, self.gear) - return VehicleState(pose,v=self.speed,accelerator_pedal_position=self.accelerator_pedal_position,brake_pedal_position=self.brake_pedal_position, - acceleration=acc,gear=self.gear,steering_wheel_angle=self.steering_wheel_angle, - front_wheel_angle=front_wheel_angle,heading_rate=turn_rate, - left_turn_indicator=self.left_turn_signal,right_turn_indicator=self.right_turn_signal, - horn_on=self.horn_on,wiper_level=self.wiper_level,headlights_on=self.headlights_on) - - -@dataclass -@serialization.register -class GEMVehicleCommand: - """All items that can be directly commanded to the vehicle's actuators.""" - gear : int #follows convention in state.vehicle.VehicleState. -2: park, -1 reverse: 0: neutral, 1..n: forward - accelerator_pedal_position : float - accelerator_pedal_speed : float - brake_pedal_position : float - brake_pedal_speed : float - steering_wheel_angle : float - steering_wheel_speed : float - left_turn_signal : bool = False - right_turn_signal : bool = False - headlights_on : bool = False - horn_on : bool = False - wiper_level : int = 0 - - -@dataclass -class GNSSReading: - pose : ObjectPose - speed : float - status : str - - -class GEMInterface: - """Base class for simulated / physical GEM vehicle. - """ - def __init__(self): - self.last_command = None # type: GEMVehicleCommand - self.last_reading = None # type: GEMVehicleReading - - def start(self): - pass - - def stop(self): - pass - - def time(self) -> float: - """Returns the current time""" - raise NotImplementedError() - - def get_reading(self) -> GEMVehicleReading: - """Returns current read state of the vehicle""" - raise NotImplementedError() - - def send_command(self, cmd : GEMVehicleCommand): - """Sends a command to the vehicle""" - raise NotImplementedError() - - def sensors(self) -> List[str]: - """Returns all available sensors""" - return ['gnss','imu','top_lidar','front_camera','front_depth','front_radar'] - - def subscribe_sensor(self, name : str, callback : Callable, type = None) -> None: - """Subscribes to a sensor with a given callback. - - If type is not None, it should be the expected type of the message produced - by the sensor callback. - """ - raise NotImplementedError() - - def hardware_faults(self) -> List[str]: - """Returns a list of hardware faults, naming the failed component. - - Can be any sensor, actuator, or other component. - """ - raise NotImplementedError() - - def simple_command(self, acceleration_mps2 : float, steering_wheel_angle : float, state : VehicleState = None) -> GEMVehicleCommand: - """" - Returns a command according to a desired acceleration and steering angle - - Args: - acceleration_mps2: acceleration in m/s^2 - steering_wheel_angle: steering angle in radians - state: current vehicle state - """ - pitch = state.pose.pitch if state is not None and state.pose.pitch is not None else 0.0 - v = state.v if state is not None else 0.0 - gear = state.gear if state is not None else 1 - acc_pos,brake_pos,gear = acceleration_to_pedal_positions(acceleration_mps2, v, pitch, gear) - - cmd = GEMVehicleCommand(gear=gear, - accelerator_pedal_position=acc_pos, - brake_pedal_position=brake_pos, - steering_wheel_angle=steering_wheel_angle, - accelerator_pedal_speed=settings.get('vehicle.control_defaults.accelerator_pedal_speed'), - brake_pedal_speed = settings.get('vehicle.control_defaults.brake_pedal_speed'), - steering_wheel_speed = settings.get('vehicle.control_defaults.steering_wheel_speed')) - if state is not None: - #preserve indicators - cmd.left_turn_signal = state.left_turn_indicator - cmd.right_turn_signal = state.right_turn_indicator - cmd.headlights_on = state.headlights_on - cmd.horn_on = state.horn_on - cmd.wiper_level = state.wiper_level - - return cmd - - def command_from_reading(self, reading: GEMVehicleReading = None) -> GEMVehicleCommand: - """Returns a command that maintains all the current elements in the - provided vehicle reading. If reading=None, then the last reading - is used. - """ - if reading is None: - reading = self.last_reading - if reading is None: - raise RuntimeError("Can't get command from reading, no reading available") - return GEMVehicleCommand(gear=reading.gear, - accelerator_pedal_position=reading.accelerator_pedal_position, - brake_pedal_position=reading.brake_pedal_position, - steering_wheel_angle=reading.steering_wheel_angle, - accelerator_pedal_speed=settings.get('vehicle.control_defaults.accelerator_pedal_speed'), - brake_pedal_speed = settings.get('vehicle.control_defaults.brake_pedal_speed'), - steering_wheel_speed = settings.get('vehicle.control_defaults.steering_wheel_speed'), - left_turn_signal = reading.left_turn_signal, - right_turn_signal = reading.right_turn_signal, - headlights_on = reading.headlights_on, - horn_on = reading.horn_on, +from dataclasses import dataclass +from ...utils import settings,serialization +from ...state import VehicleState, ObjectPose, ObjectFrameEnum +from ...knowledge.vehicle.geometry import front2steer,steer2front,heading_rate +from ...knowledge.vehicle.dynamics import pedal_positions_to_acceleration, acceleration_to_pedal_positions +from typing import List,Optional,Callable + +@dataclass +@serialization.register +class GEMVehicleReading: + """All items that the vehicle reports directly from its internal sensors.""" + speed : float = 0 # in m/s + gear : int = 0 # 0 neutral, -1 reverse, -2 park, > 0 forward + accelerator_pedal_position : float = 0 # in range [0,1] + brake_pedal_position : float = 0 # in range [0,1] + steering_wheel_angle : float = 0 # in radians + left_turn_signal : bool = False + right_turn_signal : bool = False + headlights_on : bool = False + horn_on : bool = False + wiper_level : int = 0 + battery_level : Optional[float] = None # in range [0,1] + fuel_level : Optional[float] = None # in liters + driving_range : Optional[float] = None # remaining range left, in km + + def from_state(self, state: VehicleState) -> None: + """Sets the readings that would be approximately sensed at the given + VehicleState. + + Does not change the battery_level, fuel_level, or driving_range values. + """ + self.speed = state.v + self.steering_wheel_angle = state.steering_wheel_angle + pitch = state.pose.pitch if state.pose.pitch is not None else 0.0 + + #acc_pos,brake_pos,gear = acceleration_to_pedal_positions(state.acceleration, state.v, pitch, state.gear) + self.accelerator_pedal_position = state.accelerator_pedal_position + self.brake_pedal_position = state.brake_pedal_position + self.gear = state.gear + self.left_turn_signal = state.left_turn_indicator + self.right_turn_signal = state.right_turn_indicator + self.horn_on = state.horn_on + self.wiper_level = state.wiper_level + self.headlights_on = state.headlights_on + + def to_state(self, pose : ObjectPose = None) -> VehicleState: + """Returns a VehicleState representing the vehicle's current state given + these readings. + + Note: the acceleration attribute is totally bogus, and should be ignored + until the dynamics are calibrated better. + """ + if pose is None: + pose = ObjectPose(frame = ObjectFrameEnum.CURRENT,t=0,x=0,y=0,yaw=0) + pitch = pose.pitch if pose.pitch is not None else 0.0 + wheel_base = settings.get('vehicle.geometry.wheelbase') + front_wheel_angle=steer2front(self.steering_wheel_angle) + turn_rate=heading_rate(front_wheel_angle,self.speed,wheel_base) + acc = pedal_positions_to_acceleration(self.accelerator_pedal_position, self.brake_pedal_position, self.speed, pitch, self.gear) + return VehicleState(pose,v=self.speed,accelerator_pedal_position=self.accelerator_pedal_position,brake_pedal_position=self.brake_pedal_position, + acceleration=acc,gear=self.gear,steering_wheel_angle=self.steering_wheel_angle, + front_wheel_angle=front_wheel_angle,heading_rate=turn_rate, + left_turn_indicator=self.left_turn_signal,right_turn_indicator=self.right_turn_signal, + horn_on=self.horn_on,wiper_level=self.wiper_level,headlights_on=self.headlights_on) + + +@dataclass +@serialization.register +class GEMVehicleCommand: + """All items that can be directly commanded to the vehicle's actuators.""" + gear : int #follows convention in state.vehicle.VehicleState. -2: park, -1 reverse: 0: neutral, 1..n: forward + accelerator_pedal_position : float + accelerator_pedal_speed : float + brake_pedal_position : float + brake_pedal_speed : float + steering_wheel_angle : float + steering_wheel_speed : float + left_turn_signal : bool = False + right_turn_signal : bool = False + headlights_on : bool = False + horn_on : bool = False + wiper_level : int = 0 + + +@dataclass +class GNSSReading: + pose : ObjectPose + speed : float + status : str + + +class GEMInterface: + """Base class for simulated / physical GEM vehicle. + """ + def __init__(self): + self.last_command = None # type: GEMVehicleCommand + self.last_reading = None # type: GEMVehicleReading + + def start(self): + pass + + def stop(self): + pass + + def time(self) -> float: + """Returns the current time""" + raise NotImplementedError() + + def get_reading(self) -> GEMVehicleReading: + """Returns current read state of the vehicle""" + raise NotImplementedError() + + def send_command(self, cmd : GEMVehicleCommand): + """Sends a command to the vehicle""" + raise NotImplementedError() + + def sensors(self) -> List[str]: + """Returns all available sensors""" + return ['gnss','imu','top_lidar','front_camera','front_depth','front_radar'] + + def subscribe_sensor(self, name : str, callback : Callable, type = None) -> None: + """Subscribes to a sensor with a given callback. + + If type is not None, it should be the expected type of the message produced + by the sensor callback. + """ + raise NotImplementedError() + + def hardware_faults(self) -> List[str]: + """Returns a list of hardware faults, naming the failed component. + + Can be any sensor, actuator, or other component. + """ + raise NotImplementedError() + + def simple_command(self, acceleration_mps2 : float, steering_wheel_angle : float, state : VehicleState = None) -> GEMVehicleCommand: + """" + Returns a command according to a desired acceleration and steering angle + + Args: + acceleration_mps2: acceleration in m/s^2 + steering_wheel_angle: steering angle in radians + state: current vehicle state + """ + pitch = state.pose.pitch if state is not None and state.pose.pitch is not None else 0.0 + v = state.v if state is not None else 0.0 + gear = state.gear if state is not None else 1 + acc_pos,brake_pos,gear = acceleration_to_pedal_positions(acceleration_mps2, v, pitch, gear) + + cmd = GEMVehicleCommand(gear=gear, + accelerator_pedal_position=acc_pos, + brake_pedal_position=brake_pos, + steering_wheel_angle=steering_wheel_angle, + accelerator_pedal_speed=settings.get('vehicle.control_defaults.accelerator_pedal_speed'), + brake_pedal_speed = settings.get('vehicle.control_defaults.brake_pedal_speed'), + steering_wheel_speed = settings.get('vehicle.control_defaults.steering_wheel_speed')) + if state is not None: + #preserve indicators + cmd.left_turn_signal = state.left_turn_indicator + cmd.right_turn_signal = state.right_turn_indicator + cmd.headlights_on = state.headlights_on + cmd.horn_on = state.horn_on + cmd.wiper_level = state.wiper_level + + return cmd + + def command_from_reading(self, reading: GEMVehicleReading = None) -> GEMVehicleCommand: + """Returns a command that maintains all the current elements in the + provided vehicle reading. If reading=None, then the last reading + is used. + """ + if reading is None: + reading = self.last_reading + if reading is None: + raise RuntimeError("Can't get command from reading, no reading available") + return GEMVehicleCommand(gear=reading.gear, + accelerator_pedal_position=reading.accelerator_pedal_position, + brake_pedal_position=reading.brake_pedal_position, + steering_wheel_angle=reading.steering_wheel_angle, + accelerator_pedal_speed=settings.get('vehicle.control_defaults.accelerator_pedal_speed'), + brake_pedal_speed = settings.get('vehicle.control_defaults.brake_pedal_speed'), + steering_wheel_speed = settings.get('vehicle.control_defaults.steering_wheel_speed'), + left_turn_signal = reading.left_turn_signal, + right_turn_signal = reading.right_turn_signal, + headlights_on = reading.headlights_on, + horn_on = reading.horn_on, wiper_level = reading.wiper_level) \ No newline at end of file diff --git a/GEMstack/onboard/planning/creep_planning.py b/GEMstack/onboard/planning/creep_planning.py deleted file mode 100644 index 57e4805ca..000000000 --- a/GEMstack/onboard/planning/creep_planning.py +++ /dev/null @@ -1,229 +0,0 @@ -# File: GEMstack/onboard/planning/creep_planning.py -from typing import List, Tuple -import math -from ..component import Component -from ...state import AllState, VehicleState, EntityRelation, EntityRelationEnum, Path, Trajectory, Route, \ - ObjectFrameEnum -from ...utils import serialization -from ...mathutils import transforms -import numpy as np -from .longitudinal_planning import longitudinal_plan, longitudinal_brake - - -DEBUG = False # Set to False to disable debug output - - -''' -# OTHER CREEP CONTROL CODE - -def inverse_speed_function(distance_to_object): - -# max_creep_speed = 1.0 # Maximum speed when creeping (m/s) -# min_creep_speed = 0.2 # Minimum speed to maintain (m/s) -# safe_distance = 5.0 # Distance at which to start slowing down (m) - -# # Almost stop when very close (≤0.25m) -# if distance_to_object <= 0.25: -# return 0.0 # Practically zero speed - -# # Slow creep when close (≤1.5m) -# if distance_to_object <= 3.0: -# return min_creep_speed -# speed = min_creep_speed + (max_creep_speed - min_creep_speed) * (distance_to_object / safe_distance) -# return min(speed, max_creep_speed) - -def pid_speed_control(distance_to_object, target_distance, current_speed, prev_error, integral, dt, - kp=0.5, ki=0.1, kd=0.05, min_speed=0.2, max_speed=1.0): - if distance_to_object <= 0.25: - return 0.0, 0.0, 0.0 - error = distance_to_object - target_distance - integral += error * dt - integral = max(-5.0, min(integral, 5.0)) # Prevent integral windup - derivative = (error - prev_error) / dt if dt > 0 else 0 - p_term = kp * error - i_term = ki * integral - d_term = kd * derivative - speed_adjustment = p_term + i_term + d_term - base_speed = 0.5 # Base creep speed - target_speed = base_speed + speed_adjustment - target_speed = max(min_speed, min(target_speed, max_speed)) - return target_speed, error, integral - - -''' - - - -class CreepTrajectoryPlanner(Component): - """Follows the given route. Brakes if the ego–vehicle must yield - (e.g. to a pedestrian) or if the end of the route is near; otherwise, - it accelerates (or cruises) toward a desired speed. - """ - - def __init__(self): - self.route_progress = None - self.t_last = None - self.acceleration = 5 - self.desired_speed = 2.0 - self.deceleration = 2.0 - self.emergency_brake = 8.0 - # Parameters for end-of-route linear deceleration - self.end_stop_distance = 15 # Distance in meters to start linear deceleration - # Did 15, 10, 7.5, 17.5, 20, 12.5 - def state_inputs(self): - return ['all'] - - def state_outputs(self) -> List[str]: - return ['trajectory'] - - def rate(self): - return 10.0 - - def check_end_of_route(self, route, current_parameter): - """Check if vehicle is approaching the end of the route and calculate - appropriate linear deceleration parameters. - - Args: - route: The complete route - current_parameter: Current position along the route - - Returns: - (bool, float): Tuple containing: - - Whether the vehicle should start decelerating - - Adjusted speed if deceleration needed, otherwise desired_speed - """ - route_length = route.length() - - distance_remaining = route_length - current_parameter - - if DEBUG: - print(f"[DEBUG] check_end_of_route: Route length = {route_length}, " - f"Current position = {current_parameter}, " - f"Distance remaining = {distance_remaining}") - - if distance_remaining <= self.end_stop_distance: - if distance_remaining > 0.1: # Avoid division by very small numbers - required_decel = (self.desired_speed ** 2) / (2 * distance_remaining) - - linear_factor = distance_remaining / self.end_stop_distance - adjusted_speed = self.desired_speed * linear_factor - if DEBUG: - print(f"[DEBUG] End deceleration active: {distance_remaining:.2f}m remaining, " - f"required deceleration = {required_decel:.2f} m/s², " - f"speed adjusted to {adjusted_speed:.2f} m/s") - - return True, adjusted_speed - else: - return True, 0.0 - - return False, self.desired_speed - - def update(self, state: AllState): - vehicle = state.vehicle # type: VehicleState - route = state.route # type: Route - t = state.t - - if DEBUG: - print("[DEBUG] CreepTrajectoryPlanner.update: t =", t) - - if self.t_last is None: - self.t_last = t - dt = t - self.t_last - if DEBUG: - print("[DEBUG] CreepTrajectoryPlanner.update: dt =", dt) - - curr_x = vehicle.pose.x - curr_y = vehicle.pose.y - curr_v = vehicle.v - if DEBUG: - print(f"[DEBUG] CreepTrajectoryPlanner.update: Vehicle position = ({curr_x}, {curr_y}), speed = {curr_v}, ") - - # Determine progress along the route. - if self.route_progress is None: - self.route_progress = 0.0 - _, closest_parameter = route.closest_point_local( - [curr_x, curr_y], - (self.route_progress - 5.0, self.route_progress + 5.0) - ) - if DEBUG: - print("[DEBUG] CreepTrajectoryPlanner.update: Closest parameter on route =", closest_parameter) - self.route_progress = closest_parameter - - # Check if approaching end of route and get adjusted speed - approaching_end, target_speed = self.check_end_of_route(route, closest_parameter) - if DEBUG and approaching_end: - print("[DEBUG] CreepTrajectoryPlanner.update: Vehicle is approaching end of route") - print(f"[DEBUG] CreepTrajectoryPlanner.update: Target speed = {target_speed}") - - # Extract a 10 m segment of the route for planning lookahead. - route_with_lookahead = route.trim(closest_parameter, closest_parameter + 10.0) - if DEBUG: - print("[DEBUG] CreepTrajectoryPlanner.update: Route Lookahead =", route_with_lookahead) - - print("[DEBUG] state", state.relations) - # Check whether any yield relations (e.g. due to pedestrians) require braking. - stay_braking = False - pointSet = set() - for i in range(len(route_with_lookahead.points)): - if tuple(route_with_lookahead.points[i]) in pointSet: - stay_braking = True - break - pointSet.add(tuple(route_with_lookahead.points[i])) - - should_brake = any( - r.type == EntityRelationEnum.STOPPING_AT and r.obj1 == '' - for r in state.relations - ) - should_decelerate = any( - r.type == EntityRelationEnum.YIELDING and r.obj1 == '' - for r in state.relations - ) if should_brake == False else False - - # If approaching end of route, override the target speed - if approaching_end: - should_accelerate = (not should_brake and not should_decelerate and curr_v < target_speed) - else: - should_accelerate = (not should_brake and not should_decelerate and curr_v < self.desired_speed) - - if DEBUG: - print("[DEBUG] CreepTrajectoryPlanner.update: stay_braking =", stay_braking) - print("[DEBUG] CreepTrajectoryPlanner.update: should_brake =", should_brake) - print("[DEBUG] CreepTrajectoryPlanner.update: should_accelerate =", should_accelerate) - print("[DEBUG] CreepTrajectoryPlanner.update: should_decelerate =", should_decelerate) - print("[DEBUG] CreepTrajectoryPlanner.update: target_speed =", target_speed if approaching_end else self.desired_speed) - - if stay_braking: - traj = longitudinal_brake(route_with_lookahead, 0.0, 0.0, 0.0) - if DEBUG: - print("[DEBUG] CreepTrajectoryPlanner.update: Using longitudinal_brake (stay braking).") - elif should_brake: - traj = longitudinal_brake(route_with_lookahead, self.emergency_brake, curr_v) - if DEBUG: - print("[DEBUG] CreepTrajectoryPlanner.update: Using longitudinal_brake.") - elif should_decelerate: - traj = longitudinal_brake(route_with_lookahead, self.deceleration, curr_v) - if DEBUG: - print("[DEBUG] CreepTrajectoryPlanner.update: Using longitudinal_brake.") - elif approaching_end: - # Use linear deceleration to stop at end of route - traj = longitudinal_plan(route_with_lookahead, self.acceleration, - self.deceleration, target_speed, curr_v) - if DEBUG: - print("[DEBUG] CreepTrajectoryPlanner.update: Using longitudinal_plan with end-of-route deceleration.") - elif should_accelerate: - traj = longitudinal_plan(route_with_lookahead, self.acceleration, - self.deceleration, self.desired_speed, curr_v) - if DEBUG: - print("[DEBUG] CreepTrajectoryPlanner.update: Using longitudinal_plan (accelerate).") - else: - # Maintain current speed if not accelerating or braking. - traj = longitudinal_plan(route_with_lookahead, 0.0, self.deceleration, self.desired_speed, curr_v) - if DEBUG: - print( - "[DEBUG] CreepTrajectoryPlanner.update: Maintaining current speed with longitudinal_plan (0 accel).") - - self.t_last = t - if DEBUG: - print(f'[DEBUG] Current Velocity: {curr_v}, Target Speed: {target_speed if approaching_end else self.desired_speed}') - print("[DEBUG] CreepTrajectoryPlanner.update: Returning trajectory with", len(traj.points), "points.") - return traj diff --git a/GEMstack/utils/klampt_visualization.py b/GEMstack/utils/klampt_visualization.py index 188c8d4f9..063f690c7 100644 --- a/GEMstack/utils/klampt_visualization.py +++ b/GEMstack/utils/klampt_visualization.py @@ -1,299 +1,299 @@ -from klampt import vis -from klampt.math import vectorops,so3,se3 -from klampt.model.trajectory import Trajectory -from klampt import Geometry3D, GeometricPrimitive, TriangleMesh -import numpy as np -from . import settings -from ..state import ObjectFrameEnum,ObjectPose,PhysicalObject,VehicleState,VehicleGearEnum,Path,Obstacle,AgentState,AgentEnum,Roadgraph,RoadgraphLane,RoadgraphLaneEnum,RoadgraphCurve,RoadgraphCurveEnum,RoadgraphRegion,RoadgraphRegionEnum,RoadgraphSurfaceEnum,Trajectory,Route,SceneState,AllState - -#KH: there is a bug on some system where the visualization crashes with an OpenGL error when drawing curves -#this is a workaround. We really should find the source of the bug! -MAX_POINTS_IN_CURVE = 50 - -OBJECT_COLORS = { - AgentEnum.CAR : (1,1,0,1), - AgentEnum.PEDESTRIAN : (0,1,0,1), - AgentEnum.BICYCLIST : (0,0,1,1), - AgentEnum.MEDIUM_TRUCK : (1,0,1,1), - AgentEnum.LARGE_TRUCK : (0,1,1,1), - None: (0.7,0.7,0.7,1), -} - -AUX_BBOX_COLOR = (0,0,1,1) - -CURVE_TO_STYLE = { - RoadgraphCurveEnum.LANE_BOUNDARY : {'color':(1,1,1,1),'width':2,'pointSize':0}, - RoadgraphCurveEnum.CURB : {'color':(0.5,0.5,0.5,1),'width':2,'pointSize':0}, - RoadgraphCurveEnum.CLIFF : {'color':(1,0,0,1),'width':2,'pointSize':0}, - RoadgraphCurveEnum.CROSSING_BOUNDARY : {'color':(1,1,1,1),'width':1,'pointSize':0}, - RoadgraphCurveEnum.PARKING_SPOT_BOUNDARY : {'color':(1,1,1,1),'width':1,'pointSize':0}, - RoadgraphCurveEnum.STOP_LINE : {'color':(1,1,1,1),'width':2,'pointSize':0}, - RoadgraphCurveEnum.WALL : {'color':(0,0,1,1),'width':2,'pointSize':0}, - None : {'color':(1,1,1,1),'width':1,'pointSize':0}, -} - -SURFACE_TO_STYLE = { - RoadgraphSurfaceEnum.PAVEMENT : {'color':(0.5,0.5,0.5,0.2),'pointSize':0}, - RoadgraphSurfaceEnum.DIRT : {'color':(160/255.0,82/255.0,45/255.0,0.2),'pointSize':0}, - RoadgraphSurfaceEnum.GRASS : {'color':(50/255.0,255/255.0,50/255.0,0.2),'pointSize':0}, - RoadgraphSurfaceEnum.GRAVEL : {'color':(0.7,0.7,0.7,0.2),'pointSize':0}, - None: {'color':(1,0,0,0.2),'pointSize':0}, -} - -REGION_TO_STYLE = { - RoadgraphRegionEnum.INTERSECTION : {'color':(0,1,0,0.5),'width':1,'pointSize':0}, - RoadgraphRegionEnum.PARKING_LOT : {'color':(0,0,1,0.5),'width':1,'pointSize':0}, - RoadgraphRegionEnum.CLOSED_COURSE : {'color':(1,0,0,0.5),'width':1,'pointSize':0}, - RoadgraphRegionEnum.VIRTUAL : {'color':(0,0,0,0.5),'width':1,'pointSize':0}, -} - -def plot_pose(name : str, pose : ObjectPose, axis_len=0.1, label=True): - """Plots the pose in the given axes. The coordinates - of the pose are plotted in the pose's indicated frame.""" - R = pose.rotation() - t = pose.translation() - T = (so3.from_matrix(R),t) - vis.add(name, T, length=axis_len, hide_label=(not label)) - if pose.frame == ObjectFrameEnum.START: - vis.add(name+"_origin",t,color=(0,0,1,1),size=5,hide_label=True) - elif pose.frame == ObjectFrameEnum.CURRENT: - vis.add(name+"_origin",t,color=(1,0,0,1),size=5,hide_label=True) - elif pose.frame in [ObjectFrameEnum.GLOBAL,ObjectFrameEnum.ABSOLUTE_CARTESIAN]: - vis.add(name+"_origin",t,color=(0,1,0,1),size=5,hide_label=True) - else: - raise ValueError("Unknown frame %s" % pose.frame) - -def plot_object(name : str, obj : PhysicalObject, type=None, axis_len=None, outline=True, solid=True, bbox=True, label=True): - """Shows an object in the given axes. - - If axis_len is given, shows the object's pose with - a coordinate frame of the given length. - - If outline is True, shows the object's outline. - - If bbox is True, shows the object's bounding box. - """ - height = obj.dimensions[2] - core_color = OBJECT_COLORS[type] - bbox_color = AUX_BBOX_COLOR - if label: - #add a point at the object's origin - vis.add(name,obj.pose.translation(),size=5,color=(0,0,0,1)) - if axis_len: - plot_pose(name+"_pose", obj.pose, axis_len=0) - #plot bounding box - R = obj.pose.rotation() - t = obj.pose.translation() - if bbox or (outline and obj.outline is None): - bounds = obj.bounds() - (xmin,xmax),(ymin,ymax),(zmin,zmax) = bounds - if not solid: - corners = [[xmin,ymin,0.01],[xmin,ymax,0.01],[xmax,ymax,0.01],[xmax,ymin,0.01]] - corners = [list(R.dot(c)+t) for c in corners] - corners.append(corners[0]) - if not bbox: - vis.add(name+"_bbox1",corners,width=1,color=core_color,pointSize=0,hide_label=True) - else: - vis.add(name+"_bbox1",corners,width=1,color=AUX_BBOX_COLOR,pointSize=0,hide_label=True) - if height > 0: - corners = [[xmin,ymin,height],[xmin,ymax,height],[xmax,ymax,height],[xmax,ymin,height]] - corners = [list(R.dot(c)+t) for c in corners] - corners.append(corners[0]) - if not bbox: - vis.add(name+"_bbox2",corners,width=1,color=core_color,pointSize=0,hide_label=True) - else: - vis.add(name+"_bbox2",corners,width=1,color=AUX_BBOX_COLOR,pointSize=0,hide_label=True) - else: - prim = GeometricPrimitive() - prim.setAABB([xmin,ymin,0],[xmax,ymax,height]) - g = Geometry3D(prim) - g.setCurrentTransform(so3.from_matrix(R),t) - vis.add(name+"_bbox",g,color=core_color[:3]+(0.5,),hide_label=True) - - #plot outline - if outline and obj.outline: - outline = [R.dot((p[0],p[1],0))+t for p in obj.outline] - outline.append(outline[0]) - vis.add(name+"_outline1",outline,width=1,color=core_color,hide_label=True) - if height > 0: - outline = [R.dot((p[0],p[1],height))+t for p in obj.outline] - outline.append(outline[0]) - vis.add(name+"_outline2",outline,width=1,color=core_color,hide_label=True) - - -def plot_vehicle(vehicle : VehicleState, vehicle_model=None, axis_len=1.0): - """Plots the vehicle in the given axes. The coordinates - of the vehicle are plotted in the vehicle's indicated frame.""" - plot_object("vehicle",vehicle.to_object(), type=None, axis_len=0, solid=False, label=False) - R = vehicle.pose.rotation() - t = vehicle.pose.translation() - T = (so3.from_matrix(R),t) - vis.add("vehicle", T, length=axis_len, hide_label=True) - - xbounds,ybounds,zbounds = settings.get('vehicle.geometry.bounds') - #plot velocity arrow - R = vehicle.pose.rotation2d() - t = np.array([vehicle.pose.x,vehicle.pose.y]) - v = R.dot([vehicle.v,0]) - front_pt = vehicle.pose.apply((xbounds[1],0.0)) - h = (vehicle.pose.z if vehicle.pose.z is not None else 0) + zbounds[1] - vis.add("vehicle_velocity",[[front_pt[0],front_pt[1],h],[front_pt[0]+v[0],front_pt[1]+v[1],h]],width=2,color=(0,1,0,1),pointSize=0) - - #plot front wheel angles - wheelbase = settings.get('vehicle.geometry.wheelbase') - wheel_spacing = 0.8*settings.get('vehicle.geometry.width') / 2 - phi = vehicle.front_wheel_angle - if vehicle_model: - q = vehicle_model.getConfig() - lwhindex = vehicle_model.link("left_steering_hinge_link").index - rwhindex = vehicle_model.link("right_steering_hinge_link").index - lwindex = vehicle_model.link("front_left_wheel_link").index - rwindex = vehicle_model.link("front_right_wheel_link").index - rlwindex = vehicle_model.link("rear_left_wheel_link").index - rrwindex = vehicle_model.link("rear_right_wheel_link").index - q[lwhindex] = phi - q[rwhindex] = phi - q[lwindex] += vehicle.v * 0.2 - q[rwindex] += vehicle.v * 0.2 - q[rlwindex] += vehicle.v * 0.2 - q[rrwindex] += vehicle.v * 0.2 - vehicle_model.setConfig(q) - else: - left_wheel_origin = t + R.dot([wheelbase,wheel_spacing]) - right_wheel_origin = t + R.dot([wheelbase,-wheel_spacing]) - wheel_width = 0.5 #meters - wheel_offset = R.dot(np.array([np.cos(phi),np.sin(phi)]))*wheel_width*0.5 - vis.add("left_wheel",[vectorops.sub(left_wheel_origin,wheel_offset),vectorops.add(left_wheel_origin,wheel_offset)],color=(0,0,0,1),width=2) - vis.add("right_wheel",[vectorops.sub(right_wheel_origin,wheel_offset),vectorops.add(right_wheel_origin,wheel_offset)],color=(0,0,0,1),width=2) - - #plot gear - if vehicle.gear in [VehicleGearEnum.NEUTRAL,VehicleGearEnum.REVERSE,VehicleGearEnum.PARK]: - if vehicle.gear == VehicleGearEnum.NEUTRAL: - gear = 'N' - elif vehicle.gear == VehicleGearEnum.REVERSE: - gear = 'R' - else: - gear = 'P' - vis.addText("gear",gear,position=(t[0],t[1],1.5),color=(0,0,0,1)) - - #plot lights - light_point_size = 4 - light_size = 0.15 - light_color = (1,1,0,1) - turn_indicator_height = 0.7 - headlight_height = 0.6 - if vehicle.left_turn_indicator: - lp = vehicle.pose.apply([xbounds[0],ybounds[0]+light_size,turn_indicator_height]) - vis.add("left_turn_indicator",list(lp),size=light_point_size,color=light_color) - if vehicle.right_turn_indicator: - lp = vehicle.pose.apply([xbounds[0],ybounds[1]-light_size,turn_indicator_height]) - vis.add("right_turn_indicator",list(lp),size=light_point_size,color=light_color) - if vehicle.headlights_on: - lp = vehicle.pose.apply([xbounds[1],ybounds[0]+light_size*2,headlight_height]) - vis.add("left_headlight",list(lp),size=light_point_size,color=light_color) - lp = vehicle.pose.apply([xbounds[1],ybounds[1]-light_size*2,headlight_height]) - vis.add("right_headlight",list(lp),size=light_point_size,color=light_color) - if vehicle_model is not None: - if vehicle.brake_pedal_position > 0.1: - vehicle_model.link('rear_right_stop_light_link').appearance().setColor(1,0,0,1) - vehicle_model.link('rear_left_stop_light_link').appearance().setColor(1,0,0,1) - else: - vehicle_model.link('rear_right_stop_light_link').appearance().setColor(0.3,0,0,1) - vehicle_model.link('rear_left_stop_light_link').appearance().setColor(0.3,0,0,1) - -def plot_path(name : str, path : Path, color=(0,0,0), width=1): - if len(path.points) > MAX_POINTS_IN_CURVE: # downsample due to OpenGL error? - vis.add(name,[list(p) for p in path.points[::len(path.points)//MAX_POINTS_IN_CURVE]],color=color,width=width) - else: - vis.add(name,[list(p) for p in path.points],color=color,width=width) - -def plot_curve(name : str, curve : RoadgraphCurve, color=None, width=None): - style = CURVE_TO_STYLE.get(curve.type,CURVE_TO_STYLE[None]) - if curve.crossable and curve.type == RoadgraphCurveEnum.LANE_BOUNDARY: - #style['linestyle'] = '--' - #TODO: how to indicate crossable lines? - pass - if color is not None: - style['color'] = color - if width is not None: - style['width'] = width - for i,seg in enumerate(curve.segments): - if len(seg) > MAX_POINTS_IN_CURVE: # downsample due to OpenGL error? - vis.add(name+"_%d" % i,seg[::len(seg)//MAX_POINTS_IN_CURVE],**style) - else: - vis.add(name+"_%d" % i,seg,**style) - -def plot_lane(name : str, lane : RoadgraphLane, on_route=False): - if lane.surface != RoadgraphSurfaceEnum.PAVEMENT: - style = SURFACE_TO_STYLE.get(lane.surface,SURFACE_TO_STYLE[None]) - outline = lane.outline() - vis.add(name, outline, **style) - if lane.left is not None: - plot_curve(name+"_left", lane.left) - if lane.right is not None: - plot_curve(name+"_right", lane.right) - -def plot_region(name : str, region : RoadgraphRegion, color=None, width=None): - style = REGION_TO_STYLE.get(region.type,REGION_TO_STYLE[None]) - points = region.outline() - pts = points + points[0] - if color is not None: - style['color'] = color - if width is not None: - style['width'] = width - vis.add(name, [list(p) for p in pts], **style) - -def plot_roadgraph(roadgraph : Roadgraph, route : Route = None): - #plot lanes - for k,l in roadgraph.lanes.items(): - if route is not None and k in route.lanes: - plot_lane(k,l,on_route=True) - else: - plot_lane(k,l) - for k,c in roadgraph.curves.items(): - plot_curve(k,c,color=(0,0,0,1)) - #plot intersections - for k,r in roadgraph.regions.items(): - plot_region(k,r) - #plot - for k,o in roadgraph.static_obstacles.items(): - plot_object(k,o) - -def plot_scene(scene : SceneState, ground_truth_vehicle=None, vehicle_model = None, title = None, show=True): - for i in list(vis.scene().items.keys()): - if not i.startswith("vehicle"): - if not isinstance(vis.scene().items[i],vis.VisPlot): - vis.remove(i) - #set plot range - #TODO - if vehicle_model is not None: - vis.add("vehicle_model",vehicle_model) - if ground_truth_vehicle is not None: - xform = ground_truth_vehicle.to_object().pose.transform() - else: - xform = scene.vehicle.to_object().pose.transform() - vehicle_model.link(0).setParentTransform(*se3.from_ndarray(xform)) - vehicle_model.setConfig(vehicle_model.getConfig()) - - #plot roadgraph - plot_roadgraph(scene.roadgraph,scene.route) - #plot vehicle and objects - plot_vehicle(scene.vehicle, vehicle_model) - for k,a in scene.agents.items(): - plot_object(k,a,type=a.type) - for k,o in scene.obstacles.items(): - plot_object(k,o) - if title is None: - if show: - vis.add("title","Scene at t = %.2f" % scene.t, position=(10,10)) - else: - vis.add("title",title) - if show: - vis.show() - -def plot(state : AllState, ground_truth_vehicle = None, vehicle_model=None, title=None, show=True): - plot_scene(state, ground_truth_vehicle=ground_truth_vehicle, vehicle_model=vehicle_model, title=title, show=show) - if state.route is not None: - plot_path("route",state.route,color=(1,0.5,0,1),width=2) - if state.trajectory is not None: - plot_path("trajectory",state.trajectory,color=(1,0,0,1),width=3) +from klampt import vis +from klampt.math import vectorops,so3,se3 +from klampt.model.trajectory import Trajectory +from klampt import Geometry3D, GeometricPrimitive, TriangleMesh +import numpy as np +from . import settings +from ..state import ObjectFrameEnum,ObjectPose,PhysicalObject,VehicleState,VehicleGearEnum,Path,Obstacle,AgentState,AgentEnum,Roadgraph,RoadgraphLane,RoadgraphLaneEnum,RoadgraphCurve,RoadgraphCurveEnum,RoadgraphRegion,RoadgraphRegionEnum,RoadgraphSurfaceEnum,Trajectory,Route,SceneState,AllState + +#KH: there is a bug on some system where the visualization crashes with an OpenGL error when drawing curves +#this is a workaround. We really should find the source of the bug! +MAX_POINTS_IN_CURVE = 50 + +OBJECT_COLORS = { + AgentEnum.CAR : (1,1,0,1), + AgentEnum.PEDESTRIAN : (0,1,0,1), + AgentEnum.BICYCLIST : (0,0,1,1), + AgentEnum.MEDIUM_TRUCK : (1,0,1,1), + AgentEnum.LARGE_TRUCK : (0,1,1,1), + None: (0.7,0.7,0.7,1), +} + +AUX_BBOX_COLOR = (0,0,1,1) + +CURVE_TO_STYLE = { + RoadgraphCurveEnum.LANE_BOUNDARY : {'color':(1,1,1,1),'width':2,'pointSize':0}, + RoadgraphCurveEnum.CURB : {'color':(0.5,0.5,0.5,1),'width':2,'pointSize':0}, + RoadgraphCurveEnum.CLIFF : {'color':(1,0,0,1),'width':2,'pointSize':0}, + RoadgraphCurveEnum.CROSSING_BOUNDARY : {'color':(1,1,1,1),'width':1,'pointSize':0}, + RoadgraphCurveEnum.PARKING_SPOT_BOUNDARY : {'color':(1,1,1,1),'width':1,'pointSize':0}, + RoadgraphCurveEnum.STOP_LINE : {'color':(1,1,1,1),'width':2,'pointSize':0}, + RoadgraphCurveEnum.WALL : {'color':(0,0,1,1),'width':2,'pointSize':0}, + None : {'color':(1,1,1,1),'width':1,'pointSize':0}, +} + +SURFACE_TO_STYLE = { + RoadgraphSurfaceEnum.PAVEMENT : {'color':(0.5,0.5,0.5,0.2),'pointSize':0}, + RoadgraphSurfaceEnum.DIRT : {'color':(160/255.0,82/255.0,45/255.0,0.2),'pointSize':0}, + RoadgraphSurfaceEnum.GRASS : {'color':(50/255.0,255/255.0,50/255.0,0.2),'pointSize':0}, + RoadgraphSurfaceEnum.GRAVEL : {'color':(0.7,0.7,0.7,0.2),'pointSize':0}, + None: {'color':(1,0,0,0.2),'pointSize':0}, +} + +REGION_TO_STYLE = { + RoadgraphRegionEnum.INTERSECTION : {'color':(0,1,0,0.5),'width':1,'pointSize':0}, + RoadgraphRegionEnum.PARKING_LOT : {'color':(0,0,1,0.5),'width':1,'pointSize':0}, + RoadgraphRegionEnum.CLOSED_COURSE : {'color':(1,0,0,0.5),'width':1,'pointSize':0}, + RoadgraphRegionEnum.VIRTUAL : {'color':(0,0,0,0.5),'width':1,'pointSize':0}, +} + +def plot_pose(name : str, pose : ObjectPose, axis_len=0.1, label=True): + """Plots the pose in the given axes. The coordinates + of the pose are plotted in the pose's indicated frame.""" + R = pose.rotation() + t = pose.translation() + T = (so3.from_matrix(R),t) + vis.add(name, T, length=axis_len, hide_label=(not label)) + if pose.frame == ObjectFrameEnum.START: + vis.add(name+"_origin",t,color=(0,0,1,1),size=5,hide_label=True) + elif pose.frame == ObjectFrameEnum.CURRENT: + vis.add(name+"_origin",t,color=(1,0,0,1),size=5,hide_label=True) + elif pose.frame in [ObjectFrameEnum.GLOBAL,ObjectFrameEnum.ABSOLUTE_CARTESIAN]: + vis.add(name+"_origin",t,color=(0,1,0,1),size=5,hide_label=True) + else: + raise ValueError("Unknown frame %s" % pose.frame) + +def plot_object(name : str, obj : PhysicalObject, type=None, axis_len=None, outline=True, solid=True, bbox=True, label=True): + """Shows an object in the given axes. + + If axis_len is given, shows the object's pose with + a coordinate frame of the given length. + + If outline is True, shows the object's outline. + + If bbox is True, shows the object's bounding box. + """ + height = obj.dimensions[2] + core_color = OBJECT_COLORS[type] + bbox_color = AUX_BBOX_COLOR + if label: + #add a point at the object's origin + vis.add(name,obj.pose.translation(),size=5,color=(0,0,0,1)) + if axis_len: + plot_pose(name+"_pose", obj.pose, axis_len=0) + #plot bounding box + R = obj.pose.rotation() + t = obj.pose.translation() + if bbox or (outline and obj.outline is None): + bounds = obj.bounds() + (xmin,xmax),(ymin,ymax),(zmin,zmax) = bounds + if not solid: + corners = [[xmin,ymin,0.01],[xmin,ymax,0.01],[xmax,ymax,0.01],[xmax,ymin,0.01]] + corners = [list(R.dot(c)+t) for c in corners] + corners.append(corners[0]) + if not bbox: + vis.add(name+"_bbox1",corners,width=1,color=core_color,pointSize=0,hide_label=True) + else: + vis.add(name+"_bbox1",corners,width=1,color=AUX_BBOX_COLOR,pointSize=0,hide_label=True) + if height > 0: + corners = [[xmin,ymin,height],[xmin,ymax,height],[xmax,ymax,height],[xmax,ymin,height]] + corners = [list(R.dot(c)+t) for c in corners] + corners.append(corners[0]) + if not bbox: + vis.add(name+"_bbox2",corners,width=1,color=core_color,pointSize=0,hide_label=True) + else: + vis.add(name+"_bbox2",corners,width=1,color=AUX_BBOX_COLOR,pointSize=0,hide_label=True) + else: + prim = GeometricPrimitive() + prim.setAABB([xmin,ymin,0],[xmax,ymax,height]) + g = Geometry3D(prim) + g.setCurrentTransform(so3.from_matrix(R),t) + vis.add(name+"_bbox",g,color=core_color[:3]+(0.5,),hide_label=True) + + #plot outline + if outline and obj.outline: + outline = [R.dot((p[0],p[1],0))+t for p in obj.outline] + outline.append(outline[0]) + vis.add(name+"_outline1",outline,width=1,color=core_color,hide_label=True) + if height > 0: + outline = [R.dot((p[0],p[1],height))+t for p in obj.outline] + outline.append(outline[0]) + vis.add(name+"_outline2",outline,width=1,color=core_color,hide_label=True) + + +def plot_vehicle(vehicle : VehicleState, vehicle_model=None, axis_len=1.0): + """Plots the vehicle in the given axes. The coordinates + of the vehicle are plotted in the vehicle's indicated frame.""" + plot_object("vehicle",vehicle.to_object(), type=None, axis_len=0, solid=False, label=False) + R = vehicle.pose.rotation() + t = vehicle.pose.translation() + T = (so3.from_matrix(R),t) + vis.add("vehicle", T, length=axis_len, hide_label=True) + + xbounds,ybounds,zbounds = settings.get('vehicle.geometry.bounds') + #plot velocity arrow + R = vehicle.pose.rotation2d() + t = np.array([vehicle.pose.x,vehicle.pose.y]) + v = R.dot([vehicle.v,0]) + front_pt = vehicle.pose.apply((xbounds[1],0.0)) + h = (vehicle.pose.z if vehicle.pose.z is not None else 0) + zbounds[1] + vis.add("vehicle_velocity",[[front_pt[0],front_pt[1],h],[front_pt[0]+v[0],front_pt[1]+v[1],h]],width=2,color=(0,1,0,1),pointSize=0) + + #plot front wheel angles + wheelbase = settings.get('vehicle.geometry.wheelbase') + wheel_spacing = 0.8*settings.get('vehicle.geometry.width') / 2 + phi = vehicle.front_wheel_angle + if vehicle_model: + q = vehicle_model.getConfig() + lwhindex = vehicle_model.link("left_steering_hinge_link").index + rwhindex = vehicle_model.link("right_steering_hinge_link").index + lwindex = vehicle_model.link("front_left_wheel_link").index + rwindex = vehicle_model.link("front_right_wheel_link").index + rlwindex = vehicle_model.link("rear_left_wheel_link").index + rrwindex = vehicle_model.link("rear_right_wheel_link").index + q[lwhindex] = phi + q[rwhindex] = phi + q[lwindex] += vehicle.v * 0.2 + q[rwindex] += vehicle.v * 0.2 + q[rlwindex] += vehicle.v * 0.2 + q[rrwindex] += vehicle.v * 0.2 + vehicle_model.setConfig(q) + else: + left_wheel_origin = t + R.dot([wheelbase,wheel_spacing]) + right_wheel_origin = t + R.dot([wheelbase,-wheel_spacing]) + wheel_width = 0.5 #meters + wheel_offset = R.dot(np.array([np.cos(phi),np.sin(phi)]))*wheel_width*0.5 + vis.add("left_wheel",[vectorops.sub(left_wheel_origin,wheel_offset),vectorops.add(left_wheel_origin,wheel_offset)],color=(0,0,0,1),width=2) + vis.add("right_wheel",[vectorops.sub(right_wheel_origin,wheel_offset),vectorops.add(right_wheel_origin,wheel_offset)],color=(0,0,0,1),width=2) + + #plot gear + if vehicle.gear in [VehicleGearEnum.NEUTRAL,VehicleGearEnum.REVERSE,VehicleGearEnum.PARK]: + if vehicle.gear == VehicleGearEnum.NEUTRAL: + gear = 'N' + elif vehicle.gear == VehicleGearEnum.REVERSE: + gear = 'R' + else: + gear = 'P' + vis.addText("gear",gear,position=(t[0],t[1],1.5),color=(0,0,0,1)) + + #plot lights + light_point_size = 4 + light_size = 0.15 + light_color = (1,1,0,1) + turn_indicator_height = 0.7 + headlight_height = 0.6 + if vehicle.left_turn_indicator: + lp = vehicle.pose.apply([xbounds[0],ybounds[0]+light_size,turn_indicator_height]) + vis.add("left_turn_indicator",list(lp),size=light_point_size,color=light_color) + if vehicle.right_turn_indicator: + lp = vehicle.pose.apply([xbounds[0],ybounds[1]-light_size,turn_indicator_height]) + vis.add("right_turn_indicator",list(lp),size=light_point_size,color=light_color) + if vehicle.headlights_on: + lp = vehicle.pose.apply([xbounds[1],ybounds[0]+light_size*2,headlight_height]) + vis.add("left_headlight",list(lp),size=light_point_size,color=light_color) + lp = vehicle.pose.apply([xbounds[1],ybounds[1]-light_size*2,headlight_height]) + vis.add("right_headlight",list(lp),size=light_point_size,color=light_color) + if vehicle_model is not None: + if vehicle.brake_pedal_position > 0.1: + vehicle_model.link('rear_right_stop_light_link').appearance().setColor(1,0,0,1) + vehicle_model.link('rear_left_stop_light_link').appearance().setColor(1,0,0,1) + else: + vehicle_model.link('rear_right_stop_light_link').appearance().setColor(0.3,0,0,1) + vehicle_model.link('rear_left_stop_light_link').appearance().setColor(0.3,0,0,1) + +def plot_path(name : str, path : Path, color=(0,0,0), width=1): + if len(path.points) > MAX_POINTS_IN_CURVE: # downsample due to OpenGL error? + vis.add(name,[list(p) for p in path.points[::len(path.points)//MAX_POINTS_IN_CURVE]],color=color,width=width) + else: + vis.add(name,[list(p) for p in path.points],color=color,width=width) + +def plot_curve(name : str, curve : RoadgraphCurve, color=None, width=None): + style = CURVE_TO_STYLE.get(curve.type,CURVE_TO_STYLE[None]) + if curve.crossable and curve.type == RoadgraphCurveEnum.LANE_BOUNDARY: + #style['linestyle'] = '--' + #TODO: how to indicate crossable lines? + pass + if color is not None: + style['color'] = color + if width is not None: + style['width'] = width + for i,seg in enumerate(curve.segments): + if len(seg) > MAX_POINTS_IN_CURVE: # downsample due to OpenGL error? + vis.add(name+"_%d" % i,seg[::len(seg)//MAX_POINTS_IN_CURVE],**style) + else: + vis.add(name+"_%d" % i,seg,**style) + +def plot_lane(name : str, lane : RoadgraphLane, on_route=False): + if lane.surface != RoadgraphSurfaceEnum.PAVEMENT: + style = SURFACE_TO_STYLE.get(lane.surface,SURFACE_TO_STYLE[None]) + outline = lane.outline() + vis.add(name, outline, **style) + if lane.left is not None: + plot_curve(name+"_left", lane.left) + if lane.right is not None: + plot_curve(name+"_right", lane.right) + +def plot_region(name : str, region : RoadgraphRegion, color=None, width=None): + style = REGION_TO_STYLE.get(region.type,REGION_TO_STYLE[None]) + points = region.outline() + pts = points + points[0] + if color is not None: + style['color'] = color + if width is not None: + style['width'] = width + vis.add(name, [list(p) for p in pts], **style) + +def plot_roadgraph(roadgraph : Roadgraph, route : Route = None): + #plot lanes + for k,l in roadgraph.lanes.items(): + if route is not None and k in route.lanes: + plot_lane(k,l,on_route=True) + else: + plot_lane(k,l) + for k,c in roadgraph.curves.items(): + plot_curve(k,c,color=(0,0,0,1)) + #plot intersections + for k,r in roadgraph.regions.items(): + plot_region(k,r) + #plot + for k,o in roadgraph.static_obstacles.items(): + plot_object(k,o) + +def plot_scene(scene : SceneState, ground_truth_vehicle=None, vehicle_model = None, title = None, show=True): + for i in list(vis.scene().items.keys()): + if not i.startswith("vehicle"): + if not isinstance(vis.scene().items[i],vis.VisPlot): + vis.remove(i) + #set plot range + #TODO + if vehicle_model is not None: + vis.add("vehicle_model",vehicle_model) + if ground_truth_vehicle is not None: + xform = ground_truth_vehicle.to_object().pose.transform() + else: + xform = scene.vehicle.to_object().pose.transform() + vehicle_model.link(0).setParentTransform(*se3.from_ndarray(xform)) + vehicle_model.setConfig(vehicle_model.getConfig()) + + #plot roadgraph + plot_roadgraph(scene.roadgraph,scene.route) + #plot vehicle and objects + plot_vehicle(scene.vehicle, vehicle_model) + for k,a in scene.agents.items(): + plot_object(k,a,type=a.type) + for k,o in scene.obstacles.items(): + plot_object(k,o) + if title is None: + if show: + vis.add("title","Scene at t = %.2f" % scene.t, position=(10,10)) + else: + vis.add("title",title) + if show: + vis.show() + +def plot(state : AllState, ground_truth_vehicle = None, vehicle_model=None, title=None, show=True): + plot_scene(state, ground_truth_vehicle=ground_truth_vehicle, vehicle_model=vehicle_model, title=title, show=show) + if state.route is not None: + plot_path("route",state.route,color=(1,0.5,0,1),width=2) + if state.trajectory is not None: + plot_path("trajectory",state.trajectory,color=(1,0,0,1),width=3) diff --git a/GEMstack/utils/serialization.py b/GEMstack/utils/serialization.py index 9ee161143..c16228a9d 100644 --- a/GEMstack/utils/serialization.py +++ b/GEMstack/utils/serialization.py @@ -97,7 +97,7 @@ def deserialize(data): `data` can be a `str`, `bytes`, `dict`, or ROS std_msgs/String object. """ - global REGISTRY ##causing error while pushing changes + global REGISTRY name,version,data = deserialize_raw(data) if name not in REGISTRY: raise IOError("Class of type {} not found in registry".format(name)) @@ -170,4 +170,4 @@ def load(file): def save(obj,file): """Saves a JSON file containing serialized data into a registered class.""" - file.write(serialize(obj)) + file.write(serialize(obj)) \ No newline at end of file diff --git a/scenes/xyhead_demo.yaml b/scenes/xyhead_demo.yaml index 52d943b34..c6d97477a 100644 --- a/scenes/xyhead_demo.yaml +++ b/scenes/xyhead_demo.yaml @@ -3,7 +3,7 @@ agents: ped1: type: pedestrian position: [15.0, 2.0] - nominal_velocity: -0.5 + nominal_velocity: 0.5 target: [15.0,10.0] behavior: loop - + \ No newline at end of file From 91e4cd280200338efa180ef4ac9cce7a1bea2c42 Mon Sep 17 00:00:00 2001 From: PatrickWu Date: Fri, 16 May 2025 12:50:14 -0500 Subject: [PATCH 70/71] Update mpc.py --- GEMstack/onboard/planning/mpc.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/GEMstack/onboard/planning/mpc.py b/GEMstack/onboard/planning/mpc.py index ab29f5402..d811567ec 100644 --- a/GEMstack/onboard/planning/mpc.py +++ b/GEMstack/onboard/planning/mpc.py @@ -6,6 +6,7 @@ import numpy as np import casadi import math +import time ########################### # Bo-Hao Wu's code # @@ -83,6 +84,7 @@ def angle_between(p1, p2): def compute(self, state: VehicleState, component: Component = None): """Compute the control commands using MPC.""" + start_time = time.time() if self.iter < 10 and self.prev_x is not None: self.iter += 1 # return float(self.prev_u[self.iter, 0]), float(self.prev_x[self.iter + 1, 4]) @@ -292,6 +294,8 @@ def model(x, u): position_error = np.linalg.norm([vehicle_x - path_x, vehicle_y - path_y]) heading_error = ((vehicle_yaw - path_yaw + np.pi) % (2 * np.pi)) - np.pi + + total_time_elapse = time.time() - start_time ###Jugal's code end here### if component is not None: component.debug("mpc/accel", acc) @@ -305,6 +309,7 @@ def model(x, u): component.debug("mpc/target_x", self.path.points[self.current_path_parameter][0]) component.debug("mpc/target_y", self.path.points[self.current_path_parameter][1]) component.debug("mpc/target_theta", target_angles[0]) + component.debug("mpc/time_elapsed", total_time_elapse) # xy_array = [f"np.array([{round(self.prev_x[t,0],8)}, {round(self.prev_x[t,1],8)}])" for t in range(self.prev_x.shape[0])] # print("mpc = [", ", ".join(xy_array), "]") From acdb8e26278e89753948dcc5c94cab48201c9d44 Mon Sep 17 00:00:00 2001 From: Patrick Wu Date: Fri, 16 May 2025 13:08:48 -0500 Subject: [PATCH 71/71] update current.yaml --- GEMstack/knowledge/defaults/current.yaml | 102 +++++++++++------------ 1 file changed, 51 insertions(+), 51 deletions(-) diff --git a/GEMstack/knowledge/defaults/current.yaml b/GEMstack/knowledge/defaults/current.yaml index cb7f6641f..307635fba 100644 --- a/GEMstack/knowledge/defaults/current.yaml +++ b/GEMstack/knowledge/defaults/current.yaml @@ -1,51 +1,51 @@ -# ********* Main settings entry point for behavior stack *********** - -# Configure settings for the vehicle / vehicle model -vehicle: !include ../vehicle/gem_e4.yaml - -#arguments for algorithm components here -model_predictive_controller: - dt: 0.1 - lookahead: 20 -control: - recovery: - brake_amount : 0.5 - brake_speed : 2.0 - - # Pure Pursuit controller parameters - pure_pursuit: - lookahead: 2.0 # Base lookahead distance (meters) - lookahead_scale: 3.0 # Velocity-dependent lookahead scaling factor - crosstrack_gain: 0.5 # Gain for crosstrack error correction (default: 1) - desired_speed: trajectory # Speed reference source: can be "trajectory", "path", or a constant value - - # Stanley controller parameters (fine tune this) - stanley: - control_gain: 1.5 - softening_gain: 0.2 - desired_speed: trajectory # Speed reference source: can be "trajectory", "path", or a constant value - - # MPC controller parameters - mpc: - dt: 0.2 # Time step for the MPC controller (seconds) - horizon: 30 # Prediction horizon for the MPC controller (number of time steps) - switch_gear: False # Whether to switch gears during runtime - desired_speed: trajectory # Speed reference source: can be "trajectory", "path", or a constant value - - # Shared longitudinal control parameters - longitudinal_control: - pid_p: 1.0 # Proportional gain for speed PID controller - pid_i: 0.1 # Integral gain for speed PID controller - pid_d: 1.0 # Derivative gain for speed PID controller - -#configure the simulator, if using -simulator: - dt: 0.01 - real_time_multiplier: 1.0 # make the simulator run faster than real time by making this > 1 - gnss_emulator: - dt: 0.1 #10Hz - #position_noise: 0.1 #10cm noise - #orientation_noise: 0.04 #2.3 degrees noise - #velocity_noise: - # constant: 0.04 #4cm/s noise - # linear: 0.02 #2% noise +# ********* Main settings entry point for behavior stack *********** + +# Configure settings for the vehicle / vehicle model +vehicle: !include ../vehicle/gem_e4.yaml + +#arguments for algorithm components here +model_predictive_controller: + dt: 0.1 + lookahead: 20 +control: + recovery: + brake_amount : 0.5 + brake_speed : 2.0 + + # Pure Pursuit controller parameters + pure_pursuit: + lookahead: 2.0 # Base lookahead distance (meters) + lookahead_scale: 3.0 # Velocity-dependent lookahead scaling factor + crosstrack_gain: 0.5 # Gain for crosstrack error correction (default: 1) + desired_speed: trajectory # Speed reference source: can be "trajectory", "path", or a constant value + + # Stanley controller parameters (fine tune this) + stanley: + control_gain: 1.5 + softening_gain: 0.2 + desired_speed: trajectory # Speed reference source: can be "trajectory", "path", or a constant value + + # MPC controller parameters + mpc: + dt: 0.2 # Time step for the MPC controller (seconds) + horizon: 30 # Prediction horizon for the MPC controller (number of time steps) + switch_gear: False # Whether to switch gears during runtime + desired_speed: trajectory # Speed reference source: can be "trajectory", "path", or a constant value + + # Shared longitudinal control parameters + longitudinal_control: + pid_p: 1.0 # Proportional gain for speed PID controller + pid_i: 0.1 # Integral gain for speed PID controller + pid_d: 1.0 # Derivative gain for speed PID controller + +#configure the simulator, if using +simulator: + dt: 0.01 + real_time_multiplier: 1.0 # make the simulator run faster than real time by making this > 1 + gnss_emulator: + dt: 0.1 #10Hz + #position_noise: 0.1 #10cm noise + #orientation_noise: 0.04 #2.3 degrees noise + #velocity_noise: + # constant: 0.04 #4cm/s noise + # linear: 0.02 #2% noise