PyDiffGame 0.1.0__py3-none-any.whl → 0.1.2__py3-none-any.whl

PyDiffGame/examples/MassesWithSpringsComparison.py

@@ -0,0 +1,218 @@
+import numpy as np
+from typing import Sequence, Optional, Union
+
+from PyDiffGame.PyDiffGame import PyDiffGame
+from PyDiffGame.PyDiffGameLQRComparison import PyDiffGameLQRComparison
+from PyDiffGame.Objective import GameObjective, LQRObjective
+
+
+class MassesWithSpringsComparison(PyDiffGameLQRComparison):
+    def __init__(self,
+                 N: int,
+                 m: float,
+                 k: float,
+                 q: Union[float, Sequence[float], Sequence[Sequence[float]]],
+                 r: float,
+                 Ms: Optional[Sequence[np.array]] = None,
+                 x_0: Optional[np.array] = None,
+                 x_T: Optional[np.array] = None,
+                 T_f: Optional[float] = None,
+                 epsilon_x: Optional[float] = PyDiffGame.epsilon_x_default,
+                 epsilon_P: Optional[float] = PyDiffGame.epsilon_P_default,
+                 L: Optional[int] = PyDiffGame.L_default,
+                 eta: Optional[int] = PyDiffGame.eta_default):
+        I_N = np.eye(N)
+        Z_N = np.zeros((N, N))
+
+        M_masses = m * I_N
+        K = k * (2 * I_N - np.array([[int(abs(i - j) == 1) for j in range(N)] for i in range(N)]))
+        M_masses_inv = np.linalg.inv(M_masses)
+
+        M_inv_K = M_masses_inv @ K
+
+        if Ms is None:
+            eigenvectors = np.linalg.eig(M_inv_K)[1]
+            Ms = [eigenvector.reshape(1, N) for eigenvector in eigenvectors]
+
+        A = np.block([[Z_N, I_N],
+                      [-M_inv_K, Z_N]])
+        B = np.block([[Z_N],
+                      [M_masses_inv]])
+
+        Qs = [np.diag([0.0] * i + [q] + [0.0] * (N - 1) + [q] + [0.0] * (N - i - 1))
+              if isinstance(q, (int, float)) else
+              np.diag([0.0] * i + [q[i]] + [0.0] * (N - 1) + [q[i]] + [0.0] * (N - i - 1)) for i in range(N)]
+
+        M = np.concatenate(Ms,
+                           axis=0)
+
+        assert np.all(np.abs(np.linalg.inv(M) - M.T) < 10e-12)
+
+        Q_mat = np.kron(a=np.eye(2),
+                        b=M)
+
+        Qs = [Q_mat.T @ Q @ Q_mat for Q in Qs]
+
+        Rs = [np.array([r])] * N
+        R_lqr = 1 / 4 * r * I_N
+        Q_lqr = q * np.eye(2 * N) if isinstance(q, (int, float)) else np.diag(2 * q)
+
+        state_variables_names = ['x_{' + str(i) + '}' for i in range(1, N + 1)] + \
+                                ['\\dot{x}_{' + str(i) + '}' for i in range(1, N + 1)]
+        args = {'A': A,
+                'B': B,
+                'x_0': x_0,
+                'x_T': x_T,
+                'T_f': T_f,
+                'state_variables_names': state_variables_names,
+                'epsilon_x': epsilon_x,
+                'epsilon_P': epsilon_P,
+                'L': L,
+                'eta': eta}
+
+        lqr_objective = [LQRObjective(Q=Q_lqr,
+                                      R_ii=R_lqr)]
+        game_objectives = [GameObjective(Q=Q,
+                                         R_ii=R,
+                                         M_i=M_i) for Q, R, M_i in zip(Qs, Rs, Ms)]
+        games_objectives = [lqr_objective,
+                            game_objectives]
+
+        self.figure_filename_generator = lambda g: ('LQR' if g.is_LQR() else f'{N}-players') + \
+                                                                             f'_large_{1 if q[0] > q[1] else 2}'
+
+        super().__init__(args=args,
+                         M=M,
+                         games_objectives=games_objectives,
+                         continuous=True)
+
+    @staticmethod
+    def are_all_orthogonal(vectors: Sequence[np.array]) -> bool:
+        """
+        Check if a set of vectors are all orthogonal to each other.
+
+        Parameters
+        ----------
+
+        vectors: sequence of vectors of len(n)
+            A sequence of numpy arrays representing the vectors
+
+        Returns
+        ----------
+
+        all_orthogonal: bool
+            True if all the vectors are orthogonal to each other, False otherwise
+        """
+
+        all_orthogonal = not any([any([abs(vectors[i] @ vectors[j].T) > 1e-10 for j in range(i + 1, len(vectors))])
+                                  for i in range(len(vectors))])
+
+        return all_orthogonal
+
+    @staticmethod
+    def are_all_unit_vectors(vectors: Sequence[np.array]) -> bool:
+        """
+        Check if a set of vectors are all of length 1.
+
+        Parameters
+        ----------
+
+        vectors: sequence of vectors of len(n)
+            A sequence of numpy arrays representing the vectors
+
+        Returns
+        ----------
+
+        all_orthogonal: bool
+            True if all the vectors are orthogonal to each other, False otherwise
+        """
+
+        all_unit_vectors = all([abs(np.linalg.norm(v) - 1) < 1e-10 for v in vectors])
+
+        return all_unit_vectors
+
+
+N2_output_variables_names = ['$\\frac{x_1 + x_2}{\\sqrt{2}}$',
+                             '$\\frac{x_2 - x_1}{\\sqrt{2}}$',
+                             '$\\frac{\\dot{x}_1 + \\dot{x}_2}{\\sqrt{2}}$',
+                             '$\\frac{\\dot{x}_2 - \\dot{x}_1}{\\sqrt{2}}$']
+
+
+def multiprocess_worker_function(N: int,
+                                 k: float,
+                                 m: float,
+                                 q: float,
+                                 r: float,
+                                 epsilon_x: float,
+                                 epsilon_P: float):
+    x_0 = np.array([10 * i for i in range(1, N + 1)] + [0] * N)
+    x_T = x_0 * 10 if N == 2 else np.array([(10 * i) ** 3 for i in range(1, N + 1)] + [0] * N)
+
+    output_variables_names = N2_output_variables_names \
+        if N == 2 else [f'$q_{i}$' for i in range(1, N + 1)] + ['$\\dot{q}_{' + str(i) + '}$' for i in range(1, N + 1)]
+
+    T_f = 25
+    masses_with_springs = MassesWithSpringsComparison(N=N,
+                                                      m=m,
+                                                      k=k,
+                                                      q=q,
+                                                      r=r,
+                                                      x_0=x_0,
+                                                      x_T=x_T,
+                                                      T_f=T_f,
+                                                      epsilon_x=epsilon_x,
+                                                      epsilon_P=epsilon_P)
+
+    masses_with_springs(plot_state_spaces=True,
+                        plot_Mx=True,
+                        output_variables_names=output_variables_names,
+                        save_figure=True,
+                        figure_filename=masses_with_springs.figure_filename_generator
+                        )
+
+
+if __name__ == '__main__':
+    d = 0.2
+    N = 8
+
+    Ns = [N]
+    ks = [10]
+    ms = [50]
+    rs = [1]
+    epsilon_xs = [10e-8]
+    epsilon_Ps = [10e-8]
+    qs = [[500, 2000], [500, 250]] if N == 2 else \
+        [[500 * (1 + d) ** i for i in range(N)], [500 * (1 - d) ** i for i in range(N)]]
+
+    # N, k, m, r, epsilon_x, epsilon_P = Ns[0], ks[0], ms[0], rs[0], epsilon_xs[0], epsilon_Ps[0]
+    # q = [500, 2000] if N == 2 else [500 * (1 + d) ** i for i in range(N)]
+    # x_0 = np.array([10 * i for i in range(1, N + 1)] + [0] * N)
+    # x_T = x_0 * 10 if N == 2 else np.array([(10 * i) ** 3 for i in range(1, N + 1)] + [0] * N)
+    #
+    # masses_with_springs = MassesWithSpringsComparison(N=N,
+    #                                                   m=m,
+    #                                                   k=k,
+    #                                                   q=q,
+    #                                                   r=r,
+    #                                                   x_0=x_0,
+    #                                                   x_T=x_T,
+    #                                                   T_f=25,
+    #                                                   epsilon_x=epsilon_x,
+    #                                                   epsilon_P=epsilon_P)
+    #
+    # output_variables_names = N2_output_variables_names \
+    #     if N == 2 else [f'$q_{i}$' for i in range(1, N + 1)] + ['$\\dot{q}_{' + str(i) + '}$' for i in range(1, N + 1)]
+    #
+    # masses_with_springs(plot_state_spaces=True,
+    #                     plot_Mx=True,
+    #                     output_variables_names=output_variables_names,
+    #                     save_figure=True,
+    #                     figure_filename=masses_with_springs.figure_filename_generator
+    #                     )
+
+    #
+
+    params = [Ns, ks, ms, qs, rs, epsilon_xs, epsilon_Ps]
+    PyDiffGameLQRComparison.run_multiprocess(multiprocess_worker_function=multiprocess_worker_function,
+                                             values=params)
+

