Finished ADRC and added Readme

Author: mmcza

README.md

@@ -0,0 +1,21 @@
+# TSwR Student - Laboratory
+
+Miniprojects from the course "Control Theory in Robotics" during my 1st semester of the Master's.
+
+## List of implemented algorithms:
+
+1. [Feedback Linearization Controller](/controllers/feedback_linearization.py)
+2. [Multi-model Adaptive Controller](/controllers/mma_controller.py)
+3. [PD Controller](/controllers/pd_controller.py)
+4. [Active Disturbance Rejection Controller](/controllers/adrc_joint_controller.py)
+5. [Active Disturbance Rejection Controller with Feedback Linearization](/controllers/adrc_flc_controller.py)
+
+## List of implemented modules:
+
+1. [Extended State Observer](/observers/eso.py)
+2. [Polynomial Trajectory Generator](/trajectory_generators/poly3.py)
+3. [Robot Model](/models/manipulator_model.py)
+
+# Final result:
+
+![Final result for sinusoidal trajectory](/pictures/TSwR.gif)

adrc.py

@@ -17,14 +17,14 @@
 traj_gen = Sinusoidal(np.array([0., 1.]), np.array([2., 2.]), np.array([0., 0.]))
 # traj_gen = Poly3(np.array([0., 0.]), np.array([pi/4, pi/6]), end)
 
-b_est_1 = None
-b_est_2 = None
-kp_est_1 = None
-kp_est_2 = None
-kd_est_1 = None
-kd_est_2 = None
-p1 = None
-p2 = None
+b_est_1 = 2
+b_est_2 = 10
+kp_est_1 = 100
+kp_est_2 = 20
+kd_est_1 = 50
+kd_est_2 = 10
+p1 = 100
+p2 = 100
 
 q0, qdot0, _ = traj_gen.generate(0.)
 q1_0 = np.array([q0[0], qdot0[0]])

adrflc.py

@@ -7,20 +7,20 @@
 from utils.simulation import simulate
 
 Tp = 0.001
-end = 5
+end = 30
 
 # traj_gen = ConstantTorque(np.array([0., 1.0])[:, np.newaxis])
 traj_gen = Sinusoidal(np.array([0., 1.]), np.array([2., 2.]), np.array([0., 0.]))
 # traj_gen = Poly3(np.array([0., 0.]), np.array([pi/4, pi/6]), end)
 
-b_est_1 = None
-b_est_2 = None
-kp_est_1 = None
-kp_est_2 = None
-kd_est_1 = None
-kd_est_2 = None
-p1 = None
-p2 = None
+b_est_1 = 2
+b_est_2 = 10
+kp_est_1 = 100
+kp_est_2 = 20
+kd_est_1 = 50
+kd_est_2 = 10
+p1 = 100
+p2 = 100
 
 q0, qdot0, _ = traj_gen.generate(0.)
 q1_0 = np.array([q0[0], qdot0[0]])

controllers/__pycache__/adrc_controller.cpython-310.pyc

@@ -12,7 +12,7 @@ def __init__(self, Tp, params):
     def calculate_control(self, x, q_d, q_d_dot, q_d_ddot):
         u = []
         for i, controller in enumerate(self.joint_controllers):
-            u.append(controller.calculate_control([x[i], x[i+2]], q_d[i], q_d_dot[i], q_d_ddot[i]))
+            u.append(controller.calculate_control(x, q_d[i], q_d_dot[i], q_d_ddot[i], i))
         u = np.array(u)[:, np.newaxis]
         return u
 

controllers/__pycache__/adrc_flc_controller.cpython-310.pyc

@@ -1,29 +1,49 @@
 import numpy as np
 
-from models.free_model import FreeModel
+#from models.free_model import FreeModel
 from observers.eso import ESO
 from .adrc_joint_controller import ADRCJointController
 from .controller import Controller
-from models.ideal_model import IdealModel
+#from models.ideal_model import IdealModel
+from models.manipulator_model import ManiuplatorModel
 
 
 class ADRFLController(Controller):
     def __init__(self, Tp, q0, Kp, Kd, p):
-        self.model = None
+        self.model = ManiuplatorModel(Tp)
         self.Kp = Kp
         self.Kd = Kd
-        self.L = None
-        W = None
-        A = None
-        B = None
-        self.eso = ESO(A, B, W, self.L, q0, Tp)
+        self.L = np.array([[3*p[0], 0], [0, 3*p[1]], [3*p[0]**2, 0], [0, 3*p[1]**2], [p[0]**3, 0], [0, p[1]**3]])
+        W = np.array([[1., 0., 0., 0., 0., 0.], [0., 1., 0., 0., 0., 0.]])
+        self.A = np.array([[0., 0., 1., 0., 0., 0.], [0., 0., 0., 1., 0., 0.], [0., 0., 0., 0., 1., 0.],
+                      [0., 0., 0., 0., 0., 1.], [0., 0., 0., 0., 0., 0.], [0., 0., 0., 0., 0., 0.]])
+        self.B = np.zeros((6, 2))
+        self.eso = ESO(self.A, self.B, W, self.L, q0, Tp)
         self.update_params(q0[:2], q0[2:])
 
     def update_params(self, q, q_dot):
         ### TODO Implement procedure to set eso.A and eso.B
