import math
|
|
import pytest
|
|
from hitl.dynamics import DifferentialDriveModel
|
|
|
def test_straight_motion_updates_position():
|
model = DifferentialDriveModel()
|
model.reset()
|
state = model.step(target_linear=1.0, target_angular=0.0, dt=1.0)
|
assert state.east == pytest.approx(1.0, rel=0.05)
|
assert state.north == pytest.approx(0.0, abs=0.05)
|
assert state.heading == pytest.approx(0.0, abs=1e-3)
|
|
|
def test_in_place_rotation_changes_heading():
|
model = DifferentialDriveModel()
|
model.reset()
|
state = model.step(target_linear=0.0, target_angular=math.radians(90.0), dt=1.0)
|
assert math.degrees(state.heading) == pytest.approx(90.0, abs=1.0)
|
assert abs(state.linear_velocity) < 1e-6
|
|
|
def test_velocity_limits_respected():
|
model = DifferentialDriveModel(max_linear_speed=0.5, max_linear_accel=0.5)
|
model.reset()
|
state = model.step(target_linear=2.0, target_angular=0.0, dt=0.2)
|
assert state.linear_velocity <= model.max_linear_speed + 1e-6
|
|
|
def test_body_acceleration_contains_gravity():
|
model = DifferentialDriveModel()
|
model.reset()
|
state = model.step(target_linear=0.0, target_angular=0.0, dt=0.1)
|
ax, ay, az = state.body_accel_g
|
assert -1.05 < az < -0.95 # 重力方向
|
assert abs(ax) < 0.05
|
assert abs(ay) < 0.05
|
