40 beginning pipeline simulation

cones.json

@@ -0,0 +1,12 @@
+{
+  "cones": [
+    { "id": 0, "color": "red",   "position": [5.0, 0.0, 0.0] },
+    { "id": 1, "color": "blue",  "position": [3.54, 3.54, 0.0] },
+    { "id": 2, "color": "yellow","position": [0.0, 5.0, 0.0] },
+    { "id": 3, "color": "red",   "position": [-3.54, 3.54, 0.0] },
+    { "id": 4, "color": "blue",  "position": [-5.0, 0.0, 0.0] },
+    { "id": 5, "color": "yellow","position": [-3.54, -3.54, 0.0] },
+    { "id": 6, "color": "red",   "position": [0.0, -5.0, 0.0] },
+    { "id": 7, "color": "blue",  "position": [3.54, -3.54, 0.0] }
+  ]
+}

cones_plot.py

@@ -0,0 +1,95 @@
+import json
+import numpy as np
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d.art3d import Poly3DCollection
+
+
+## loading the json file
+with open("/Users/atvyas/Desktop/coding/FS/sim_int/cones.json", "r") as f:
+    data = json.load(f)
+
+cones = data["cones"]
+positions = np.array([cone["position"] for cone in cones])
+colors = [cone["color"] for cone in cones]
+
+
+## Drawing the cones:
+def draw_cone(ax, x, y, z, height=0.4, radius=0.5, color="orange"):
+    # sqaure base to the cones
+    half = radius
+    square = [
+        [x - half, y - half, z],
+        [x + half, y - half, z],
+        [x + half, y + half, z],
+        [x - half, y + half, z]
+    ]
+
+    base = Poly3DCollection(
+        [square],
+        facecolors=color,
+        alpha=0.6
+    )
+    ax.add_collection3d(base)
+
+    # the actual cones
+    theta = np.linspace(0, 2 * np.pi, 1)
+    r = np.linspace(0, radius, 2)
+    T, R = np.meshgrid(theta, r)
+
+    X = x + R * np.cos(T)
+    Y = y + R * np.sin(T)
+    Z = z + height * (1 - R / radius)
+
+    ax.plot_surface(X, Y, Z, color=color, alpha=0.7, linewidth=0)
+
+
+    # Cone surface
+    theta = np.linspace(0, 2*np.pi, 5)
+    r = np.linspace(0, radius, 2)
+    T, R = np.meshgrid(theta, r)
+
+    X = x + R * np.cos(T)
+    Y = y + R * np.sin(T)
+    Z = z + height * (1 - R / radius)
+
+    ax.plot_surface(X, Y, Z, color=color, alpha=0.7, linewidth=0)
+
+
+## Plotting the cones in 3-D
+fig = plt.figure(figsize=(14, 7))
+
+# 3d
+ax3d = fig.add_subplot(121, projection="3d")
+
+for (x, y, z), color in zip(positions, colors):
+    draw_cone(ax3d, x, y, z, height=0.5, radius=0.5, color=color)
+
+ax3d.set_title("3D Cone View")
+ax3d.set_xlabel("X")
+ax3d.set_ylabel("Y")
+ax3d.set_zlabel("Z")
+
+ax3d.set_xlim(-6, 6)
+ax3d.set_ylim(-6, 6)
+ax3d.set_zlim(0, 3)
+
+# top down
+ax2d = fig.add_subplot(122)
+
+x = positions[:, 0]
+y = positions[:, 1]
+
+ax2d.scatter(x, y, c=colors, s=120, edgecolors="black")
+
+# setting up the grid
+ax2d.set_title("Top-Down View (X-Y)")
+ax2d.set_xlabel("X")
+ax2d.set_ylabel("Y")
+ax2d.set_aspect("equal")
+ax2d.set_xlim(-6, 6)
+ax2d.set_ylim(-6, 6)
+ax2d.grid(True)
+
+# to display the graphs
+plt.tight_layout()
+plt.show()

data_rep.py

