PID

ros_ws/low_level/low_level/PID.py

+# coding: utf-8
+
+"""
+PID class.
+
+It computes the command given the gains and the estimated derivate and integral
+of the error.
+
+The equations of the cart pole example are taken from
+"Correct equations for the dynamics of the cart-pole system" R.V. Florian(2005)
+https://coneural.org/florian/papers/05_cart_pole.pdf
+"""
+
+import datetime
+import numpy as np
+
+class PID:
+
+    def __init__(self, gains):
+        '''
+        Builds a PID
+
+        Arguments:
+            gains (dict): with keys Kp, Kd, Ki
+        '''
+        self.gains = gains.copy()
+        self.errors = {'error': 0.,
+                       'd_error': 0.,
+                       'i_error': 0.,
+                       'time': None
+                      }
+        if not ('Kp' in gains and 'Kd' in gains and 'Ki' in gains):
+            raise RuntimeError('''Some keys are missing in the gains, '''
+                               '''we expect Kp, Kd and Ki''')
+
+
+    def reset(self):
+        '''
+        Reset the errors of the PID
+        '''
+        self.errors = {'error': 0.,
+                       'd_error': 0.,
+                       'i_error': 0.,
+                       'time': None
+                      }
+
+    def update(self,
+               time: datetime.datetime,
+               error: float):
+        '''
+        Update the error, its derivative and integral
+        '''
+        prev_error = self.errors['error']
+        prev_time = self.errors['time']
+        if prev_time is None:
+            self.errors['error'], self.errors['time'] = error, time
+            return
+
+        self.errors['error'] = error
+        self.errors['time'] = time
+        dt = (time - prev_time).total_seconds()
+        self.errors['i_error'] += dt * (prev_error + error)/2.0
+        self.errors['d_error'] = (error - prev_error)/dt
+
+    @property
+    def command(self):
+        return self.gains['Kp'] * self.errors['error'] + \
+                self.gains['Kd'] * self.errors['d_error'] + \
+                self.gains['Ki'] * self.errors['i_error']
+
+
+if __name__ == '__main__':
+    
+    import matplotlib.pyplot as plt
+
+    # Illustration of the PID on the inverted pendulum
+    dt = 0.02  # s.
+    g = 9.81  # kg/s^-2
+    l = 0.5   # m.
+    mc = 1.0  # kg
+    mp = 0.1  # kg
+    mT = mc + mp
+    muc = 5*1e-4
+    mup = 2*1e-6
+
+    # The equations for the dynamics of the system
+    def f_Nc(theta, dtheta, ddtheta):
+        ct, st = np.cos(theta), np.sin(theta)
+        return mT*g-mp*l*(ddtheta*st+dtheta**2*ct)
+
+    def f_ddtheta(F, theta, dtheta, Nc, dx):
+        ct, st = np.cos(theta), np.sin(theta)
+        return (g*st+ct*((-F-mp*l*dtheta**2*(st+muc*np.sign(Nc*dx)*ct))/mT+muc*g*np.sign(Nc*dx))-mup*dtheta/(mp*l))/(l*(4./3.-mp*ct/mT*(ct-muc*np.sign(Nc*dx))))
+
+    def f_ddx(F, theta, dtheta, ddtheta, Nc, x, dx):
+        ct, st = np.cos(theta), np.sin(theta)
+        return (F+mp*l*(dtheta**2*st-ddtheta*ct)-muc*Nc*np.sign(Nc*dx))/mT
+
+    def f_dX(F, X, ddtheta):
+        """
+        X is the state [theta, dtheta, x, dx]
+        """
+        theta, dtheta, x, dx = X
+        Nc = f_Nc(theta, dtheta, ddtheta)
+        ddtheta = f_ddtheta(F, theta, dtheta, Nc, dx)
+        Nc_2 = f_Nc(theta, dtheta, ddtheta)
+        if np.sign(Nc_2) != np.sign(Nc):
+            Nc = Nc_2
+            ddtheta = f_ddtheta(F, theta, dtheta, Nc, dx)
+
+        ddx = f_ddx(F, theta, dtheta, ddtheta, Nc, x, dx)
+
+        return np.array([
+            dtheta,
+            ddtheta,
+            dx,
+            ddx
+        ])
+
+    # Instantiate our PID with hand tuned gains
+    gains = {'Kp': 50, 'Kd': 10, 'Ki': 50.0}
+    pid = PID(gains)
+    # Define the target for the angle of the pole
+    target_theta = -0.6
+
+    # Define the initial state
+    X = np.array([0.1, 0.0, 1, 0])
+    ddtheta = 0
+    F = 0
+    t = datetime.datetime.now()
+    # We will simulate for 5 sec
+    tmax = t + datetime.timedelta(seconds=5.0)
+    dt = datetime.timedelta(milliseconds=dt*1000)
+    history = np.append(X, [F, t]).reshape((1, 6))
+    
+    # Let us go for the simulation
+    while t < tmax:
+
+        # Update the PID
+        theta = X[0]
+        pid.update(t, theta - target_theta)
+
+        # Get the force to apply
+        F = pid.command
+
+        # Integrate the dynamics with Euler
+        dX = f_dX(F, X, ddtheta)
+        X += dt.total_seconds() * dX
+        ddtheta = dX[1]
+        history = np.vstack([history, np.append(X, [F, t])])
+        t += dt
+
+    # Plot the results
+    titles = [r'$\theta$', r'$\dot{\theta}$', r'$x$', r'$\dot{x}$', r'$F$']
+    fig, axes = plt.subplots(1, 5, figsize=(20, 5))
+    for i in range(5):
+        ax = axes[i]
+        ax.plot(history[:, 5], history[:, i])
+        ax.set_title(titles[i])
+        plt.gcf().autofmt_xdate()
+    # On the first plot, plot the target
+    ax = axes[0]
+    ax.axhline(y=target_theta, linestyle='--', color='tab:red')
+
+    plt.show()

