class OutputPlotGraphics():
''' Class containing the output plot display behaviour: plotting the state and drawing the speed arrow
Can also plot other information on the graph such as a line that represents the target trajectory,
but these functions have been disabled to reduce display latency'''
def __init__(self, outputPlot):
self.decayValue = 0
self.outputPlot = outputPlot
self.measurementPatch = Rectangle((0, 0),
1,
1,
linewidth=1,
color='gray',
facecolor='none')
self.statePatch = Circle((0, 0), 1, facecolor='blue')
self.speedPatch = Arrow(0, 0, 3, 3)
self.circlePatch = Circle((0, 0),
1,
linewidth=1,
color='red',
facecolor='None')
self.linePatch = ConnectionPatch((0, 0), (1, 1), 'data')
self.model = 0
self.startLine = (0, 0)
self.needToResetStartLine = True
self.outputPlot.add_patch(self.measurementPatch)
self.outputPlot.add_patch(self.statePatch)
self.outputPlot.add_patch(self.speedPatch)
self.outputPlot.add_patch(self.circlePatch)
self.outputPlot.add_patch(self.linePatch)
self.circlePatch.set_visible(False)
self.linePatch.set_visible(False)
def setModel(self, model):
if model != self.model:
self.decayValue = 10
elif self.decayValue > 0:
self.decayValue -= 1
if self.decayValue == 5:
self.needToResetStartLine = True
self.model = model
if self.decayValue <= 0:
if model == 0:
self.speedPatch.set_visible(False)
self.linePatch.set_visible(False)
self.circlePatch.set_visible(False)
if model == 1:
self.speedPatch.set_visible(True)
self.linePatch.set_visible(True)
self.circlePatch.set_visible(False)
if model == 2:
self.speedPatch.set_visible(True)
self.linePatch.set_visible(False)
#self.circlePatch.set_visible(True)
def setState(self, x, y):
self.statePatch.center = x, y
def setMeasurement(self, x, y):
self.measurementPatch.set_xy((x, y))
def setSpeed(self, x, y, vx, vy):
self.speedPatch.remove()
self.speedPatch = Arrow(x, y, vx / 10, vy / 10)
self.outputPlot.add_patch(self.speedPatch)
def setCircle(self, x, y, R):
self.circlePatch.set_radius(R)
self.circlePatch.center = x, y
def setLine(self, x, y):
""" if self.needToResetStartLine:
self.needToResetStartLine = False
self.startLine = (x,y)
self.linePatch.remove()
self.linePatch = ConnectionPatch((self.startLine[0],self.startLine[1]), (x+1, y+1), 'data')
self.outputPlot.add_patch(self.linePatch) """
pass
class Body(object):
'''
Represent a solid body by a list of vertices, the position of its center of
mass and orientation
'''
def __init__(self,
orientation=0.0,
x=0.0,
y=0.0,
v=0.0,
acc=0.0,
omega=0.0,
vertex=None):
self.x = x
self.y = y
self.orientation = orientation
self.v = v
self.acc = acc
self.omega = omega
self.lastUpdate = 0.0
if vertex is None:
self.vertices = np.matrix([0, 0])
else:
self.vertices = np.matrix(vertex)
self.radius = self.__calculateRadius__()
# handling plot
self.plotRefreshRate = 2 # seconds
self.fig = None
self.timeOfLastPlot = -self.plotRefreshRate
self.bodyLine = None
self.directionArrow = None
@classmethod
def fromVehicle(cls, vehicle):
orientation = vehicle.getOrientation()
pos = vehicle.getPos()
vel = vehicle.getVelocity()
acc = vehicle.getAcc()
omega = vehicle.getOmega()
vertices = [(-vehicle.length / 2, -2), (-vehicle.length / 2, 2),
(vehicle.length / 2, 2), (vehicle.length / 2, -2)]
return cls(orientation=orientation,
x=pos[0],
y=pos[1],
v=vel,
acc=acc,
omega=omega,
vertex=vertices)
def setVertex(self, vertex):
self.vertices = np.matrix(vertex)
self.radius = self.__calculateRadius__()
def __calculateRadius__(self):
rows, cols = self.vertices.shape
radius = 0
for i in range(rows):
currRadius = sqrt(self.vertices[i, ] * self.vertices[i, ].T)
radius = max(radius, currRadius)
return radius
def checkBodyCollision(self, body):
d = self.getDistance(body)
if (d < self.radius + body.radius):
polygonSelf = Polygon(self.getPose())
polygonBody = Polygon(body.getPose())
return polygonSelf.intersects(polygonBody)
else:
return False
def checkLineCollision(self, line):
polygonSelf = Polygon(self.getPose())
return polygonSelf.intersects(line)
def getPolygon(self):
return Polygon(self.getPose())
def getOrigin(self):
return Point(self.x, self.y)
def getDistance(self, body):
return sqrt((self.x - body.x)**2 + (self.y - body.y)**2)
def setAcceleration(self, acc):
self.acc = acc
def setOmega(self, omega):
self.omega = omega
def updateStates(self, t):
dt = t - self.lastUpdate
self.x += self.v * cos(self.orientation)
self.y += self.v * sin(self.orientation)
self.v += self.acc * dt
self.orientation += self.omega * dt
self.lastUpdate = t
def setStates(self, x, y, v, orientation, t):
self.x = x
self.y = y
self.v = v
self.orientation = orientation
self.lastUpdate = t
def resetTime(self):
self.lastUpdate = 0
def getPosition(self):
return self.x, self.y
def getOrientation(self):
return self.orientation
def getVelocity(self):
return self.v
def createPlot(self, fig=None, ax=None):
self.timeOfLastPlot = -1
if (fig is None):
self.fig = plt.figure()
else:
self.fig = fig
if (ax is None):
self.ax = self.fig.add_subplot(111)
else:
self.ax = ax
(verticesX, verticesY) = self.getDrawingVertex()
self.bodyLine, = self.ax.plot(verticesX, verticesY, 'k')
self.directionArrow = Arrow(self.x,
self.y,
0.1 * self.v * cos(self.orientation),
0.1 * self.v * sin(self.orientation),
color='c')
self.ax.add_patch(self.directionArrow)
self.ax.set_xlim([self.x - 10, self.x + 10])
self.ax.set_ylim([self.y - 10, self.y + 10])
self.ax.set_xlabel('Distance X')
self.ax.set_ylabel('Distance Y')
def getPose(self):
verticesX = []
verticesY = []
self.R = np.matrix([(cos(self.orientation), -sin(self.orientation)),
(sin(self.orientation), cos(self.orientation))])
v = self.R * self.vertices.transpose() # applyting the rotation matrix
v = v.transpose()
rows, cols = v.shape
for i in range(rows):
verticesX.append(v[i, 0] + self.x)
verticesY.append(v[i, 1] + self.y)
return v + np.matrix([self.x, self.y])
def getDrawingVertex(self):
vertices = self.getPose()
vX = [v[0, 0] for v in vertices]
vX.append(vertices[0, 0])
vY = [v[0, 1] for v in vertices]
vY.append(vertices[0, 1])
return vX, vY
def plot(self, draw=True):
(verticesX, verticesY) = self.getDrawingVertex()
self.bodyLine.set_ydata(verticesY)
self.bodyLine.set_xdata(verticesX)
self.directionArrow.remove()
self.directionArrow = Arrow(self.x,
self.y,
0.1 * self.v * cos(self.orientation),
0.1 * self.v * sin(self.orientation),
color='c')
self.ax.add_patch(self.directionArrow)
self.ax.set_xlim([self.x - 10, self.x + 10])
self.ax.set_ylim([self.y - 10, self.y + 10])
if (draw):
self.fig.canvas.draw()
plt.pause(0.001)
class Plot_canvas(FigureCanvas):
def __init__(self):
fig = Figure(figsize=(4, 4), dpi=100)
self.ax = fig.add_subplot(111)
super().__init__(fig)
FigureCanvas.setSizePolicy(self, QSizePolicy.Expanding,
QSizePolicy.Expanding)
FigureCanvas.updateGeometry(self)
self.radars = []
def init_walls(self, pos_angle, ends, fin_pos):
self.ax.cla()
self.ax.plot(*zip(*ends),
color='k') # zip equals to tranpose, * eauals to depack
finish_area = Rectangle(xy=(fin_pos[0][0], fin_pos[1][1]),
width=fin_pos[1][0] - fin_pos[0][0],
height=fin_pos[1][1] - fin_pos[0][1],
color='red')
self.ax.add_artist(finish_area)
pos = tuple(pos_angle[:2])
angle = math.radians(pos_angle[2])
self.car = Circle(pos, radius=3, color='mediumblue')
self.head = Arrow(pos[0],
pos[1],
dx=5 * math.cos(angle),
dy=5 * math.sin(angle),
facecolor='gold',
edgecolor='k')
self.ax.add_artist(self.car)
self.ax.add_artist(self.head)
self.draw()
def update_car(self, pos_angle, inters):
self.car.remove()
self.head.remove()
pos = tuple(pos_angle[:2])
angle = math.radians(pos_angle[2])
self.car = Circle(pos, radius=3, color='mediumblue')
self.head = Arrow(pos[0],
pos[1],
dx=5 * math.cos(angle),
dy=5 * math.sin(angle),
facecolor='gold',
edgecolor='k')
if self.radars:
for radar in self.radars:
radar.remove()
self.radars = [
Line2D(*zip(pos, inter), linestyle='-', color='gray')
for inter in inters
]
for radar in self.radars:
self.ax.add_line(radar)
self.ax.add_artist(self.car)
self.ax.add_artist(self.head)
self.draw()
def collide(self):
self.car.set_color('darkred')
self.draw()
def show_path(self, path_x, path_y):
self.path = Line2D(path_x,
path_y,
linewidth=6,
solid_capstyle='round',
solid_joinstyle='round',
alpha=0.75,
color='gray')
self.ax.add_line(self.path)
self.draw()
class MyMainWindow(QMainWindow, Ui_MainWindow):
"""
Main window class
"""
def __init__(self, ):
"""
Main constructor
"""
super(MyMainWindow, self).__init__()
self.setupUi(self)
self.connect_click_handlers()
self.emitter = Emitter()
self.particleList = []
self.figure = plt.figure()
self.canvas = FigureCanvas(self.figure)
self.setLayouts()
self.ax = self.figure.add_subplot(1, 1, 1)
self.emitter_vector = Arrow(self.emitter.coordinates[0],
self.emitter.coordinates[1],
self.emitter.vector[0] / 20,
self.emitter.vector[1] / 20,
width=0.09)
self.figure.canvas.mpl_connect('button_press_event',
self.changeEmitter)
self.solar_mode = False
self.particleList = self.initialize_solar_system()
self.particleListV = supercopy(self.particleList)
self.toDraw = supercopy(self.particleList)
self.animation = FuncAnimation(self.figure,
self.draw_particles,
interval=10)
self.x_scale = None
self.inaccuracy_iter = [[], []]
self.i = 0
def draw_particles(self, frame):
"""
Function to draw all particles
"""
if self.pauseRadioButton.isChecked():
return
self.i += 1
for i in range(len(self.toDraw)):
try:
self.toDraw[i].circle.remove()
except Exception:
pass
delta_t = 0.01 if self.solar_mode else 1
odeint_list = supercopy(self.particleList)
verle_list = supercopy(self.particleListV)
start_time = datetime.datetime.now()
verle_list = overall_verle(verle_list, [0, delta_t / 2, delta_t])[0]
#verle_list = overall_verle_threading(verle_list, [0, delta_t / 2, delta_t])[0]
#verle_list = overall_verle_multiprocessing(verle_list, [0, delta_t / 2, delta_t])[0]
#verle_list = overall_verle_cython(verle_list, [0, delta_t / 2, delta_t])[0]
#verle_list = overall_verle_opencl(verle_list, [0, delta_t / 2, delta_t])[0]
print("Verle iteration: {}".format(datetime.datetime.now() -
start_time))
#start_time = datetime.datetime.now()
odeint_list = overall_odeint(odeint_list, [0, delta_t / 2, delta_t])[0]
#print("Odeint iteration: {}".format(datetime.datetime.now() - start_time))
self.particleList = to_particle_list(odeint_list, self.particleList)
self.particleListV = to_particle_list(verle_list_, self.particleListV)
metric = .0
for p_1, p_2 in zip(self.particleList, self.particleListV):
dist = np.array(p_1.coordinates) - np.array(p_2.coordinates)
metric += np.linalg.norm(dist)
self.inaccuracy_iter[0].append(self.i)
self.inaccuracy_iter[1].append(metric)
self.toDraw = supercopy(
self.particleListV) if self.methodCheckBox.isChecked(
) else supercopy(self.particleList)
if len(self.toDraw) != 0:
if self.x_scale is None:
self.x_scale = max(
[elem.coordinates[0] for elem in self.toDraw] + [100])
max_m = max([elem.mass for elem in self.toDraw])
sorted_masses = sorted([elem.mass for elem in self.toDraw])
masses_map = dict()
sizes = np.linspace(0.01, 0.03, len(sorted_masses))
for i, elem in enumerate(sorted_masses):
masses_map[elem] = sizes[i]
for i in range(len(self.toDraw)):
self.toDraw[i].create_circle(
coordinates=[
self.toDraw[i].coordinates[0] / (self.x_scale * 2.1) +
0.5, self.toDraw[i].coordinates[1] /
(self.x_scale * 2.1) + 0.5
],
size=masses_map[self.toDraw[i].mass],
color=self.toDraw[i].color)
self.ax.add_artist(self.toDraw[i].circle)
self.figure.canvas.draw_idle()
def connect_click_handlers(self):
"""
Function that connects button click events to proper handlers
"""
self.generateParticleButton.clicked.connect(self.generateParticle)
self.emitterChangeButton.clicked.connect(self.changeEmitter)
self.pauseRadioButton.clicked.connect(self.draw_inacc)
def draw_inacc(self):
plt.plot(self.inaccuracy_iter[0], self.inaccuracy_iter[1])
def setLayouts(self):
vertical_layout1 = QVBoxLayout()
vertical_layout1.addWidget(self.particleXAxisSpeedLineEdit)
vertical_layout1.addWidget(self.particleYAxisSpeedLineEdit)
vertical_layout1.addWidget(self.particleColorLineEdit)
vertical_layout1.addWidget(self.massLabel)
vertical_layout1.addWidget(self.particleMassLineEdit)
vertical_layout1.addWidget(self.generateParticleButton)
vertical_layout2 = QVBoxLayout()
vertical_layout2.addWidget(self.emitterXLineEdit)
vertical_layout2.addWidget(self.emitterYLineEdit)
vertical_layout2.addWidget(self.emitterXVectorLineEdit)
vertical_layout2.addWidget(self.emitterYVectorLineEdit)
vertical_layout2.addWidget(self.emitterChangeButton)
vertical_layout2.addWidget(self.pauseRadioButton)
horizontal_layout1 = QHBoxLayout()
horizontal_layout1.addLayout(vertical_layout1)
horizontal_layout1.addLayout(vertical_layout2)
vertical_layout3 = QVBoxLayout()
vertical_layout3.addLayout(horizontal_layout1)
vertical_layout3.addWidget(self.methodCheckBox)
horizontal_layout2 = QHBoxLayout()
horizontal_layout2.addLayout(vertical_layout3)
self.canvas.setMinimumWidth(700)
self.canvas.setMinimumHeight(700)
horizontal_layout2.addWidget(self.canvas)
self.centralwidget.setLayout(horizontal_layout2)
def generateParticle(self):
"""
Generate particle button click event handler
"""
try:
xAxisSpeed = float(self.particleXAxisSpeedLineEdit.text())
except Exception:
print("generateParticle(): x axis speed is in wrong format!")
return
try:
yAxisSpeed = float(self.particleYAxisSpeedLineEdit.text())
except Exception:
print("generateParticle(): y axis speed is in wrong format!")
return
color = self.particleColorLineEdit.text()
#mass = self.particleMassSlider.value()
try:
mass_list = self.particleMassLineEdit.text().upper().split('E')
mass = float(mass_list[0]) * pow(10, int(mass_list[1]))
except Exception as ex:
print("generateParticle(): failed to parse mass, error: " +
str(ex))
return
particle = Particle(
[self.emitter.coordinates[0], self.emitter.coordinates[1]], [
xAxisSpeed * self.emitter.vector[0],
yAxisSpeed * self.emitter.vector[1]
], mass, color)
self.particleList.append(particle)
self.x_scale = None
def changeEmitter(self):
"""
Emitter position changed button click event handler
"""
# ------------------------ Parsing new valus ---------------------------
try:
xAxis = float(self.emitterXLineEdit.text())
except Exception:
print("changeEmitter(): x axis is in wrong format!")
return
try:
yAxis = float(self.emitterYLineEdit.text())
except Exception:
print("changeEmitter(): y axis is in wrong format!")
return
try:
xVector = int(self.emitterXVectorLineEdit.text())
except Exception:
print("changeEmitter(): x axis vector is in wrong format!")
return
try:
yVector = int(self.emitterYVectorLineEdit.text())
except Exception:
print("changeEmitter(): y axis vector is in wrong format!")
return
# -----------------------------------------------------------------------
# Changing emitter position
self.emitter.change_position([xAxis, yAxis], [xVector, yVector])
# Removing old emitter (we have no emitter, don't do anything)
try:
self.emitter_vector.remove()
except Exception:
pass
# Creating new emitter
self.emitter_vector = Arrow(self.emitter.coordinates[0],
self.emitter.coordinates[1],
self.emitter.vector[0] / 20,
self.emitter.vector[1] / 20,
width=0.09)
# Redraw plot
self.figure.canvas.draw_idle()
def initialize_solar_system(self, data_file="solar_system.json"):
"""
Function that prepares solar system example
"""
self.particleList = []
with open(data_file, "r") as descr:
text_content = descr.read()
json_content = json.loads(text_content)
for val in json_content["particles"].values():
particle = Particle(coordinates=[val["x"], val["y"]],
speed=[val["u"], val["v"]],
mass=val["m"],
color=val["color"],
living_time=val["lifetime"])
self.particleList.append(particle)
self.solar_mode = True
return self.particleList
def print_particle_list(self, lst=None):
for elem in lst:
print(elem.coordinates)
print("-----")
class Enviroment(SimulationEnviroment):
'''
Creates a map of the vehicle and its surroding enviroment. This map is used
by the planner to avoid collision.
This might be expanded later with a SLAM.
'''
def __init__(self, vehicle, mapRadius=80):
self.bodies = []
self.mapRadius = mapRadius
self.centerBody = Body.fromVehicle(vehicle)
self.centerBodyLine = None
self.centerBodyArrow = None
self.vehicle = vehicle
self.radiusCircle = None
self.bodyLines = [] # might be better changing this to a dict
self.bodyArrows = [] # as to avoid having to keep bodies on track
def addBody(self, body):
if (body not in self.bodies
and self.centerBody.getDistance(body) <= self.mapRadius):
self.bodies.append(body)
def lidarScan(self):
for lidar in self.vehicle.lidarArray:
lidar.updateRays()
lidar.rayTracing(self.bodies)
def update(self, t):
bodiesToRemove = []
for idx in range(len(self.bodies)):
body = self.bodies[idx]
body.updateStates(t)
if (self.centerBody.getDistance(body) > self.mapRadius):
bodiesToRemove.append(idx)
# for removeIdx in bodiesToRemove:
# del self.bodies[removeIdx]
orientation = self.vehicle.getOrientation()
pos = self.vehicle.getPos()
vel = self.vehicle.getVelocity()
acc = self.vehicle.getAcc()
omega = self.vehicle.getOmega()
self.centerBody.setStates(pos[0], pos[1], vel, orientation, t)
self.centerBody.setAcceleration(acc)
self.centerBody.setOmega(omega)
def createPlot(self, fig=None, ax=None):
self.timeOfLastPlot = -1
if (fig is None):
self.fig = plt.figure()
else:
self.fig = fig
if (ax is None):
self.ax = self.fig.add_subplot(111)
else:
self.ax = ax
# for lidar in self.vehicle.lidarArray:
pm = [
pm.coords if pm is not None else None
for pm in self.vehicle.lidar.read()
]
bodyLine, = self.ax.plot(pm, 'r-', linewidth=0.1)
self.bodyLines.append(bodyLine)
(verticesX, verticesY) = self.centerBody.getDrawingVertex()
self.centerBodyLine, = self.ax.plot(verticesX, verticesY, 'r')
self.centerBodyArrow = Arrow(
self.centerBody.x,
self.centerBody.y,
0.1 * self.centerBody.v * cos(self.centerBody.orientation),
0.1 * self.centerBody.v * sin(self.centerBody.orientation),
color='c')
self.ax.add_patch(self.centerBodyArrow)
self.radiusCircle = Circle((self.centerBody.x, self.centerBody.y),
self.mapRadius,
color='k',
linestyle=':',
fill=False)
self.ax.add_patch(self.radiusCircle)
self.ax.set_xlim([
self.centerBody.x - self.mapRadius * 1.1,
self.centerBody.x + self.mapRadius * 1.1
])
self.ax.set_ylim([
self.centerBody.y - self.mapRadius * 1.1,
self.centerBody.y + self.mapRadius * 1.1
])
self.ax.set_xlabel('Distance X')
self.ax.set_ylabel('Distance Y')
def plot(self, draw=True):
# for idx in range(len(self.vehicle.lidarArray)):
idx = 0
lidar = self.vehicle.lidar
pm_x = [(self.centerBody.x, pm.xy[0][0]) if pm is not None else None
for pm in lidar.read()]
pm_y = [(self.centerBody.y, pm.xy[1][0]) if pm is not None else None
for pm in lidar.read()]
bodyLine = self.bodyLines[idx]
bodyLine.set_ydata(pm_y)
bodyLine.set_xdata(pm_x)
(verticesX, verticesY) = self.centerBody.getDrawingVertex()
self.centerBodyLine.set_ydata(verticesY)
self.centerBodyLine.set_xdata(verticesX)
self.centerBodyArrow.remove()
self.centerBodyArrow = Arrow(
self.centerBody.x,
self.centerBody.y,
0.1 * self.centerBody.v * cos(self.centerBody.orientation),
0.1 * self.centerBody.v * sin(self.centerBody.orientation),
color='c')
self.ax.add_patch(self.centerBodyArrow)
self.radiusCircle.remove()
self.radiusCircle = Circle((self.centerBody.x, self.centerBody.y),
self.mapRadius,
color='k',
linestyle=':',
fill=False)
self.ax.add_patch(self.radiusCircle)
self.ax.set_xlim([
self.centerBody.x - self.mapRadius * 1.1,
self.centerBody.x + self.mapRadius * 1.1
])
self.ax.set_ylim([
self.centerBody.y - self.mapRadius * 1.1,
self.centerBody.y + self.mapRadius * 1.1
])
if (draw):
self.fig.canvas.draw()
plt.pause(0.001)
class SimulationEnviroment(object):
'''
Creates a map of the vehicle and its surroding enviroment. This map is used
by the planner to avoid collision.
This might be expanded later with a SLAM.
'''
def __init__(self, centerBody, mapRadius=80):
self.bodies = []
self.mapRadius = mapRadius
self.centerBody = centerBody
self.centerBodyLine = None
self.centerBodyArrow = None
self.radiusCircle = None
self.bodyLines = [] # might be better changin g this to a dict
self.bodyArrows = [] # as to avoid having to keep bodies on track
self.lidarArray = []
def addLIDAR(self, lidar):
self.lidarArray.append(lidar)
def getPointMap(self):
pm = []
for lidar in self.lidarArray:
pm.extend(lidar.pointMap)
return pm
def lidarScan(self):
for lidar in self.lidarArray:
lidar.updateRays()
lidar.rayTracing(self.bodies)
def addBody(self, body):
if (body not in self.bodies):
self.bodies.append(body)
def update(self, t):
for b in self.bodies:
b.updateStates(t)
# prevPos = self.centerBody.getPosition()
# prevOrientation = self.centerBody.getOrientation()
# self.centerBody.updateStates(t)
pos = (self.centerBody.x, self.centerBody.y)
# delta_x, delta_y = (currPos[0] - prevPos[0], currPos[1] - prevPos[1])
# delta_theta = self.centerBody.getOrientation() - prevOrientation
for lidar in self.lidarArray:
lidar.setPose(pos, 0)
self.lidarScan()
# self.centerBody.update(t)
def createPlot(self, fig=None, ax=None):
self.timeOfLastPlot = -1
if (fig is None):
self.fig = plt.figure()
else:
self.fig = fig
if (ax is None):
self.ax = self.fig.add_subplot(111)
else:
self.ax = ax
for b in self.bodies:
(verticesX, verticesY) = b.getDrawingVertex()
bodyLine, = self.ax.plot(verticesX, verticesY, 'k')
self.bodyLines.append(bodyLine)
directionArrow = Arrow(b.x,
b.y,
0.1 * b.v * cos(b.orientation),
0.1 * b.v * sin(b.orientation),
color='c')
self.bodyArrows.append(directionArrow)
self.ax.add_patch(directionArrow)
(verticesX, verticesY) = self.centerBody.getDrawingVertex()
self.centerBodyLine, = self.ax.plot(verticesX, verticesY, 'r')
self.centerBodyArrow = Arrow(
self.centerBody.x,
self.centerBody.y,
0.1 * b.v * cos(self.centerBody.orientation),
0.1 * b.v * sin(self.centerBody.orientation),
color='c')
self.ax.add_patch(self.centerBodyArrow)
self.radiusCircle = Circle((self.centerBody.x, self.centerBody.y),
self.mapRadius,
color='k',
linestyle=':',
fill=False)
self.ax.add_patch(self.radiusCircle)
self.ax.set_xlim([
self.centerBody.x - self.mapRadius * 1.1,
self.centerBody.x + self.mapRadius * 1.1
])
self.ax.set_ylim([
self.centerBody.y - self.mapRadius * 1.1,
self.centerBody.y + self.mapRadius * 1.1
])
self.ax.set_xlabel('Distance X')
self.ax.set_ylabel('Distance Y')
def plot(self, draw=True):
for idx in range(len(self.bodies)):
b = self.bodies[idx]
(verticesX, verticesY) = b.getDrawingVertex()
bodyLine = self.bodyLines[idx]
bodyLine.set_ydata(verticesY)
bodyLine.set_xdata(verticesX)
directionArrow = self.bodyArrows[idx]
directionArrow.remove()
directionArrow = Arrow(b.x,
b.y,
0.1 * b.v * cos(b.orientation),
0.1 * b.v * sin(b.orientation),
color='c')
self.ax.add_patch(directionArrow)
self.bodyArrows[idx] = directionArrow
(verticesX, verticesY) = self.centerBody.getDrawingVertex()
self.centerBodyLine.set_ydata(verticesY)
self.centerBodyLine.set_xdata(verticesX)
self.centerBodyArrow.remove()
self.centerBodyArrow = Arrow(
self.centerBody.x,
self.centerBody.y,
0.1 * self.centerBody.v * cos(self.centerBody.orientation),
0.1 * self.centerBody.v * sin(self.centerBody.orientation),
color='c')
self.ax.add_patch(self.centerBodyArrow)
self.radiusCircle.remove()
self.radiusCircle = Circle((self.centerBody.x, self.centerBody.y),
self.mapRadius,
color='k',
linestyle=':',
fill=False)
self.ax.add_patch(self.radiusCircle)
self.ax.set_xlim([
self.centerBody.x - self.mapRadius * 1.1,
self.centerBody.x + self.mapRadius * 1.1
])
self.ax.set_ylim([
self.centerBody.y - self.mapRadius * 1.1,
self.centerBody.y + self.mapRadius * 1.1
])
if (draw):
self.fig.canvas.draw()
plt.pause(0.001)
class BikeTrailer(object):
def __init__(self,
theta=0.0,
x_0=0.0,
y_0=0.0,
lengthTractor=4.0,
lengthTrailer=10.0,
tailDistance=0.5):
self.tractor = Bike(theta,
x_0,
y_0,
acc=1.0,
dPhi=1.0 * pi / 180.0,
v=0.0,
phi=0.0,
length=lengthTractor,
tailDistance=tailDistance)
self.trailer = Bike(theta,
self.tractor.x - cos(self.tractor.orientation) *
(self.tractor.length + self.tractor.tailDistance),
self.tractor.y - sin(self.tractor.orientation) *
(self.tractor.length + self.tractor.tailDistance),
acc=1.0,
dPhi=1.0 * pi / 180.0,
v=0.0,
phi=0.0,
length=lengthTrailer,
tailDistance=tailDistance)
# handling plot
self.plotRefreshRate = 2 # seconds
self.fig = None
self.timeOfLastPlot = -self.plotRefreshRate
self.tractorFront = None
self.trailerFront = None
self.tractorLine = None
self.trailerLine = None
self.directionArrow = None
def setNoise(self, errorPos, errorPhi, errorOrientation, errorVelocity):
self.tractor.setNoise(errorPos, errorPhi, errorOrientation,
errorVelocity)
self.trailer.setNoise(errorPos, errorPhi, errorOrientation,
errorVelocity)
def update_posture(self, X):
l_1 = self.tractor.length
l_off = self.tractor.tailDistance
x_1, y_1, delta_1, delta_2 = X
x_2 = x_1 - (l_1 + l_off) * cos(delta_1)
y_2 = y_1 - (l_1 + l_off) * sin(delta_1)
self.tractor.set_pose(delta_1, x_1, y_1)
self.trailer.set_pose(delta_2, x_2, y_2)
def getStates(self):
return [
self.tractor.x, self.tractor.y, self.tractor.orientation,
self.trailer.orientation
]
def setStates(self, tractorX, tractorY, tractorOrientation,
trailerOrientation):
self.tractor.x = tractorX
self.tractor.y = tractorY
self.tractor.orientation = tractorOrientation
self.trailer.orientation = trailerOrientation
def move(self, finalTime):
X0 = self.getStates()
self.tractor.resetTime()
X = integrate.odeint(self.diff, X0, [0, finalTime])
self.update_posture(X[-1])
return X
def update(self, finalTime):
self.move(finalTime)
def diff(self, X, t):
delta_1 = X[2]
delta_2 = X[3]
self.update_posture(X)
dt = t - self.tractor.lastUpdate
self.tractor.updateStates(dt)
self.tractor.lastUpdate = t
phi_1 = self.tractor.phi
v_1 = self.tractor.v
l_1 = self.tractor.length
l_2 = self.trailer.length
l_off = self.tractor.tailDistance
theta_1 = delta_1 + phi_1
dDelta_1 = (1.0 / l_1) * (v_1 * sin(phi_1))
if (abs(phi_1) > 0.0):
self.radius = l_1 / sin(phi_1)
self.tractor.acc_cent = v_1 * v_1 / self.radius
else:
self.tractor.acc_cent = 0.0
alpha = delta_1 - delta_2
c_1 = v_1 * sin(phi_1)
c_2 = v_1 * cos(phi_1)
c_3 = sin(alpha)
c_4 = cos(alpha)
dDelta_2 = (1 / l_2) * (c_2 * c_3) - (l_off / (l_1 * l_2)) * c_1 * c_4
dX_1 = v_1 * cos(theta_1)
dY_1 = v_1 * sin(theta_1)
self.setStates(dX_1, dY_1, dDelta_1, dDelta_2)
if (t - self.timeOfLastPlot >= self.plotRefreshRate):
self.plot()
self.timeOfLastPlot = t
return [dX_1, dY_1, dDelta_1, dDelta_2]
'''
def printStatus(self):
if(sys.version_info.major == 3):
print('Tractor pose')
self.tractor.printStatus()
print('Trailer pose')
self.trailer.printStatus()
print('Velocity')
print(self.tractor.cruiseControl.v)
print('angle')
print(self.tractor.cruiseControl.phi)
else:
print 'Tractor pose'
self.tractor.printStatus()
print 'Trailer pose'
self.trailer.printStatus()
print 'Velocity'
print self.tractor.cruiseControl.v
print 'angle'
print self.tractor.cruiseControl.phi
'''
def createPlot(self, fig=None, ax=None):
self.timeOfLastPlot = -1
if (fig is None):
self.fig = plt.figure()
else:
self.fig = fig
if (ax is None):
self.ax = self.fig.add_subplot(111)
else:
self.ax = ax
self.tractorFront, = self.ax.plot(self.tractor.x,
self.tractor.y,
'r*',
label='Tractor Front')
self.tractorLine, = self.ax.plot([self.tractor.x, self.trailer.x],
[self.tractor.y, self.trailer.y],
'k',
label='Tractor Body')
self.trailerFront, = self.ax.plot(self.trailer.x,
self.trailer.y,
'm*',
label='Trailer Front')
self.trailerLine, = self.ax.plot([
self.trailer.x,
self.trailer.x - self.trailer.length * cos(self.trailer.length)
], [
self.trailer.y,
self.trailer.y - self.trailer.length * sin(self.trailer.length)
],
'b',
label='Trailer Body')
self.directionArrow = Arrow(
self.tractor.x,
self.tractor.y,
0.1 * self.tractor.v *
cos(self.tractor.orientation + self.tractor.phi),
0.1 * self.tractor.v *
sin(self.tractor.orientation + self.tractor.phi),
color='c',
label='Wheel Direction')
self.ax.add_patch(self.directionArrow)
self.ax.set_xlim([-10, 10])
self.ax.set_ylim([-10, 10])
self.ax.set_xlabel('Distance X')
self.ax.set_ylabel('Distance Y')
self.ax.legend()
def plot(self, draw=True):
self.tractorFront.set_ydata(self.tractor.y)
self.tractorFront.set_xdata(self.tractor.x)
self.tractorLine.set_ydata([self.tractor.y, self.trailer.y])
self.tractorLine.set_xdata([self.tractor.x, self.trailer.x])
self.directionArrow.remove()
self.directionArrow = Arrow(
self.tractor.x,
self.tractor.y,
0.1 * self.tractor.v *
cos(self.tractor.orientation + self.tractor.phi),
0.1 * self.tractor.v *
sin(self.tractor.orientation + self.tractor.phi),
color='c')
self.ax.add_patch(self.directionArrow)
self.trailerFront.set_ydata(self.trailer.y)
self.trailerFront.set_xdata(self.trailer.x)
x_4 = self.trailer.x - (self.trailer.length + self.trailer.tailDistance
) * cos(self.trailer.orientation)
y_4 = self.trailer.y - (self.trailer.length + self.trailer.tailDistance
) * sin(self.trailer.orientation)
self.trailerLine.set_ydata([y_4, self.trailer.y])
self.trailerLine.set_xdata([x_4, self.trailer.x])
self.ax.set_xlim([self.trailer.x - 10, self.trailer.x + 10])
self.ax.set_ylim([self.trailer.y - 10, self.trailer.y + 10])
if (draw):
self.fig.canvas.draw()
plt.pause(0.001)