@@ -0,0 +1,56 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from state_test import (
+    position, speed, heading,
+    acceleration, throttle, brakes, lastCurrent
+)
+
+# sim time set up
+dt = 0.1           # time step (seconds)
+t_end = 10.0       # total simulation time
+time = np.arange(0, t_end, dt)
+
+# sim values over sim time
+speed_t = speed + acceleration * time
+heading_t = heading + 0.02 * time          # slow turn
+throttle_t = np.full_like(time, throttle)
+brakes_t = np.full_like(time, brakes)
+current_t = lastCurrent + 0.5 * time       # example trend
+
+# Position integration
+x = position[0] + np.cumsum(speed_t * np.cos(heading_t) * dt)
+y = position[1] + np.cumsum(speed_t * np.sin(heading_t) * dt)
+
+## plots
+fig, axs = plt.subplots(3, 2, figsize=(12, 10))
+axs = axs.flatten()
+
+axs[0].plot(time, speed_t)
+axs[0].set_title("Speed vs Time")
+axs[0].set_ylabel("m/s")
+
+axs[1].plot(time, heading_t)
+axs[1].set_title("Heading vs Time")
+axs[1].set_ylabel("radians")
+
+axs[2].plot(time, throttle_t)
+axs[2].set_title("Throttle vs Time")
+
+axs[3].plot(time, brakes_t)
+axs[3].set_title("Brakes vs Time")
+
+axs[4].plot(time, current_t)
+axs[4].set_title("Current vs Time")
+axs[4].set_ylabel("Amps")
+
+axs[5].plot(x, y)
+axs[5].set_title("Position (X vs Y)")
+axs[5].set_xlabel("X")
+axs[5].set_ylabel("Y")
+
+for ax in axs:
+    ax.set_xlabel("Time (s)")
+    ax.grid(True)
+
+plt.tight_layout()
+plt.show()

sim-int-issue1.py

@@ -0,0 +1,85 @@
+
+'''Take vechile postion, speed, and last current as input from state.py and use numpy to find the
+ postions of the cones and the car and graph it on a coordinate system.'''
+
+import numpy as np
+import matplotlib.pyplot as plt
+
+# Veichel state form state.py
+stepSize = 0.1
+position = np.array([12.5, 7.8])   # (x, y)
+speed = 18.2                       # m/s
+heading = np.deg2rad(15)           # radians
+acceleration = 1.3                 # m/s^2
+throttle = 0.65                    # 0–1
+brakes = 0.1                       # 0–1
+lastCurrent = 42.0                 # amps
+
+# cones postions coming from a json file
+cones = np.array([
+    [5, 5],
+    [8, 6],
+    [10, 7],
+    [13, 8],
+    [16, 9],
+    [18, 10]
+])
+
+# finding velcoity from heading
+velocity = np.array([
+    np.cos(heading),
+    np.sin(heading)
+]) * speed
+
+#creating layout
+fig, axs = plt.subplots(3, 1, figsize=(9, 12))
+
+# cones and psition
+axs[0].scatter(cones[:, 0], cones[:, 1],
+               marker="^", s=150, label="Cones")
+
+axs[0].scatter(position[0], position[1],
+               marker="o", s=200, label="Vehicle")
+
+axs[0].arrow(
+    position[0], position[1],
+    velocity[0] * 0.2, velocity[1] * 0.2,
+    head_width=0.4,
+    length_includes_head=True,
+    label="Velocity"
+)
+
+axs[0].text(
+    position[0] + 0.5,
+    position[1] + 0.5,
+    f"Speed: {speed:.1f} m/s",
+    fontsize=10
+)
+
+axs[0].set_title("Vehicle & Cone Map")
+axs[0].set_xlabel("X Position (m)")
+axs[0].set_ylabel("Y Position (m)")
+axs[0].axis("equal")
+axs[0].grid(True)
+axs[0].legend()
+
+#Motion data
+axs[1].bar(
+    ["Speed (m/s)", "Acceleration (m/s²)"],
+    [speed, acceleration]
+)
+
+axs[1].set_title("Vehicle Motion State")
+axs[1].grid(True)
+
+#electrical data
+axs[2].bar(
+    ["Throttle", "Brakes", "Motor Current (A)"],
+    [throttle, brakes, lastCurrent]
+)
+
+axs[2].set_title("Control & Electrical State")
+axs[2].grid(True)
+
+plt.tight_layout()
+plt.show()

state_test.py

@@ -0,0 +1,11 @@
+import numpy as np
+
+
+# Vehicle state
+position = np.array([12.0, 7.0])
+speed = 18.2
+heading = np.deg2rad(15)
+acceleration = 1.3
+throttle = 0.65
+brakes = 0.1
+lastCurrent = 42.0