-        self.eso.A = None
-        self.eso.B = None
+        x = np.concatenate([q, q_dot])
+        inv_M = np.linalg.inv(self.model.M(x.flatten()))
+        inv_M_C = inv_M @ self.model.C(x.flatten())
+        self.A[2:4, 2:4] = - inv_M_C
+        self.B[2:4, :] = inv_M
+        self.eso.A = self.A
+        self.eso.B = self.B
 
     def calculate_control(self, x, q_d, q_d_dot, q_d_ddot):
         ### TODO implement centralized ADRFLC
-        return NotImplementedError
+        z = self.eso.get_state()
+        z = z[:, np.newaxis]
+        v = q_d_ddot - self.Kp @ (x[:2] - q_d) - self.Kd @ (x[2:] - q_d_dot)
+        # print(v.shape)
+        # print(z[:4].shape)
+        # print(z[4:].shape)
+        # print(z[2:4].shape)
+        # print(x.shape)
+        #u = self.model.M(z[0:4]) @ (v[:, np.newaxis] - z[4:]) + self.model.C(z[0:4]) @ z[2:4]
+        u = self.model.M(z[0:4].flatten()) @ (v[:, np.newaxis] - z[4:]) + self.model.C(z[0:4].flatten()) @ z[2:4]
+        self.update_params(z[0:2], z[2:4])
+        self.eso.update(x[:2], u)
+        return u
+        #return NotImplementedError

controllers/__pycache__/adrc_joint_controller.cpython-310.pyc

@@ -1,24 +1,43 @@
 import numpy as np
 from observers.eso import ESO
 from .controller import Controller
+from models.manipulator_model import ManiuplatorModel
 
 
 class ADRCJointController(Controller):
     def __init__(self, b, kp, kd, p, q0, Tp):
         self.b = b
         self.kp = kp
         self.kd = kd
+        self.model = ManiuplatorModel(Tp)
 
-        A = None
-        B = None
-        L = None
-        W = None
+        A = np.array([[0., 1., 0.], [0., 0., 1.], [0., 0., 0.]])
+        B = np.array([[0.], [self.b], [0.]])
+        L = np.array([[3*p], [3*p**2], [p**3]])
+        W = np.array([1., 0., 0.])
         self.eso = ESO(A, B, W, L, q0, Tp)
+        self.prev_u = 0.
 
     def set_b(self, b):
         ### TODO update self.b and B in ESO
-        return NotImplementedError
+        self.b = b
+        B = np.array([[0.], [self.b], [0.]])
+        self.eso.set_B(B)
 
-    def calculate_control(self, x, q_d, q_d_dot, q_d_ddot):
+    def calculate_control(self, x, q_d, q_d_dot, q_d_ddot, i):
         ### TODO implement ADRC
-        return NotImplementedError
+
+        u_arr = np.array([self.prev_u])
+        self.eso.update(x[i], u_arr[:, np.newaxis])
+        z = self.eso.get_state()
+        print("controller")
+        print(z)
+        q, q_dot, f = z
+        v = q_d_ddot + self.kd * (q_d_dot - q_dot) + self.kp * (q_d - q)
+        u =  (v - f) / self.b
+        self.prev_u = u
+
+        B = np.linalg.inv(self.model.M(x))
+        self.set_b(B[i, i])
+
+        return u

controllers/__pycache__/feedback_linearization_controller.cpython-310.pyc

@@ -9,5 +9,5 @@ def __init__(self, kp, kd):
 
     def calculate_control(self, q, q_dot, q_d, q_d_dot, q_d_ddot):
         ### TODO: Please implement me
-        u = None
+        u = self.kp * (q_d - q) + self.kd * (q_d_dot - q_dot)
         return u

controllers/adrc_controller.py

@@ -10,7 +10,7 @@
 
 Tp = 0.01
 start = 0
-end = 3
+end = 30
 
 """
 Switch to FeedbackLinearizationController as soon as you implement it
@@ -23,8 +23,8 @@
 At the end implement Point2point trajectory generator to move your manipulator to some desired state.
 """
 #traj_gen = ConstantTorque(np.array([0., 1.0])[:, np.newaxis])
-# traj_gen = Sinusoidal(np.array([0., 1.]), np.array([2., 2.]), np.array([0., 0.]))
-traj_gen = Poly3(np.array([0., 0.]), np.array([np.pi/4, np.pi/6]), end)
+traj_gen = Sinusoidal(np.array([0., 1.]), np.array([2., 2.]), np.array([0., 0.]))
+# traj_gen = Poly3(np.array([0., 0.]), np.array([np.pi/4, np.pi/6]), end)
 
 
 Q, Q_d, u, T = simulate("PYBULLET", traj_gen, controller, Tp, end)

controllers/adrc_flc_controller.py

@@ -21,7 +21,7 @@ def __init__(self, Tp, m3=0.1, r3=0.05):
         self.gamma = self.m2*(self.l2/2)**2 + self.I_2 + self.m3 * self.l2**2 + self.I_3
 
         self.Kp = np.array([[15, 0], [0, 15]])
-        self.Kd = np.array([[30, 0], [0, 10]])
+        self.Kd = np.array([[10, 0], [0, 10]])
 
     def M(self, x):
         """

controllers/adrc_joint_controller.py

@@ -18,6 +18,9 @@ def set_B(self, B):
     def update(self, q, u):
         self.states.append(copy(self.state))
         ### TODO implement ESO update
+        z_hat = self.state[:, np.newaxis]
+        z_hat_dot = self.A @ z_hat + self.B @ u + np.array(self.L @ (q.reshape(-1, 1) - self.W @ z_hat))
+        self.state = (z_hat + z_hat_dot * self.Tp).flatten()
 
     def get_state(self):
         return self.state