Adjust bicycle kinematics to use front wheel reference

- the code now uses the front wheels as the reference
  point for the calculations
- this fixes the unit test, which were using the front wheels
  while the code used the back wheels.

donkeycar/parts/kinematics.py

@@ -12,7 +12,7 @@ def limit_angle(angle:float):
     """
     limit angle to pi to -pi radians (one full circle)
     """
-    return math.atan2(math.sin(angle), math.cos(angle));
+    return math.atan2(math.sin(angle), math.cos(angle))
     # twopi = math.pi * 2
     # while(angle > math.pi):
     #     angle -= twopi
@@ -30,15 +30,16 @@ def __init__(self, x:float=0.0, y:float=0.0, angle:float=0.0) -> None:
 
 class Bicycle:
     """
-    Bicycle forward kinematics for a car-like vehicle (Ackerman steering)
+    Bicycle forward kinematics for a car-like vehicle (Ackerman steering),
+    using the point midway between the front wheels as a reference point,
     takes the steering angle in radians and output of the odometer 
     and turns those into:
-    - forward distance and velocity,
+    - forward distance and velocity of the reference point between the front wheels,
     - pose; angle aligned (x,y) position and orientation in radians
     - pose velocity; change in angle aligned position and orientation per second
     @param wheel_base: distance between the front and back wheels
 
-    NOTE: this version uses the point midway between the rear wheels
+    NOTE: this version uses the point midway between the front wheels
           as the point of reference.
     see https://thef1clan.com/2020/09/21/vehicle-dynamics-the-kinematic-bicycle-model/
     """
@@ -48,27 +49,31 @@ def __init__(self, wheel_base:float, debug=False):
         self.timestamp:float = 0
         self.forward_distance:float = 0
         self.forward_velocity:float = 0
+        self.steering_angle = None
         self.pose = Pose2D()
         self.pose_velocity = Pose2D()
         self.running:bool = True
 
     def run(self, forward_distance:float, steering_angle:float, timestamp:float=None) -> Tuple[float, float, float, float, float, float, float, float, float]:
         """
         params
-            forward_distance: distance the reference point has travelled
+            forward_distance: distance the reference point between the
+                              front wheels has travelled
             steering_angle: angle in radians of the front 'wheel' from forward.
                             In this case left is positive, right is negative,
                             and directly forward is zero.
             timestamp: time of distance readings or None to use current time
         returns
             distance
             velocity
-            x is horizontal position of point midway between wheels
-            y is vertical position of point midway between wheels
-            angle is orientation in radians around point midway between wheels
-            x' is the horizontal velocity
-            y' is the vertical velocity
-            angle' is the angular velocity
+            x is horizontal position of reference point midway between front wheels
+            y is vertical position of reference point midway between front wheels
+            angle is orientation in radians of the vehicle along it's wheel base
+                  (along the line between the reference points midway between
+                   the front wheels and and midway between the back wheels)
+            x' is the horizontal velocity (rate of change of reference point along horizontal axis)
+            y' is the vertical velocity (rate of change of reference point along vertical axis)
+            angle' is the angular velocity (rate of change of orientation)
             timestamp
 
         """
