def __init__(self, cX, cY, width, height):
self.x = cX
self.y = cY
self.width = width
self.height = height
_x1 = cX - (float(width) / 2.0)
_x2 = cX + (float(width) / 2.0)
self.x1 = min(_x1, _x2)
self.x2 = max(_x1, _x2)
_y1 = cY - (float(height) / 2.0)
_y2 = cY + (float(height) / 2.0)
self.y1 = min(_y1, _y2)
self.y2 = max(_y1, _y2)
self.pt1 = Pt(self.x1, self.y1)
self.pt2 = Pt(self.x2, self.y1)
self.pt3 = Pt(self.x2, self.y2)
self.pt4 = Pt(self.x1, self.y2)
# Array of (p, r)
self.segments = [
(self.pt1, self.pt2 - self.pt1),
(self.pt2, self.pt3 - self.pt2),
(self.pt3, self.pt4 - self.pt3),
(self.pt4, self.pt1 - self.pt4)
]
def getPoints(self):
pt1 = Pt(self.x - self.robotSize[0] / 2.0,
self.y - self.robotSize[1] / 2.0)
pt2 = Pt(self.x + self.robotSize[0] / 2.0,
self.y - self.robotSize[1] / 2.0)
pt3 = Pt(self.x + self.robotSize[0] / 2.0,
self.y + self.robotSize[1] / 2.0)
pt4 = Pt(self.x - self.robotSize[0] / 2.0,
self.y + self.robotSize[1] / 2.0)
return [pt1, pt2, pt3, pt4]
def __init__(self, x = 0.0, y = 0.0, theta = 0.0, timestamp = 0.0):
self.x = x
self.y = y
self.theta = theta
self.time = timestamp
self.robotSize = (2.0, 2.0)
self.leftSensorOffset = Pt(1.0, 0.8)
self.rightSensorOffset = Pt(1.0, -0.8)
self.sensorRange = SENSOR_MAX_RANGE # cm
self.minRange = SENSOR_MIN_RANGE
def step(self, action):
l, r = 0, 0
if action == 0:
l = SPEED
r = -SPEED
elif action == 1:
l = SPEED
r = SPEED
elif action == 2:
l = -SPEED
r = SPEED
elif action == 3:
l = -SPEED
r = -SPEED
self.world.simulate(l, r, TIMESTEP)
self.time = self.time + 1
closestObstacle = sorted(
self.world.obstacles,
key=lambda obs: Pt(obs.x, obs.y).dist(
Pt(self.world.robot.x, self.world.robot.y)))[-1]
dist = Pt(closestObstacle.x, closestObstacle.y).dist(
Pt(self.world.robot.x, self.world.robot.y))
reward = Pt(self.world.robot.x, self.world.robot.y).dist(
self.startPoint) * 100 - (10000
if self.world.checkCollision() else 0)
if dist < 3:
reward = reward - ((3 - dist) * 350)
proxL = self.world.proxL if self.world.proxL is not None else SENSOR_MAX_RANGE * 2
proxR = self.world.proxR if self.world.proxR is not None else SENSOR_MAX_RANGE * 2
return (np.array([proxL, proxR]), reward, self.time > 1000
or self.world.checkCollision(), {})
def intersect(self, pt, angle, maxLen):
q = pt
s = Pt(maxLen, 0.0).rotate(angle)
intersections = []
for seg in self.segments:
r = seg[1]
a = (q - seg[0])
b = r.cross(s)
if _epsilonEquals(b, 0):
pass
else:
t = a.cross(s) / b
u = a.cross(r) / b
if t >= 0 and t <= 1 and u >= 0 and u <= 1 and seg[0] + r*t == q + s*u:
intersections.append(q + s*u)
if len(intersections) == 0:
return None
else:
return min(intersections, key=lambda p: p.dist(pt))
def getRightSensorPoint(self, dist):
point = Pt(self.x, self.y)
point = point + self.rightSensorOffset.rotate(self.theta)
point = point + Pt(dist, 0.0).rotate(self.theta)
return point
def getPosition(self):
return Pt(self.x, self.y)
class Robot:
# Coords = robot wheelbase center
def __init__(self, x = 0.0, y = 0.0, theta = 0.0, timestamp = 0.0):
self.x = x
self.y = y
self.theta = theta
self.time = timestamp
self.robotSize = (2.0, 2.0)
self.leftSensorOffset = Pt(1.0, 0.8)
self.rightSensorOffset = Pt(1.0, -0.8)
self.sensorRange = SENSOR_MAX_RANGE # cm
self.minRange = SENSOR_MIN_RANGE
def getPoints(self):
pt1 = Pt(self.x - self.robotSize[0] / 2.0, self.y - self.robotSize[1] / 2.0)
pt2 = Pt(self.x + self.robotSize[0] / 2.0, self.y - self.robotSize[1] / 2.0)
pt3 = Pt(self.x + self.robotSize[0] / 2.0, self.y + self.robotSize[1] / 2.0)
pt4 = Pt(self.x - self.robotSize[0] / 2.0, self.y + self.robotSize[1] / 2.0)
return [pt1, pt2, pt3, pt4]
def simulate(self, velR, velL, timestamp):
dt = float(timestamp - self.time)
self.time = timestamp
if _isZero(velR - velL, DRIVING_STRAIGHT):
vel = float(velR + velL) / 2
self.x = self.x + vel * dt * cos(self.theta)
self.y = self.y + vel * dt * sin(self.theta)
else:
oldTheta = float(self.theta)
theta = (velR - velL) * dt / B + oldTheta
coeff = float(B * (velR + velL)) / float(2.0 * (velR - velL))
self.x = self.x + coeff * (sin(theta) - sin(oldTheta))
self.y = self.y - coeff * (cos(theta) - cos(oldTheta))
self.theta = theta % (2 * pi)
def getPosition(self):
return Pt(self.x, self.y)
def getOrientation(self):
return self.theta
def getLeftSensorPoint(self, dist):
point = Pt(self.x, self.y)
point = point + self.leftSensorOffset.rotate(self.theta)
point = point + Pt(dist, 0.0).rotate(self.theta)
return point
def getRightSensorPoint(self, dist):
point = Pt(self.x, self.y)
point = point + self.rightSensorOffset.rotate(self.theta)
point = point + Pt(dist, 0.0).rotate(self.theta)
return point
def getLeftSensorPos(self):
return self.getLeftSensorPoint(0.0)
def getRightSensorPos(self):
return self.getRightSensorPoint(0.0)
def getRightFloorSensorPos(self):
point = Pt(self.x, self.y)
point = point + self.rightFloorSensorOffset.rotate(self.theta)
return point
def __init__(self):
self.world = World(1000, 1000, 0)
self.startPoint = Pt(-16, 0)
self.time = 0
def in_collision(self, a, x, y):
# REAL ROBOT ONLY
#return False
def intersectLine(pt1, pt2, ptA, ptB):
q = pt1
s = pt2 - pt1
intersections = []
r = ptB - ptA
a = (q - ptA)
b = r.cross(s)
if _epsilonEquals(b, 0):
pass
else:
t = a.cross(s) / b
u = a.cross(r) / b
if t >= 0 and t <= 1 and u >= 0 and u <= 1:
point = q + u * s
#self.canvas.create_oval(point[0] - 2, point[1] - 2, point[0] + 2, point[1] + 2)
return True
return False
canvas_width = self.canvas_width
canvas_height = self.canvas_height
pi4 = 3.1415 / 4 # quarter pi
a1 = a + pi4
a2 = a + 3*pi4
a3 = a + 5*pi4
a4 = a + 7*pi4
x1 = canvas_width + self.vrobot.l * math.sin(a1) + x
x2 = canvas_width + self.vrobot.l * math.sin(a2) + x
x3 = canvas_width + self.vrobot.l * math.sin(a3) + x
x4 = canvas_width + self.vrobot.l * math.sin(a4) + x
y1 = canvas_height - self.vrobot.l * math.cos(a1) - y
y2 = canvas_height - self.vrobot.l * math.cos(a2) - y
y3 = canvas_height - self.vrobot.l * math.cos(a3) - y
y4 = canvas_height - self.vrobot.l * math.cos(a4) - y
pt1 = Pt(x1, y1)
pt2 = Pt(x2, y2)
pt3 = Pt(x3, y3)
pt4 = Pt(x4, y4)
for rect in self.map:
_x1 = canvas_width + rect[0]
_y1= canvas_height - rect[1]
_x2= canvas_width + rect[2]
_y2 = canvas_height - rect[3]
_pt1 = Pt(_x1, _y1)
_pt2 = Pt(_x1, _y2)
_pt3 = Pt(_x2, _y2)
_pt4 = Pt(_x2, _y1)
for line in [(_pt1, _pt2), (_pt2, _pt3), (_pt3, _pt4), (_pt4, _pt1)]:
for line2 in [(pt1, pt2), (pt2, pt3), (pt3, pt4), (pt4, pt1)]:
if intersectLine(line[0], line[1], line2[0], line2[1]):
return True
return False