(Q1.1) Plot the trajectory of the vehicle on the X-Y plane

LanekeepingQ1part2.py

import numpy as np
from scipy.integrate import solve_ivp
import matplotlib.pyplot as plt


class PidController:
    """PidController
    Documentation goes here
    """

    def __init__(self, kp, ki, kd, ts):
        """Documentation goes here

        :param kp: proportional gain
        :param ki: integral gain
        :param kd:
        :param ts:
        """
        self.__kp = kp
        self.__kd = kd / ts  # discrete-time Kd
        self.__ki = ki * ts
        self.__previous_error = None
        self.__error_sum = 0.

    def control(self, y, set_point=2.3):
        """Documentation goes here

        :param y:
        :param set_point:
        :return:
        """
        error = set_point - y  # compute the control error
        steering_action = self.__kp * error  # P controller

        # D component:
        if self.__previous_error is not None:
            error_diff = error - self.__previous_error
            steering_action += self.__kd * error_diff

        # I component:
        # TODO: Do this as an exercise. Introduce the I component
        # here (don't forget to update the sum of errors).

        self.__previous_error = error
        return steering_action

class Car:

    def __init__(self,
                 length=2.3,
                 velocity=1,
                 x_pos_init=0, y_pos_init=0, pose_init=(5*(np.pi)/(180))):
        self.__length = length
        self.__velocity = velocity
        self.__x = x_pos_init
        self.__y = y_pos_init
        self.__pose = pose_init

    def move(self, steering_angle, dt):
        # This method computes the position and orientation (pose)
        # of the car after time `dt` starting from its current
        # position and orientation by solving an IVP

        def bicycle_model(_t, z):
            x = z[0]
            y = z[1]
            theta = z[2]
            return [self.__velocity * np.cos(theta),
                    self.__velocity * np.sin(theta),
                    self.__velocity * np.tan(steering_angle)
                    / self.__length]

        sol = solve_ivp(bicycle_model,
                        [0, dt],
                        [self.__x, self.__y, self.__pose])

        self.__x = sol.y[0, -1]
        self.__y = sol.y[1, -1]
        self.__pose = sol.y[2, -1]

    def y(self):
        return self.__y
    def x(self):
        return self.__x
    def pose(self):
        return self.__pose
t_sampling = 0.01
car = Car(y_pos_init=0.3, velocity=5)
pid = PidController(kp=-(np.pi)/180, kd=0.00, ki=0.0, ts=t_sampling)

n_sim_points = 200
y_cache = np.array([car.y()], dtype=float)
x_cache = np.array([car.x()], dtype=float)
pose_cache = np.array([car.pose()], dtype=float)
for i in range(n_sim_points):
    u = pid.control(car.y())
    car.move(u, t_sampling)
    y_cache = np.append(y_cache, car.y())
    x_cache = np.append(x_cache, car.x())
    pose_cache = np.append(pose_cache, car.pose())

t_span = t_sampling * np.arange(n_sim_points + 1)
plt.plot(x_cache,y_cache)
plt.grid()
plt.xlabel('X(t) (m)')
plt.ylabel('Y(t) (m)')
plt.show()