diff --git a/README.md b/README.md index a8e58c2..980bd19 100644 --- a/README.md +++ b/README.md @@ -22,6 +22,13 @@ PathSim-Vehicle extends the [PathSim](https://github.com/pathsim/pathsim) simulation framework with blocks for vehicle dynamics. All blocks follow the standard PathSim block interface and can be connected into simulation diagrams. +## Features + +- **Kinematic bicycle model** — 4-state model (position, heading, speed) suitable for low-to-moderate speeds and path planning +- **Dynamic bicycle model** — 6-state model with linear tire forces for higher-fidelity lateral dynamics +- **Vehicle parameter sets** — including Hyundai Azera test vehicle from Kong et al. (IEEE IV 2015) +- **Input constraints** — steering angle/rate and acceleration/jerk bounds + ## Install ```bash diff --git a/examples/example_lane_change.py b/examples/example_lane_change.py new file mode 100644 index 0000000..ac057ec --- /dev/null +++ b/examples/example_lane_change.py @@ -0,0 +1,165 @@ +######################################################################################### +## +## PathSim-Vehicle: Kinematic Bicycle Lane Change Example +## +## Simulates a double lane change maneuver comparing the kinematic and dynamic +## bicycle models from Kong et al. (IEEE IV, 2015). +## +######################################################################################### + +# IMPORTS =============================================================================== + +import numpy as np +import matplotlib.pyplot as plt + +from pathsim import Simulation, Connection +from pathsim.blocks import Source, Scope +from pathsim.solvers import RKCK54 + +from pathsim_vehicle import ( + KinematicBicycle, + DynamicBicycle, + hyundai_azera, +) + + +# VEHICLE PARAMETERS =================================================================== + +params = hyundai_azera() + +# Initial conditions +v0 = 15.0 # Highway speed [m/s] (~54 km/h) + + +# STEERING INPUT ======================================================================== + +# Double lane change: two-cycle sinusoidal steering profile +delta_max = np.radians(5.0) # Peak steering angle [rad] +t_start = 1.0 # Maneuver start time [s] +t_duration = 4.0 # Maneuver duration [s] (2 cycles × 2s each) +t_end = t_start + t_duration # Maneuver end time [s] + +def steering_input(t): + """Double lane change steering profile. + + Two full sine periods: the first cycle moves the vehicle to the + adjacent lane, the second cycle returns it to the original lane. + """ + if t < t_start or t > t_end: + return 0.0 + # Two full cycles over t_duration + phase = 2.0 * (2.0 * np.pi) * (t - t_start) / t_duration + return delta_max * np.sin(phase) + + +# KINEMATIC MODEL SETUP ================================================================ + +kin = KinematicBicycle(params, v0=v0) +src_delta_k = Source(steering_input) +src_accel_k = Source(lambda t: 0.0) # Constant speed + +sco_kin = Scope(labels=["x [m]", "y [m]", "ψ [rad]", "v [m/s]"]) + +blocks_kin = [src_delta_k, src_accel_k, kin, sco_kin] + +connections_kin = [ + Connection(src_delta_k, kin["delta_f"]), + Connection(src_accel_k, kin["a"]), + Connection(kin["x"], sco_kin[0]), + Connection(kin["y"], sco_kin[1]), + Connection(kin["psi"], sco_kin[2]), + Connection(kin["v"], sco_kin[3]), +] + +Sim_kin = Simulation(blocks_kin, connections_kin, Solver=RKCK54, dt=0.01) + + +# DYNAMIC MODEL SETUP ================================================================== + +dyn = DynamicBicycle(params, vx0=v0) +src_delta_d = Source(steering_input) +src_accel_d = Source(lambda t: 0.0) + +sco_dyn = Scope(labels=["vx [m/s]", "vy [m/s]", "ψ [rad]", "ψ̇ [rad/s]", "X [m]", "Y [m]"]) + +blocks_dyn = [src_delta_d, src_accel_d, dyn, sco_dyn] + +connections_dyn = [ + Connection(src_delta_d, dyn["delta_f"]), + Connection(src_accel_d, dyn["a_x"]), + Connection(dyn["vx"], sco_dyn[0]), + Connection(dyn["vy"], sco_dyn[1]), + Connection(dyn["psi"], sco_dyn[2]), + Connection(dyn["psi_dot"], sco_dyn[3]), + Connection(dyn["X"], sco_dyn[4]), + Connection(dyn["Y"], sco_dyn[5]), +] + +Sim_dyn = Simulation(blocks_dyn, connections_dyn, Solver=RKCK54, dt=0.01) + + +# SIMULATION ============================================================================ + +T_sim = 7.0 # Total simulation time [s] + + +# Run Example =========================================================================== + +if __name__ == "__main__": + + # Run both simulations + Sim_kin.run(T_sim) + Sim_dyn.run(T_sim) + + # Read results + t_k, data_k = sco_kin.read() + t_d, data_d = sco_dyn.read() + + # ---- Plot ---- + + fig, axes = plt.subplots(2, 2, figsize=(14, 9)) + fig.suptitle("Kinematic vs Dynamic Bicycle Model — Double Lane Change", fontsize=14) + + # (0,0) XY trajectory + ax = axes[0, 0] + ax.plot(data_k[0], data_k[1], label="Kinematic", lw=2) + ax.plot(data_d[4], data_d[5], "--", label="Dynamic", lw=2) + ax.set_xlabel("X [m]") + ax.set_ylabel("Y [m]") + ax.set_title("Trajectory (top view)") + ax.legend() + ax.set_aspect("equal") + ax.grid(True, alpha=0.3) + + # (0,1) Heading angle + ax = axes[0, 1] + ax.plot(t_k, np.degrees(data_k[2]), label="Kinematic ψ", lw=2) + ax.plot(t_d, np.degrees(data_d[2]), "--", label="Dynamic ψ", lw=2) + ax.set_xlabel("Time [s]") + ax.set_ylabel("Heading ψ [°]") + ax.set_title("Heading Angle") + ax.legend() + ax.grid(True, alpha=0.3) + + # (1,0) Steering input + ax = axes[1, 0] + t_steer = np.linspace(0, T_sim, 500) + delta_vals = np.array([steering_input(t) for t in t_steer]) + ax.plot(t_steer, np.degrees(delta_vals), "k-", lw=2) + ax.set_xlabel("Time [s]") + ax.set_ylabel("Steering δ_f [°]") + ax.set_title("Steering Input") + ax.grid(True, alpha=0.3) + + # (1,1) Lateral displacement + ax = axes[1, 1] + ax.plot(t_k, data_k[1], label="Kinematic y", lw=2) + ax.plot(t_d, data_d[5], "--", label="Dynamic Y", lw=2) + ax.set_xlabel("Time [s]") + ax.set_ylabel("Lateral displacement [m]") + ax.set_title("Lateral Position vs Time") + ax.legend() + ax.grid(True, alpha=0.3) + + plt.tight_layout() + plt.show() diff --git a/pyproject.toml b/pyproject.toml index f4e7dc9..d53726f 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -26,7 +26,7 @@ classifiers = [ "Topic :: Scientific/Engineering", ] dependencies = [ - "pathsim", + "pathsim>=0.20", "numpy>=1.15,<2", "scipy>=1.2", ] diff --git a/src/pathsim_vehicle/__init__.py b/src/pathsim_vehicle/__init__.py index 7335b0a..6fbfb4a 100644 --- a/src/pathsim_vehicle/__init__.py +++ b/src/pathsim_vehicle/__init__.py @@ -10,4 +10,19 @@ except ImportError: __version__ = "unknown" -__all__ = ["__version__"] +from .parameters import BicycleParameters, DynamicBicycleParameters, hyundai_azera +from .constraints import BicycleConstraints, clip_steering, clip_acceleration +from .kinematic_bicycle import KinematicBicycle +from .dynamic_bicycle import DynamicBicycle + +__all__ = [ + "__version__", + "BicycleParameters", + "DynamicBicycleParameters", + "hyundai_azera", + "BicycleConstraints", + "clip_steering", + "clip_acceleration", + "KinematicBicycle", + "DynamicBicycle", +] diff --git a/src/pathsim_vehicle/constraints.py b/src/pathsim_vehicle/constraints.py new file mode 100644 index 0000000..c54908d --- /dev/null +++ b/src/pathsim_vehicle/constraints.py @@ -0,0 +1,90 @@ +"""Input constraints for bicycle vehicle models. + +Based on Table II in: Kong, J., Pfeiffer, M., Schildbach, G., and Borrelli, F. +"Kinematic and Dynamic Vehicle Models for Autonomous Driving Control Design", +IEEE IV, 2015. +""" + +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np + + +@dataclass +class BicycleConstraints: + """Input and rate constraints for the bicycle model (Table II in Kong et al.). + + All angular values are stored in radians. + + Parameters + ---------- + delta_min : float + Minimum steering angle [rad]. + delta_max : float + Maximum steering angle [rad]. + a_min : float + Minimum (braking) acceleration [m/s²]. + a_max : float + Maximum acceleration [m/s²]. + delta_dot_min : float + Minimum steering rate [rad/s]. + delta_dot_max : float + Maximum steering rate [rad/s]. + a_dot_min : float + Minimum jerk [m/s³]. + a_dot_max : float + Maximum jerk [m/s³]. + """ + + delta_min: float = -0.6458 # -37° in rad + delta_max: float = 0.6458 # +37° in rad + a_min: float = -1.5 # m/s² + a_max: float = 1.0 # m/s² + delta_dot_min: float = -0.17453 # -10°/s in rad/s + delta_dot_max: float = 0.17453 # +10°/s in rad/s + a_dot_min: float = -3.0 # m/s³ + a_dot_max: float = 1.5 # m/s³ + + +def clip_steering( + delta: float | np.ndarray, + constraints: BicycleConstraints, +) -> float | np.ndarray: + """Clip steering angle to constraint bounds. + + Parameters + ---------- + delta : float or ndarray + Steering angle [rad]. + constraints : BicycleConstraints + Constraint bounds. + + Returns + ------- + float or ndarray + Clipped steering angle. + """ + return np.clip(delta, constraints.delta_min, constraints.delta_max) + + +def clip_acceleration( + a: float | np.ndarray, + constraints: BicycleConstraints, +) -> float | np.ndarray: + """Clip acceleration to constraint bounds. + + Parameters + ---------- + a : float or ndarray + Acceleration [m/s²]. + constraints : BicycleConstraints + Constraint bounds. + + Returns + ------- + float or ndarray + Clipped acceleration. + """ + return np.clip(a, constraints.a_min, constraints.a_max) diff --git a/src/pathsim_vehicle/dynamic_bicycle.py b/src/pathsim_vehicle/dynamic_bicycle.py new file mode 100644 index 0000000..172a33a --- /dev/null +++ b/src/pathsim_vehicle/dynamic_bicycle.py @@ -0,0 +1,141 @@ +"""Dynamic bicycle model block for PathSim. + +Implements Eq. (2) from: Kong, J., Pfeiffer, M., Schildbach, G., and Borrelli, F. +"Kinematic and Dynamic Vehicle Models for Autonomous Driving Control Design", +IEEE IV, 2015. + +State vector: [vx, vy, psi, psi_dot, X, Y] + vx — longitudinal velocity in body frame [m/s] + vy — lateral velocity in body frame [m/s] + psi — heading angle [rad] + psi_dot — yaw rate [rad/s] + X — inertial X position [m] + Y — inertial Y position [m] + +Inputs: [delta_f, a_x] + delta_f — front steering angle [rad] + a_x — longitudinal acceleration [m/s²] + +Tire model: linear (Eq. 3), F_c,i = -C_alpha_i * alpha_i +""" + +from __future__ import annotations + +import numpy as np + +from pathsim.blocks.dynsys import DynamicalSystem + +from .parameters import DynamicBicycleParameters + + +class DynamicBicycle(DynamicalSystem): + """Dynamic bicycle model with linear tire forces (Eq. 2-3 in Kong et al. IV 2015). + + A higher-fidelity vehicle model that captures lateral tire slip effects. + Uses body-frame velocities and a linear tire model for the lateral forces. + + .. math:: + + \\ddot{x} = \\dot{\\psi} \\, \\dot{y} + a_x + + \\ddot{y} = -\\dot{\\psi} \\, \\dot{x} + \\frac{1}{m}(F_{c,f} \\cos \\delta_f + F_{c,r}) + + \\ddot{\\psi} = \\frac{1}{I_z}(l_f F_{c,f} - l_r F_{c,r}) + + \\dot{X} = \\dot{x} \\cos \\psi - \\dot{y} \\sin \\psi + + \\dot{Y} = \\dot{x} \\sin \\psi + \\dot{y} \\cos \\psi + + where the linear tire forces are :math:`F_{c,i} = -C_{\\alpha i} \\, \\alpha_i` + and :math:`C_{\\alpha i}` is the total axle cornering stiffness (both tires combined). + + Parameters + ---------- + params : DynamicBicycleParameters + Vehicle parameters (l_f, l_r, m, I_z, C_af, C_ar). + vx0 : float + Initial longitudinal velocity [m/s]. + vy0 : float + Initial lateral velocity [m/s]. + psi0 : float + Initial heading angle [rad]. + psi_dot0 : float + Initial yaw rate [rad/s]. + X0 : float + Initial inertial X position [m]. + Y0 : float + Initial inertial Y position [m]. + """ + + input_port_labels = { + "delta_f": 0, + "a_x": 1, + } + + output_port_labels = { + "vx": 0, + "vy": 1, + "psi": 2, + "psi_dot": 3, + "X": 4, + "Y": 5, + } + + def __init__( + self, + params: DynamicBicycleParameters, + vx0: float = 0.0, + vy0: float = 0.0, + psi0: float = 0.0, + psi_dot0: float = 0.0, + X0: float = 0.0, + Y0: float = 0.0, + ): + self.params = params + + def _fn_dyn(state, u, t): + vx, vy, psi, psi_dot, X, Y = state + delta_f, a_x = u + + l_f = self.params.l_f + l_r = self.params.l_r + m = self.params.m + I_z = self.params.I_z + C_f = self.params.C_af + C_r = self.params.C_ar + + # Use vx for computing dynamics, with floor for low-speed safety + vx_safe = max(abs(vx), 1.0) + + # Standard 2-DOF lateral-yaw state-space model + # d/dt [vy, psi_dot] = A * [vy, psi_dot] + B * delta_f + # + # A = [-(C_f+C_r)/(m*vx), -vx - (l_f*C_f - l_r*C_r)/(m*vx)] + # [-(l_f*C_f-l_r*C_r)/Iz, -(l_f²*C_f + l_r²*C_r)/(Iz*vx) ] + # + # B = [C_f/m, ] + # [l_f*C_f/Iz ] + + dvx = a_x + dvy = (-(C_f + C_r) / (m * vx_safe)) * vy \ + + (-vx_safe - (l_f * C_f - l_r * C_r) / (m * vx_safe)) * psi_dot \ + + (C_f / m) * delta_f + dpsi = psi_dot + dpsi_dot = (-(l_f * C_f - l_r * C_r) / I_z) * vy \ + + (-(l_f**2 * C_f + l_r**2 * C_r) / (I_z * vx_safe)) * psi_dot \ + + (l_f * C_f / I_z) * delta_f + + # Inertial position (nonlinear kinematics) + dX = vx * np.cos(psi) - vy * np.sin(psi) + dY = vx * np.sin(psi) + vy * np.cos(psi) + + return np.array([dvx, dvy, dpsi, dpsi_dot, dX, dY]) + + def _fn_alg(state, u, t): + return state.copy() + + super().__init__( + func_dyn=_fn_dyn, + func_alg=_fn_alg, + initial_value=np.array([vx0, vy0, psi0, psi_dot0, X0, Y0]), + ) diff --git a/src/pathsim_vehicle/kinematic_bicycle.py b/src/pathsim_vehicle/kinematic_bicycle.py new file mode 100644 index 0000000..a5bd5d4 --- /dev/null +++ b/src/pathsim_vehicle/kinematic_bicycle.py @@ -0,0 +1,106 @@ +"""Kinematic bicycle model block for PathSim. + +Implements Eq. (1) from: Kong, J., Pfeiffer, M., Schildbach, G., and Borrelli, F. +"Kinematic and Dynamic Vehicle Models for Autonomous Driving Control Design", +IEEE IV, 2015. + +State vector: [x, y, psi, v] + x — inertial X position [m] + y — inertial Y position [m] + psi — heading angle [rad] + v — speed [m/s] + +Inputs: [delta_f, a] + delta_f — front steering angle [rad] + a — acceleration [m/s²] +""" + +from __future__ import annotations + +import numpy as np + +from pathsim.blocks.dynsys import DynamicalSystem + +from .parameters import BicycleParameters + + +class KinematicBicycle(DynamicalSystem): + """Kinematic bicycle model (Eq. 1 in Kong et al. IV 2015). + + A simple nonlinear vehicle model that captures position, heading, + and speed dynamics using only the wheelbase geometry. No tire model + is required, making it valid across all speeds including zero. + + .. math:: + + \\dot{x} = v \\cos(\\psi + \\beta) + + \\dot{y} = v \\sin(\\psi + \\beta) + + \\dot{\\psi} = \\frac{v}{l_r} \\sin(\\beta) + + \\dot{v} = a + + where :math:`\\beta = \\arctan\\!\\left(\\frac{l_r}{l_f + l_r} \\tan(\\delta_f)\\right)`. + + Parameters + ---------- + params : BicycleParameters + Vehicle geometry (l_f, l_r). + x0 : float + Initial X position [m]. + y0 : float + Initial Y position [m]. + psi0 : float + Initial heading [rad]. + v0 : float + Initial speed [m/s]. + """ + + input_port_labels = { + "delta_f": 0, + "a": 1, + } + + output_port_labels = { + "x": 0, + "y": 1, + "psi": 2, + "v": 3, + } + + def __init__( + self, + params: BicycleParameters, + x0: float = 0.0, + y0: float = 0.0, + psi0: float = 0.0, + v0: float = 0.0, + ): + self.params = params + + def _fn_dyn(state, u, t): + x, y, psi, v = state + delta_f, a = u + + l_f = self.params.l_f + l_r = self.params.l_r + + # sideslip angle at center of gravity + beta = np.arctan2(l_r * np.tan(delta_f), l_f + l_r) + + dx = v * np.cos(psi + beta) + dy = v * np.sin(psi + beta) + dpsi = (v / l_r) * np.sin(beta) + dv = a + + return np.array([dx, dy, dpsi, dv]) + + def _fn_alg(state, u, t): + return state.copy() + + super().__init__( + func_dyn=_fn_dyn, + func_alg=_fn_alg, + initial_value=np.array([x0, y0, psi0, v0]), + ) diff --git a/src/pathsim_vehicle/parameters.py b/src/pathsim_vehicle/parameters.py new file mode 100644 index 0000000..83e78d1 --- /dev/null +++ b/src/pathsim_vehicle/parameters.py @@ -0,0 +1,81 @@ +"""Vehicle parameter dataclasses for bicycle models. + +Based on: Kong, J., Pfeiffer, M., Schildbach, G., and Borrelli, F. +"Kinematic and Dynamic Vehicle Models for Autonomous Driving Control Design", +IEEE IV, 2015. +""" + +from __future__ import annotations + +from dataclasses import dataclass + + +@dataclass +class BicycleParameters: + """Parameters for the kinematic bicycle model (Eq. 1 in Kong et al.). + + Only two geometric parameters are needed: the distances from the + center of gravity to the front and rear axles. + + Parameters + ---------- + l_f : float + Distance from center of gravity to front axle [m]. + l_r : float + Distance from center of gravity to rear axle [m]. + """ + + l_f: float + l_r: float + + @property + def wheelbase(self) -> float: + """Total wheelbase l_f + l_r [m].""" + return self.l_f + self.l_r + + +@dataclass +class DynamicBicycleParameters(BicycleParameters): + """Extended parameters for the dynamic bicycle model (Eq. 2 in Kong et al.). + + Adds mass, yaw inertia, and linear tire cornering stiffnesses + to the kinematic parameters. + + Parameters + ---------- + l_f : float + Distance from center of gravity to front axle [m]. + l_r : float + Distance from center of gravity to rear axle [m]. + m : float + Vehicle mass [kg]. + I_z : float + Yaw moment of inertia [kg·m²]. + C_af : float + Front axle cornering stiffness (both tires combined) [N/rad]. + C_ar : float + Rear axle cornering stiffness (both tires combined) [N/rad]. + """ + + m: float = 0.0 + I_z: float = 0.0 + C_af: float = 0.0 + C_ar: float = 0.0 + + +def hyundai_azera() -> DynamicBicycleParameters: + """Hyundai Azera test vehicle from Kong et al. IV 2015. + + Wheelbase: 2.843 m, l_f = 1.105 m, l_r = 1.738 m. + Mass and inertia values are representative estimates for a + mid-size sedan (~1600 kg). Cornering stiffness values are + typical for a passenger car axle (~80,000 N/rad total, ~40,000 per tire). + """ + return DynamicBicycleParameters( + l_f=1.105, + l_r=1.738, + m=1600.0, + I_z=2250.0, + C_af=126000.0, # proportional to rear load fraction l_r/(l_f+l_r) + C_ar=80000.0, # proportional to front load fraction l_f/(l_f+l_r) + ) diff --git a/tests/test_constraints.py b/tests/test_constraints.py new file mode 100644 index 0000000..9ff2759 --- /dev/null +++ b/tests/test_constraints.py @@ -0,0 +1,62 @@ +"""Tests for bicycle constraint functions.""" + +import numpy as np +import pytest + +from pathsim_vehicle.constraints import ( + BicycleConstraints, + clip_steering, + clip_acceleration, +) + + +@pytest.fixture() +def c(): + """Default constraints from Table II.""" + return BicycleConstraints() + + +class TestBicycleConstraints: + + def test_defaults_symmetric_steering(self, c): + assert c.delta_min == pytest.approx(-c.delta_max) + + def test_defaults_from_paper(self, c): + # 37° ≈ 0.6458 rad + assert c.delta_max == pytest.approx(0.6458, abs=0.001) + assert c.a_max == pytest.approx(1.0) + assert c.a_min == pytest.approx(-1.5) + + +class TestClipSteering: + + def test_within_bounds(self, c): + assert clip_steering(0.3, c) == pytest.approx(0.3) + + def test_clip_positive(self, c): + assert clip_steering(2.0, c) == pytest.approx(c.delta_max) + + def test_clip_negative(self, c): + assert clip_steering(-2.0, c) == pytest.approx(c.delta_min) + + def test_array_input(self, c): + vals = np.array([-2.0, 0.0, 2.0]) + result = clip_steering(vals, c) + np.testing.assert_allclose(result, [c.delta_min, 0.0, c.delta_max]) + + +class TestClipAcceleration: + + def test_within_bounds(self, c): + assert clip_acceleration(0.5, c) == pytest.approx(0.5) + + def test_clip_positive(self, c): + assert clip_acceleration(5.0, c) == pytest.approx(c.a_max) + + def test_clip_negative(self, c): + assert clip_acceleration(-5.0, c) == pytest.approx(c.a_min) + + def test_array_input(self, c): + vals = np.array([-10.0, 0.0, 10.0]) + result = clip_acceleration(vals, c) + np.testing.assert_allclose(result, [c.a_min, 0.0, c.a_max]) diff --git a/tests/test_dynamic_bicycle.py b/tests/test_dynamic_bicycle.py new file mode 100644 index 0000000..83722df --- /dev/null +++ b/tests/test_dynamic_bicycle.py @@ -0,0 +1,143 @@ +"""Tests for the dynamic bicycle model block.""" + +import numpy as np +import pytest + +from pathsim import Simulation, Connection +from pathsim.blocks import Source, Scope + +from pathsim_vehicle import DynamicBicycle, DynamicBicycleParameters + + +@pytest.fixture() +def params(): + return DynamicBicycleParameters( + l_f=1.105, + l_r=1.738, + m=1600.0, + I_z=2250.0, + C_af=80000.0, + C_ar=80000.0, + ) + + +class TestDynamicBicycleODE: + """Test the ODE right-hand side directly.""" + + def test_straight_line(self, params): + """Zero steering, straight-line cruise → no lateral forces.""" + bike = DynamicBicycle(params, vx0=10.0) + state = np.array([10.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + u = np.array([0.0, 0.0]) # delta_f=0, a_x=0 + f = bike.func_dyn(state, u, 0.0) + + # dvx = 0, dvy = 0, dpsi = 0 + assert f[0] == pytest.approx(0.0, abs=1e-10) + assert f[1] == pytest.approx(0.0, abs=1e-10) + assert f[2] == pytest.approx(0.0, abs=1e-10) + # dX = vx*cos(0) = 10 + assert f[4] == pytest.approx(10.0, abs=1e-10) + # dY = vx*sin(0) = 0 + assert f[5] == pytest.approx(0.0, abs=1e-10) + + def test_acceleration(self, params): + """Pure longitudinal acceleration.""" + bike = DynamicBicycle(params, vx0=5.0) + state = np.array([5.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + u = np.array([0.0, 2.0]) # a_x = 2 m/s² + f = bike.func_dyn(state, u, 0.0) + + assert f[0] == pytest.approx(2.0, abs=1e-10) # dvx = a_x + + +class TestDynamicBicycleSimulation: + """Integration tests using PathSim simulation. + + Scope.read() returns (time, data) where data has shape (n_channels, n_steps). + Channel order: 0=vx, 1=vy, 2=psi, 3=psi_dot, 4=X, 5=Y. + """ + + def test_straight_line_integration(self, params): + """Constant speed, zero steering → straight line.""" + bike = DynamicBicycle(params, vx0=10.0) + src_delta = Source(lambda t: 0.0) + src_a = Source(lambda t: 0.0) + sco = Scope(labels=["vx", "vy", "psi", "psi_dot", "X", "Y"]) + + sim = Simulation( + [src_delta, src_a, bike, sco], + [ + Connection(src_delta, bike), + Connection(src_a, bike[1]), + Connection(bike, sco), + Connection(bike[1], sco[1]), + Connection(bike[2], sco[2]), + Connection(bike[3], sco[3]), + Connection(bike[4], sco[4]), + Connection(bike[5], sco[5]), + ], + dt=0.01, + ) + sim.run(1.0) + + _, data = sco.read() + # X ≈ 10 m after 1s at 10 m/s + assert data[4, -1] == pytest.approx(10.0, abs=0.1) + # Y ≈ 0 + assert data[5, -1] == pytest.approx(0.0, abs=0.01) + + def test_steering_produces_lateral_motion(self, params): + """Steering at speed → lateral motion appears.""" + bike = DynamicBicycle(params, vx0=10.0) + src_delta = Source(lambda t: 0.1) # constant small steer + src_a = Source(lambda t: 0.0) + sco = Scope(labels=["vx", "vy", "psi", "psi_dot", "X", "Y"]) + + sim = Simulation( + [src_delta, src_a, bike, sco], + [ + Connection(src_delta, bike), + Connection(src_a, bike[1]), + Connection(bike, sco), + Connection(bike[1], sco[1]), + Connection(bike[2], sco[2]), + Connection(bike[3], sco[3]), + Connection(bike[4], sco[4]), + Connection(bike[5], sco[5]), + ], + dt=0.01, + ) + sim.run(2.0) + + _, data = sco.read() + # Should have turned — Y nonzero + assert abs(data[5, -1]) > 0.5 + # Heading changed + assert abs(data[2, -1]) > 0.05 + + def test_low_speed_safe(self, params): + """At very low speed, model should not blow up (slip guard).""" + bike = DynamicBicycle(params, vx0=0.1) + src_delta = Source(lambda t: 0.2) + src_a = Source(lambda t: 0.0) + sco = Scope(labels=["vx", "vy", "psi", "psi_dot", "X", "Y"]) + + sim = Simulation( + [src_delta, src_a, bike, sco], + [ + Connection(src_delta, bike), + Connection(src_a, bike[1]), + Connection(bike, sco), + Connection(bike[1], sco[1]), + Connection(bike[2], sco[2]), + Connection(bike[3], sco[3]), + Connection(bike[4], sco[4]), + Connection(bike[5], sco[5]), + ], + dt=0.01, + ) + sim.run(0.5) + + _, data = sco.read() + # All values should be finite (no NaN/Inf from division by zero) + assert np.all(np.isfinite(data)) diff --git a/tests/test_kinematic_bicycle.py b/tests/test_kinematic_bicycle.py new file mode 100644 index 0000000..1016347 --- /dev/null +++ b/tests/test_kinematic_bicycle.py @@ -0,0 +1,134 @@ +"""Tests for the kinematic bicycle model block.""" + +import numpy as np +import pytest + +from pathsim import Simulation, Connection +from pathsim.blocks import Source, Scope + +from pathsim_vehicle import KinematicBicycle, BicycleParameters + + +@pytest.fixture() +def params(): + return BicycleParameters(l_f=1.105, l_r=1.738) + + +class TestKinematicBicycleODE: + """Test the ODE right-hand side directly.""" + + def test_straight_line(self, params): + """Zero steering → straight-line motion along heading.""" + bike = KinematicBicycle(params, v0=10.0) + state = np.array([0.0, 0.0, 0.0, 10.0]) + u = np.array([0.0, 0.0]) # delta_f=0, a=0 + f = bike.func_dyn(state, u, 0.0) + + # dx = v*cos(0) = 10, dy = v*sin(0) = 0, dpsi = 0, dv = 0 + np.testing.assert_allclose(f, [10.0, 0.0, 0.0, 0.0], atol=1e-12) + + def test_acceleration(self, params): + """Pure acceleration from rest → dv = a.""" + bike = KinematicBicycle(params, v0=0.0) + state = np.array([0.0, 0.0, 0.0, 0.0]) + u = np.array([0.0, 2.0]) # delta_f=0, a=2 + f = bike.func_dyn(state, u, 0.0) + + assert f[3] == pytest.approx(2.0) # dv = a + assert f[0] == pytest.approx(0.0) # dx = 0 (v=0) + + def test_steering_produces_yaw(self, params): + """Nonzero steering at nonzero speed → nonzero yaw rate.""" + bike = KinematicBicycle(params, v0=5.0) + state = np.array([0.0, 0.0, 0.0, 5.0]) + u = np.array([0.3, 0.0]) # delta_f = 0.3 rad + f = bike.func_dyn(state, u, 0.0) + + assert abs(f[2]) > 0 # dpsi != 0 + + +class TestKinematicBicycleSimulation: + """Integration tests using PathSim simulation. + + Scope.read() returns (time, data) where data has shape (n_channels, n_steps). + Channel order: 0=x, 1=y, 2=psi, 3=v. + """ + + def test_straight_line_integration(self, params): + """Constant speed, zero steering → straight line.""" + bike = KinematicBicycle(params, v0=10.0) + src_delta = Source(lambda t: 0.0) + src_a = Source(lambda t: 0.0) + sco = Scope(labels=["x", "y", "psi", "v"]) + + sim = Simulation( + [src_delta, src_a, bike, sco], + [ + Connection(src_delta, bike), + Connection(src_a, bike[1]), + Connection(bike, sco), + Connection(bike[1], sco[1]), + Connection(bike[2], sco[2]), + Connection(bike[3], sco[3]), + ], + dt=0.01, + ) + sim.run(1.0) + + _, data = sco.read() + # After 1s at v=10 m/s: x ≈ 10, y ≈ 0 + assert data[0, -1] == pytest.approx(10.0, abs=0.1) # x + assert data[1, -1] == pytest.approx(0.0, abs=0.01) # y + + def test_circular_trajectory(self, params): + """Constant steering + constant speed → circular arc.""" + bike = KinematicBicycle(params, v0=5.0) + delta_f = 0.2 # rad + src_delta = Source(lambda t: delta_f) + src_a = Source(lambda t: 0.0) + sco = Scope(labels=["x", "y", "psi", "v"]) + + sim = Simulation( + [src_delta, src_a, bike, sco], + [ + Connection(src_delta, bike), + Connection(src_a, bike[1]), + Connection(bike, sco), + Connection(bike[1], sco[1]), + Connection(bike[2], sco[2]), + Connection(bike[3], sco[3]), + ], + dt=0.01, + ) + sim.run(2.0) + + _, data = sco.read() + # Should have turned — psi should change significantly + assert abs(data[2, -1]) > 0.1 # psi changed + # y should be nonzero (turning) + assert abs(data[1, -1]) > 0.1 + + def test_zero_velocity_no_motion(self, params): + """Zero velocity, zero acceleration → no motion.""" + bike = KinematicBicycle(params, v0=0.0) + src_delta = Source(lambda t: 0.3) # steering applied but v=0 + src_a = Source(lambda t: 0.0) + sco = Scope(labels=["x", "y", "psi", "v"]) + + sim = Simulation( + [src_delta, src_a, bike, sco], + [ + Connection(src_delta, bike), + Connection(src_a, bike[1]), + Connection(bike, sco), + Connection(bike[1], sco[1]), + Connection(bike[2], sco[2]), + Connection(bike[3], sco[3]), + ], + dt=0.01, + ) + sim.run(0.5) + + _, data = sco.read() + assert data[0, -1] == pytest.approx(0.0, abs=1e-10) # no x motion + assert data[1, -1] == pytest.approx(0.0, abs=1e-10) # no y motion diff --git a/tests/test_parameters.py b/tests/test_parameters.py new file mode 100644 index 0000000..36a906f --- /dev/null +++ b/tests/test_parameters.py @@ -0,0 +1,63 @@ +"""Tests for vehicle parameter dataclasses.""" + +import pytest + +from pathsim_vehicle.parameters import ( + BicycleParameters, + DynamicBicycleParameters, + hyundai_azera, +) + + +class TestBicycleParameters: + + def test_construction(self): + p = BicycleParameters(l_f=1.0, l_r=1.5) + assert p.l_f == 1.0 + assert p.l_r == 1.5 + + def test_wheelbase(self): + p = BicycleParameters(l_f=1.0, l_r=1.5) + assert p.wheelbase == pytest.approx(2.5) + + +class TestDynamicBicycleParameters: + + def test_inherits_bicycle(self): + p = DynamicBicycleParameters( + l_f=1.0, l_r=1.5, m=1500.0, I_z=2000.0, C_af=80000.0, C_ar=80000.0 + ) + assert isinstance(p, BicycleParameters) + assert p.wheelbase == pytest.approx(2.5) + + def test_dynamic_fields(self): + p = DynamicBicycleParameters( + l_f=1.0, l_r=1.5, m=1500.0, I_z=2000.0, C_af=80000.0, C_ar=80000.0 + ) + assert p.m == 1500.0 + assert p.I_z == 2000.0 + assert p.C_af == 80000.0 + assert p.C_ar == 80000.0 + + +class TestHyundaiAzera: + + def test_returns_dynamic_params(self): + p = hyundai_azera() + assert isinstance(p, DynamicBicycleParameters) + + def test_wheelbase(self): + p = hyundai_azera() + assert p.wheelbase == pytest.approx(2.843) + + def test_axle_distances(self): + p = hyundai_azera() + assert p.l_f == pytest.approx(1.105) + assert p.l_r == pytest.approx(1.738) + + def test_mass_positive(self): + p = hyundai_azera() + assert p.m > 0 + assert p.I_z > 0 + assert p.C_af > 0 + assert p.C_ar > 0