ros_ws/low_level/low_level/speed_controller.py

@@ -6,6 +6,7 @@ from geometry_msgs.msg import Twist
 from std_msgs.msg import Float32, Bool
 from nav_msgs.msg import Odometry
 from threading import Lock
+import PID
 
 class SpeedController(Node):
     def __init__(self):
@@ -15,6 +16,7 @@ class SpeedController(Node):
         self.target_twist = Twist()  # Stores target velocity
         self.lock = Lock()           # Mutex lock to protect target_twist
         self.hover_mode = False      # Hover mode
+        self.odom_velocity = Twist()
 
         # Create publishers for image updates
         self.cmd_vel_pub = self.create_publisher(Twist, '/bebop/cmd_vel', 10)
@@ -36,6 +38,10 @@ class SpeedController(Node):
 
         # Set up control loop
         self.timer = self.create_timer(0.1, self.control_loop)  # Control frequency is 10Hz
+        self.t0 = self.get_clock().now()
+
+        self.pid_controller_x = PID(Kp=0.0, Ki=0.0, Kd=0.00, setpoint=0.0)
+        self.pid_controller_y = PID(Kp=0.0, Ki=0.0, Kd=0.00, setpoint=0.0)
 
     def reset_target_twist(self):
         # Reset target velocity
@@ -44,6 +50,7 @@ class SpeedController(Node):
 
     def odom_callback(self, msg):
         # Handle /odom topic callback
+        self.odom_velocity = msg.twist.twist.linear
         pass  # Actual velocity data acquisition and processing
 
     def linear_x_callback(self, msg):
@@ -101,7 +108,28 @@ class SpeedController(Node):
                 cmd_vel_msg.linear.z = max(min(self.target_twist.linear.z, 1.0), -1.0)
                 cmd_vel_msg.angular.z = max(min(self.target_twist.angular.z, 1.0), -1.0)
 
+                # Ubdate PID target vel
+                self.pid_controller_x.setpoint = cmd_vel_msg.linear.x
+                self.pid_controller_y.setpoint = cmd_vel_msg.linear.y
+
+                # Calcul PID error
+                error_x = cmd_vel_msg.linear.x - self.odom.velocity.x
+                error_y = cmd_vel_msg.linear.y - self.odom.velocity.y
+
+                #Update PID model
+
+                t1 = self.get_clock().now()
+                duration_in_seconds = (t1 - self.t0).nanosecounds*1e-9
+                self.t0 = t1
+                
+                
+                self.pid_controller_x.update(duration_in_seconds, error_x)
+                self.pid_controller_y.update(duration_in_seconds, error_y)
+
                 # Publish to bebop/cmd_vel and target_vel topics
+                cmd_vel_msg.linear.x = self.pid_controller_x.command
+                cmd_vel_msg.linear.y = self.pid_controller_y.command
+
                 self.cmd_vel_pub.publish(cmd_vel_msg)
                 self.target_vel_pub.publish(cmd_vel_msg)