@@ -79,8 +84,9 @@ def run(self, forward_distance:float, steering_angle:float, timestamp:float=None
 
         if self.running:
             if 0 == self.timestamp:
-                self.forward_distance = forward_distance
+                self.forward_distance = 0
                 self.forward_velocity=0
+                self.steering_angle = steering_angle
                 self.pose = Pose2D()
                 self.pose_velocity = Pose2D()
                 self.timestamp = timestamp
@@ -90,33 +96,50 @@ def run(self, forward_distance:float, steering_angle:float, timestamp:float=None
                 #
                 delta_time = timestamp - self.timestamp
                 delta_distance = forward_distance - self.forward_distance
-                forward_velocity = delta_distance / delta_time
+                delta_steering_angle = steering_angle - self.steering_angle
 
                 #
-                # new velocities
+                # new position and orientation
+                # assumes delta_time is small and so assumes movement is linear
                 #
-                angle_velocity = bicycle_angular_velocity(self.wheel_base, forward_velocity, steering_angle)
-                delta_angle = angle_velocity * delta_time
-                estimated_angle = limit_angle(self.pose.angle + delta_angle / 2)
-                x_velocity = forward_velocity * math.cos(estimated_angle)
-                y_velocity = forward_velocity * math.sin(estimated_angle)
+                # x, y, angle = update_bicycle_front_wheel_pose(
+                #     self.pose,
+                #     self.wheel_base,
+                #     self.steering_angle + delta_steering_angle / 2,
+                #     delta_distance)
+                #
+                # #
+                # # update velocities
+                # #
+                # forward_velocity = delta_distance / delta_time
+                # self.pose_velocity.angle = (angle - self.pose.angle) / delta_time
+                # self.pose_velocity.x = (x - self.pose.x) / delta_time
+                # self.pose_velocity.y = (y - self.pose.y) / delta_time
+                # #
+                # # update pose
+                # #
+                # self.pose.x = x
+                # self.pose.y = y
+                # self.pose.angle = angle
 
                 #
-                # new position and orientation
+                # new velocities
                 #
-                x = self.pose.x + x_velocity * delta_time
-                y = self.pose.y + y_velocity * delta_time
-                angle = limit_angle(self.pose.angle + delta_angle)
+                forward_velocity = delta_distance / delta_time
+                self.pose_velocity.angle = bicycle_angular_velocity(self.wheel_base, forward_velocity, steering_angle)
+                delta_angle = self.pose_velocity.angle * delta_time
+                estimated_angle = limit_angle(self.pose.angle + delta_angle / 2)
+                self.pose_velocity.x = forward_velocity * math.cos(estimated_angle)
+                self.pose_velocity.y = forward_velocity * math.sin(estimated_angle)
 
                 #
-                # update pose and velocities
+                # new pose
                 #
-                self.pose.x = x
-                self.pose.y = y
-                self.pose.angle = angle
-                self.pose_velocity.x = x_velocity
-                self.pose_velocity.y = y_velocity
-                self.pose_velocity.angle = angle_velocity
+                self.pose.x = self.pose.x + self.pose_velocity.x * delta_time
+                self.pose.y = self.pose.y + self.pose_velocity.y * delta_time
+                self.pose.angle = limit_angle(self.pose.angle + delta_angle)
+
+                self.steering_angle = steering_angle
 
                 #
                 # update odometry
@@ -185,6 +208,41 @@ def run(self, forward_velocity:float, angular_velocity:float, timestamp:float=No
         return forward_velocity, steering_angle, timestamp
 
 
+def update_bicycle_front_wheel_pose(front_wheel, wheel_base, steering_angle, distance):
+    """
+    Calculates the ending position of the front wheel of a bicycle kinematics model.
+    This is expected to be called at a high rate such that we can model the
+    the travel as a line rather than an arc.
+
+    Arguments:
+    front_wheel -- starting pose at front wheel as tuple of (x, y, angle) where
+                x -- initial x-coordinate of the front wheel (float)
+                y -- initial y-coordinate of the front wheel (float)
+                angle -- initial orientation of the vehicle along it's wheel base (in radians) (float)
+    wheel_base -- length of the wheel base (float)
+    steering_angle -- steering angle (in radians) (float)
+    distance -- distance travelled by the vehicle (float)
+
+    Returns:
+    A tuple (x_f, y_f, theta_f) representing the ending position and orientation of the front wheel.
+    x_f -- ending x-coordinate of the front wheel (float)
+    y_f -- ending y-coordinate of the front wheel (float)
+    theta_f -- ending orientation of the vehicle (in radians) (float)
+    """
+    if distance == 0:
+        return front_wheel
+
+    if steering_angle == 0:
+        x = front_wheel.x + distance * math.cos(front_wheel.angle)
+        y = front_wheel.y + distance * math.sin(front_wheel.angle)
+        theta = front_wheel.angle
+    else:
+        theta = limit_angle(front_wheel.angle + math.tan(steering_angle) * distance / wheel_base)
+        x = front_wheel.x + distance * math.cos(theta)
+        y = front_wheel.y + distance * math.sin(theta)
+    return x, y, theta
+
+
 def bicycle_steering_angle(wheel_base:float, forward_velocity:float, angular_velocity:float) -> float:
     """
     Calculate bicycle steering for the vehicle from the angular velocity.
@@ -197,7 +255,8 @@ def bicycle_steering_angle(wheel_base:float, forward_velocity:float, angular_vel
     # math.tan(steering_angle) = angular_velocity * self.wheel_base / forward_velocity
     # steering_angle = math.atan(angular_velocity * self.wheel_base / forward_velocity)
     #
-    return math.atan(angular_velocity * wheel_base / forward_velocity)
+    # return math.atan(angular_velocity * wheel_base / forward_velocity)
+    return limit_angle(math.asin(angular_velocity * wheel_base / forward_velocity))
 
 
 def bicycle_angular_velocity(wheel_base:float, forward_velocity:float, steering_angle:float) -> float:
@@ -208,9 +267,11 @@ def bicycle_angular_velocity(wheel_base:float, forward_velocity:float, steering_
     """
     #
     # for car-like (bicycle model) vehicle, for the back axle:
-    # angular_velocity = forward_velocity * (math.tan(steering_angle) /  wheel_base)
+    # angular_velocity = forward_velocity * math.tan(steering_angle) /  wheel_base if velocity is from rear wheels
+    # angular_velocity = forward_velocity * math.tan(steering_angle) /  wheel_base if velocity is from front wheels
     #
-    return forward_velocity * math.tan(steering_angle) / wheel_base
+    # return forward_velocity * math.tan(steering_angle) / wheel_base # velocity for rear wheel
+    return forward_velocity * math.sin(steering_angle) / wheel_base  # velocity of front wheel
 
 
 class BicycleNormalizeAngularVelocity:

donkeycar/tests/test_kinematics.py

@@ -138,7 +138,7 @@ def do_90degree_turn(self, steering_angle):
         # calculate length of circle's perimeter
         # see [Rear-axle reference point](https://thef1clan.com/2020/09/21/vehicle-dynamics-the-kinematic-bicycle-model/)
         #
-        R = abs(wheel_base / math.tan(steering_angle))  # Radius of turn (radius to Instantaneous Center of Rotation)
+        R = abs(wheel_base / math.sin(steering_angle))  # Radius of turn (radius to Instantaneous Center of Rotation)
         C = 2 * math.pi * R                             # Circumference of turn
 
         turn_distance = C / 4  # drive the 90 degrees of perimeter
@@ -486,9 +486,10 @@ def test_differential_steering(self):
 
 from donkeycar.templates.complete import add_odometry
 from donkeycar.vehicle import Vehicle
-from donkeycar.parts.kinematics import NormalizeSteeringAngle
+from donkeycar.parts.kinematics import NormalizeSteeringAngle, limit_angle
 import math
 
+
 def calc_center_of_rotation(wheelbase, front_wheel, orientation, steering_angle):
     """
     Calculate the center of rotation for a bicycle turn.
@@ -510,12 +511,13 @@ def calc_center_of_rotation(wheelbase, front_wheel, orientation, steering_angle)
     if steering_angle == 0:
         return (x, y, 0)
     else:
-        R = wheelbase / math.tan(steering_angle)
+        R = wheelbase / math.sin(steering_angle)
         cx = x - R * math.sin(orientation)
         cy = y + R * math.cos(orientation)
-        return (cx, cy, R)
+        return (cx, cy, abs(R))  # radius is always positive
+
 
-def calc_end_point(center, radius, point, radians):
+def calc_arc_end_point(center, radius, point, radians):
     """
     Calculates the ending point on a circle given the center of the circle, the radius of the circle,
     a point on the circle and a distance along the circle in radians from the given point.
@@ -529,11 +531,18 @@ def calc_end_point(center, radius, point, radians):
     Returns:
     end_point -- the ending point on the circle represented as a tuple (x, y) (tuple)
     """
-    x = center[0] + radius * math.cos(math.atan2(point[1] - center[1], point[0] - center[0]) + radians)
-    y = center[1] + radius * math.sin(math.atan2(point[1] - center[1], point[0] - center[0]) + radians)
+    center_x, center_y = center
+    point_x, point_y = point
+
+    theta = math.atan2(point_y - center_y, point_x - center_x)
+    theta_end = theta + radians
+
+    x = center_x + radius * math.cos(theta_end)
+    y = center_y + radius * math.sin(theta_end)
     end_point = (x, y)
     return end_point
 
+
 def calc_bicycle_pose(wheelbase, front_wheel, orientation, steering_angle, distance):
     """
     Calculate the ending pose for a bicycle with the given wheel base,
@@ -546,23 +555,36 @@ def calc_bicycle_pose(wheelbase, front_wheel, orientation, steering_angle, dista
     :return: ending pose as tuple of (x, y, orientation)
     """
 
-    # - calculate the center and radius of the turn,
-    center_x, center_y, turn_radius = calc_center_of_rotation(wheelbase,
-                                                           front_wheel,
-                                                           orientation,
-                                                           steering_angle)
-    # - calculate the circumference of a full turn
-    turn_circumference = 2 * math.pi * turn_radius
+    if distance == 0:
+        return front_wheel.x, front_wheel.y, orientation
+
+    if steering_angle == 0:
+        ending_x = front_wheel[0] + distance * math.cos(orientation)
+        ending_y = front_wheel[1] + distance * math.sin(orientation)
+        ending_angle = orientation
+    else:
+        # - calculate the center and radius of the turn,
+        center_x, center_y, turn_radius = calc_center_of_rotation(wheelbase,
+                                                               front_wheel,
+                                                               orientation,
+                                                               steering_angle)
+        print(f"Center of rotation = ({center_x}, {center_y})")
+        print(f"Radius of turn = {turn_radius}")
 
-    # - calculate the angular distance travelled in radians
-    turn_radians = distance / turn_circumference * 2 * math.pi if turn_circumference > 0 else 0
+        # - calculate the circumference of a full turn
+        turn_circumference = 2 * math.pi * turn_radius
 
-    # - calculate the end position on the arc
-    ending_x, ending_y = calc_end_point((center_x, center_y),
-                                        turn_radius, (0, 0), turn_radians)
+        # - calculate the angular distance travelled in radians
+        turn_radians = (distance / turn_circumference) * 2 * math.pi
+        turn_radians *= sign(steering_angle)  # right turn is negative angle
+
+        # - calculate the end position on the arc
+        ending_x, ending_y = calc_arc_end_point((center_x, center_y),
+                                                turn_radius, front_wheel, turn_radians)
+
+        # - calculate the end orientation
+        ending_angle = limit_angle(orientation + math.tan(steering_angle) * distance / wheelbase)
 
-    # - calculate the end orientation
-    ending_angle = 0 + math.tan(steering_angle) * distance / wheelbase
     return ending_x, ending_y, ending_angle
 
 
@@ -594,7 +616,7 @@ class Config:
         # cfg.__setattr__('DONKEY_GYM', True)
         # cfg.SIM_RECORD_DISTANCE = True
 
-        cfg.DRIVE_LOOP_HZ = 30
+        cfg.DRIVE_LOOP_HZ = 50
 
         cfg.DRIVE_TRAIN_TYPE = "mock"
         cfg.AXLE_LENGTH = 0.175  # length of axle; distance between left and right wheels in meters
@@ -641,9 +663,9 @@ def do_drive(self, cfg, forward_velocity, steering_angle):
         #
         turn_distance = None
         if steering_angle != 0.0:
-            R = abs(wheel_base / math.tan(steering_angle))  # Radius of turn (radius to Instantaneous Center of Rotation)
-            C = 2 * math.pi * R                             # Circumference of turn
-            turn_distance = C / 4  # drive the 90 degrees of perimeter
+            R = wheel_base / math.sin(steering_angle)  # Radius of turn (radius to Instantaneous Center of Rotation)
+            C = 2 * math.pi * R                        # Circumference of turn
+            turn_distance = abs(C / 4)  # drive the 90 degrees of perimeter
         else:
             # going straight
             turn_distance = forward_velocity * 2 # just use 2 seconds of driving
@@ -666,11 +688,9 @@ def do_drive(self, cfg, forward_velocity, steering_angle):
         cfg.MOCK_TICKS_PER_SECOND = ticks_per_second
 
         # set throttle and steering angle
-        # normalize_velocity = VelocityNormalize(min_speed=cfg.MIN_SPEED, max_speed=cfg.MAX_SPEED, min_normal_speed=cfg.MIN_THROTTLE)
-        # throttle = normalize_velocity.run(forward_velocity)
-        throttle = 1.0
+        throttle = 1.0  # full throttle, so we can use the cfg.MOCK_TICKS_PER_SECOND as the encoder rate
         normalize_steering = NormalizeSteeringAngle(max_steering_angle=cfg.MAX_STEERING_ANGLE)
-        steering = normalize_steering.run(steering_angle);
+        steering = normalize_steering.run(steering_angle)
 
         # run the vehicle and then stop it
         vehicle = self.initialize_vehicle(cfg)
@@ -690,7 +710,6 @@ def do_drive(self, cfg, forward_velocity, steering_angle):
         self.assertAlmostEqual(loop_distance, distance, 2)
         self.assertLessEqual(abs(loop_distance - distance) / distance, 0.005)  # final error less than 0.5%
 
-
         #
         # calculate the expected ending position using the center of the turn,
         # the radius of the turn, the circumference of the turn
@@ -699,20 +718,14 @@ def do_drive(self, cfg, forward_velocity, steering_angle):
         #
         ending_x, ending_y, ending_angle = calc_bicycle_pose(cfg.WHEEL_BASE, (0, 0), 0, steering_angle, distance)
 
-
         print(f"Expected Distance = {loop_distance}")
         print(f"Calculated Pose = ({pose_x}, {pose_y}, {pose_angle})")
         print(f"Expected ending Pose = ({ending_x}, {ending_y}, {ending_angle})")
 
-
-        #
-        # TODO: using loop_distance, calculate the point on the traversed
-        #       arc where the vehicle finished.
-        #
         if steering_angle != 0.0:
-            self.assertLessEqual(abs(ending_x - pose_x) / ending_x, 0.005)  # final error less than 0.5%
-            self.assertLessEqual(abs(ending_y - pose_y) / ending_y, 0.005)  # final error less than 0.5%
-            self.assertLessEqual(abs(ending_angle - pose_angle) / ending_angle, 0.005)
+            self.assertLessEqual(abs((ending_x - pose_x) / loop_distance), 0.005)  # final error less than 0.5%
+            self.assertLessEqual(abs((ending_y - pose_y) / loop_distance), 0.005)  # final error less than 0.5%
+            self.assertLessEqual(abs((ending_angle - pose_angle) / (2 * math.pi)), 0.01)  # final error less than 1%
 
     def test_drive_straight(self):
         # 12.5 degrees steering angle