Simulator: Bicycle model + Discrete-time PID controller in Python

task1.py

from scipy.integrate import solve_ivp
import numpy as np


# Define the system dynamics as a function of the
# form f(t, z)
def dynamics(_t, z):
    return np.sin(z)


t_final = 10
z_init = 0.7
num_points = 100  # resolution

# Simulate the dynamical system
sol = solve_ivp(dynamics,
                [0, t_final],
                [z_init],
                t_eval=np.linspace(0, t_final, num_points))

# solve_ivp returns an object of type \texttt{OdeResult}; the solution
# (t, x(t)) is stored in `sol.t` and `sol.y`. Both are numpy arrays.
# Print `sol` to inspect it and run: print(sol.y.shape)
print(sol)
print(sol.t)
print(sol.y)
print(sol.y.shape)

task2.py

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


# Define the system dynamics as a function of the
# form f(t, z)
def dynamics(_t, z):
    return np.sin(z)


t_final = 10
z_init = 0.7
num_points = 100  # resolution

# Simulate the dynamical system
sol = solve_ivp(dynamics,
                [0, t_final],
                [z_init],
                t_eval=np.linspace(0, t_final, num_points))

plt.plot(sol.t, sol.y.T)
plt.show()

task3.py

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


# Define the system dynamics as a function of the
# form f(t, z)
def dynamics(_t, z):
    x = z[0]
    y = z[1]
    return [-y**2, x*y]


t_final = 40
z_init = [1, -3]
num_points = 1000  # resolution

# Simulate the dynamical system
sol = solve_ivp(dynamics,
                [0, t_final],
                z_init,
                t_eval=np.linspace(0, t_final, num_points))

plt.plot(sol.t, sol.y.T)
plt.show()

task4.py

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=0.):
        """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


pid = PidController(10, 0.1, 0.5, 0.01)
u = pid.control(0.3)
u = pid.control(0.15)
print(u)

task5.py

import numpy as np
from scipy.integrate import solve_ivp


class Car:

    def __init__(self,
                 length=0.9,
                 velocity=1,
                 x_pos_init=0, y_pos_init=0, pose_init=0):
        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 x(self):
        return self.__x
    
    def y(self):
        return self.__y

    def pose(self):
        return self.__pose

car = Car(velocity=7)
car.move(0.03, 1)

# get the new coordinates:
x = car.x()
y = car.y()
pose = car.pose()

task6.py

u = 3. * np.pi / 180.
car = Car(velocity=7)
car.move(u, 0.1)
new_state = car.state()
print(new_state)

# Prints:
# [0.6998061734407899, 0.01426458691293885, 0.04076160610903204]

task7.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=0.):
        """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=0.9,
                 velocity=1,
                 x_pos_init=0, y_pos_init=0, pose_init=0):
        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


t_sampling = 0.05
car = Car(y_pos_init=0.5, velocity=7)
pid = PidController(kp=0.5, kd=0.05, ki=0.1, ts=t_sampling)

n_sim_points = 200
y_cache = np.array([car.y()], 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())

t_span = t_sampling * np.arange(n_sim_points+1)
plt.plot(t_span, y_cache)
plt.grid()
plt.show()