Revise L07

Author: SebastianZug

07_Regelungstechnik/07_Regelungstechnik.md

@@ -0,0 +1,84 @@
+# https://www.csestack.org/control-systems-simulation-python-example/
+import math
+import matplotlib.pyplot as plt
+import numpy as np
+
+steps = 1000
+
+class SecondOrderSystem:
+  def __init__(self, d1, d2):
+    if d1 > 0:
+      e1 = -1.0 / d1
+      x1 = math.exp(e1)
+    else: x1 = 0
+    if d2 > 0:
+      e2 = -1.0 / d2
+      x2 = math.exp(e2)
+    else: x2 = 0
+    a = 1.0 - x1    # b = x1
+    c = 1.0 - x2    # d = x2
+    self.ac = a * c
+    self.bpd = x1 + x2
+    self.bd = x1 * x2
+    self.init_system()
+
+  def init_system(self):
+    self.Yppr = 0
+    self.Ypr = 0
+
+  def __call__(self, X):
+    Y = self.ac * X + self.bpd * self.Ypr - self.bd * self.Yppr
+    self.Yppr = self.Ypr
+    self.Ypr = Y
+    return Y
+
+class PIDControlBlock:
+  def __init__(self, Kp, Ki, Kd):
+    self.Kp = Kp
+    self.Ki = Ki
+    self.Kd = Kd
+    self.Epr = 0
+    self.Eppr = 0
+    self.Epppr = 0
+    self.Sum = 0
+
+  def __call__(self, E):
+    self.Sum += 0.5 * self.Ki * (E + self.Epr)      # where T ~1
+    U = self.Kp * E + self.Sum + 0.1667 * self.Kd * (E - self.Epppr + 3.0 * (self.Epr - self.Eppr))
+    self.Epppr = self.Eppr
+    self.Eppr = self.Epr
+    self.Epr = E
+    return U
+
+class ClosedLoopSystem:
+  def __init__(self, controller, plant) :
+    self.P = plant
+    self.C = controller
+    self.Ypr = 0
+
+  def __call__(self, X):
+    E = X - self.Ypr
+    U = self.C(E)
+    Y = self.P(U)
+    self.Ypr = Y
+    return Y
+
+Plant = SecondOrderSystem(250, 100)     
+#Pid = PIDControlBlock(5, 0.0143, 356.25)
+Pid = PIDControlBlock(2, 0, 0)
+Ctrl = ClosedLoopSystem(Pid, Plant)
+t = np.arange(0, 1001)
+setpoints = np.zeros(len(t))
+setpoints[np.where(t > 50)]= 100
+y = np.arange(0, 1001, dtype = float)
+for index, value in enumerate(setpoints):
+    y[index]=Ctrl(value)
+fig, ax = plt.subplots()
+ax.plot(t, setpoints, label = "Setpoint")
+ax.plot(t, y, label = "Controled speed level")
+plt.xlabel("Time")
+plt.ylabel("Speed")
+plt.legend()
+plt.grid()
+plt.show()
+#plt.savefig('foo.png')

07_Regelungstechnik/code/control.py

@@ -0,0 +1,51 @@
+# https://www.csestack.org/control-systems-simulation-python-example/
+import math
+import matplotlib.pyplot as plt
+import numpy as np
+
+steps = 2000
+
+class SecondOrderSystem:
+  def __init__(self, d1, d2):
+    if d1 > 0:
+      e1 = -1.0 / d1
+      x1 = math.exp(e1)
+    else: x1 = 0
+    if d2 > 0:
+      e2 = -1.0 / d2
+      x2 = math.exp(e2)
+    else: x2 = 0
+    a = 1.0 - x1    # b = x1
+    c = 1.0 - x2    # d = x2
+    self.ac = a * c
+    self.bpd = x1 + x2
+    self.bd = x1 * x2
+    self.init_system()
+
+  def init_system(self):
+    self.Yppr = 0
+    self.Ypr = 0
+
+  def __call__(self, X):
+    Y = self.ac * X + self.bpd * self.Ypr - self.bd * self.Yppr
+    self.Yppr = self.Ypr
+    self.Ypr = Y
+    return Y
+
+fig, ax = plt.subplots()
+
+Plant = SecondOrderSystem(250, 100)     
+t = np.arange(0, steps)
+y = np.arange(0, steps, dtype = float)
+for speed in [10, 100, 150, 200]:
+  for index, value in enumerate(t):
+      y[index]=Plant(speed)
+  ax.plot(t, y, label = f"Voltage level '{speed}'")
+  Plant.init_system()
+
+plt.xlabel("Time")
+plt.ylabel("Speed")
+plt.legend()
+plt.grid()  
+#plt.show()
+plt.savefig('foo.png')