PyDiffGame/examples/PVTOL.py

@@ -0,0 +1,222 @@
+# pvtol_lqr.m - LQR design for vectored thrust aircraft
+# RMM, 14 Jan 03
+#
+# This file works through an LQR based design problem, using the
+# planar vertical takeoff and landing (PVTOLComparison.py) aircraft example from
+# Astrom and Murray, Chapter 5.  It is intended to demonstrate the
+# basic functionality of the python-control package.
+#
+
+import os
+import numpy as np
+import matplotlib.pyplot as plt  # MATLAB-like plotting functions
+import control as ct
+
+#
+# System dynamics
+#
+# These are the dynamics for the PVTOLComparison.py system, written in state space
+# form.
+#
+
+# System parameters
+m = 4       # mass of aircraft
+J = 0.0475  # inertia around pitch axis
+r = 0.25    # distance to center of force
+g = 9.8     # gravitational constant
+c = 0.05    # damping factor (estimated)
+
+# State space dynamics
+xe = [0, 0, 0, 0, 0, 0]  # equilibrium point of interest
+ue = [0, m * g]  # (note these are lists, not matrices)
+
+# This will involve re-working the subsequent equations as the shapes
+# See below.
+
+A = np.array(
+    [[0, 0, 0, 1, 0, 0],
+     [0, 0, 0, 0, 1, 0],
+     [0, 0, 0, 0, 0, 1],
+     [0, 0, (-ue[0]*np.sin(xe[2]) - ue[1]*np.cos(xe[2]))/m, -c/m, 0, 0],
+     [0, 0, (ue[0]*np.cos(xe[2]) - ue[1]*np.sin(xe[2]))/m, 0, -c/m, 0],
+     [0, 0, 0, 0, 0, 0]]
+)
+
+# Input matrix
+B = np.array(
+    [[0, 0], [0, 0], [0, 0],
+     [np.cos(xe[2])/m, -np.sin(xe[2])/m],
+     [np.sin(xe[2])/m, np.cos(xe[2])/m],
+     [r/J, 0]]
+)
+
+# Output matrix 
+C = np.array([[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0]])
+D = np.array([[0, 0], [0, 0]])
+
+#
+# Construct inputs and outputs corresponding to steps in xy position
+#
+# The vectors xd and yd correspond to the states that are the desired
+# equilibrium states for the system.  The matrices Cx and Cy are the 
+# corresponding outputs.
+#
+# The way these vectors are used is to compute the closed loop system
+# dynamics as
+#
+#   xdot = Ax + B u  =>  xdot = (A-BK)x + K xd
+#      u = -K(x - xd)       y = Cx
+#
+# The closed loop dynamics can be simulated using the "step" command, 
+# with K*xd as the input vector (assumes that the "input" is unit size,
+# so that xd corresponds to the desired steady state.
+#
+
+xd = np.array([[1], [0], [0], [0], [0], [0]])
+yd = np.array([[0], [1], [0], [0], [0], [0]])
+
+#
+# Extract the relevant dynamics for use with SISO library
+#
+# The current python-control library only supports SISO transfer
+# functions, so we have to modify some parts of the original MATLAB
+# code to extract out SISO systems.  To do this, we define the 'lat' and
+# 'alt' index vectors to consist of the states that are are relevant
+# to the lateral (x) and vertical (y) dynamics.
+#
+
+# Indices for the parts of the state that we want
+lat = (0, 2, 3, 5)
+alt = (1, 4)
+
+# Decoupled dynamics
+Ax = A[np.ix_(lat, lat)]
+Bx = B[np.ix_(lat, [0])]
+Cx = C[np.ix_([0], lat)]
+Dx = D[np.ix_([0], [0])]
+
+Ay = A[np.ix_(alt, alt)]
+By = B[np.ix_(alt, [1])]
+Cy = C[np.ix_([1], alt)]
+Dy = D[np.ix_([1], [1])]
+
+# Label the plot
+plt.clf()
+plt.suptitle("LQR controllers for vectored thrust aircraft (pvtol-lqr)")
+
+#
+# LQR design
+#
+
+# Start with a diagonal weighting
+Qx1 = np.diag([1, 1, 1, 1, 1, 1])
+Qu1a = np.diag([1, 1])
+K1a, X, E = ct.lqr(A, B, Qx1, Qu1a)
+
+# Close the loop: xdot = Ax - B K (x-xd)
+#
+# Note: python-control requires we do this 1 input at a time
+# H1a = ss(A-B*K1a, B*K1a*concatenate((xd, yd), axis=1), C, D);
+# (T, Y) = step_response(H1a, T=np.linspace(0,10,100));
+#
+
+# Step response for the first input
+H1ax = ct.ss(Ax - Bx @ K1a[np.ix_([0], lat)],
+             Bx @ K1a[np.ix_([0], lat)] @ xd[lat, :], Cx, Dx)
+Tx, Yx = ct.step_response(H1ax, T=np.linspace(0, 10, 100))
+
+# Step response for the second input
+H1ay = ct.ss(Ay - By @ K1a[np.ix_([1], alt)],
+             By @ K1a[np.ix_([1], alt)] @ yd[alt, :], Cy, Dy)
+Ty, Yy = ct.step_response(H1ay, T=np.linspace(0, 10, 100))
+
+plt.subplot(221)
+plt.title("Identity weights")
+# plt.plot(T, Y[:,1, 1], '-', T, Y[:,2, 2], '--')
+plt.plot(Tx.T, Yx.T, '-', Ty.T, Yy.T, '--')
+plt.plot([0, 10], [1, 1], 'k-')
+
+plt.axis([0, 10, -0.1, 1.4])
+plt.ylabel('position')
+plt.legend(('x', 'y'), loc='lower right')
+
+# Look at different input weightings
+Qu1a = np.diag([1, 1])
+K1a, X, E = ct.lqr(A, B, Qx1, Qu1a)
+H1ax = ct.ss(Ax - Bx @ K1a[np.ix_([0], lat)],
+             Bx @ K1a[np.ix_([0], lat)] @ xd[lat, :], Cx, Dx)
+
+Qu1b = (40 ** 2)*np.diag([1, 1])
+K1b, X, E = ct.lqr(A, B, Qx1, Qu1b)
+H1bx = ct.ss(Ax - Bx @ K1b[np.ix_([0], lat)],
+             Bx @ K1b[np.ix_([0], lat)] @ xd[lat, :], Cx, Dx)
+
+Qu1c = (200 ** 2)*np.diag([1, 1])
+K1c, X, E = ct.lqr(A, B, Qx1, Qu1c)
+H1cx = ct.ss(Ax - Bx @ K1c[np.ix_([0], lat)],
+             Bx @ K1c[np.ix_([0], lat)] @ xd[lat, :], Cx, Dx)
+
+T1, Y1 = ct.step_response(H1ax, T=np.linspace(0, 10, 100))
+T2, Y2 = ct.step_response(H1bx, T=np.linspace(0, 10, 100))
+T3, Y3 = ct.step_response(H1cx, T=np.linspace(0, 10, 100))
+
+plt.subplot(222)
+plt.title("Effect of input weights")
+plt.plot(T1.T, Y1.T, 'b-')
+plt.plot(T2.T, Y2.T, 'b-')
+plt.plot(T3.T, Y3.T, 'b-')
+plt.plot([0, 10], [1, 1], 'k-')
+
+plt.axis([0, 10, -0.1, 1.4])
+
+# arcarrow([1.3, 0.8], [5, 0.45], -6)
+plt.text(5.3, 0.4, r'$\rho$')
+
+# Output weighting - change Qx to use outputs
+Qx2 = C.T @ C
+Qu2 = 0.1 * np.diag([1, 1])
+K2, X, E = ct.lqr(A, B, Qx2, Qu2)
+
+H2x = ct.ss(Ax - Bx @ K2[np.ix_([0], lat)],
+            Bx @ K2[np.ix_([0], lat)] @ xd[lat, :], Cx, Dx)
+H2y = ct.ss(Ay - By @ K2[np.ix_([1], alt)],
+            By @ K2[np.ix_([1], alt)] @ yd[alt, :], Cy, Dy)
+
+plt.subplot(223)
+plt.title("Output weighting")
+T2x, Y2x = ct.step_response(H2x, T=np.linspace(0, 10, 100))
+T2y, Y2y = ct.step_response(H2y, T=np.linspace(0, 10, 100))
+plt.plot(T2x.T, Y2x.T, T2y.T, Y2y.T)
+plt.ylabel('position')
+plt.xlabel('time')
+plt.ylabel('position')
+plt.legend(('x', 'y'), loc='lower right')
+
+#
+# Physically motivated weighting
+#
+# Shoot for 1 cm error in x, 10 cm error in y.  Try to keep the angle
+# less than 5 degrees in making the adjustments.  Penalize side forces
+# due to loss in efficiency.
+#
+
+Qx3 = np.diag([100, 10, 2*np.pi/5, 0, 0, 0])
+Qu3 = 0.1*np.diag([1, 10])
+K3, X, E = ct.lqr(A, B, Qx3, Qu3)
+
+H3x = ct.ss(Ax - Bx @ K3[np.ix_([0], lat)],
+            Bx @ K3[np.ix_([0], lat)] @ xd[lat, :], Cx, Dx)
+H3y = ct.ss(Ay - By @ K3[np.ix_([1], alt)],
+            By @ K3[np.ix_([1], alt)] @ yd[alt, :], Cy, Dy)
+plt.subplot(224)
+# step_response(H3x, H3y, 10)
+T3x, Y3x = ct.step_response(H3x, T=np.linspace(0, 10, 100))
+T3y, Y3y = ct.step_response(H3y, T=np.linspace(0, 10, 100))
+plt.plot(T3x.T, Y3x.T, T3y.T, Y3y.T)
+plt.title("Physically motivated weights")
+plt.xlabel('time')
+plt.legend(('x', 'y'), loc='lower right')
+plt.tight_layout()
+
+if 'PYCONTROL_TEST_EXAMPLES' not in os.environ:
+    plt.show()

