From 646f434235cfbbe84bb8ade33d9c7378bac0df04 Mon Sep 17 00:00:00 2001 From: Gabriel Zaragoza Date: Sat, 3 Feb 2024 16:44:21 -0500 Subject: [PATCH] Initial implementation --- rb_ws/src/buggy/scripts/2d_sim/engine.py | 27 +++++++++--------------- 1 file changed, 10 insertions(+), 17 deletions(-) diff --git a/rb_ws/src/buggy/scripts/2d_sim/engine.py b/rb_ws/src/buggy/scripts/2d_sim/engine.py index 8bb34526..03a5f6b6 100755 --- a/rb_ws/src/buggy/scripts/2d_sim/engine.py +++ b/rb_ws/src/buggy/scripts/2d_sim/engine.py @@ -137,23 +137,16 @@ def step(self): velocity = self.velocity steering_angle = self.steering_angle - # Calculate new position - if steering_angle == 0.0: - # Straight - e_utm_new = e_utm + (velocity / self.rate) * np.cos(np.deg2rad(heading)) - n_utm_new = n_utm + (velocity / self.rate) * np.sin(np.deg2rad(heading)) - heading_new = heading - else: - # steering radius - radius = self.get_steering_arc() - - distance = velocity / self.rate - - delta_heading = distance / radius - heading_new = heading + np.rad2deg(delta_heading) / 2 - e_utm_new = e_utm + (velocity / self.rate) * np.cos(np.deg2rad(heading_new)) - n_utm_new = n_utm + (velocity / self.rate) * np.sin(np.deg2rad(heading_new)) - heading_new = heading_new + np.rad2deg(delta_heading) / 2 + h = 1/self.rate + state = np.array([e_utm, n_utm, heading, steering_angle]) + k1 = self.dynamics(state, velocity) + k2 = self.dynamics(state + h/2 * k1, velocity) + k3 = self.dynamics(state + h/2 * k2, velocity) + k4 = self.dynamics(state + h/2 * k3, velocity) + + final_state = state + h/6 * (k1 + 2 * k2 + 2 * k3 + k4) + + e_utm_new, n_utm_new, heading_new, _ = final_state with self.lock: self.e_utm = e_utm_new