07_Regelungstechnik/code/main.py

@@ -0,0 +1,210 @@
+"""
+Move to specified pose
+Author: Daniel Ingram (daniel-s-ingram)
+        Atsushi Sakai (@Atsushi_twi)
+        Seied Muhammad Yazdian (@Muhammad-Yazdian)
+P. I. Corke, "Robotics, Vision & Control", Springer 2017, ISBN 978-3-319-54413-7
+"""
+
+import matplotlib.pyplot as plt
+import numpy as np
+from random import random
+
+
+class PathFinderController:
+    """
+    Constructs an instantiate of the PathFinderController for navigating a
+    3-DOF wheeled robot on a 2D plane
+    Parameters
+    ----------
+    Kp_rho : The linear velocity gain to translate the robot along a line
+             towards the goal
+    Kp_alpha : The angular velocity gain to rotate the robot towards the goal
+    Kp_beta : The offset angular velocity gain accounting for smooth merging to
+              the goal angle (i.e., it helps the robot heading to be parallel
+              to the target angle.)
+    """
+
+    def __init__(self, Kp_rho, Kp_alpha, Kp_beta):
+        self.Kp_rho = Kp_rho
+        self.Kp_alpha = Kp_alpha
+        self.Kp_beta = Kp_beta
+
+    def calc_control_command(self, x_diff, y_diff, theta, theta_goal):
+        """
+        Returns the control command for the linear and angular velocities as
+        well as the distance to goal
+        Parameters
+        ----------
+        x_diff : The position of target with respect to current robot position
+                 in x direction
+        y_diff : The position of target with respect to current robot position
+                 in y direction
+        theta : The current heading angle of robot with respect to x axis
+        theta_goal: The target angle of robot with respect to x axis
+        Returns
+        -------
+        rho : The distance between the robot and the goal position
+        v : Command linear velocity
+        w : Command angular velocity
+        """
+
+        # Description of local variables:
+        # - alpha is the angle to the goal relative to the heading of the robot
+        # - beta is the angle between the robot's position and the goal
+        #   position plus the goal angle
+        # - Kp_rho*rho and Kp_alpha*alpha drive the robot along a line towards
+        #   the goal
+        # - Kp_beta*beta rotates the line so that it is parallel to the goal
+        #   angle
+        #
+        # Note:
+        # we restrict alpha and beta (angle differences) to the range
+        # [-pi, pi] to prevent unstable behavior e.g. difference going
+        # from 0 rad to 2*pi rad with slight turn
+
+        rho = np.hypot(x_diff, y_diff)
+        alpha = (np.arctan2(y_diff, x_diff)
+                 - theta + np.pi) % (2 * np.pi) - np.pi
+        beta = (theta_goal - theta - alpha + np.pi) % (2 * np.pi) - np.pi
+        v = self.Kp_rho * rho
+        w = self.Kp_alpha * alpha - self.Kp_beta * beta
+
+        if alpha > np.pi / 2 or alpha < -np.pi / 2:
+            v = -v
+
+        return rho, v, w
+
+
+def move_to_pose(controller, x_start, y_start, theta_start, x_goal, y_goal, theta_goal, trajectory_color):
+    x = x_start
+    y = y_start
+    theta = theta_start
+
+    x_diff = x_goal - x
+    y_diff = y_goal - y
+
+    x_traj, y_traj = [], []
+
+    rho = np.hypot(x_diff, y_diff)
+    while rho > 0.003 or (abs(theta - theta_goal) > 0.01):
+        x_traj.append(x)
+        y_traj.append(y)
+
+        x_diff = x_goal - x
+        y_diff = y_goal - y
+
+        rho, v, w = controller.calc_control_command(
+            x_diff, y_diff, theta, theta_goal)
+
+        if abs(v) > MAX_LINEAR_SPEED:
+            v = np.sign(v) * MAX_LINEAR_SPEED
+
+        if abs(w) > MAX_ANGULAR_SPEED:
+            w = np.sign(w) * MAX_ANGULAR_SPEED
+
+        theta = theta + w * dt
+        x = x + v * np.cos(theta) * dt
+        y = y + v * np.sin(theta) * dt
+
+        if show_animation:  # pragma: no cover
+            plt.cla()
+            plot_scene(x_start, y_start, theta_start,
+                       x_goal, y_goal, theta_goal,
+                       x, y, theta, 
+                       x_traj, y_traj,
+                       trajectory_color)
+
+    plot_scene(x_start, y_start, theta_start,
+               x_goal, y_goal, theta_goal,
+               x, y, theta, 
+               x_traj, y_traj,
+               trajectory_color)
+
+def plot_scene(x_start, y_start, theta_start,
+                       x_goal, y_goal, theta_goal,
+                       x, y, theta, 
+                       x_traj, y_traj,
+                       trajectory_color):
+    plt.text(10, 10, "diff_x = {:>2.2f}\ndiff_y = {:>2.2f}\ndiff_t = {:>2.2f}".
+        format(abs(x_goal - x), 
+               abs(y_goal - y), 
+               abs(theta_goal- theta)
+               ))
+    plt.arrow(x_start, y_start, np.cos(theta_start),
+                  np.sin(theta_start), color='r', width=0.1)
+    plt.arrow(x_goal, y_goal, np.cos(theta_goal),
+                  np.sin(theta_goal), color='g', width=0.1)
+    plot_vehicle(x, y, theta, x_traj, y_traj, trajectory_color)
+
+
+def plot_vehicle(x, y, theta, x_traj, y_traj, trajectory_color):  # pragma: no cover
+    # Corners of triangular vehicle when pointing to the right (0 radians)
+    p1_i = np.array([0.5, 0, 1]).T
+    p2_i = np.array([-0.5, 0.25, 1]).T
+    p3_i = np.array([-0.5, -0.25, 1]).T
+
+    T = transformation_matrix(x, y, theta)
+    p1 = np.matmul(T, p1_i)
+    p2 = np.matmul(T, p2_i)
+    p3 = np.matmul(T, p3_i)
+
+    plt.plot([p1[0], p2[0]], [p1[1], p2[1]], 'k-')
+    plt.plot([p2[0], p3[0]], [p2[1], p3[1]], 'k-')
+    plt.plot([p3[0], p1[0]], [p3[1], p1[1]], 'k-')
+
+    plt.plot(x_traj, y_traj, trajectory_color)
+
+    # for stopping simulation with the esc key.
+    plt.gcf().canvas.mpl_connect(
+        'key_release_event',
+        lambda event: [exit(0) if event.key == 'escape' else None])
+
+    plt.xlim(0, 20)
+    plt.ylim(0, 20)
+
+    plt.pause(dt)
+
+
+def transformation_matrix(x, y, theta):
+    return np.array([
+        [np.cos(theta), -np.sin(theta), x],
+        [np.sin(theta), np.cos(theta), y],
+        [0, 0, 1]
+    ])
+
+# Robot specifications
+MAX_LINEAR_SPEED = 15
+MAX_ANGULAR_SPEED = 8
+
+# Animated or static visualisation
+show_animation = False
+dt = 0.005
+
+def main():
+  x_start = 3.25
+  y_start = 16.0
+  theta_start = 0.38
+  x_goal = 15
+  y_goal = 2.5
+  theta_goal = -1.2
+
+  print("Initial x: %.2f m\nInitial y: %.2f m\nInitial theta: %.2f rad\n" %
+        (x_start, y_start, theta_start))
+  print("Goal x: %.2f m\nGoal y: %.2f m\nGoal theta: %.2f rad\n" %
+        (x_goal, y_goal, theta_goal))
+
+  print("Press Esc to stop.")
+
+  controller_A = PathFinderController(5, 15, 3)
+  move_to_pose(controller_A, x_start, y_start, theta_start, x_goal, y_goal, theta_goal, '--b')
+
+  controller_B = PathFinderController(5, 3, 3)
+  move_to_pose(controller_B, x_start, y_start, theta_start, x_goal, y_goal, theta_goal, '--r')
+
+  #plt.savefig('move_to_pose.png')
+  plt.show()
+  print("Aus die Maus")
+
+if __name__ == '__main__':
+    main()