Update to project 2

irlc/project2/Latex/02465project2_handin.tex

+\documentclass[12pt,twoside]{article}
+%\usepackage[table]{xcolor} % important to avoid options clash.
+%\input{02465shared_preamble}
+%\usepackage{cleveref}
+\usepackage{url}
+\usepackage{graphics}
+\usepackage{multicol}
+\usepackage{rotate}
+\usepackage{rotating}
+\usepackage{booktabs}
+\usepackage{hyperref}
+\usepackage{pifont}
+\usepackage{latexsym}
+\usepackage[english]{babel}
+\usepackage{epstopdf}
+\usepackage{etoolbox}
+\usepackage{amsmath}
+\usepackage{amssymb}
+\usepackage{multirow,epstopdf}
+\usepackage{fancyhdr}
+\usepackage{booktabs}
+\usepackage{xcolor}
+\newcommand\redt[1]{ {\textcolor[rgb]{0.60, 0.00, 0.00}{\textbf{ #1} } } }
+
+
+\newcommand{\m}[1]{\boldsymbol{ #1}}
+\newcommand{\yoursolution}{ \redt{(your solution here) } } 
+
+
+
+\title{ Report 2 hand-in }
+\date{ \today }
+\author{Alice (\texttt{s000001})\and  Bob (\texttt{s000002})\and Clara (\texttt{s000003}) } 
+
+\begin{document}
+\maketitle
+
+\begin{table}[ht!]
+\caption{Attribution table. Feel free to add/remove rows and columns}
+\begin{tabular}{llll}
+\toprule
+                                                                    & Alice   & Bob    & Clara   \\
+\midrule
+ 1: Formulate Yodas pendulum as a linear problem                    & 0-100\%  & 0-100\% & 0-100\%  \\
+ 2: State at a later time                                           & 0-100\%  & 0-100\% & 0-100\%  \\
+ 3: State at a later time II                                        & 0-100\%  & 0-100\% & 0-100\%  \\
+ 4: Eigenvalues and powers                                          & 0-100\%  & 0-100\% & 0-100\%  \\
+ 5: Analytical expression of Eigenvalues using Euler discretization & 0-100\%  & 0-100\% & 0-100\%  \\
+ 6: Bound using Euler discretization                                & 0-100\%  & 0-100\% & 0-100\%  \\
+ 7: Matrix norm of Exponential discretization (harder)              & 0-100\%  & 0-100\% & 0-100\%  \\
+ 8: Stability                                                       & 0-100\%  & 0-100\% & 0-100\%  \\
+ 9: Discretization                                                  & 0-100\%  & 0-100\% & 0-100\%  \\
+ 10: Linearization                                                  & 0-100\%  & 0-100\% & 0-100\%  \\
+ 11: Unitgrade self-check                                           & 0-100\%  & 0-100\% & 0-100\%  \\
+ 12: Optimal planning                                               & 0-100\%  & 0-100\% & 0-100\%  \\
+ 13: Control using simple linearization                             & 0-100\%  & 0-100\% & 0-100\%  \\
+ 14: MPC                                                            & 0-100\%  & 0-100\% & 0-100\%  \\
+\bottomrule
+\end{tabular}
+\end{table}
+
+%\paragraph{Statement about collaboration:}
+%Please edit this section to reflect how you have used external resources. The following statement will in most cases suffice: 
+%\emph{The code in the irls/project1 directory is entirely}
+
+%\paragraph{Main report:}
+Headings have been inserted in the document for readability. You only have to edit the part which says \yoursolution. 
+
+\section{Master Yodas pendulum (\texttt{yoda.py})}\label{yoda1}
+\subsubsection*{{\color{red}Problem 1:  Formulate Yodas pendulum as a linear problem}}
+	
+	\begin{align}
+		A & = \begin{bmatrix} \cdots \end{bmatrix}  \\
+		B & = \begin{bmatrix} \cdots \end{bmatrix}
+	\end{align}
+	\yoursolution 	
+	
+\subsubsection*{{\color{red}Problem 2:  State at a later time}}
+	
+		To solve the first part, we can write $\m x_N = \begin{bmatrix} \cdots \end{bmatrix}$
+		
+		As for the second part we get:
+\begin{align}
+\tilde A_0 & = \begin{bmatrix} \cdots \end{bmatrix}, \quad A_0 = \begin{bmatrix} \cdots \end{bmatrix}
+\end{align}
+		\yoursolution 	
+\subsubsection*{{\color{red}Problem 4:  Eigenvalues and powers}}
+	
+Assume $\lambda_1, \lambda_2$ are the eigenvalues ... then the Eigenvalues of $M$ is ... similarly for $\tilde M$ ... 
+\yoursolution
+	
+\subsubsection*{{\color{red}Problem 5:  Analytical expression of Eigenvalues using Euler discretization}}
+
+... we get a characteristic polynomial of ... and therefore it follows from Mat1 that the two Eigenvalues are ... 
+\yoursolution
+
+\subsubsection*{{\color{red}Problem 6:  Bound using Euler discretization}}
+
+	Using Euler discretization we get the upper bound:
+	$$
+\| \m x_N \| \leq \cdots
+$$
+\yoursolution
+	
+\subsubsection*{{\color{red}Problem 7:  Matrix norm of Exponential discretization (harder)}}
+	
+Using exponential discretization we get an upper bound of:
+		$$
+		\| \m x_N \| \leq \cdots
+		$$
+		\yoursolution
+	
+\section{R2D2 and control (\texttt{r2d2.py})}
+\subsubsection*{{\color{red}Problem 9:  Discretization}}
+
+	$$
+	\m x_{k+1} = \m f_k(\m x_k, \m u_k) = \begin{bmatrix} \cdots \\ \cdots \\ \cdots \end{bmatrix}$$
+
+\subsubsection*{{\color{red}Problem 10:  Linearization}}
+	
+$$
+		\m x_{k+1} \approx \begin{bmatrix} \cdots \\ \cdots \\ \cdots \end{bmatrix} \m x_k + 
+		\begin{bmatrix} \cdots \\ \cdots \\ \cdots \end{bmatrix} \m u_k +  
+		\begin{bmatrix} \vdots \end{bmatrix} 
+$$
+	
+\subsubsection*{{\color{red}Problem 12:  Optimal planning}}
+		
+	\begin{center}\includegraphics[width=.5\linewidth]{figures/your_answer}~
+		\includegraphics[width=.5\linewidth]{figures/your_answer} \end{center}
+
+\subsubsection*{{\color{red}Problem 13:  Control using simple linearization}}
+			
+		% Just generate the figures using the script and change the path below. 
+		\begin{center}\includegraphics[width=.5\linewidth]{figures/your_answer}~
+		\includegraphics[width=.5\linewidth]{figures/your_answer} \end{center}
+Intuitively, the second case fails because... \yoursolution	
+	
+\subsubsection*{{\color{red}Problem 14:  MPC}}
+			
+		\begin{center}\includegraphics[width=.6\linewidth]{figures/your_answer}%~
+		%	\includegraphics[width=.5\linewidth]{figures/your_answer} 
+	\end{center}
+		Iterative linearization solves the problem because... \yoursolution
+	
+\end{document}
\ No newline at end of file

irlc/project2/__init__.py

+# This file may not be shared/redistributed without permission. Please read copyright notice in the git repo. If this file contains other copyright notices disregard this text.
+"""This file is required for the test system but should otherwise be empty."""

irlc/project2/project2_tests.py

+# This file may not be shared/redistributed without permission. Please read copyright notice in the git repo. If this file contains other copyright notices disregard this text.
+from unitgrade import UTestCase, Report
+import irlc
+import numpy as np
+
+class YodaProblem1(UTestCase):
+    """ Test the get_A_B() function (Section 1, Problem 1) """
+    def test_A_B(self):
+        from irlc.project2.yoda import get_A_B
+        for g in [9.82, 5.1]:
+            for L in [0.2, 0.5, 1.1]:
+                A,B = get_A_B(g,L)
+                # To get the expected output of a test (in the cases where it is not specified manually),
+                # simply use the line self.get_expected_test_value() *right before* running the test-function itself.
+                # print("The expected value is", self.get_expected_test_value())
+                # If the code does not work, you need to upgrade unitgrade to the most recent version:
+                # pip install unitgrade --upgrade --no-cache
+                print(A)
+                self.assertLinf(A)
+                print(B)
+                self.assertLinf(B)
+
+class YodaProblem2(UTestCase):
+    r""" Yodas pendulum: Problem 2 """
+    def test_A0(self):
+        from irlc.project2.yoda import A_ei, A_euler
+        for g in [9.2, 10]:
+            for L in [0.2, 0.4]:
+                for Delta in [0.1, 0.2]:
+                    self.assertLinf(A_euler(g, L, Delta)) # Test Euler discretization
+                    self.assertLinf(A_ei(g, L, Delta))    # Test exponential discretization
+
+
+class YodaProblem3(UTestCase):
+    r""" Yodas pendulum: Problem 3 """
+    def test_M(self):
+        from irlc.project2.yoda import M_ei, M_euler
+        for g in [9.2, 10]:
+            for L in [0.2, 0.4]:
+                for Delta in [0.1, 0.2]:
+                    for N in [3, 5]:
+                        self.assertLinf(M_ei(g, L, Delta, N)) # Test Euler discretization
+                        self.assertLinf(M_euler(g, L, Delta, N))    # Test exponential discretization
+
+
+class YodaProblem6(UTestCase):
+    r""" Yodas pendulum: Bound using Euler discretization Problem 6 """
+    def test_xN_euler_bound(self):
+        from irlc.project2.yoda import xN_bound_euler
+        for g in [9.2, 10]:
+            for L in [0.2, 0.4]:
+                for Delta in [0.1, 0.2]:
+                    for N in [3, 5]:
+                        self.assertLinf(xN_bound_euler(g, L, Delta, N))
+
+class YodaProblem7(UTestCase):
+    r"""Yodas pendulum: Bound using exponential discretization Problem 7 """
+    def test_xN_euler_bound(self):
+        from irlc.project2.yoda import xN_bound_ei
+        for g in [9.2, 10]:
+            for L in [0.2, 0.4]:
+                for Delta in [0.1, 0.2]:
+                    for N in [3, 5]:
+                        self.assertLinf(xN_bound_ei(g, L, Delta, N))
+
+
+class R2D2Problem15(UTestCase):
+    r"""R2D2: Tests the linearization and discretization code in Problem 9 and Problem 10"""
+    def test_f_euler_zeros(self):
+        # Test in a simple case:
+        x = np.zeros((3,))
+        u = np.asarray([1,0])
+        from irlc.project2.r2d2 import f_euler
+        self.assertLinf(f_euler(x, u, Delta=0.05))
+        self.assertLinf(f_euler(x, u, Delta=0.1))
+
+    def test_f_euler(self):
+        np.random.seed(42)
+        for _ in range(4):
+            x = np.random.randn(3)
+            u = np.random.randn(2)
+            from irlc.project2.r2d2 import f_euler
+            self.assertLinf(f_euler(x, u, Delta=0.05))
+            self.assertLinf(f_euler(x, u, Delta=0.1))
+
+    def checklin(self, x_bar, u_bar):
+        from irlc.project2.r2d2 import linearize
+        A, B, d = linearize(x_bar, u_bar, Delta=0.05)
+        self.assertLinf(A)
+        self.assertLinf(B)
+        self.assertLinf(d)
+
+    def test_linearization1(self):
+        x_bar = np.asarray([0, 0, 0])
+        u_bar = np.asarray([1, 0])
+        self.checklin(x_bar, u_bar)
+
+    def test_linearization2(self):
+        x_bar = np.asarray([0, 0, 0.24])
+        u_bar = np.asarray([1, 0])
+        self.checklin(x_bar, u_bar)
+
+    def test_linearization3(self):
+        np.random.seed(42)
+        for _ in range(10):
+            x_bar = np.random.randn(3)
+            u_bar = np.asarray([1, 0])
+            self.checklin(x_bar, u_bar)
+
+class R2D2Direct(UTestCase):
+    r"""Problem 12: R2D2 and direct methods """
+    def chk_direct(self, x_target):
+        from irlc.project2.r2d2 import drive_to_direct
+        states = drive_to_direct(x_target=x_target, plot=False)
+        self.assertIsInstance(states, np.ndarray)  # Test states are an ndarray
+        self.assertEqualC(states.shape)  # Test states have the right shape
+        self.assertL2(states, tol=0.03)
+
+    def test_direct_1(self):
+        x_target = (2, 0, 0)
+        self.chk_direct(x_target)
+
+    def test_direct_2(self):
+        x_target = (2, 2, np.pi / 2)
+        self.chk_direct(x_target)
+
+
+class R2D2Linearization(UTestCase):
+    """Problem 13: R2D2 and simple linearization."""
+    def chk_linearization(self, x_target):
+        from irlc.project2.r2d2 import drive_to_linearization
+        states = drive_to_linearization(x_target=x_target, plot=False)
+        self.assertIsInstance(states, np.ndarray)  # Test states are an ndarray
+        self.assertEqualC(states.shape)  # Test states have the right shape
+        self.assertL2(states, tol=0.03)
+
+    def test_linearization_1(self):
+        x_target = (2, 0, 0)
+        self.chk_linearization(x_target)
+
+    def test_linearization_2(self):
+        x_target = (2, 2, np.pi / 2)
+        self.chk_linearization(x_target)
+
+class R2D2_MPC(UTestCase):
+    r"""Problem 14: R2D2 and MPC."""
+    def chk_mpc(self, x_target):
+        from irlc.project2.r2d2 import drive_to_mpc
+        states = drive_to_mpc(x_target=x_target, plot=False)
+        self.assertIsInstance(states, np.ndarray)  # Test states are an ndarray
+        self.assertEqualC(states.shape)  # Test states have the right shape
+        self.assertL2(states, tol=0.03)
+
+    def test_mpc_1(self):
+        self.chk_mpc(x_target=(2,0,0) )
+
+    def test_mpc_2(self):
+        self.chk_mpc(x_target=(2, 2, np.pi / 2))
+
+class Project2(Report):
+    title = "Project part 2: Control"
+    pack_imports = [irlc]
+
+    yoda = [
+        (YodaProblem1, 10),
+        (YodaProblem2, 10),
+        (YodaProblem3, 10),
+        (YodaProblem6, 8),
+        (YodaProblem7, 2)
+             ]
+    r2d2 = [
+            (R2D2Problem15, 10),
+            (R2D2Direct, 10),
+            (R2D2Linearization, 10),
+            (R2D2_MPC, 10),
+            ]
+
+    questions = []
+    questions += yoda
+    questions += r2d2
+
+if __name__ == '__main__':
+    from unitgrade import evaluate_report_student
+    evaluate_report_student(Project2() )

irlc/project2/r2d2.py

+# This file may not be shared/redistributed without permission. Please read copyright notice in the git repo. If this file contains other copyright notices disregard this text.
+import time
+import numpy as np
+import sympy as sym
+import matplotlib.pyplot as plt
+from gymnasium.spaces import Box
+# matplotlib.use('Qt5Agg') This line may be useful if you are having matplotlib problems on Linux.
+from irlc.ex04.discrete_control_model import DiscreteControlModel
+from irlc.ex04.control_environment import ControlEnvironment
+from irlc.ex03.control_model import ControlModel
+from irlc.ex03.control_cost import SymbolicQRCost
+from irlc.ex05.direct_agent import DirectAgent
+from irlc.ex05.direct import get_opts, guess
+from irlc.ex07.linearization_agent import LinearizationAgent
+from irlc.project2.utils import R2D2Viewer
+from irlc import Agent, train, plot_trajectory, savepdf
+
+dt = 0.05 # Time discretization Delta
+Tmax = 5  # Total simulation time (in all instances). This means that N = Tmax/dt = 100.
+x22 = (2, 2, np.pi / 2)  # Where we want to drive to: x_target
+
+class R2D2Model(ControlModel): # This may help you get started.
+    state_labels = ["$x$", "$y$", r"$\gamma$"]
+    action_labels = ["Cart velocity $v$", r'Yaw rate $\omega$'] # Define constants as needed here (look at other environments); Note there is an easy way to add labels!
+
+    def __init__(self, x_target=(2,2,np.pi/2), Q0=1.): # This constructor is one possible choice.
+        # Q0:       The Q-matrix for the cF-term in the cost function (see problem description)
+        # x_target: The state we will drive towards.
+        self.x_target = np.asarray(x_target)
+        self.Q0 = Q0
+        self.Tmax = 5  # Plan for a maximum of 5 seconds.
+        # Set up a variable for rendering (optional) and call superclass.
+        self.viewer = None
+        super().__init__()
+
+    def get_cost(self) -> SymbolicQRCost:
+        # The cost function uses self.Q0 to define the appropriate cost. It has the same meaning as the lecture description
+        cost = SymbolicQRCost(Q=np.zeros(3), R=np.eye(2))
+        cost += cost.goal_seeking_cost(x_target=self.x_target)*self.Q0
+        return cost
+
+    def tF_bound(self) -> Box:
+        return Box(self.Tmax, self.Tmax, shape=(1,))
+
+    def x0_bound(self) -> Box:
+        return Box(0, 0, shape=(self.state_size,))
+
+    def xF_bound(self) -> Box: 
+        # TODO: 1 lines missing.
+        raise NotImplementedError("Complete this function to specify the target of R2D2.")
+
+    # TODO: 3 lines missing.
+    raise NotImplementedError("Complete model dynamics here.")
+
+    """ These are two helper functions. They add rendering functionality so you can eventually use the environment as
+    
+    > env = R2D2Environment(render_mode='human') 
+    
+    and see a small animation. 
+    """
+    def close(self):
+        if self.viewer is not None:
+            self.viewer.close()
+
+    def render(self, x, render_mode="human"): 
+        if self.viewer is None:
+            self.viewer = R2D2Viewer(x_target=self.x_target) # Target is the red cross.
+        self.viewer.update(x)
+        time.sleep(0.05)
+        return self.viewer.blit(render_mode=render_mode) 
+
+
+class R2D2Environment(ControlEnvironment):
+    def __init__(self, Tmax=Tmax, Q0=0., x_target=x22, dt=None, render_mode=None):
+        assert dt is not None, "Remember to specify the discretization time!"
+        model = R2D2Model(Q0=Q0, x_target=x_target) # Create an R2D2 ControlModel with the given parameters.
+        dmodel = DiscreteControlModel(model, dt=dt)   # Create a discrete version of the R2D2 ControlModel
+        super().__init__(dmodel, Tmax=Tmax, render_mode=render_mode)
+
+# TODO: 9 lines missing.
+raise NotImplementedError("Your code here.")
+
+def f_euler(x : np.ndarray, u : np.ndarray, Delta=0.05) -> np.ndarray: 
+    """ Solve Problem 9. The function should compute
+    > x_next = f_k(x, u)
+    """
+    # TODO: 1 lines missing.
+    raise NotImplementedError("return next state")
+    return x_next
+
+def linearize(x_bar, u_bar, Delta=0.05):
+    """ Linearize R2D2's dynamics around the two vectors x_bar, u_bar
+    and return A, B, d so that
+
+    x_{k+1} = A x_k + B u_k + d (approximately).
+
+    The function should return linearization matrices A, B and d.
+    """
+    # Create A, B, d as numpy ndarrays.
+    # TODO: 4 lines missing.
+    raise NotImplementedError("Insert your solution and remove this error.")
+    return A, B, d
+
+def drive_to_linearization(x_target, plot=True): 
+    """
+    Plan in a R2D2 model with specific value of x_target (in the cost function). We use Q0=1.0.
+
+    this function will linearize the dynamics around xbar=0, ubar=0 to get a linear approximation of the model,
+    and then use that to plan on a horizon of N=50 steps to get a control law (L_0, l_0). This is then applied
+    to generate actions.
+
+    Plot is an optional parameter to control plotting. the plot_trajectory(trajectory, env) method may be useful.
+
+    The function should return the states visited as a (samples x state-dimensions) matrix, i.e. same format
+    as the default output of trajectories when you use train(...).
+
+    Hints:
+        * The control method is identical to one we have seen in the exercises/notes. You can re-purpose the code from that week.
+        * Remember to set Q0=1
+    """
+    # TODO: 7 lines missing.
+    raise NotImplementedError("Implement function body")
+    return traj[0].state
+
+def drive_to_direct(x_target, plot=False): 
+    """
+    Optimal planning in the R2D2 model with specific value of x_target using the direct method.
+    Remember that for this problem we set Q0=0, and implement x_target as an end-point constraint (see examples from exercises).
+
+    Plot is an optional parameter to control plotting, and to (optionally) visualize the environment using code such as::
+
+    env = R2D2Environment(..., render_mode='human' if plot else None)
+
+    For making the actual plot, the plot_trajectory(trajectory, env) method may be useful (see examples from exercises to see how labels can be specified)
+
+    The function should return the states visited as a (samples x state-dimensions) matrix, i.e. same format
+    as the default output of trajectories when you use train(...).
+
+    Hints:
+        * The control method (Direct method) is identical to what we did in the exercises, but you have to specify the options
+        to implement the correct grid-refinement of N=10, N=20 and N=40.
+        * Remember to set Q0=0.
+    """
+    # TODO: 10 lines missing.
+    raise NotImplementedError("Implement function body")
+    return traj[0].state
+
+def drive_to_mpc(x_target, plot=True) -> np.ndarray: 
+    """
+    Plan in a R2D2 model with specific value of x_target (in the cost function) using iterative MPC (see problem text).
+    Use Q0 = 1. in the cost function (see the R2D2 model class)
+
+    Plot is an optional parameter to control plotting. the plot_trajectory(trajectory, env) method may be useful.
+
+    The function should return the states visited as a (samples x state-dimensions) matrix, i.e. same format
+    as the default output of trajectories when you use train(...).
+
+    Hints:
+     * The control method is *nearly* identical to the linearization control method. Think about the differences,
+       and how a solution to one can be used in another.
+     * A bit more specific: Linearization is handled similarly to the LinearizationAgent, however, we need to update
+       (in each step) the xbar/ubar states/actions we are linearizing about, and then just use the immediate action computed
+       by the linearization agent.
+     * My approach was to implement a variant of the LinearizationAgent.
+    """
+    # TODO: 6 lines missing.
+    raise NotImplementedError("Implement function body")
+    return traj[0].state
+
+if __name__ == "__main__":
+    r2d2 = R2D2Model()
+    print(r2d2) # This will print out details of your R2D2 model.
+
+    # Check Problem 10
+    x = np.asarray( [0, 0, 0] )
+    u = np.asarray( [1,0])
+    print("x_k =", x, "u_k =", u, "x_{k+1} =", f_euler(x, u, dt))
+
+    A,B,d = linearize(x_bar=x, u_bar=u, Delta=dt)
+    print("x_{k+1} ~ A x_k + B u_k + d")
+    print("A:", A)
+    print("B:", B)
+    print("d:", d)
+
+    # Test the simple linearization method (Problem 12)
+    states = drive_to_direct(x22, plot=True)
+    savepdf('r2d2_direct')
+    plt.show()
+    # plt.close()
+    plt.figure()
+    # Build plot assuming that states is in the format (samples x coordinates-of-state).
+    plt.plot(states[:,0], states[:,1], 'k-', label="R2D2's (x, y) trajectory")
+    plt.legend()
+    plt.xlabel("x")
+    plt.ylabel("y")
+    savepdf('r2d2_direct_B')
+    plt.show()
+
+    # Test the simple linearization method (Problem 13)
+    drive_to_linearization((2,0,0), plot=True)
+    savepdf('r2d2_linearization_1')
+    plt.show()
+
+    drive_to_linearization(x22, plot=True)
+    savepdf('r2d2_linearization_2')
+    plt.show()
+
+    # Test iterative LQR (Problem 14)
+    state = drive_to_mpc(x22, plot=True)
+    print(state[-1])
+    savepdf('r2d2_iterative_1')
+    plt.show()

irlc/project2/utils.py

+# This file may not be shared/redistributed without permission. Please read copyright notice in the git repo. If this file contains other copyright notices disregard this text.
+from irlc.utils.graphics_util_pygame import UpgradedGraphicsUtil, rotate_around
+import numpy as np
+
+""" This file contains code you can either use (or not) to render the R2D2 robot. class is already called correctly by your R2D2 class, 
+and you don't really have to think too carefully about what the code does unless you want to R2D2 to look better.
+"""
+
+
+class R2D2Viewer(UpgradedGraphicsUtil):
+    def __init__(self, x_target = (0,0)):
+        self.x_target = x_target
+        width = 800
+        self.scale = width / 1000
+        xlim = 3
+        self.dw = self.scale * 0.1
+        super().__init__(screen_width=width, xmin=-xlim, xmax=xlim, ymin=xlim, ymax=-xlim, title='R2D2')
+        self.xlim = xlim
+    def render(self):
+        # self.
+        self.draw_background(background_color=(255, 255, 255))
+        dw = self.dw
+        self.line("t1", (-self.xlim, 0), (self.xlim, 0), width=1, color=(0,) * 3)
+        self.line("t1", (0, -self.xlim), (0, self.xlim), width=1, color=(0,) * 3)
+
+
+        self.circle("r2d2", pos=(self.x[0], self.x[1]), r=24, outlineColor=(100, 100, 200), fillColor=(100, 100, 200))
+        self.circle("r2d2", pos=(self.x[0], self.x[1]), r=20, outlineColor=(100, 100, 200), fillColor=(150, 150, 255))
+        self.circle("r2d2", pos=(self.x[0], self.x[1]), r=2, outlineColor=(100, 100, 200), fillColor=(0,)*3)
+
+        dx = 0.13
+        dy = dx/2.5
+        wheel = [(-dx, dy), (dx, dy), (dx, -dy), (-dx, -dy) ]
+        ddy = 0.20
+        w1 = [ (x, y + ddy) for x, y in wheel]
+        w1 = rotate_around(w1, (0,0), angle=self.x[2] / np.pi * 180)
+
+        w2 = [(x, y - ddy) for x, y in wheel]
+        w2 = rotate_around(w2, (0, 0), angle=self.x[2] / np.pi * 180)
+
+
+        self.polygon("wheel1", coords=[ (x +  self.x[0], self.x[1] + y) for x, y in w1], filled=True, fillColor=(200,)*3, outlineColor=(100,)*3, closed=True)
+        self.polygon("wheel2", coords=[ (x +  self.x[0], self.x[1] + y) for x, y in w2], filled=True, fillColor=(200,)*3, outlineColor=(100,)*3, closed=True)
+
+        dc = 0.1
+        xx = self.x_target[0]
+        yy = self.x_target[1]
+        self.line("t1", (xx-dc, yy+dc), (xx+dc, yy-dc), width=4, color=(200, 100, 100))
+        self.line("t1", (xx-dc, yy-dc), (xx+dc, yy+dc), width=4, color=(200, 100, 100))
+
+
+    def update(self, x):
+        self.x = x

irlc/project2/yoda.py

+# This file may not be shared/redistributed without permission. Please read copyright notice in the git repo. If this file contains other copyright notices disregard this text.
+import numpy as np
+from scipy.linalg import expm  # Computes the matrix exponential e^A for a square matrix A
+from numpy.linalg import matrix_power  # Computes A^n for matrix A and integer n
+
+
+def get_A_B(g : float, L: float, m=0.1): 
+    r""" Compute the two matrices A, B (see Problem 1) here and return them.
+    The matrices should be numpy ndarrays. """
+    # TODO: 2 lines missing.
+    raise NotImplementedError("Compute numpy matrices A and B here")
+    return A, B
+
+
+def A_euler(g : float,L : float, Delta : float) -> np.ndarray: 
+    r""" Compute \tilde{A}_0 (Euler discretization), see Problem 2.
+
+    Hints:
+        * get_A_B can perhaps save you a line or two.
+    """
+    # TODO: 2 lines missing.
+    raise NotImplementedError("Implement function body")
+    return A0_tilde
+
+def A_ei(g : float,L : float, Delta : float) -> np.ndarray: 
+    r""" Compute A_0 (Exponential discretization), see Problem 2
+
+    Hints:
+        * The special function expm(X) computes the matrix exponential e^X. See the lecture notes for more information.
+    """
+    # TODO: 2 lines missing.
+    raise NotImplementedError("Implement function body")
+    return A0
+
+def M_euler(g : float, L : float, Delta : float, N : int) -> np.ndarray: 
+    r""" Compute \tilde{M} (Euler discretization), see Problem 3
+    Hints:
+        * the matrix_power(X,n) function can compute expressions such as X^n where X is a square matrix and n is a number
+    """
+    # TODO: 1 lines missing.
+    raise NotImplementedError("Implement function body")
+    return M_tilde
+
+def M_ei(g : float,L : float, Delta : float, N : int) -> np.ndarray: 
+    r""" Compute M (Exponential discretization), see Problem 3 """
+    # TODO: 1 lines missing.
+    raise NotImplementedError("Implement function body")
+    return M
+
+def xN_bound_euler(g : float, L : float,Delta : float,N : int) -> float: 
+    r""" Compute upper bound on |x_N| when using Euler discretization, see Problem 6.
+    The function should just return a number.
+
+    Hints:
+        * This function uses all input arguments.
+    """
+    # TODO: 1 lines missing.
+    raise NotImplementedError("Implement function body")
+    return bound
+
+def xN_bound_ei(g: float,L : float,Delta : float,N : int) -> float: 
+    r""" Compute upper bound on |x_N| when using exponential discretization, see Problem 7.
+
+    Hints:
+        * This function does NOT use all input arguments.
+        * This will be the hardest problem to solve, but the easiest function to implement.
+    """
+    # TODO: 1 lines missing.
+    raise NotImplementedError("Implement function body")
+    return bound
+
+if __name__ == '__main__':
+    g = 9.82 # gravitational constant
+    L = 5 # Length of string
+    m = 0.1 # Mass of pendulum (in kg)
+    Delta = 0.3 # Time-discretization constant Delta (in seconds)
+    N = 100 # Time steps
+
+    # Solve Problem 2
+    print("A0_euler")
+    print(A_euler(g, L, Delta))
+
+    print("A0_ei")
+    print(A_ei(g, L, Delta))
+
+    # Solve Problem 3
+    print("M_euler")
+    print(M_euler(g, L, Delta, N))
+
+    print("M_ei")
+    print(M_ei(g, L, Delta, N))
+
+    # Solve Problem 7, upper bound on x_N using Euler discretization
+    print("|x_N| <= ", xN_bound_euler(g, L, Delta, N))
+
+    # Solve Problem 8, upper bound on x_N using Exponential discretization
+    print("|x_N| <= ", xN_bound_ei(g, L, Delta, N))