Skip to content
Snippets Groups Projects
Commit 98f043ed authored by Maximilian Gruber's avatar Maximilian Gruber
Browse files

Added simulation class

parent 08e9e131
Branches
No related tags found
No related merge requests found
import numpy as np import numpy as np
import scipy.interpolate as sci
import matplotlib as mpl import matplotlib as mpl
import matplotlib.pyplot as plt
import itertools import itertools
import time import time
class Simulation:
def __init__(self, vehicle, dt=1.0):
self.vehicle = vehicle
self.dt = dt
self.path = []
def append_to_path(self, t, x, x_dot_norm = 0.1):
if t == None:
delta_x = np.linalg.norm(x - self.path[-1][1])
delta_t = delta_x / x_dot_norm
t = self.path[-1][0] + delta_t
self.path.append([t,x])
def update_path_interpolation(self):
t_path, x_path = self.path_to_arrays()
if t_path.size >= 2:
self.path_interpolation = sci.interp1d(t_path, x_path, kind="linear", axis=0, bounds_error=True)
else:
self.path_interpolation = sci.interp1d(t_path, x_path, kind="nearest", axis=0, bounds_error=True)
def path_to_arrays(self):
# collect data
t_path = np.array([waypoint[0] for waypoint in self.path])
x_path = np.array([waypoint[1] for waypoint in self.path])
return t_path, x_path
def define_random_path(self, length=10):
# define time and locations
t = np.cumsum(1 + np.random.random(length)*0.2)
x = 5*np.random.random((length,2))
# add them to path
for i in range(len(t)):
self.append_to_path(t[i], x[i], x_dot_norm=None)
def simulate(self):
self.define_random_path()
self.update_path_interpolation()
t_path, x_path = self.path_to_arrays()
t_sim = np.arange(np.min(t_path), np.max(t_path), step=self.dt)
x_sim = self.path_interpolation(t_sim)
plt.plot(x_path[:,0], x_path[:,1])
plt.scatter(x_sim[:,0], x_sim[:,1])
plt.show()
class Vehicle: class Vehicle:
def __init__(self, mass=1.0, floor=None): def __init__(self, mass=1.0, floor=None, init_position=np.array([0,0]), init_rotation=np.array(0)):
# mechanics and state # mechanics and state
self.position = np.array([1,1]) # meter self.position = init_position # meter
self.rotation = np.array(1) # radian self.rotation = init_rotation # radian
self.mass = mass self.mass = mass
self.wheels_weight = np.array([2,2,1,1]) / 6 # weight self.wheels_weight = np.array([2,2,1,1]) / 6 # weight
......
from base import Simulation
s = Simulation(None, dt=0.1)
s.simulate()
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment