Introduce left/right slip factors for WheelsSimulation

rosys/hardware/wheels.py

@@ -81,16 +81,35 @@ class WheelsSimulation(Wheels, ModuleSimulation):
     A simulated pose is regularly updated with these velocities, while the velocities are emitted as an event.
     """
 
-    def __init__(self) -> None:
+    def __init__(self, width: float = 0.5) -> None:
         super().__init__()
 
+        self.width: float = width
+        """The distance between the wheels -- used to calculate actual drift when slip_factor_* is used."""
+
         self.pose: Pose = Pose()
+        """Provides the actual pose of the robot which can alter due to slippage."""
+
         self.linear_velocity: float = 0
+        """The current linear velocity of the robot."""
+
         self.angular_velocity: float = 0
+        """The current angular velocity of the robot."""
+
         self.inertia_factor: float = 0.0
+        """The factor of inertia for the wheels (0 = no inertia, 1 = full inertia)."""
+
         self.friction_factor: float = 0.0
+        """The factor of friction for the wheels (0 = no friction, 1 = full friction)."""
+
         self.is_blocking: bool = False
-        self.is_slipping: bool = False
+        """If True, the wheels are blocking and the robot will not move."""
+
+        self.slip_factor_left: float = 0
+        """The factor of slippage for the left wheel (0 = no slippage, 1 = full slippage)."""
+
+        self.slip_factor_right: float = 0
+        """The factor of slippage for the right wheel (0 = no slippage, 1 = full slippage)."""
 
     async def drive(self, linear: float, angular: float) -> None:
         await super().drive(linear, angular)
@@ -101,7 +120,12 @@ async def drive(self, linear: float, angular: float) -> None:
     async def step(self, dt: float) -> None:
         self.linear_velocity *= 1 - self.friction_factor
         self.angular_velocity *= 1 - self.friction_factor
-        if not self.is_slipping:
-            self.pose += PoseStep(linear=dt*self.linear_velocity, angular=dt*self.angular_velocity, time=rosys.time())
+        left_speed = self.linear_velocity - self.angular_velocity * self.width / 2
+        right_speed = self.linear_velocity + self.angular_velocity * self.width / 2
+        left_speed *= 1 - self.slip_factor_left
+        right_speed *= 1 - self.slip_factor_right
+        self.pose += PoseStep(linear=dt * (left_speed + right_speed) / 2,
+                              angular=dt * (right_speed - left_speed) / self.width,
+                              time=rosys.time())
         velocity = Velocity(linear=self.linear_velocity, angular=self.angular_velocity, time=rosys.time())
         self.VELOCITY_MEASURED.emit([velocity])

tests/test_robot.py

@@ -2,6 +2,7 @@
 import pytest
 
 import rosys
+from rosys.driving.odometer import Odometer
 from rosys.hardware import Robot, Wheels, WheelsSimulation
 from rosys.testing import assert_pose, forward
 
@@ -27,10 +28,19 @@ async def test_drive(wheels: Wheels, robot: Robot) -> None:
 
 
 @pytest.mark.usefixtures('odometer')
-async def test_stopping_robot_when_rosys_stops(wheels: Wheels, robot: Robot) -> None:
+async def test_stopping_robot_when_rosys_stops(wheels: WheelsSimulation, robot: Robot) -> None:
     await wheels.drive(1, 0)
     await forward(x=1.0, y=0.0)
     await rosys.shutdown()
-    assert isinstance(wheels, WheelsSimulation)
     assert wheels.linear_velocity == 0
     assert wheels.angular_velocity == 0
+
+
+async def test_robot_can_slip_in_simulation(wheels: WheelsSimulation, robot: Robot, odometer: Odometer) -> None:
+    wheels.slip_factor_right = 0.3
+    await wheels.drive(1, 0)
+    await forward(seconds=3.0)
+    assert odometer.prediction.x == pytest.approx(3.0, abs=0.1)
+    assert odometer.prediction.y == pytest.approx(0.0, abs=0.001)
+    assert wheels.pose.x == pytest.approx(1.4, abs=0.1)
+    assert wheels.pose.y == pytest.approx(-1.7, abs=0.1)