Merge pull request udacity#27 from autti/acc

Naive PID implementation

acc/acc/cruise.py

@@ -1,10 +1,57 @@
 
-def control(speed, acceleration, car_in_front, steer_torque):
-    """Adaptive Cruise Control
-    """
-    # --- Implement your solution here ---#
-    brake = 0
-    gas = 0
-    # ------------------------------------#
-
-    return brake, gas
+
+def control(speed=0, acceleration=0, car_in_front=200, gap=5, cruise_speed=None, state=None):
+        """Adaptive Cruise Control
+
+           speed: Current car speed (m/s)
+           acceleration: Current car acceleration (m/s^2)
+           gas: last signal sent. Real number.
+           brake: last signal sent. Real number.
+           car_in_front: distance in meters to the car in front. (m)
+           gap: maximum distance to the car in front (m)
+           cruise_speed: desired speed, set via the cruise control buttons. (m/s)
+           status: a convenience dictionary to keep state between runs.
+        """
+
+        if state is None:
+            state = dict(K_p=0.15, K_d=0., K_i=0.0003, d_front_prev=100,
+                         t_safe=.5, prev_setpoint=0., integral_setpoint=0.,
+                         maintaining_distance=False)
+
+        delta_distance = car_in_front - 2 * gap - speed**2 / (2 * 5)
+
+        # if the car ahead does not allow to get to cruise speed
+        # use safe following distance as a measure until cruise speed is reached again
+        if delta_distance < 0:
+            state['maintaining_distance'] = True
+        elif speed >= cruise_speed:
+            state['maintaining_distance'] = False
+
+        if state['maintaining_distance']:
+            # But override it if we are too close to the car in front.
+            set_point = delta_distance
+        else:
+            # if the distance is not too close maintain cruise speed
+            set_point = cruise_speed - speed
+
+        control = (state['K_p'] * set_point +
+                   state['K_d'] * (set_point - state['prev_setpoint']) +
+                   state['K_i'] * state['integral_setpoint'])
+
+        if control > 1:
+            control = 1.
+        elif control < -1:
+            control = -1.
+
+        if control >= 0:
+            gas = control
+            brake = 0
+        if control < 0:
+            gas = 0
+            brake = -control
+
+        # ------set variables from previous value-----
+        state['prev_setpoint'] = set_point
+        state['integral_setpoint'] = state['integral_setpoint'] + set_point
+
+        return brake, gas, state

acc/acc/tests/maneuver.py

@@ -1,7 +1,19 @@
 from .plant import Plant
+from .visualize import Visualizer
 import numpy as np
 
 
+class CV:
+    MPH_TO_MS = 1.609 / 3.6
+    MS_TO_MPH = 3.6 / 1.609
+    KPH_TO_MS = 1. / 3.6
+    MS_TO_KPH = 3.6
+    MPH_TO_KPH = 1.609
+    KPH_TO_MPH = 1. / 1.609
+    KNOTS_TO_MS = 1 / 1.9438
+    MS_TO_KNOTS = 1.9438
+
+
 class Maneuver(object):
 
     def __init__(self, title, duration, **kwargs):
@@ -17,12 +29,12 @@ def __init__(self, title, duration, **kwargs):
         self.speed_lead_breakpoints = kwargs.get(
             "speed_lead_values", [0.0, duration])
 
-        self.cruise_button_presses = kwargs.get("cruise_button_presses", [])
+        self.cruise_speeds = kwargs.get("cruise_speeds", [])
 
         self.duration = duration
         self.title = title
 
-    def evaluate(self, control=None, verbosity=0, min_gap=5):
+    def evaluate(self, control=None, verbosity=0, gap=10, plot=False, animate=False):
         """runs the plant sim and returns (score, run_data)"""
         plant = Plant(
             lead_relevancy=self.lead_relevancy,
@@ -31,19 +43,40 @@ def evaluate(self, control=None, verbosity=0, min_gap=5):
             verbosity=verbosity,
         )
 
-        buttons_sorted = sorted(self.cruise_button_presses, key=lambda a: a[1])
-        current_button = 0
+        speeds_sorted = sorted(self.cruise_speeds, key=lambda a: a[1])
+        cruise_speed = 0
 
         brake = 0
         gas = 0
         steer_torque = 0
 
+        # dictionary used by the cruise control function to maintan state between invocations.
+        state = None
+
+        # 3 possible states(accelerating(1), not accelerating(0), braking(-1))
+        previous_state = 0
+        score = 0.
+        prev_accel = 0.
+        # TODO: calibrate this threshold to denote maximum discomfort allowed
+        score_threshold = 200.
+        # TODO: calibrate this constant for scaling rate of acceleration
+        accel_const = 1.
+
+        # Initialize the Visualizer. Set animate = False to only display the plots at the end of the maneuver,
+        # this will be faster than showing in real time with animate = True
+        # max_speed, max_accel, max_score set the maximum for the y-axis
+        # TODO: make this dynamic?
+
+        if verbosity >= 4:
+            vis = Visualizer(animate=animate, show=plot, max_speed=80, max_accel=10, max_score=20)
+
         while plant.current_time() < self.duration:
-            while buttons_sorted and plant.current_time() >= buttons_sorted[0][1]:
-                current_button = buttons_sorted[0][0]
-                buttons_sorted = buttons_sorted[1:]
+            while speeds_sorted and plant.current_time() >= speeds_sorted[0][1]:
+                # getting the current cruise speed
+                cruise_speed = speeds_sorted[0][0]
+                speeds_sorted = speeds_sorted[1:]
                 if verbosity > 1:
-                    print("current button changed to", current_button)
+                    print("current cruise speed changed to", cruise_speed)
 
             grade = np.interp(plant.current_time(),
                               self.grade_breakpoints, self.grade_values)
@@ -53,18 +86,64 @@ def evaluate(self, control=None, verbosity=0, min_gap=5):
             speed, acceleration, car_in_front, steer_torque = plant.step(brake=brake,
                                                                          gas=gas,
                                                                          v_lead=speed_lead,
-                                                                         cruise_buttons=current_button,
                                                                          grade=grade)
 
-            # If the car in front is less than min_gap away, give it the worst score
-            # and abort.
-            if car_in_front < min_gap:
-                return 0
+            # Assert the gap parameter is respected during all the maneuver.
+            assert car_in_front >= gap
+
+            brake, gas, state = control(speed=speed,
+                                        acceleration=acceleration,
+                                        car_in_front=car_in_front,
+                                        gap=gap,
+                                        cruise_speed=cruise_speed)
+
+            if gas > 0:
+                # accelerating
+                new_state = 1
+            elif brake > 0:
+                # braking
+                new_state = -1
+            else:
+                # not accelerating
+                new_state = 0
+
+            # getting the rate of change of acceleration
+            # TODO: add division by exact time, if relevent(did not delve deep into timekeeping)
+            rate_accel = acceleration - prev_accel
+            prev_accel = acceleration
+
+            # based on acceptable jerk values given in
+            # A SURVEY OF LONGITUDINAL ACCELERATION COMFORT STUDIES
+            # IN GROUND TRANSPORTATION VEHICLES by l. l. HOBEROCK
+            # not sure about the validity, real tests should proove this
+            # uncomment this with the acceptable values, this will affect pid consts
+            # assert -0.3 * 9.81 < rate_accel < 0.3 * 9.81
+
+            # The higher the value of score, worse the controller.
+            # multiplication with rate_accel scales the change based on the speed of change.
+            score += abs((new_state - previous_state) + rate_accel * accel_const)
+            previous_state = new_state
+
+            # this updates the plots with latest state
+
+            if verbosity >= 4:
+                vis.update_data(cur_time=plant.current_time(), speed=speed,
+                                acceleration=acceleration, gas_control=gas, brake_control=brake,
+                                car_in_front=car_in_front, steer_torque=steer_torque, score=score)
+
+        score /= self.duration
+        assert score <= score_threshold
+
+        # Assert the desired speed matches the actual speed at the end of the maneuver.
+        # only valid when lead distance is greater than distance to be maintained
+        if car_in_front >= 200:
+            # TODO: Make atol lower once we have a decent solution.
+            assert np.isclose(speed, cruise_speed, atol=2)
+
+        # TODO: if there is a car in front, assert the speed matches that car's speed at the end of the manuever.
 
-            brake, gas = control(speed, acceleration,
-                                 car_in_front, steer_torque)
+        # this cleans up the plots for this maneuver and pauses until user presses [Enter]
+        if verbosity >= 4 and plot:
+            vis.show_final_plots()
 
-        # TODO: Calculate score, for now it always returns 10.
-        # It should be 0 when the car crashes and higher if it doesn't.
-        score = 10
-        return score
+        return

acc/acc/tests/plant.py

@@ -49,14 +49,16 @@ def monitor_time(self):
 
 def car_plant(pos, speed, grade, gas, brake):
     # vehicle parameters
+    g = 9.81
     mass = 1700
     aero_cd = 0.3
-    force_peak = mass * 3.
-    force_brake_peak = -mass * 10.  # 1g
+    # force_peak is set to get maximum acceleration allowed of 0.4g
+    force_peak = mass * 1.24
+    # force_brake_peak is set to get maximum decceleration allowed -0.9g
+    force_brake_peak = -mass * 43
     power_peak = 100000   # 100kW
     speed_base = power_peak / force_peak
     rolling_res = 0.01
-    g = 9.81
     air_density = 1.225
     gas_to_peak_linear_slope = 3.33
     brake_to_peak_linear_slope = 0.2
