diff --git a/src/roboticstoolbox/mobile/Vehicle.py b/src/roboticstoolbox/mobile/Vehicle.py index 50a5ce0a3..35dc14eca 100644 --- a/src/roboticstoolbox/mobile/Vehicle.py +++ b/src/roboticstoolbox/mobile/Vehicle.py @@ -804,6 +804,9 @@ def step( .. note:: Vehicle control input limits are applied. + The distance component of the odometry is signed: reverse motion gives + a negative distance. + .. seealso:: :func:`control`, ``update``, :func:`run` """ # determine vehicle control @@ -814,6 +817,8 @@ def step( # update state xd = self._dt * self.deriv(self._x, u) # delta state + theta = self._x[2] + distance = xd[0] * cos(theta) + xd[1] * sin(theta) # update state vector self._x += xd @@ -822,7 +827,7 @@ def step( # print('VEH', u, self.x) # odometry comes from change in state vector - odo = np.r_[np.linalg.norm(xd[0:2]), xd[2]] + odo = np.r_[distance, xd[2]] if self._V is not None: odo += self.random.multivariate_normal((0, 0), self._V) diff --git a/tests/test_vehicle_estimators.py b/tests/test_vehicle_estimators.py index b91f0c0ff..cca90984d 100644 --- a/tests/test_vehicle_estimators.py +++ b/tests/test_vehicle_estimators.py @@ -22,6 +22,7 @@ import matplotlib.pyplot as plt import numpy as np import numpy.testing as nt +import pytest from roboticstoolbox import ( EKF, @@ -32,6 +33,7 @@ RandomPath, RangeBearingSensor, Unicycle, + VehicleBase, ) T = TypeVar("T") @@ -55,6 +57,28 @@ def numerical_jacobian( return np.column_stack(cols) +@pytest.mark.parametrize( + "model, control", + [ + (Bicycle, (-1.0, 0.2)), + (Unicycle, (-1.0, 0.2)), + (DiffSteer, (-1.2, -0.8)), + (DiffSteer, (0.2, -0.6)), + ], +) +@pytest.mark.parametrize("direction", [-1, 1]) +def test_step_odometry_reconstructs_forward_and_reverse_motion( + model: type[VehicleBase], control: tuple[float, float], direction: int +) -> None: + vehicle = model(x0=[2.0, -3.0, 1.2], dt=0.1) + initial = vehicle.x.copy() + + odometry = vehicle.step(np.array(control) * direction, animate=False) + + nt.assert_allclose(vehicle.f(initial, odometry), vehicle.x, atol=1e-14) + assert np.sign(odometry[0]) == -direction + + class TestBicycle(unittest.TestCase): def test_deriv(self) -> None: bike = Bicycle(L=2)