Semester start

.gitignore

+**/__pycache__/*
+solutions/exam
+solutions
+exam_tabular_examples
+
+solutions/ex01
+solutions/ex02
+solutions/ex03
+solutions/ex04
+solutions/ex05
+solutions/ex06
+solutions/ex07
+solutions/ex08
+solutions/ex09
+solutions/ex10
+solutions/ex11
+solutions/ex12
+solutions/ex13
+
+#irlc/ex03
+#irlc/ex04
+#irlc/ex05
+#irlc/ex06
+#irlc/ex07
+#irlc/ex08
+#irlc/ex09
+#irlc/ex10
+#irlc/ex11
+#irlc/ex12
+#irlc/ex13
+irlc/tests/tests_week01.py
+irlc/tests/tests_week02.py
+irlc/tests/tests_week03.py
+irlc/tests/tests_week04.py
+irlc/tests/tests_week05.py
+irlc/tests/tests_week06.py
+irlc/tests/tests_week07.py
+irlc/tests/tests_week08.py
+irlc/tests/tests_week09.py
+irlc/tests/tests_week10.py
+irlc/tests/tests_week11.py
+irlc/tests/tests_week12.py
+irlc/tests/tests_week13.py
+
+# irlc/project2
+# irlc/project3
+# irlc/project3i
+irlc/project0/fruit*_complete*.py
+*tests_complete_grade*.py
+
+# irlc/exam/exam2024spring/*.zip
+# irlc/exam/exam2024spring/*.pdf
+
+irlc/exam/exam202*/*.zip
+irlc/exam/exam202*/*.pdf
+
+irlc/exam/exam2024august/*.zip
+irlc/exam/exam2024august/*.pdf
+
+irlc/exam/exam2025*/*.zip
+irlc/exam/exam2025*/*.pdf
+
+#irlc/exam/midterm2023a
+#irlc/exam/midterm2023b
+
+irlc/lectures/lec01
+irlc/lectures/lec02
+irlc/lectures/lec03
+irlc/lectures/lec04
+irlc/lectures/lec05
+irlc/lectures/lec06
+irlc/lectures/lec07
+irlc/lectures/lec08
+irlc/lectures/lec09
+irlc/lectures/lec10
+irlc/lectures/lec11
+irlc/lectures/lec12
+irlc/lectures/lec13
+
+
+

.vscode/keybindings.json

+[
+    {
+        "key": "shift+alt+e",
+        "command": "editor.debug.action.selectionToRepl"
+    }
+]

.vscode/settings.json

+{
+    "python.testing.unittestArgs": [
+        "-v",
+        "-s",
+        "./irlc",
+        "-p", "*tests*.py"
+    ],
+    "python.testing.pytestEnabled": false,
+    "python.testing.unittestEnabled": true,
+
+}

LICENSE

+LICENCE
+
+This repository contains code for the course 02465, Introduction to reinforcement learning and control theory.
+
+Some of the code in this repository is not written by me and the licensing terms are as indicated in the relevant files.
+
+Files authored by me or other DTU employees is only intended for educational purposes for students enrolled in the course. 
+Please do not re-distribute or publish this code without written permision from Tue Herlau (tuhe@dtu.dk).
\ No newline at end of file

README.md

+# 02465 Introduction to reinforcement learning and control theory
+
+This repository contains code for 02465, introduction to machine learning and control theory. For installation instructions and course material please see:
+
+ - https://www2.compute.dtu.dk/courses/02465/information/installation.html

irlc/__init__.py

+""" Source code for 02466, Introduction to reinforcement learning and control, offered at DTU """
+__version__ = "0.0.1"
+
+# Do not import Matplotlib (or imports which import matplotlib) in case you have to run in headless mode.
+import shutil
+import inspect
+import pickle
+
+import gymnasium
+import numpy as np
+import os
+
+# Global imports from across the API. Allows imports like
+# > from irlc import Agent, train
+from irlc.utils.irlc_plot import main_plot as main_plot
+from irlc.utils.irlc_plot import plot_trajectory as plot_trajectory
+try:
+    from irlc.ex01.agent import Agent as Agent, train as train
+    from irlc.ex09.rl_agent import TabularAgent, ValueAgent
+except ImportError:
+    pass
+from irlc.utils.player_wrapper import interactive as interactive
+from irlc.utils.lazylog import LazyLog # This one is unclear. Is it required?
+from irlc.utils.timer import Timer
+
+def get_irlc_base():
+    dir_path = os.path.dirname(os.path.realpath(__file__))
+    return dir_path
+
+def get_students_base():
+    return os.path.join(get_irlc_base(), "../../../02465students/")
+
+
+def pd2latex_(pd, index=False, escape=False, column_spec=None, **kwargs): # You can add column specs.
+    for c in pd.columns:
+        if pd[c].values.dtype == 'float64' and all(pd[c].values - np.round(pd[c].values)==0):
+            pd[c] = pd[c].astype(int)
+    ss = pd.to_latex(index=index, escape=escape, **kwargs)
+    return fix_bookstabs_latex_(ss,column_spec=column_spec)
+
+def fix_bookstabs_latex_(ss, linewidth=True, first_column_left=True, column_spec=None):
+    to_tabular_x = linewidth
+
+    if to_tabular_x:
+        ss = ss.replace("tabular", "tabularx")
+    lines = ss.split("\n")
+    hd = lines[0].split("{")
+    if column_spec is None:
+        adj = (('l' if to_tabular_x else 'l') if first_column_left else 'C') + ("".join(["C"] * (len(hd[-1][:-1]) - 1)))
+    else:
+        adj = column_spec
+
+    # adj = ( ('l' if to_tabular_x else 'l') if first_column_left else 'C') + ("".join(["C"] * (len(hd[-1][:-1])-1)))
+    if linewidth:
+        lines[0] = "\\begin{tabularx}{\\linewidth}{" + adj + "}"
+    else:
+        lines[0] = "\\begin{tabular}{" + adj.lower() + "}"
+
+    ss = '\n'.join(lines)
+    return ss
+
+def plotenv(env : gymnasium.Env):
+    """
+    Given a Gymnasium environment instance, this function will plot the environment as a matplotlib image. Remember to call ``plt.show()`` to actually see the image.
+
+    For this function to work, you must create the environment with :python:`render_mode='human'`.
+
+    .. note::
+
+        This function may not work for all gymnasium environments, however, it will work for most environments we use in this course.
+
+    :param env: The environment to plot.
+    """
+
+    from PIL import Image
+    import matplotlib.pyplot as plt
+    if hasattr(env, 'render_mode') and not env.render_mode == 'rgb_array':
+        env.render_mode, rmt = 'rgb_array', env.render_mode
+    frame = env.render()
+    if hasattr(env, 'render_mode') and not env.render_mode == 'rgb_array':
+        env.render_mode = rmt
+
+    im = Image.fromarray(frame)
+
+    plt.figure(figsize=(16, 16))
+    plt.imshow(im)
+    plt.axis('off')
+    plt.tight_layout()
+
+
+
+
+def _savepdf_env(file, env):
+    from PIL import Image
+    import matplotlib.pyplot as plt
+    if hasattr(env, 'render_mode') and not env.render_mode == 'rgb_array':
+        env.render_mode, rmt = 'rgb_array', env.render_mode
+    frame = env.render()
+    if hasattr(env, 'render_mode') and not env.render_mode == 'rgb_array':
+        env.render_mode = rmt
+
+    im = Image.fromarray(frame)
+    snapshot_base = file
+    if snapshot_base.endswith(".png"):
+        sf = snapshot_base[:-4]
+        fext = 'png'
+    else:
+        fext = 'pdf'
+        if snapshot_base.endswith(".pdf"):
+            sf = snapshot_base[:-4]
+        else:
+            sf = snapshot_base
+
+    sf = f"{sf}.{fext}"
+    dn = os.path.dirname(sf)
+    if len(dn) > 0 and not os.path.isdir(dn):
+        os.makedirs(dn)
+    print("Saving snapshot of environment to", os.path.abspath(sf))
+    if fext == 'png':
+        im.save(sf)
+        from irlc import _move_to_output_directory
+        _move_to_output_directory(sf)
+    else:
+        plt.figure(figsize=(16, 16))
+        plt.imshow(im)
+        plt.axis('off')
+        plt.tight_layout()
+        from irlc import savepdf
+        savepdf(sf, verbose=True)
+        # plt.show()
+
+
+
+def savepdf(pdf, verbose=False, watermark=False, env=None):
+    """
+    Convenience function for saving PDFs. Just call it after you have created your plot as ``savepdf('my_file.pdf')``
+    to save a PDF of the plot.
+    You can also pass an environment, in which case the environment will be stored to a pdf file.
+
+
+    :param pdf: The file to save to, for instance ``"my_pdf.pdf"``
+    :param verbose: Print output destination (optional)
+    :param watermark: Include a watermark (optional)
+    :return: Full path of the created PDF.
+    """
+    if env is not None:
+        _savepdf_env(pdf, env)
+        return
+
+    import matplotlib.pyplot as plt
+    pdf = os.path.normpath(pdf.strip())
+    pdf = pdf+".pdf" if not pdf.endswith(".pdf") else pdf
+
+    if os.sep in pdf:
+        pdf = os.path.abspath(pdf)
+    else:
+        pdf = os.path.join(os.getcwd(), "pdf", pdf)
+    if not os.path.isdir(os.path.dirname(pdf)):
+        os.makedirs(os.path.dirname(pdf))
+
+
+
+    # filename = None
+    stack = inspect.stack()
+    modules = [inspect.getmodule(s[0]) for s in inspect.stack()]
+    files = [m.__file__ for m in modules if m is not None]
+    if any( [f.endswith("RUN_OUTPUT_CAPTURE.py") for f in files] ):
+        return
+
+    # for s in stack:
+    #     print(s)
+    # print(stack)
+    # for k in range(len(stack)-1, -1, -1):
+    #     frame = stack[k]
+    #     module = inspect.getmodule(frame[0])
+    #     filename = module.__file__
+    #     print(filename)
+    #     if not any([filename.endswith(f) for f in ["pydev_code_executor.py", "pydevd.py", "_pydev_execfile.py", "pydevconsole.py", "pydev_ipython_console.py"] ]):
+    #         # print("breaking c. debugger", filename)
+    #         break
+    # if any( [filename.endswith(f) for f in ["pydevd.py", "_pydev_execfile.py"]]):
+    #     print("pdf path could not be resolved due to debug mode being active in pycharm", filename)
+    #     return
+    # print("Selected filename", filename)
+    # wd = os.path.dirname(filename)
+    # pdf_dir = wd +"/pdf"
+    # if filename.endswith("_RUN_OUTPUT_CAPTURE.py"):
+    #     return
+    # if not os.path.isdir(pdf_dir):
+    #     os.mkdir(pdf_dir)
+    wd = os.getcwd()
+    irlc_base = os.path.dirname(__file__)
+    if False:
+        pass
+    else:
+        plt.savefig(fname=pdf)
+    outf = os.path.normpath(os.path.abspath(pdf))
+    print("> [savepdf]", pdf + (f" [full path: {outf}]" if verbose else ""))
+
+    return outf
+
+
+def _move_to_output_directory(file):
+    """
+    Hidden function: Move file given file to static output dir.
+    """
+    if not is_this_my_computer():
+        return
+    CDIR = os.path.dirname(os.path.realpath(__file__)).replace('\\', '/')
+    shared_output_dir = CDIR + "/../../shared/output"
+    shutil.copy(file, shared_output_dir + "/"+ os.path.basename(file) )
+
+
+def bmatrix(a):
+    if False:
+        return a.__str__()
+    else:
+        np.set_printoptions(suppress=True)
+        """Returns a LaTeX bmatrix
+        :a: numpy array
+        :returns: LaTeX bmatrix as a string
+        """
+        if len(a.shape) > 2:
+            raise ValueError('bmatrix can at most display two dimensions')
+        lines = str(a).replace('[', '').replace(']', '').splitlines()
+        rv = [r'\begin{bmatrix}']
+        rv += ['  ' + ' & '.join(l.split()) + r'\\' for l in lines]
+        rv +=  [r'\end{bmatrix}']
+        return '\n'.join(rv)
+
+
+def is_this_my_computer():
+    CDIR = os.path.dirname(os.path.realpath(__file__)).replace('\\', '/')
+    return os.path.exists(CDIR + "/../../Exercises")
+
+def cache_write(object, file_name, only_on_professors_computer=False, verbose=True, protocol=-1): # -1 is default protocol. Fix crash issue with large files.
+    import lzma
+    if only_on_professors_computer and not is_this_my_computer():
+        """ Probably for your own good :-). """
+        return
+
+    dn = os.path.dirname(file_name)
+    if not os.path.exists(dn):
+        os.mkdir(dn)
+    if verbose: print("Writing cache...", file_name)
+    with lzma.open(file_name, 'wb') as f:
+        pickle.dump(object, f)
+        # compress_pickle.dump(object, f, compression="lzma", protocol=protocol)
+    if verbose:
+        print("Done!")
+
+
+def cache_exists(file_name):
+    return os.path.exists(file_name)
+
+def cache_read(file_name):
+    import lzma
+    if os.path.exists(file_name):
+        with lzma.open(file_name, 'rb') as f:
+            return pickle.load(f)
+    else:
+        return None

irlc/car/__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.

irlc/car/car_model.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.car.car_viewer import CarViewer
+from irlc.car.car_viewer import CarViewerPygame
+import numpy as np
+import sympy as sym
+from scipy.optimize import Bounds
+from gymnasium.spaces import Box
+from irlc.car.sym_map import SymMap, wrap_angle
+from irlc.ex03.control_model import ControlModel
+from irlc.ex03.control_cost import SymbolicQRCost
+from irlc.ex04.discrete_control_model import DiscreteControlModel
+from irlc.ex04.control_environment import ControlEnvironment
+from sympy import Heaviside
+
+# from irlc.ex03.control_specification import ControlSpecification
+
+"""
+class MySpecification():
+    def get_bounds(self):
+        return bounds
+
+    def get_cost(self):
+        pass
+
+    def sym_f(self):
+        return ...
+
+    def simulate(self):
+        # Simulate using RK4.
+
+        pass
+
+
+spec = MySpecification()
+model = Model(spec)
+model.simulate(...)
+
+
+
+"""
+
+
+class SymbolicBicycleModel(ControlModel):
+    metadata = {
+        'render_modes': ['human', 'rgb_array'],
+        'render_fps': 30
+    }
+    def __init__(self, map_width=0.8, simple_bounds=None, cost=None, hot_start=False, verbose=True):
+        s = """
+        Coordinate system of the car:
+        State x consist of
+        x[0] = Vx (speed in direction of the car body)
+        x[1] = Vy (speed perpendicular to car body)
+        x[2] = wz (Yaw rate; how fast the car is turning)
+        x[3] = e_psi (Angle of rotation between car body and centerline)
+        x[4] = s (How far we are along the track)
+        x[5] = e_y (Distance between car body and closest point on centerline)
+
+        Meanwhile the actions are
+        u[0] : Angle between wheels and car body (i.e. are we steering to the right or to the left)
+        u[1] : Engine force (applied to the rear wheels, i.e. accelerates car)
+        """
+        if verbose:
+            print(s)
+        # if simple_bounds is None:
+        #     simple_bounds = dict()
+        self.map = SymMap(width=map_width)
+        self.v_max = 3.0
+
+        self.viewer = None  # rendering
+        self.hot_start = hot_start
+        # self.observation_space = Box(low=np.asarray([-np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -map_width], dtype=float),
+        #                              high=np.asarray([v_max, np.inf, np.inf, np.inf, np.inf, map_width]), dtype=float)
+        # self.action_space = Box(low=np.asarray([-0.5, -1]), high=np.asarray([0.5, 1]), dtype=float)
+
+        # xl = np.zeros((6,))
+        # xl[4] = self.map.TrackLength
+        # simple_bounds = {'x0': Bounds([-np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -map_width], [v_max, np.inf, np.inf, np.inf, np.inf, map_width]),
+        #                 'xF': Bounds(list(xl), list(xl)), **simple_bounds}
+        # n = 6
+        # d = 2
+        # if cost is None:
+        #     cost = SymbolicQRCost(Q=np.zeros((6,6)), R=np.eye(2)*10, qc=0*1.)
+        # bounds = dict(x_low=[-np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -map_width], x_high=[self.v_max, np.inf, np.inf, np.inf, np.inf, map_width],
+        #               u_low=[-0.5, -1], u_high=[0.5, 1])
+
+        super().__init__()
+
+    def get_cost(self) -> SymbolicQRCost:
+        return SymbolicQRCost(Q=np.zeros((6,6)), R=np.eye(2)*10, qc=1.*0)
+
+    def x_bound(self) -> Box:
+        return Box(np.asarray([-np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -self.map.width]),
+                   np.asarray([self.v_max, np.inf, np.inf, np.inf, np.inf, self.map.width]))
+
+    def u_bound(self) -> Box:
+        return Box(np.asarray([-0.5, -1]),np.asarray([0.5, 1]))
+
+    def render(self, x, render_mode='human'):
+        if self.viewer == None:
+            self.viewer = CarViewerPygame(self)
+
+        self.viewer.update(self.x_curv2x_XY(x))
+        return self.viewer.blit(render_mode=render_mode)
+        # return self.viewer.render(return_rgb_array=mode == 'rgb_array')
+
+    def close(self):
+        if self.viewer is not None:
+            self.viewer.close()
+
+    def x_curv2x_XY(self, x_curv):
+        """
+        Utility function for converting x (including velocities, etc.) from local (curvilinear) coordinates to global XY position.
+        """
+        Xc, Yc, vangle = self.map.getGlobalPosition(s=x_curv[4], ey=x_curv[5], epsi=x_curv[3])
+        dglob = np.asarray([x_curv[0], x_curv[1], x_curv[2], vangle, Xc, Yc])
+        return dglob
+
+    def sym_f(self, x, u, t=None, curvelinear_coordinates=True, curvature_s=None):
+        r"""
+        Implements the symbolic dynamics
+
+        .. math::
+
+            \dot{x} = f(x, u)
+
+        We will both create it in curvelinear coordinates or normal (global) coordinates.
+        """
+        # Vehicle Parameters
+        m = 1.98
+        lf = 0.125
+        lr = 0.125
+        Iz = 0.024
+        Df = 0.8 * m * 9.81 / 2.0
+        Cf = 1.25
+        Bf = 1.0
+        Dr = 0.8 * m * 9.81 / 2.0
+        Cr = 1.25
+        Br = 1.0
+
+        vx = x[0]
+        vy = x[1]
+        wz = x[2]
+        if curvelinear_coordinates:
+            epsi = x[3]
+            s = x[4]
+            ey = x[5]
+        else:
+            psi = x[3]
+
+        delta = u[0]
+        a = u[1]
+
+        alpha_f = delta - sym.atan2(vy + lf * wz, vx)
+        alpha_r = -sym.atan2(vy - lf * wz, vx)
+
+        # Compute lateral force at front and rear tire
+        Fyf = 2 * Df * sym.sin(Cf * sym.atan(Bf * alpha_f))
+        Fyr = 2 * Dr * sym.sin(Cr * sym.atan(Br * alpha_r))
+
+        d_vx = (a - 1 / m * Fyf * sym.sin(delta) + wz * vy)
+        d_vy = (1 / m * (Fyf * sym.cos(delta) + Fyr) - wz * vx)
+        d_wz = (1 / Iz * (lf * Fyf * sym.cos(delta) - lr * Fyr))
+
+        if curvelinear_coordinates:
+            # cur = 0
+            cur = self.map.sym_curvature(s)
+            d_epsi = (wz - (vx * sym.cos(epsi) - vy * sym.sin(epsi)) / (1 - cur * ey) * cur)
+            d_s = ((vx * sym.cos(epsi) - vy * sym.sin(epsi)) / (1 - cur * ey))
+            """
+            Compute derivative of e_y here (d_ey). See paper for details. 
+            """
+            d_ey = (vx * sym.sin(epsi) + vy * sym.cos(epsi)) # Old ex here ! b ! b
+            # implement the ODE governing ey (distane from center of road) in curveliner coordinates
+            xp = [d_vx, d_vy, d_wz, d_epsi, d_s, d_ey]
+
+        else:
+            d_psi = wz
+            d_X = ((vx * sym.cos(psi) - vy * sym.sin(psi)))
+            d_Y = (vx * sym.sin(psi) + vy * sym.cos(psi))
+
+            xp = [d_vx, d_vy, d_wz, d_psi, d_X, d_Y]
+        return xp
+
+    def fix_angles(self, x):
+        # fix angular component of x
+        if x.size == self.state_size:
+            x[3] = wrap_angle(x[3])
+        elif x.shape[1] == self.state_size:
+            x[:,3] = wrap_angle(x[:,3])
+        return x
+
+
+class DiscreteCarModel(DiscreteControlModel): 
+    def __init__(self, dt=0.1, cost=None, **kwargs): 
+        model = SymbolicBicycleModel(**kwargs)
+        # self.observation_space = model.observation_space
+        # self.action_space = model.action_space 
+        # n = 6
+        # d = 2
+        # if cost is None:
+        #     from irlc.ex04.cost_discrete import DiscreteQRCost
+        #     cost = DiscreteQRCost(Q=np.zeros((model.state_size, model.state_size)), R=np.eye(model.action_size))
+        super().__init__(model=model, dt=dt, cost=cost)
+        # self.cost = cost
+        self.map = model.map
+
+
+class CarEnvironment(ControlEnvironment): 
+    def __init__(self, Tmax=10, noise_scale=1.0, cost=None, max_laps=10, hot_start=False, render_mode=None, **kwargs):
+        discrete_model = DiscreteCarModel(cost=cost, hot_start=hot_start, **kwargs)
+        super().__init__(discrete_model, Tmax=Tmax, render_mode=render_mode) 
+        self.map = discrete_model.map
+        self.noise_scale = noise_scale
+        self.cost = cost
+        self.completed_laps = 0
+        self.max_laps = max_laps
+
+    def simple_bounds(self):
+        simple_bounds = {'x': Bounds(self.observation_space.low, self.observation_space.high),
+                         't0': Bounds([0], [0]),
+                         'u': Bounds(self.action_space.low, self.action_space.high)}
+        return simple_bounds
+
+    """ We add a bit of noise for backward compatibility.  """
+    def step(self, u):
+        # We don't want to render the car before we have added jitter (below). These lines therefore disable rendering
+        self.render_mode, rmt_ = None, self.render_mode
+        xp, cost, terminated, truncated, info = super().step(u)
+        self.render_mode = rmt_
+
+        x = xp
+        if hasattr(self, 'seed') and self.seed is not None and not callable(self.seed):
+            np.random.seed(self.seed)
+
+        noise_vx = np.maximum(-0.05, np.minimum(np.random.randn() * 0.01, 0.05))
+        noise_vy = np.maximum(-0.1, np.minimum(np.random.randn() * 0.01, 0.1))
+        noise_wz = np.maximum(-0.05, np.minimum(np.random.randn() * 0.005, 0.05))
+        if True: #self.noise_scale > 0:
+            x[0] = x[0] + 0.03 * noise_vx #* self.noise_scale
+            x[1] = x[1] + 0.03 * noise_vy #* self.noise_scale
+            x[2] = x[2] + 0.03 * noise_wz #* self.noise_scale
+
+        if x[4] > self.map.TrackLength:
+            self.completed_laps += 1
+            x[4] -= self.map.TrackLength
+
+        done = self.completed_laps >= self.max_laps
+        if x[4] < 0:
+            assert(False)
+        if self.render_mode == 'human':
+            self.render()
+        return x, cost, done, False, info
+
+    def L(self, x):
+        '''
+        Implement whether we have obtained the terminal condition. see eq. 4 in "Autonomous Racing using LMPC"
+
+        :param x:
+        :return:
+        '''
+        return x[4] > self.map.TrackLength
+
+    def epoch_reset(self, x):
+        '''
+        After completing one epoch, i.e. when L(x) == True, reset the x-vector using this method to
+        restart the epoch. In practice, take one more lap on the track.
+
+        :param x:
+        :return:
+        '''
+        x = x.copy()
+        x[4] -= self.map.TrackLength
+        return x
+
+    def _get_initial_state(self):
+        x0 = np.zeros((6,))
+        if self.discrete_model.continuous_model.hot_start:
+            x0[0] = 0.5  # Start velocity is 0.5
+        # self.render()
+        return x0
+
+
+
+def _alt_curv(map, s):
+    s = s - map.TrackLength * sym.floor(s / map.TrackLength)
+    n = map.PointAndTangent.shape[0]
+    # pw = []
+
+    # lt =  s - (map.PointAndTangent[:, 3] + map.PointAndTangent[:, 4]) <= 0
+
+
+    x = map.PointAndTangent[:, 3] + map.PointAndTangent[:, 4]
+    y = map.PointAndTangent[:, 5]
+
+
+    yy = 0
+    for i in range(n):
+        irange = Heaviside( x[i] - s ) #  (x[i] < s < x[i+1] ) #if i < n-1 else  (x[i] < s )
+        if i > 0:
+            irange = irange * Heaviside( s - x[i -1 ] )
+        # tau = ( s - x[i]) / ( x[i+1] - x[i])
+        yy += y[i] * irange
+        # yy += ( y[i] * (1-tau) + y[i+1]*tau ) * irange
+
+    return yy
+
+
+    for i in range(n):
+        pw.append( (map.PointAndTangent[i,5], s - (map.PointAndTangent[i, 3] + map.PointAndTangent[i, 4]) <= 0) )
+
+
+    p = sym.Piecewise(*pw)
+    return p
+
+
+if __name__ == "__main__":
+    env = CarEnvironment()
+    s = sym.Symbol('s')
+    map = env.discrete_model.continuous_model.map
+    s0 = 3.1
+    from irlc.utils.timer import Timer
+    t = Timer()
+    for s0 in np.linspace(0, 2*map.TrackLength, 230):
+        t.tic("y1")
+        c = map.sym_curvature(s)
+        y1 = c.subs(s, s0)
+        t.toc()
+        t.tic("y2")
+        x = _alt_curv(map, s)
+        y2 = x.subs(s,s0)
+        t.toc()
+        if np.abs(y1- y2) > 1e-5:
+            print(y1, y2, y1-y2)
+    print( t.display() )
+
+    env = CarEnvironment(render_mode='human')
+    env.metadata['render_fps'] = 10000
+    env.reset()
+    import time
+    t0 = time.time()
+    n = 300
+    for _ in range(n):
+        u = env.action_space.sample()
+        u[0] = 0
+        u[1] = 0.01
+        s, cost, done, truncated, info = env.step(u)
+    env.close()
+    tpf = (time.time()- t0)/n
+    print("TPF", tpf, "fps", 1/tpf)

irlc/car/car_viewer.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 pygame
+from irlc.utils.graphics_util_pygame import UpgradedGraphicsUtil
+import numpy as np
+
+track_outline = (0, 0, 0)
+track_middle = (220, 25, 25)
+
+class CarViewerPygame(UpgradedGraphicsUtil):
+    def __init__(self, car):
+        n = int(10 * (car.map.PointAndTangent[-1, 3] + car.map.PointAndTangent[-1, 4]))
+        center = [car.map.getGlobalPosition(i * 0.1, 0) for i in range(n)]
+        outer = [car.map.getGlobalPosition(i * 0.1, -car.map.width) for i in range(n)]
+        inner = [car.map.getGlobalPosition(i * 0.1, car.map.width) for i in range(n)]
+        fudge = 0.2
+        xs, ys = zip(*outer)
+        super().__init__(screen_width=1000, xmin=min(xs) - fudge, xmax=max(xs) + fudge,
+                         ymax=min(ys) - fudge, ymin=max(ys) + fudge, title="Racecar environment")
+        self.center = center
+        self.outer = outer
+        self.inner = inner
+        from irlc.utils.graphics_util_pygame import Object
+        self.car = Object("car.png", image_width=90)
+
+
+    def render(self):
+        green = (126, 200, 80)
+        track = (144,)*3
+        self.draw_background(background_color=green)
+
+        self.polygon("safd", self.outer, fillColor=track, outlineColor=track_outline, width=3)
+        self.polygon("in", self.inner, fillColor=green, outlineColor=track_outline, width=3)
+        self.polygon("in", self.center, fillColor=None, filled=False, outlineColor=(100, 100, 100), width=5)
+        # Now draw the pretty car.
+        x, y, psi = self.xglob[4], self.xglob[5], self.xglob[3]
+        xy = self.fixxy((x,y))
+        self.car.rect.center = xy
+
+        self.car.rotate(psi / (2*np.pi) * 360)
+        self.car.blit(self.surf)
+        self.circle("in", (x,y), 4, fillColor=(255, 0, 0)) # drawn on the center of the car.
+
+    def update(self, xglob):
+        self.xglob = xglob

irlc/car/sym_map.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 pdb
+import matplotlib.pyplot as plt
+import numpy as np
+import numpy.linalg as la
+import sympy as sym
+
+"""
+This is a bunch of pretty awful code to define a map and compute useful quantities like tangents, etc. 
+Defining a map is pretty straight forward (it consist of circle archs and lines), but 
+don't try to read on.
+"""
+
+# class TrackInterpolant(sym.Function):
+#     # def __init__(self, map):
+#     #     self.map = map
+#
+#     @classmethod
+#     def eval(self, x):
+#         s = x
+#         self = self.map
+#         s = s - self.TrackLength * sym.floor(s / self.TrackLength)
+#         n = self.PointAndTangent.shape[0]
+#         x = self.PointAndTangent[:, 3] + self.PointAndTangent[:, 4]
+#         y = self.PointAndTangent[:, 5]
+#         yy = 0
+#
+#         for i in range(n):
+#             irange = sym.sign(x[i] - s)
+#             if i > 0:
+#                 irange = irange * sym.sign(s - x[i - 1])
+#             yy += y[i] * irange
+#         return yy
+#
+#     @classmethod
+#     def diff(self, *symbols, **assumptions):
+#         return 0
+#
+#     def __str__(self):
+#         return "ff(s)"
+
+
+class SymMap:
+    def plot(self, show=False):
+        PointAndTangent, TrackLength, extra = self.spec2PointAndTangent(self.spec)
+        for i in range(PointAndTangent.shape[0]-1):
+            extra_ = extra[i]
+            if 'CenterX' in extra_:
+                CenterX, CenterY = extra_['CenterX'], extra_['CenterY']
+                angle, spanAng = extra_['angle'], extra_['spanAng']
+                r = self.spec[i,1]
+                direction = 1 if r >= 0 else -1
+
+                # Plotting. Ignore this
+                plt.plot(CenterX, CenterY, 'ro')
+                tt = np.linspace(angle, angle + direction * spanAng)
+                plt.plot(CenterX + np.cos(tt) * np.abs(r), CenterY + np.abs(r) * np.sin(tt), 'r-')
+
+        x, y = PointAndTangent[:, 0], PointAndTangent[:, 1]
+        plt.plot(x, y, '.-')
+        print(np.sum(np.sum(np.abs(self.PointAndTangent - PointAndTangent))))
+
+        if show:
+            plt.show()
+    '''
+    Format:
+        PointAndTangent = [x, 
+        y, 
+        psi: angle of tangent vector at the last point of segment, 
+        total-distance-travelled, 
+        segment-length, curvature]
+    
+    Also creates a symbolic expression to evaluate track position.    
+    '''
+    def spec2PointAndTangent(self, spec):
+        # also create a symbolic piecewise expression to evaluate the curvature as a function of track length location.
+
+        # spec = self.spec
+        # PointAndTangent = self.PointAndTangent.copy()
+        PointAndTangent = np.zeros((spec.shape[0] + 1, 6))
+        extra = []
+
+        N = spec.shape[0]
+        segment_s_cur = 0  # Distance travelled to start of segment (s-coordinate).
+        angle_prev = 0  # Angle of the tangent vector at the starting point of the segment
+        x_prev, y_prev = 0, 0  # x,y coordinate of last point of previous segment.
+        for i in range(N):
+            l, r = spec[i,0], spec[i,1]  # Length of segment and radius of curvature
+            ang = angle_prev  # Angle of the tangent vector at the starting point of the segment
+
+            if r == 0.0:              # If the current segment is a straight line
+                x = x_prev + l * np.cos(ang)  # x coordinate of the last point of the segment
+                y = y_prev + l * np.sin(ang)  # y coordinate of the last point of the segment
+                psi = ang  # Angle of the tangent vector at the last point of the segment
+                curvature = 0
+                extra_ = {}
+            else:
+                direction = 1 if r >= 0 else -1
+                CenterX = x_prev + np.abs(r) * np.cos(ang + direction * np.pi / 2)  # x coordinate center of circle
+                CenterY = y_prev + np.abs(r) * np.sin(ang + direction * np.pi / 2)  # y coordinate center of circle
+                spanAng = l / np.abs(r)  # Angle spanned by the circle
+                psi = wrap_angle(ang + spanAng * np.sign(r))  # Angle of the tangent vector at the last point of the segment
+                angleNormal = wrap_angle((direction * np.pi / 2 + ang))
+                angle = -(np.pi - np.abs(angleNormal)) * (sign(angleNormal))
+                x = CenterX + np.abs(r) * np.cos(angle + direction * spanAng)  # x coordinate of the last point of the segment
+                y = CenterY + np.abs(r) * np.sin(angle + direction * spanAng)  # y coordinate of the last point of the segment
+                curvature = 1/r
+
+                extra_ = {'CenterX': CenterX,
+                          'CenterY': CenterY,
+                          'angle': angle,
+                          'direction': direction,
+                          'spanAng': spanAng}
+
+            extra.append(extra_)
+            NewLine = np.array([x, y, psi, segment_s_cur, l, curvature])
+            PointAndTangent[i, :] = NewLine  # Write the new info
+            x_prev, y_prev, angle_prev = PointAndTangent[i, 0], PointAndTangent[i, 1], PointAndTangent[i, 2]
+            segment_s_cur += l
+
+        xs = PointAndTangent[-2, 0]
+        ys = PointAndTangent[-2, 1]
+        xf = 0
+        yf = 0
+        psif = 0
+
+        l = np.sqrt((xf - xs) ** 2 + (yf - ys) ** 2)
+
+        NewLine = np.array([xf, yf, psif, PointAndTangent[-2, 3] + PointAndTangent[-2, 4], l, 0])
+        PointAndTangent[-1, :] = NewLine
+        TrackLength = PointAndTangent[-1, 3] + PointAndTangent[-1, 4]
+
+        return PointAndTangent, TrackLength, extra
+
+
+    """map object
+    Attributes:
+        getGlobalPosition: convert position from (s, ey) to (X,Y)
+    """
+    def __init__(self, width):
+        """Initialization
+        width: track width
+        Modify the vector spec to change the geometry of the track
+        """
+        self.width = width
+        self.halfWidth = 0.4
+        self.slack = 0.45
+        lengthCurve = 3.5  # 3.0
+        straight = 1.0
+        spec = np.array([[1.0, 0],
+                         [lengthCurve, lengthCurve / np.pi],
+                         # Note s = 1 * np.pi / 2 and r = -1 ---> Angle spanned = np.pi / 2
+                         [straight, 0],
+                         [lengthCurve / 2, -lengthCurve / np.pi],
+                         [straight, 0],
+                         [lengthCurve, lengthCurve / np.pi],
+                         [lengthCurve / np.pi * 2 + 1.0, 0],
+                         [lengthCurve / 2, lengthCurve / np.pi]])
+
+
+        PointAndTangent, TrackLength, extra = self.spec2PointAndTangent(spec)
+        self.PointAndTangent = PointAndTangent
+        self.TrackLength = TrackLength
+        self.spec = spec
+
+    def _alt_curv(self, s):
+        return sym.floor(s)
+        return sym.sign(s)
+        s = s - self.TrackLength * sym.floor(s / self.TrackLength)
+        return sym.floor(s)
+
+        n = self.PointAndTangent.shape[0]
+        x = self.PointAndTangent[:, 3] + self.PointAndTangent[:, 4]
+        y = self.PointAndTangent[:, 5]
+        yy = 0
+        return sym.sign(x[0] - s)
+
+        for i in range(n):
+            irange = sym.sign(x[i] - s)
+            if i > 0:
+                irange = irange * sym.sign(s - x[i - 1])
+            yy += y[i] * irange
+        return yy
+
+
+    def sym_curvature(self, s):
+        s = s - self.TrackLength * sym.floor(s / self.TrackLength)
+        n = self.PointAndTangent.shape[0]
+        pw = []
+        for i in range(n):
+            pw.append( (self.PointAndTangent[i,5], s - (self.PointAndTangent[i, 3] + self.PointAndTangent[i, 4]) <= 0) )
+        p = sym.Piecewise(*pw)
+        return p
+
+    def getGlobalPosition(self, s, ey, epsi=None, vangle_true=None):
+        """coordinate transformation from curvilinear reference frame (e, ey) to inertial reference frame (X, Y)
+        (s, ey): position in the curvilinear reference frame
+        """
+        s = np.mod(s, self.TrackLength)
+        PointAndTangent = self.PointAndTangent
+        index = np.all([[s >= PointAndTangent[:, 3]], [s < PointAndTangent[:, 3] + PointAndTangent[:, 4]]], axis=0)
+        dx = np.where(np.squeeze(index))
+        if len(dx) < 1:
+            raise Exception("Cannot find map position. Is the map empty?")
+        try:
+            i = int(np.where(np.squeeze(index))[0])
+        except Exception as e:
+            print(e)
+
+        if PointAndTangent[i, 5] == 0.0:  # If segment is a straight line
+            # Extract the first final and initial point of the segment
+            xf = PointAndTangent[i, 0]
+            yf = PointAndTangent[i, 1]
+            xs = PointAndTangent[i - 1, 0]
+            ys = PointAndTangent[i - 1, 1]
+            psi = PointAndTangent[i, 2]
+
+            # Compute the segment length
+            deltaL = PointAndTangent[i, 4]
+            reltaL = s - PointAndTangent[i, 3]
+
+            # Do the linear combination
+            x = (1 - reltaL / deltaL) * xs + reltaL / deltaL * xf + ey * np.cos(psi + np.pi / 2)
+            y = (1 - reltaL / deltaL) * ys + reltaL / deltaL * yf + ey * np.sin(psi + np.pi / 2)
+            if epsi is not None:
+                vangle = psi + epsi
+        else:
+            r = 1 / PointAndTangent[i, 5]  # Extract curvature
+            ang = PointAndTangent[i - 1, 2]  # Extract angle of the tangent at the initial point (i-1)
+            # Compute the center of the arc
+            direction = 1 if r >= 0 else -1
+
+            CenterX = PointAndTangent[i - 1, 0] + np.abs(r) * np.cos(ang + direction * np.pi / 2)  # x coordinate center of circle
+            CenterY = PointAndTangent[i - 1, 1] + np.abs(r) * np.sin(ang + direction * np.pi / 2)  # y coordinate center of circle
+
+            spanAng = (s - PointAndTangent[i, 3]) / (np.pi * np.abs(r)) * np.pi
+
+            angleNormal = wrap_angle(direction * np.pi / 2 + ang)
+
+            angle = -(np.pi - np.abs(angleNormal)) * (sign(angleNormal))
+
+            x = CenterX + (np.abs(r) - direction * ey) * np.cos(angle + direction * spanAng)  # x coordinate of the last point of the segment
+            y = CenterY + (np.abs(r) - direction * ey) * np.sin(angle + direction * spanAng)  # y coordinate of the last point of the segment
+
+            if epsi is not None:
+                vangle = epsi + direction * spanAng + PointAndTangent[i - 1, 2]
+
+        if epsi is None:
+            return x,y
+        else:
+            vangle = wrap_angle(vangle)
+            if vangle_true is not None:
+                vangle_true = wrap_angle(vangle_true)
+                # vangle, vangle_true = np.unwrap([vangle, vangle_true])
+                if err(vangle - vangle_true, exception=False) > 1e-3:  # debug code
+                    print([vangle_true, vangle])
+                    print("Bad angle, delta: ", vangle - vangle_true)
+                    raise Exception("bad angle")
+            return x, y, vangle
+
+    def getLocalPosition(self, x, y, psi):
+        """coordinate transformation from inertial reference frame (X, Y) to curvilinear reference frame (s, ey)
+        (X, Y): position in the inertial reference frame
+        """
+        PointAndTangent = self.PointAndTangent
+        CompletedFlag = 0
+
+        for i in range(0, PointAndTangent.shape[0]):
+            if CompletedFlag == 1:
+                break
+
+            if PointAndTangent[i, 5] == 0.0:  # If segment is a straight line
+                # Extract the first final and initial point of the segment
+                xf = PointAndTangent[i, 0]
+                yf = PointAndTangent[i, 1]
+                xs = PointAndTangent[i - 1, 0]
+                ys = PointAndTangent[i - 1, 1]
+
+                psi_unwrap = np.unwrap([PointAndTangent[i - 1, 2], psi])[1]
+                epsi = psi_unwrap - PointAndTangent[i - 1, 2]
+                # Check if on the segment using angles
+                if (la.norm(np.array([xs, ys]) - np.array([x, y]))) == 0:
+                    s  = PointAndTangent[i, 3]
+                    ey = 0
+                    CompletedFlag = 1
+
+                elif (la.norm(np.array([xf, yf]) - np.array([x, y]))) == 0:
+                    s = PointAndTangent[i, 3] + PointAndTangent[i, 4]
+                    ey = 0
+                    CompletedFlag = 1
+                else:
+                    if np.abs(computeAngle( [x,y] , [xs, ys], [xf, yf])) <= np.pi/2 and np.abs(computeAngle( [x,y] , [xf, yf], [xs, ys])) <= np.pi/2:
+                        v1 = np.array([x,y]) - np.array([xs, ys])
+                        angle = computeAngle( [xf,yf] , [xs, ys], [x, y])
+                        s_local = la.norm(v1) * np.cos(angle)
+                        s       = s_local + PointAndTangent[i, 3]
+                        ey      = la.norm(v1) * np.sin(angle)
+
+                        if np.abs(ey)<= self.width:
+                            CompletedFlag = 1
+
+            else:
+                xf = PointAndTangent[i, 0]
+                yf = PointAndTangent[i, 1]
+                xs = PointAndTangent[i - 1, 0]
+                ys = PointAndTangent[i - 1, 1]
+
+                r = 1 / PointAndTangent[i, 5]  # Extract curvature
+                direction = 1 if r >= 0 else -1
+                # if r >= 0:
+                #     direction = 1
+                # else:
+                #     direction = -1
+                ang = PointAndTangent[i - 1, 2]  # Extract angle of the tangent at the initial point (i-1)
+
+                # Compute the center of the arc
+                CenterX = xs + np.abs(r) * np.cos(ang + direction * np.pi / 2)  # x coordinate center of circle
+                CenterY = ys + np.abs(r) * np.sin(ang + direction * np.pi / 2)  # y coordinate center of circle
+
+                # Check if on the segment using angles
+                if (la.norm(np.array([xs, ys]) - np.array([x, y]))) == 0:
+                    ey = 0
+                    psi_unwrap = np.unwrap([ang, psi])[1]
+                    epsi = psi_unwrap - ang
+                    s = PointAndTangent[i, 3]
+                    CompletedFlag = 1
+                elif (la.norm(np.array([xf, yf]) - np.array([x, y]))) == 0:
+                    s = PointAndTangent[i, 3] + PointAndTangent[i, 4]
+                    ey = 0
+                    psi_unwrap = np.unwrap([PointAndTangent[i, 2], psi])[1]
+                    epsi = psi_unwrap - PointAndTangent[i, 2]
+                    CompletedFlag = 1
+                else:
+                    arc1 = PointAndTangent[i, 4] * PointAndTangent[i, 5]
+                    arc2 = computeAngle([xs, ys], [CenterX, CenterY], [x, y])
+                    if np.sign(arc1) == np.sign(arc2) and np.abs(arc1) >= np.abs(arc2):
+                        v = np.array([x, y]) - np.array([CenterX, CenterY])
+                        s_local = np.abs(arc2)*np.abs(r)
+                        s    = s_local + PointAndTangent[i, 3]
+                        ey   = -np.sign(direction) * (la.norm(v) - np.abs(r))
+                        psi_unwrap = np.unwrap([ang + arc2, psi])[1]
+                        epsi = psi_unwrap - (ang + arc2)
+
+                        if np.abs(ey) <= self.width:
+                            CompletedFlag = 1
+
+        if epsi>1.0:
+            raise Exception("epsi very large; car in wrong direction")
+            pdb.set_trace()
+
+        if CompletedFlag == 0:
+            s    = 10000
+            ey   = 10000
+            epsi = 10000
+            print("Error!! POINT OUT OF THE TRACK!!!! <==================")
+            raise Exception("car outside track")
+        return s, ey, epsi, CompletedFlag
+
+    def curvature_and_angle(self, s):
+        """curvature computation
+        s: curvilinear abscissa at which the curvature has to be evaluated
+        PointAndTangent: points and tangent vectors defining the map (these quantities are initialized in the map object)
+        """
+        PointAndTangent = self.PointAndTangent
+        TrackLength = PointAndTangent[-1, 3] + PointAndTangent[-1, 4]
+
+        # In case on a lap after the first one
+        while (s > TrackLength):
+            s = s - TrackLength
+
+        # Given s \in [0, TrackLength] compute the curvature
+        # Compute the segment in which system is evolving
+        index = np.all([[s >= PointAndTangent[:, 3]], [s < PointAndTangent[:, 3] + PointAndTangent[:, 4]]], axis=0)
+        i = int(np.where(np.squeeze(index))[0])
+        curvature = PointAndTangent[i, 5]
+        angle = PointAndTangent[i, 4]  # tangent angle of path
+        return curvature, angle, i
+
+
+
+# ======================================================================================================================
+# ======================================================================================================================
+# ====================================== Internal utilities functions ==================================================
+# ======================================================================================================================
+# ======================================================================================================================
+def computeAngle(point1, origin, point2):
+    # The orientation of this angle matches that of the coordinate system. Tha is why a minus sign is needed
+    v1 = np.array(point1) - np.array(origin)
+    v2 = np.array(point2) - np.array(origin)
+
+    dot = v1[0] * v2[0] + v1[1] * v2[1]  # dot product between [x1, y1] and [x2, y2]
+    det = v1[0] * v2[1] - v1[1] * v2[0]  # determinant
+    angle = np.arctan2(det, dot)  # atan2(y, x) or atan2(sin, cos)
+    return angle
+
+'''
+This is used because np.sign(a) return 0 when a=0, which is pretty stupid.
+'''
+def sign(a):
+    return 1 if a >= 0 else -1
+
+def wrap_angle(angle):
+    return np.mod(angle+np.pi, 2 * np.pi) - np.pi
+
+'''
+Compute difference of these two vectors taking into account the angular component wraps
+'''
+def xy_diff(x,y):
+    dx = x-y
+    if len(dx.shape) == 1:
+        dx[3] = wrap_angle(dx[3])
+    else:
+        dx[:,3] = wrap_angle(dx[:,3])
+    return dx
+
+
+def unityTestChangeOfCoordinates(map, ClosedLoopData):
+    """For each point in ClosedLoopData change (X, Y) into (s, ey) and back to (X, Y) to check accurancy
+    """
+    TestResult = 1
+    for i in range(0, ClosedLoopData.x.shape[0]):
+        xdat = ClosedLoopData.x
+        xglobdat = ClosedLoopData.x_glob
+
+        s, ey, epsi, _ = map.getLocalPosition(x=xglobdat[i, 4], y=xglobdat[i, 5], psi=xglobdat[i, 3])
+        v1 = np.array([epsi, s, ey])
+        v2 = np.array(xdat[i, 3:6])
+        x,y,vangle = np.array(map.getGlobalPosition(s=v1[1], ey=v1[2],epsi=v1[0], vangle_true=xglobdat[i,3] ))
+        v3 = np.array([ vangle, x, y])
+        v4 = np.array( [wrap_angle( xglobdat[i, 3] )] + xglobdat[i, 4:6].tolist() )
+        # print(i)
+        if np.abs( wrap_angle( xglobdat[i, 3] ) - vangle ) > 0.1:
+            print("BAD")
+            raise Exception("bad angle test result")
+
+        if np.dot(v3 - v4, v3 - v4) > 0.00000001:
+            TestResult = 0
+            print("ERROR", v1, v2, v3, v4)
+            # pdb.set_trace()
+            v1 = np.array(map.getLocalPosition(xglobdat[i, 4], xglobdat[i, 5]))
+            v2 = np.array(xdat[i, 4:6])
+            v3 = np.array(map.getGlobalPosition(v1[0], v1[1]))
+            v4 = np.array([xglobdat[i, 4], xglobdat[i, 5]])
+            print(np.dot(v3 - v4, v3 - v4))
+            # pdb.set_trace()
+
+    if TestResult == 1:
+        print("Change of coordinates test passed!")
+
+
+def err(x, exception=True, tol=1e-5, message="Error too large!"):
+    er = np.mean(np.abs(x).flat)
+    if er > tol:
+        print(message)
+        print(x)
+        print(er)
+        if exception:
+            raise Exception(message)
+    return er
+
+
+if __name__ == "__main__":
+    map = SymMap()
+    import sympy as sym
+
+
+    pass

irlc/ex00/__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 directory contains the exercises for week 0."""

irlc/ex00/fruit_homework.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.
+def add(a, b): 
+    """  This function shuold return the sum of a and b. I.e. if print(add(2,3)) should print '5'. """
+    # TODO: 1 lines missing.
+    raise NotImplementedError("Implement function body")
+
+def misterfy(animals): 
+    """
+    Given a list of animals like animals=["cat", "wolf", "elephans"], this function should return
+    a list like ["mr cat", "mr wolf", "mr elephant"]  """
+    # TODO: 1 lines missing.
+    raise NotImplementedError("Implement function body")
+
+def mean_value(p_dict): 
+    """
+    Given a dictionary of the form: {x: probability_of_x, ...} compute the mean value of
+    x, i.e. sum_i x_i * p(x_i). The recommended way is to use list comprehension and not numpy.
+    Hint: Look at the .items() method and the build-in sum(my_list) method. """
+    # TODO: 1 lines missing.
+    raise NotImplementedError("Implement function body")
+
+def fruits_ordered(order_dict): 
+    # TODO: 1 lines missing.
+    raise NotImplementedError("Implement function body")
+
+class BasicFruitShop:
+    """ This is a simple class that represents a fruit-shop.
+    You instantiate it with a dictionary of prices """
+    def __init__(self, name, prices):
+        """ prices is a dictionary of the form {fruit_name: cost}. For instance
+        prices = {'apple': 5, 'orange': 6} """
+        self.name = name
+        self.prices = prices
+
+    def cost(self, fruit): 
+        """ Return the cost in pounds of the fruit with name 'fruit'. It uses the self.prices variable
+        to get the price.
+        You don't need to do exception handling here. """
+        # TODO: 1 lines missing.
+        raise NotImplementedError("Return cost of fruit as a floating point number")
+
+class OnlineFruitShop(BasicFruitShop):
+    def price_of_order(self, order): 
+        """
+        order_dict = {'apple': 5, 'pear': 2, ...} where the numbers are the quantity ordered.
+
+        Hints: Dictionary comprehension like:
+         > for fruit, pounds in order_dict.items()
+         > self.getCostPerPound(fruit) allows you to get cost of a fruit
+         > the total is sum of {pounds} * {cost_per_pound}
+        """
+        # TODO: 1 lines missing.
+        raise NotImplementedError("return the total cost of the order")
+
+
+def shop_smart(order, fruit_shops): 
+    """
+        order_dict: dictionary {'apple': 3, ...} of fruits and the pounds ordered
+        fruitShops: List of OnlineFruitShops
+
+    Hints:
+        > Remember there is a s.price_of_order method
+        > Use this method to first make a list containing the cost of the order at each fruit shop
+        > List has form [cost1, cost2], then find the index of the smallest value (the list has an index-function)
+        > return fruitShops[lowest_index].
+    """
+    # TODO: 2 lines missing.
+    raise NotImplementedError("Implement function body")
+    return best_shop
+
+
+if __name__ == '__main__':
+    "This code runs when you invoke the script from the command line (but not otherwise)"
+
+    """ Quesion 1: Lists and basic data types """
+    print("add(2,5) function should return 7, and it returned", add(2, 5))  
+
+    animals = ["cat", "giraffe", "wolf"] 
+    print("The nice animals are", misterfy(animals)) 
+
+    """  
+    This problem represents the probabilities of a loaded die as a dictionary such that     
+    > p(roll=3) = p_dict[3] = 0.15.
+    """
+    p_die = {1: 0.20,
+             2: 0.10,
+             3: 0.15,
+             4: 0.05,
+             5: 0.10,
+             6: 0.40}
+    print("Mean roll of die, sum_{i=1}^6 i * p(i) =", mean_value(p_die)) 
+
+    order = {'apples': 1.0, 
+              'oranges': 3.0}
+    print("The different fruits in the fruit-order is", fruits_ordered(order)) 
+
+    """ Part B: A simple class """
+    price1 = {"apple": 4, "pear": 8, 'orange': 10} 
+    shop1 = BasicFruitShop("Alis Funky Fruits", price1)
+
+    price2 = {'banana': 9, "apple": 5, "pear": 7, 'orange': 11}
+    shop2 = BasicFruitShop("Hansen Fruit Emporium", price2)
+
+    fruit = "apple"
+    print("The cost of", fruit, "in", shop1.name, "is", shop1.cost(fruit))
+    print("The cost of", fruit, "in", shop2.name, "is", shop2.cost(fruit)) 
+
+    """ Part C: Class inheritance """
+    price_of_fruits = {'apples': 2, 'oranges': 1, 'pears': 1.5, 'mellon': 10} 
+    shopA = OnlineFruitShop('shopA', price_of_fruits)
+    print("The price of the given order in shopA is", shopA.price_of_order(order))  
+
+    """ Part C: Using classes """
+    shopB = OnlineFruitShop('shopB', {'apples': 1.0, 'oranges': 5.0}) 
+
+    shops = [shopA, shopB]
+    print("For the order", order, " the best shop is", shop_smart(order, shops).name)
+    order = {'apples': 3.0}  # test with a new order.
+    print("For the order", order, " the best shop is", shop_smart(order, shops).name) 

irlc/exam/__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 to find the tests in the exam.

irlc/exam/exam2025spring/__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.

irlc/exam/exam2025spring/readme.md

+This directory is purposefully left empty. During the exam, you will be given a `.zip` file with the content of this directory. 
+Replace this directory with the corresponding directory from the `.zip` file to begin working on the exam.