@@ -98,7 +100,6 @@ def __init__(self, lead_relevancy=False, rate=100,
 
         self.angle_steer = 0.
         self.gear_choice = 0
-        self.speed, self.speed_prev = 0., 0.
 
         self.esp_disabled = 0
         self.main_on = 1
@@ -107,6 +108,7 @@ def __init__(self, lead_relevancy=False, rate=100,
         self.brake_pressed = 0
         self.distance, self.distance_prev = 0., 0.
         self.speed, self.speed_prev = speed, speed
+        self.acceleration_prev = None
         self.steer_error, self.brake_error, self.steer_not_allowed = 0, 0, 0
         self.gear_shifter = 4   # D gear
         self.pedal_gas = 0
@@ -119,21 +121,15 @@ def __init__(self, lead_relevancy=False, rate=100,
         self.lead_relevancy = lead_relevancy
 
         # lead car
-        self.distance_lead, self.distance_lead_prev = distance_lead, distance_lead
+        self.distance_lead_prev = distance_lead
 
         self.rk = Ratekeeper(rate, print_delay_threshold=100)
         self.ts = 1. / rate
 
-    def speed_sensor(self, speed):
-        if speed < 0.3:
-            return 0
-        else:
-            return speed
-
     def current_time(self):
         return float(self.rk.frame) / self.rate
 
-    def step(self, brake=0, gas=0, steer_torque=0, v_lead=0.0, cruise_buttons=None, grade=0.0):
+    def step(self, brake=0, gas=0, steer_torque=0, v_lead=0.0, grade=0.0):
         # dbc_f, sgs, ivs, msgs, cks_msgs, frqs = initialize_can_struct(self.civic, self.brake_only)
 
         distance_lead = self.distance_lead_prev + v_lead * self.ts
@@ -152,49 +148,53 @@ def step(self, brake=0, gas=0, steer_torque=0, v_lead=0.0, cruise_buttons=None,
 
         # *** radar model ***
         if self.lead_relevancy:
-            d_rel = np.maximum(0., self.distance_lead - distance)
+            d_rel = np.maximum(0., distance_lead - distance)
             v_rel = v_lead - speed
         else:
             d_rel = 200.
             v_rel = 0.
 
+        # set sensible value to acceleration for first iteration
+        if self.acceleration_prev is None:
+            self.acceleration_prev = acceleration
         # print at 5hz
-        if (self.rk.frame % (self.rate / 5)) == 0:
-            msg_tmpl = ("%6.2f m  %6.2f m/s  %6.2f m/s2   %.2f ang "
+        # if (self.rk.frame % (self.rate / 5)) == 0:
+        if True:
+            msg_tmpl = ("%6.2f m  %6.2f m/s  %6.2f m/s2   %.2f m/s3 "
                         "  gas: %.2f  brake: %.2f  steer: %5.2f "
                         "  lead_rel: %6.2f m  %6.2f m/s")
-            msg = msg_tmpl % (distance, speed, acceleration, self.angle_steer,
+            msg = msg_tmpl % (distance, speed, acceleration, acceleration - self.acceleration_prev,
                               gas, brake, steer_torque, d_rel, v_rel)
 
             if self.verbosity > 2:
                 print(msg)
 
         # ******** publish the car ********
-        vls = [self.speed_sensor(speed), self.speed_sensor(speed),
-               self.speed_sensor(speed), self.speed_sensor(speed),
-               self.angle_steer, 0, self.gear_choice, speed != 0,
-               0, 0, 0, 0,
-               self.v_cruise, not self.seatbelt, self.seatbelt, self.brake_pressed,
-               self.user_gas, cruise_buttons, self.esp_disabled, 0,
-               self.user_brake, self.steer_error, self.speed_sensor(
-                   speed), self.brake_error,
-               self.brake_error, self.gear_shifter, self.main_on, self.acc_status,
-               self.pedal_gas, self.cruise_setting,
-               # left_blinker, right_blinker, counter
-               0, 0, 0,
-               # interceptor_gas
-               0, 0]
-
-        # TODO: Use vls for something
-        assert vls is not None
+        # vls = [self.speed_sensor(speed), self.speed_sensor(speed),
+        #        self.speed_sensor(speed), self.speed_sensor(speed),
+        #        self.angle_steer, 0, self.gear_choice, speed != 0,
+        #        0, 0, 0, 0,
+        #        self.v_cruise, not self.seatbelt, self.seatbelt, self.brake_pressed,
+        #        self.user_gas, cruise_buttons, self.esp_disabled, 0,
+        #        self.user_brake, self.steer_error, self.speed_sensor(
+        #            speed), self.brake_error,
+        #        self.brake_error, self.gear_shifter, self.main_on, self.acc_status,
+        #        self.pedal_gas, self.cruise_setting,
+        #        # left_blinker, right_blinker, counter
+        #        0, 0, 0,
+        #        # interceptor_gas
+        #        0, 0]
+
+        # # TODO: Use vls for something
+        # assert vls is not None
 
         # ******** update prevs ********
         self.speed_prev = speed
         self.distance_prev = distance
         self.distance_lead_prev = distance_lead
+        self.acceleration_prev = acceleration
 
-        car_in_front = distance_lead - \
-            distance if self.lead_relevancy else 200.
+        car_in_front = distance_lead - distance if self.lead_relevancy else 200.
 
         self.rk.keep_time()
         return (speed, acceleration, car_in_front, steer_torque)