initial commit

base.py

+import numpy as np
+import matplotlib as mpl
+import itertools
+
+class Vehicle:
+
+    def __init__(self, mass=1.0, floor=None):
+
+        # mechanics and state
+        self.position = np.array([1,1])  # meter
+        self.rotation = np.array(1)    # radian
+
+        self.mass = mass
+        self.wheels_weight = np.array([2,2,1,1]) / 6  # weight
+        self.wheels_position_delta = np.array([[1,2],[-1,2],[-1,-2],[1,-2]]) / 5
+        self.wheels_position = self.get_wheel_positions()
+
+
+    def get_wheel_positions(self):
+        # build rotation matrix R
+        c = np.cos(self.rotation)
+        s = np.sin(self.rotation)
+        R = np.array([[c, -s], [s, c]])
+
+        # return absolute wheel positions
+        return self.position + np.matmul(R, self.wheels_position_delta.T).T
+
+
+    def representation(self):
+        # represent vehicle by a rectangle from wheel positions
+        shape = mpl.patches.Polygon(self.wheels_position)
+
+        # arrow direction
+        x = self.position[0]
+        y = self.position[1]
+        dx = 1.0
+        dy = np.arctan(self.rotation)
+        arrow = mpl.patches.Arrow(x, y, dx, dy, width=0.2)
+
+        return shape, arrow
+
+
+    def update_representation(self, shape, arrow):
+        pass
+
+    def connect(self):
+        pass
+
+    def on_press(self, event):
+        pass
+
+    def on_motion(self, event):
+        pass
+
+    def on_release(self, event):
+        pass
+
+
+
+class Floor():
+
+    def __init__(self):
+        self.shape = (4,5)
+        self.tile_length = 0.5  # meter
+
+        # 
+        self.tiles = self.init_floor_tiles()
+        self.draw_limits = np.array([[0, self.shape[0]+1], [0, self.shape[1]+1]]) * self.tile_length
+
+
+    def init_floor_tiles(self):
+
+        tiles = {}
+        tile_iterator = itertools.product(*[range(d) for d in self.shape])
+
+        standard_rectangle = np.array([[0,0], [1,0], [1,1], [0,1]])
+
+        for i, index in enumerate(tile_iterator):
+            corners = (np.array(index) + standard_rectangle) * self.tile_length
+            tiles[i] = {"index": index, "corners": corners, "sensor": None}
+
+        return tiles
+    
+
+    def representation(self):
+        x = np.linspace(0, self.shape[0], self.shape[0]+1) * self.tile_length
+        y = np.linspace(0, self.shape[1], self.shape[1]+1) * self.tile_length
+
+        print(x.size, y.size)
+        X,Y = np.meshgrid(x,y, indexing="ij")
+
+        C = np.zeros(self.shape)
+
+        for i, tile in self.tiles.items():
+            C[tile["index"]] = np.random.random()
+        
+        return X,Y,C
\ No newline at end of file

main.py

+from base import Vehicle, Floor
+import matplotlib.pyplot as plt
+
+
+
+v = Vehicle()
+f = Floor()
+
+fig = plt.figure()
+ax = fig.add_subplot(111)
+
+# vehicle representation
+vr = v.representation()
+v_shape = vr[0]
+v_arrow = vr[1]
+
+ax.add_patch(v_shape)
+ax.add_patch(v_arrow)
+
+
+# draw floor
+fx, fy, fc = f.representation()
+ax.pcolormesh(fx, fy, fc, cmap="Greens")
+
+
+# plot the scene
+ax.set_xlim(*f.draw_limits[0])
+ax.set_ylim(*f.draw_limits[1])
+ax.set_aspect("equal")
+plt.show()
\ No newline at end of file