PyDiffGame/examples/PVTOLComparison.py

@@ -0,0 +1,111 @@
+import numpy as np
+from typing import Optional
+
+from PyDiffGame.PyDiffGame import PyDiffGame
+from PyDiffGame.PyDiffGameLQRComparison import PyDiffGameLQRComparison
+from PyDiffGame.Objective import GameObjective, LQRObjective
+
+
+class PVTOLComparison(PyDiffGameLQRComparison):
+    def __init__(self,
+                 x_0: Optional[np.array] = None,
+                 x_T: Optional[np.array] = None,
+                 T_f: Optional[float] = None,
+                 epsilon_x: Optional[float] = PyDiffGame.epsilon_x_default,
+                 epsilon_P: Optional[float] = PyDiffGame.epsilon_P_default,
+                 L: Optional[int] = PyDiffGame.L_default,
+                 eta: Optional[int] = PyDiffGame.eta_default):
+        m = 4  # mass of aircraft
+        J = 0.0475  # inertia around pitch axis
+        r = 0.25  # distance to center of force
+        c = 0.05  # damping factor (estimated)
+
+        A_11 = np.zeros((3, 3))
+        A_12 = np.eye(3)
+        A_21 = np.array([[0, 0, -PyDiffGame.g],
+                         [0, 0, 0],
+                         [0, 0, 0]])
+        A_22 = np.diag([-c / m, -c / m, 0])
+
+        A = np.block([[A_11, A_12],
+                      [A_21, A_22]])
+
+        # Input matrix
+        B = np.array(
+            [[0, 0],
+             [0, 0],
+             [0, 0],
+             [1 / m, 0],
+             [0, 1 / m],
+             [r / J, 0]]
+        )
+
+        psi = 1
+
+        M_x = [1, 0]
+        M_y = [0, 1]
+        M_theta = [1, 0]
+
+        Ms = [np.array(M_i).reshape(1, 2) for M_i in [M_x, M_y, M_theta]]
+        M = np.concatenate(Ms, axis=0)
+
+        self.figure_filename_generator = lambda g: ('LQR_' if g.is_LQR() else '') + f"PVTOL{str(psi).replace('.', '')}"
+
+        Q_x_diag = [psi, 0, 0]
+        Q_y_diag = [0, 1, 0]
+        Q_theta_diag = [0, 0, 1]
+
+        r_x = 1
+        r_y = 1
+        r_theta = 1
+
+        Qs = [np.diag(Q_i_diag * 2) for Q_i_diag in [Q_x_diag, Q_y_diag, Q_theta_diag]]
+        Rs = [np.array([r_i]) for r_i in [r_x, r_y, r_theta]]
+
+        variables = ['x', 'y', '\\theta']
+
+        self.state_variables_names = [f'{v}(t)' for v in variables] + ['\\dot{' + str(v) + '}(t)' for v in variables]
+
+        Q_lqr = sum(Qs)
+        R_lqr = np.diag([r_x + r_theta, r_y])
+
+        lqr_objective = [LQRObjective(Q=Q_lqr,
+                                      R_ii=R_lqr)]
+        game_objectives = [GameObjective(Q=Q_i,
+                                         R_ii=R_i,
+                                         M_i=M_i) for Q_i, R_i, M_i in zip(Qs, Rs, Ms)]
+        games_objectives = [lqr_objective,
+                            game_objectives]
+
+        args = {'A': A,
+                'B': B,
+                'x_0': x_0,
+                'x_T': x_T,
+                'T_f': T_f,
+                'state_variables_names': self.state_variables_names,
+                'epsilon_x': epsilon_x,
+                'epsilon_P': epsilon_P,
+                'L': L,
+                'eta': eta}
+
+        super().__init__(args=args,
+                         M=M,
+                         games_objectives=games_objectives)
+
+
+if __name__ == '__main__':
+    epsilon_x = epsilon_P = 10 ** (-8)
+
+    x_0 = np.zeros((6,))
+    x_d = y_d = 50
+    x_T = np.array([x_d, y_d] + [0] * 4)
+    T_f = 25
+
+    pvtol = PVTOLComparison(x_0=x_0,
+                            x_T=x_T,
+                            T_f=T_f,
+                            epsilon_x=epsilon_x,
+                            epsilon_P=epsilon_P)
+    pvtol(plot_state_spaces=True,
+          save_figure=True,
+          figure_filename=pvtol.figure_filename_generator)