Smanor/mpc fix#78
Merged
Merged
Conversation
Comment on lines
+575
to
+582
| # Corridor enforcement is a soft quadratic penalty (c_bnd above). | ||
| # Pick the action by averaging the top `mppi_elite_frac × K` samples' | ||
| # control sequences (CEM-style elite mean). The reported "best" | ||
| # cost is still the elite minimum so feasibility / failsafe still | ||
| # gate on the truly-best trajectory. | ||
| n_elite = max(1, int(round(self.mppi_elite_frac * self.K))) | ||
| elite_idx = np.argpartition(cost, n_elite - 1)[:n_elite] | ||
| best = int(elite_idx[np.argmin(cost[elite_idx])]) |
Comment on lines
676
to
+716
| @@ -623,7 +695,49 @@ def _publish_status(self, mode, success, t0, s, d, psi_track, v, v_target, | |||
| float({"off": 0, "shadow": 1, "apply": 2}.get(self.residual.mode, 1)), | |||
| float(np.linalg.norm(self.residual.theta_s)), | |||
| float(np.linalg.norm(self.residual.theta_d)), | |||
| # Raw GPS pose (EKF-vs-GPS diff). NaN until first /gps fix. | |||
| float(self._gps_x), float(self._gps_y), | |||
| float(self._gps_yaw), float(self._gps_v), | |||
| # Wheel speed + last-GPS-event EKF prior/posterior. seq lets the | |||
| # analyst dedupe MPC ticks (60 Hz) against GPS events (10 Hz). | |||
| float(self._wheel_v), float(self._gps_event_seq), | |||
| float(self._gps_have_yaw), float(self._gps_have_speed), | |||
| float(self._ekf_prior_x), float(self._ekf_prior_y), | |||
| float(self._ekf_prior_yaw), float(self._ekf_prior_v), | |||
| float(self._ekf_post_x), float(self._ekf_post_y), | |||
| float(self._ekf_post_yaw), float(self._ekf_post_v), | |||
| # ---- Phase 1 residual telemetry (Phase 2 fields stay 0/NaN here) ---- | |||
| float(self.residual.buffer.size if self.residual.buffer is not None else 0), | |||
| float(self.residual.buffer.capacity if self.residual.buffer is not None else 0), | |||
| float(last.train_wall_ms if last is not None else float("nan")), | |||
| float(last.n_samples if last is not None else 0), | |||
| float(trainer.train_seq if trainer else 0), | |||
| float(last.train_mae_s if last is not None else float("nan")), | |||
| float(last.train_mae_d if last is not None else float("nan")), | |||
Comment on lines
+11
to
+21
| class TrainBuffer: | ||
| def __init__(self, capacity: int, feature_dim: int): | ||
| self.capacity = int(capacity) | ||
| self.feature_dim = int(feature_dim) | ||
| self._X = np.zeros((self.capacity, self.feature_dim), dtype=np.float64) | ||
| self._ys = np.zeros(self.capacity, dtype=np.float64) | ||
| self._yd = np.zeros(self.capacity, dtype=np.float64) | ||
| self._t = np.zeros(self.capacity, dtype=np.int64) | ||
| self._write_idx = 0 # next index to write | ||
| self._count = 0 # min(filled, capacity) | ||
| self._lock = threading.Lock() |
Comment on lines
+38
to
+41
| ```bash | ||
| ros2 bag record -s mcap -o /ws/bags/$(date +%Y%m%d_%H%M%S) /mpc/status /mpc/residual_mode /mpc/residual_revert /cmd_drive /odom /gps /imu /imu/calibration_status /e_comms/kart_speed_m_per_s /e_comms/throttle_pwm /e_comms/steering_pwm /e_comms/adcb_state /e_comms/rc_mode /system_state /track_angles /manual_commands /pathfinder_params | ||
| ``` | ||
| docker cp ros2-dev:/ws/bags . |
Comment on lines
+1
to
+7
| def test_package_importable(): | ||
| import sim # noqa: F401 | ||
|
|
||
|
|
||
| def test_torch_available(): | ||
| import torch | ||
| assert torch.__version__ |
Comment on lines
+1
to
+8
| import numpy as np | ||
| import torch | ||
|
|
||
| from sim.residual_mlp import ( | ||
| FEATURE_DIM, | ||
| ResidualMLP, | ||
| build_features, | ||
| ) |
|
|
||
| def test_mlp_reduces_to_bicycle_off_distribution(tmp_path, temp_artifacts): | ||
| import json | ||
| import torch |
Comment on lines
4
to
+6
| min_speed: 0.0 # m/s | ||
| max_steering: 100.0 # % | ||
| min_steering: -100.0 # % | ||
| max_steering: 60.0 # % | ||
| min_steering: -60.0 # % |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
This pull request introduces several improvements and new features across the autonomous kart software stack, with a focus on enhanced localization accuracy, better simulation fidelity, and improved telemetry for offline analysis. The most significant changes are the addition of per-GPS-event EKF state snapshots for offline analysis, improved IMU calibration and handling, support for steering slop in the simulation model, and expanded data handling in the pathfinder and master nodes.
Localization and Telemetry Enhancements:
localization_node.py, enabling detailed offline analysis of dead-reckoning drift and GPS corrections. This includes a new publisher for/localization/gps_eventand logic to capture and broadcast EKF prior/posterior state vectors. [1] [2]IMU Calibration and Handling:
R) to account for chassis tilt, storing and loading this matrix in the calibration cache for more accurate orientation estimation. [1] [2] [3]Simulation Model Improvements:
Miscellaneous and Infrastructure:
Summary of Most Important Changes:
Localization and Telemetry:
IMU Calibration:
Simulation Model:
Data Handling and Passthrough:
Infrastructure: