car.py

   1 """Vehicle methods copied from esmini."""
   2 from math import atan, cos, fmod, pi, sin, tan
   3
   4
   5 class Car:
   6     max_sp = 70.0
   7     max_acc = 20.0
   8     max_dec = 20.0
   9     st_rate = 5.0
  10     st_return_factor = 4.0
  11     st_max_angle = (60.0 * pi / 180)
  12     w_rad = 0.35
  13     length = 5.0
  14
  15     def __init__(self, x, y, h, sp, wa):
  16         """Create new `Car` object.
  17
  18         :param x: ``x`` coordinate.
  19         :param y: ``y`` coordinate.
  20         :param h: The heading.
  21         :param sp: The speed.
  22         :param wa: Wheel angle.
  23         """
  24         self.x = x
  25         self.y = y
  26         self.h = h
  27         self.sp = sp
  28         self.wa = wa
  29         self.wr = 0.0  # wheel rotation
  30         self.va = 0.0  # angular velocity
  31         self.vx = 0.0  # velocity in x
  32         self.vy = 0.0  # velocity in y
  33
  34     def ctrl(self, dt=0, throttle=0, steering=0):
  35         """Corresponds to vehicle's analog control.
  36
  37         :param dt: GetSimTimeStep.
  38         :param throttle: Longitudinal control
  39             -1: maximum brake
  40             0: no acceleration
  41             +1: maximum acceleration
  42         :param steering: Lateral control
  43             -1: max left
  44             0: straight
  45             +1: max right
  46         """
  47         acc = self.max_dec if throttle < 0 else self.max_acc
  48         self.sp += acc * throttle * dt
  49
  50         if abs(throttle) < 1e-3:
  51             # Apply drag
  52             self.sp *= (1 - 1e-3)
  53         self.sp = min(self.max_sp, max(0, self.sp))
  54
  55         # Make steering slightly wheel speed dependent
  56         st_scale = 1.0 / (1 + 0.005 * self.sp**2)
  57         self.wa += st_scale * self.st_rate * steering * dt
  58
  59         # Self-align
  60         self.wa *= (1.0 - self.st_return_factor * dt)
  61
  62         # Limit wheel angle
  63         ma = st_scale * self.st_max_angle
  64         self.wa = min(ma, max(-ma, self.wa))
  65
  66         self.update(dt)
  67
  68     def update(self, dt=0):
  69         # Calculate wheel rot: https://en.wikipedia.org/wiki/Arc_(geometry)
  70         self.wr = fmod(self.wr + self.sp * dt / self.w_rad, 2 * pi)
  71
  72         # Calculate vehicle kinematics according to simple bicycle model, see
  73         # http://www.me.berkeley.edu/~frborrel/pdfpub/IV_KinematicMPC_jason.pdf
  74
  75         var = atan(0.15 * tan(self.wa))
  76         self.va = var + self.h
  77         hd = self.sp * sin(var) / (self.length * 0.5)
  78
  79         self.vx = self.sp * cos(self.va)
  80         self.vy = self.sp * sin(self.va)
  81
  82         self.h = fmod(self.h + dt * hd, 2 * pi)
  83         while self.h < 0:
  84             self.h += 2 * pi
  85
  86         self.x += dt * self.vx
  87         self.y += dt * self.vy