From 4a25d5295ccb8e2b1432267549ccb1c4996dd194 Mon Sep 17 00:00:00 2001 From: Xiayun Sun Date: Wed, 24 Oct 2018 00:42:21 +0100 Subject: [PATCH 1/6] pyglet environment working for humans --- showcases/parallel_parking/__init__.py | 0 .../parallel_parking/pyglet_playground.py | 260 ++++++++++++++++++ 2 files changed, 260 insertions(+) create mode 100644 showcases/parallel_parking/__init__.py create mode 100644 showcases/parallel_parking/pyglet_playground.py diff --git a/showcases/parallel_parking/__init__.py b/showcases/parallel_parking/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/showcases/parallel_parking/pyglet_playground.py b/showcases/parallel_parking/pyglet_playground.py new file mode 100644 index 0000000..75afca2 --- /dev/null +++ b/showcases/parallel_parking/pyglet_playground.py @@ -0,0 +1,260 @@ +import math + +import pyglet + +''' +pyglet works with opengl without any other libs + +We want to: +- render a window, showing 3 cars (2 boxes, 1 with wheel and controllable by keyboard and external data feed) +- controls: steering (constant angular velocity), driving: True/False, forward: True/False +assume constant velocity for both angular and motion + +TODO: +- update steering continuously while moving +- collision detection +- parking success detection +- openai gym integration +- draw fancy cars +''' + +w = 480 +h = 640 + +# file:///Users/xiayunsun/Downloads/golf-vii-pa-dimensions.pdf +golf_h = 4.3 +golf_w = 2 +golf_wheelbase = 2.6 +golf_turning_circle = 10.9 / 2 + +car_h = 160 +pixel_to_meter = car_h / golf_h +car_w = int(golf_w * pixel_to_meter) + +offset = int(0.2 * pixel_to_meter) + + +class Point: + def __init__(self, x, y): + self.x = x + self.y = y + + def __add__(self, other): + return Point(self.x + other.x, self.y + other.y) + + def __sub__(self, other): + return Point(self.x - other.x, self.y - other.y) + + +class CarControl: + def __init__(self): + self.drive = False + self.forward = True + self.psi = 0 # in degrees, wheel pointing left is positive + + def __str__(self): + return "driving: %s, forward: %s, turning: %s" % (self.drive, self.forward, self.psi) + + +def rotate(point: Point, theta): + ''' + counter-clockwise; rotate w.r.t origin + ''' + x_new = math.cos(theta) * point.x - math.sin(theta) * point.y + y_new = math.sin(theta) * point.x + math.cos(theta) * point.y + + return Point(x_new, y_new) + + +def drive_straight(point: Point, orientation, distance, forward: bool): + if forward: + return Point( + point.x - distance * math.sin(orientation), + point.y + distance * math.cos(orientation) + ) + else: + return Point( + point.x + distance * math.sin(orientation), + point.y - distance * math.cos(orientation) + ) + + +class CarState: + # everything in pixel + def __init__(self): + self._w = car_w + self._h = car_h + self._wheel_base = golf_wheelbase * pixel_to_meter + + self.x = 3 * offset + 1.5 * self._w + self.y = offset + 3.5 * self._h + + self.center = Point(self.x, self.y) + + self.v_in_meters = 5 * 1600 / 3600. # 5 miles per hour + self.v = car_h / (golf_h / self.v_in_meters) + print("velocity: ", self.v_in_meters, self.v) + self.orientation = 0 # radius, left is positive + + # anchor positions, offsets are constant + self.bottom_left_offset = Point(- 0.5 * self._w, - 0.5 * self._h) + self.top_left_offset = Point(- 0.5 * self._w, + 0.5 * self._h) + self.top_right_offset = Point(+ 0.5 * self._w, + 0.5 * self._h) + self.bottom_right_offset = Point(0.5 * self._w, - 0.5 * self._h) + + self.bottom_left = self.center + self.bottom_left_offset + self.top_left = self.center + self.top_left_offset + self.top_right = self.center + self.top_right_offset + self.bottom_right = self.center + self.bottom_right_offset + + self.last_psi = None + self.turning_center = None + + def from_center(self): + self.bottom_left = rotate(self.bottom_left_offset, self.orientation) + self.center + self.top_left = rotate(self.top_left_offset, self.orientation) + self.center + self.top_right = rotate(self.top_right_offset, self.orientation) + self.center + self.bottom_right = rotate(self.bottom_right_offset, self.orientation) + self.center + + def update_vertices(self, control: CarControl, dt=None): + + # straight-line driving + if control.drive: + if control.psi == 0: + if self.orientation == 0: + if control.forward: + y_delta = self.v * dt + else: + y_delta = -1 * self.v * dt + self.center.y += y_delta + self.from_center() + + else: + # follow orientation + s = self.v * dt + # print("orientation: ", self.orientation) + self.center = drive_straight(self.center, orientation=self.orientation, distance=s, + forward=control.forward) + self.from_center() + + # rotate according to control.psi + # http://www.asawicki.info/Mirror/Car%20Physics%20for%20Games/Car%20Physics%20for%20Games.html section "curves" + else: + radius = control.psi / 180. * math.pi + R = self._wheel_base / (math.sin(radius)) + # print(R / pixel_to_meter) + if abs(R) < golf_turning_circle * pixel_to_meter: + if R > 0: + R = golf_turning_circle * pixel_to_meter + else: + R = -golf_turning_circle * pixel_to_meter + # print(R / pixel_to_meter) + omega = self.v / R + if control.forward: + theta = omega * dt + else: + theta = omega * dt * -1 + + self.orientation += theta + + # center should only change when steering angle changed + if control.psi != self.last_psi and control.psi != 0: + if control.psi > 0: # left + self.turning_center = Point( + x=self.top_left.x - R * math.cos(radius), + y=self.top_left.y - R * math.sin(radius) + ) + else: # right + self.turning_center = Point( + x=self.top_right.x - R * math.cos(radius), + y=self.top_right.y - R * math.sin(radius) + ) + self.last_psi = control.psi + + # print(self.turning_center.x, self.turning_center.y) + + # only rotate the middle point + # works for left turn only + self.center = rotate(self.center - self.turning_center, theta) + self.turning_center + self.from_center() + + l = [ + self.bottom_left.x, self.bottom_left.y, + self.top_left.x, self.top_left.y, + self.top_right.x, self.top_right.y, + self.bottom_right.x, self.bottom_right.y + ] + return [int(e) for e in l] + + +class MyWindow(pyglet.window.Window): + def __init__(self, height, width): + self.control = CarControl() + self.car = CarState() + super().__init__(height=height, width=width) + + +def run(): + window = MyWindow(height=h + 2 * offset, width=w + 2 * offset) + + batch = pyglet.graphics.Batch() + # draw 2 parked cars + batch.add_indexed(4, pyglet.gl.GL_TRIANGLES, None, + [0, 1, 2, 0, 2, 3], + ('v2i', (offset, offset + 3 * car_h, + offset, offset + 4 * car_h, + offset + car_w, offset + 4 * car_h, + offset + car_w, offset + 3 * car_h)) + ) + batch.add_indexed(4, pyglet.gl.GL_TRIANGLES, None, + [0, 1, 2, 0, 2, 3], + ('v2i', (offset, offset, + offset, offset + car_h, + offset + car_w, offset + car_h, + offset + car_w, offset)) + + ) + # draw player car, start with a box first + # todo: draw wheels + body, with a different color + car_vertices = batch.add_indexed(4, pyglet.gl.GL_TRIANGLES, None, + [0, 1, 2, 0, 2, 3], + ('v2i', tuple(window.car.update_vertices(window.control))) + ) + + # draw a small rectangle represent wheel direction + + @window.event + def on_key_press(symbol, modifiers): + if symbol == pyglet.window.key.F: + window.control.forward = True + elif symbol == pyglet.window.key.R: + window.control.forward = False + elif symbol == pyglet.window.key.D: + window.control.drive = not window.control.drive + # todo: continuously update psi + elif symbol == pyglet.window.key.LEFT: + window.control.psi = 60 + elif symbol == pyglet.window.key.RIGHT: + window.control.psi = -60 + elif symbol == pyglet.window.key.UP: + window.control.psi = 0 + + print("current control:", window.control) + + def update(dt): + ''' + This is where we update the controlled car positions + ''' + # print("scheduled ", dt) + # update car_vertices.vertices + window.clear() # important + car_vertices.vertices = window.car.update_vertices(window.control, dt) + batch.draw() + + pyglet.clock.schedule_interval(update, 0.05) + + pyglet.app.run() + + +if __name__ == '__main__': + run() From 860e5652d46e3cc24cd834bbc7ec17e9d4ad5411 Mon Sep 17 00:00:00 2001 From: Xiayun Sun Date: Wed, 24 Oct 2018 06:53:59 +0100 Subject: [PATCH 2/6] add some TODOs --- showcases/parallel_parking/pyglet_playground.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/showcases/parallel_parking/pyglet_playground.py b/showcases/parallel_parking/pyglet_playground.py index 75afca2..48131a7 100644 --- a/showcases/parallel_parking/pyglet_playground.py +++ b/showcases/parallel_parking/pyglet_playground.py @@ -11,7 +11,10 @@ assume constant velocity for both angular and motion TODO: -- update steering continuously while moving +- R should be calculated from top wheel position, instead of top_left/top_right +- to avoid errors between meter and pixels, make them different types +- tests +- update steering continuously while moving (pyglet's key "motion") - collision detection - parking success detection - openai gym integration @@ -22,10 +25,10 @@ h = 640 # file:///Users/xiayunsun/Downloads/golf-vii-pa-dimensions.pdf -golf_h = 4.3 +golf_h = 4 golf_w = 2 golf_wheelbase = 2.6 -golf_turning_circle = 10.9 / 2 +golf_turning_circle = 8 / 2 # todo: i believe in spec it's 2*pi*r, need to find out car_h = 160 pixel_to_meter = car_h / golf_h From 784dda40c81e79900354a4d4877aea7e0192bf1f Mon Sep 17 00:00:00 2001 From: Xiayun Sun Date: Wed, 24 Oct 2018 21:19:41 +0100 Subject: [PATCH 3/6] small changes --- showcases/parallel_parking/pyglet_playground.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/showcases/parallel_parking/pyglet_playground.py b/showcases/parallel_parking/pyglet_playground.py index 48131a7..5abd5a3 100644 --- a/showcases/parallel_parking/pyglet_playground.py +++ b/showcases/parallel_parking/pyglet_playground.py @@ -11,10 +11,10 @@ assume constant velocity for both angular and motion TODO: -- R should be calculated from top wheel position, instead of top_left/top_right +- R should be calculated from top wheel position, instead of top_left/top_right (right now we reduce turning_radius as a crude approx) +- update steering continuously while moving (pyglet's key "motion") - to avoid errors between meter and pixels, make them different types - tests -- update steering continuously while moving (pyglet's key "motion") - collision detection - parking success detection - openai gym integration @@ -25,10 +25,10 @@ h = 640 # file:///Users/xiayunsun/Downloads/golf-vii-pa-dimensions.pdf -golf_h = 4 +golf_h = 4.3 golf_w = 2 golf_wheelbase = 2.6 -golf_turning_circle = 8 / 2 # todo: i believe in spec it's 2*pi*r, need to find out +golf_turning_radius = 9 / 2 # adjust for things like we do not model the wheels exactly car_h = 160 pixel_to_meter = car_h / golf_h @@ -94,7 +94,7 @@ def __init__(self): self.center = Point(self.x, self.y) - self.v_in_meters = 5 * 1600 / 3600. # 5 miles per hour + self.v_in_meters = 3 * 1600 / 3600. # 5 miles per hour self.v = car_h / (golf_h / self.v_in_meters) print("velocity: ", self.v_in_meters, self.v) self.orientation = 0 # radius, left is positive @@ -146,11 +146,11 @@ def update_vertices(self, control: CarControl, dt=None): radius = control.psi / 180. * math.pi R = self._wheel_base / (math.sin(radius)) # print(R / pixel_to_meter) - if abs(R) < golf_turning_circle * pixel_to_meter: + if abs(R) < golf_turning_radius * pixel_to_meter: if R > 0: - R = golf_turning_circle * pixel_to_meter + R = golf_turning_radius * pixel_to_meter else: - R = -golf_turning_circle * pixel_to_meter + R = -golf_turning_radius * pixel_to_meter # print(R / pixel_to_meter) omega = self.v / R if control.forward: From d07abf93b151f9c943afd1918795c7580de7dae5 Mon Sep 17 00:00:00 2001 From: Xiayun Sun Date: Wed, 24 Oct 2018 22:07:26 +0100 Subject: [PATCH 4/6] show turning wheels --- .../parallel_parking/pyglet_playground.py | 32 +++++++++++++++---- 1 file changed, 26 insertions(+), 6 deletions(-) diff --git a/showcases/parallel_parking/pyglet_playground.py b/showcases/parallel_parking/pyglet_playground.py index 5abd5a3..25a5c2f 100644 --- a/showcases/parallel_parking/pyglet_playground.py +++ b/showcases/parallel_parking/pyglet_playground.py @@ -94,7 +94,7 @@ def __init__(self): self.center = Point(self.x, self.y) - self.v_in_meters = 3 * 1600 / 3600. # 5 miles per hour + self.v_in_meters = 2 * 1600 / 3600. # 5 miles per hour self.v = car_h / (golf_h / self.v_in_meters) print("velocity: ", self.v_in_meters, self.v) self.orientation = 0 # radius, left is positive @@ -225,6 +225,12 @@ def run(): ) # draw a small rectangle represent wheel direction + steering_vertices = batch.add_indexed(2, pyglet.gl.GL_LINES, None, + [0,1], + ('v2i', (400,600, + 400,540 + ))) + @window.event def on_key_press(symbol, modifiers): @@ -234,15 +240,28 @@ def on_key_press(symbol, modifiers): window.control.forward = False elif symbol == pyglet.window.key.D: window.control.drive = not window.control.drive - # todo: continuously update psi - elif symbol == pyglet.window.key.LEFT: - window.control.psi = 60 - elif symbol == pyglet.window.key.RIGHT: - window.control.psi = -60 elif symbol == pyglet.window.key.UP: window.control.psi = 0 + steering_vertices.vertices = [400,600,400,540] + window.clear() + batch.draw() + + # print("current control:", window.control) + + @window.event + def on_text_motion(motion): + if motion == pyglet.window.key.MOTION_LEFT: + window.control.psi = min(60, window.control.psi + 10) + elif motion == pyglet.window.key.MOTION_RIGHT: + window.control.psi = max(-60, window.control.psi - 10) print("current control:", window.control) + window.clear() + r = window.control.psi / 180. * math.pi + p1 = rotate(Point(0,30), r) + Point(400,570) + p2 = rotate(Point(0,-30), r) + Point(400, 570) + steering_vertices.vertices = [int(e) for e in [p1.x, p1.y, p2.x, p2.y]] + batch.draw() def update(dt): ''' @@ -253,6 +272,7 @@ def update(dt): window.clear() # important car_vertices.vertices = window.car.update_vertices(window.control, dt) batch.draw() + # print("current control:", window.control) pyglet.clock.schedule_interval(update, 0.05) From 1f8c15fd01761eab5719bd9da39ffb058250ea1b Mon Sep 17 00:00:00 2001 From: Xiayun Sun Date: Wed, 24 Oct 2018 22:31:14 +0100 Subject: [PATCH 5/6] reformat --- .../parallel_parking/pyglet_playground.py | 61 +++++++++---------- 1 file changed, 28 insertions(+), 33 deletions(-) diff --git a/showcases/parallel_parking/pyglet_playground.py b/showcases/parallel_parking/pyglet_playground.py index 25a5c2f..bf70a6b 100644 --- a/showcases/parallel_parking/pyglet_playground.py +++ b/showcases/parallel_parking/pyglet_playground.py @@ -11,14 +11,14 @@ assume constant velocity for both angular and motion TODO: -- R should be calculated from top wheel position, instead of top_left/top_right (right now we reduce turning_radius as a crude approx) -- update steering continuously while moving (pyglet's key "motion") +- [DONE] R should be calculated from top wheel position, instead of top_left/top_right (right now we reduce turning_radius as a crude approx) +- [DONE] update steering continuously while moving (pyglet's key "motion") +- tests & refactoring - to avoid errors between meter and pixels, make them different types -- tests - collision detection - parking success detection - openai gym integration -- draw fancy cars +- [OPTIONAL] draw fancy cars ''' w = 480 @@ -48,6 +48,12 @@ def __add__(self, other): def __sub__(self, other): return Point(self.x - other.x, self.y - other.y) + def rotate(self, theta): + # counter-clockwise; assuming subtracted reference point already + x = math.cos(theta) * self.x - math.sin(theta) * self.y + y = math.sin(theta) * self.x + math.cos(theta) * self.y + return Point(x, y) + class CarControl: def __init__(self): @@ -59,16 +65,6 @@ def __str__(self): return "driving: %s, forward: %s, turning: %s" % (self.drive, self.forward, self.psi) -def rotate(point: Point, theta): - ''' - counter-clockwise; rotate w.r.t origin - ''' - x_new = math.cos(theta) * point.x - math.sin(theta) * point.y - y_new = math.sin(theta) * point.x + math.cos(theta) * point.y - - return Point(x_new, y_new) - - def drive_straight(point: Point, orientation, distance, forward: bool): if forward: return Point( @@ -114,10 +110,10 @@ def __init__(self): self.turning_center = None def from_center(self): - self.bottom_left = rotate(self.bottom_left_offset, self.orientation) + self.center - self.top_left = rotate(self.top_left_offset, self.orientation) + self.center - self.top_right = rotate(self.top_right_offset, self.orientation) + self.center - self.bottom_right = rotate(self.bottom_right_offset, self.orientation) + self.center + self.bottom_left = self.bottom_left_offset.rotate(self.orientation) + self.center + self.top_left = self.top_left_offset.rotate(self.orientation) + self.center + self.top_right = self.top_right_offset.rotate(self.orientation) + self.center + self.bottom_right = self.bottom_right_offset.rotate(self.orientation) + self.center def update_vertices(self, control: CarControl, dt=None): @@ -178,7 +174,7 @@ def update_vertices(self, control: CarControl, dt=None): # only rotate the middle point # works for left turn only - self.center = rotate(self.center - self.turning_center, theta) + self.turning_center + self.center = (self.center - self.turning_center).rotate(theta) + self.turning_center self.from_center() l = [ @@ -204,10 +200,10 @@ def run(): # draw 2 parked cars batch.add_indexed(4, pyglet.gl.GL_TRIANGLES, None, [0, 1, 2, 0, 2, 3], - ('v2i', (offset, offset + 3 * car_h, - offset, offset + 4 * car_h, - offset + car_w, offset + 4 * car_h, - offset + car_w, offset + 3 * car_h)) + ('v2i', (offset, int(offset + 3 * car_h), + offset, int(offset + 4 * car_h), + offset + car_w, int(offset + 4 * car_h), + offset + car_w, int(offset + 3 * car_h))) ) batch.add_indexed(4, pyglet.gl.GL_TRIANGLES, None, [0, 1, 2, 0, 2, 3], @@ -226,11 +222,10 @@ def run(): # draw a small rectangle represent wheel direction steering_vertices = batch.add_indexed(2, pyglet.gl.GL_LINES, None, - [0,1], - ('v2i', (400,600, - 400,540 - ))) - + [0, 1], + ('v2i', (400, 600, + 400, 540 + ))) @window.event def on_key_press(symbol, modifiers): @@ -242,7 +237,7 @@ def on_key_press(symbol, modifiers): window.control.drive = not window.control.drive elif symbol == pyglet.window.key.UP: window.control.psi = 0 - steering_vertices.vertices = [400,600,400,540] + steering_vertices.vertices = [400, 600, 400, 540] window.clear() batch.draw() @@ -251,15 +246,15 @@ def on_key_press(symbol, modifiers): @window.event def on_text_motion(motion): if motion == pyglet.window.key.MOTION_LEFT: - window.control.psi = min(60, window.control.psi + 10) + window.control.psi = min(52, window.control.psi + 10) elif motion == pyglet.window.key.MOTION_RIGHT: - window.control.psi = max(-60, window.control.psi - 10) + window.control.psi = max(-52, window.control.psi - 10) print("current control:", window.control) window.clear() r = window.control.psi / 180. * math.pi - p1 = rotate(Point(0,30), r) + Point(400,570) - p2 = rotate(Point(0,-30), r) + Point(400, 570) + p1 = Point(0, 30).rotate(r) + Point(400, 570) + p2 = Point(0, -30).rotate(r) + Point(400, 570) steering_vertices.vertices = [int(e) for e in [p1.x, p1.y, p2.x, p2.y]] batch.draw() From 7b98c99c527acae2355bf39e5e762873a18267aa Mon Sep 17 00:00:00 2001 From: Xiayun Sun Date: Mon, 19 Nov 2018 20:00:44 +0000 Subject: [PATCH 6/6] other fixes --- showcases/parallel_parking/point.py | 30 ++++++++ .../parallel_parking/pyglet_playground.py | 73 +++++++------------ tests/test_common.py | 2 - 3 files changed, 55 insertions(+), 50 deletions(-) create mode 100644 showcases/parallel_parking/point.py diff --git a/showcases/parallel_parking/point.py b/showcases/parallel_parking/point.py new file mode 100644 index 0000000..c4c2ca8 --- /dev/null +++ b/showcases/parallel_parking/point.py @@ -0,0 +1,30 @@ +import math + + +class Point2D: + def __init__(self, x, y): + self.x = x + self.y = y + + def __add__(self, other): + return Point2D(self.x + other.x, self.y + other.y) + + def __sub__(self, other): + return Point2D(self.x - other.x, self.y - other.y) + + @staticmethod + def rotate(point, theta): + # counter-clockwise; assuming subtracted reference point already + x = math.cos(theta) * point.x - math.sin(theta) * point.y + y = math.sin(theta) * point.x + math.cos(theta) * point.y + + return Point2D(x, y) + + def move(self, orientation, distance, forward: bool): + # move on a straight line with orientation + if forward: + self.x -= distance * math.sin(orientation) + self.y += distance * math.cos(orientation) + else: + self.x += distance * math.sin(orientation) + self.y -= distance * math.cos(orientation) diff --git a/showcases/parallel_parking/pyglet_playground.py b/showcases/parallel_parking/pyglet_playground.py index bf70a6b..f702dae 100644 --- a/showcases/parallel_parking/pyglet_playground.py +++ b/showcases/parallel_parking/pyglet_playground.py @@ -2,6 +2,8 @@ import pyglet +from .point import Point2D + ''' pyglet works with opengl without any other libs @@ -17,7 +19,14 @@ - to avoid errors between meter and pixels, make them different types - collision detection - parking success detection + - parallel + - distance <= epsilon - openai gym integration + - action vector: [driving, forward, steering] + - state vector: + - image itself + - center coordinates + - simulate radar in real life - [OPTIONAL] draw fancy cars ''' @@ -37,24 +46,6 @@ offset = int(0.2 * pixel_to_meter) -class Point: - def __init__(self, x, y): - self.x = x - self.y = y - - def __add__(self, other): - return Point(self.x + other.x, self.y + other.y) - - def __sub__(self, other): - return Point(self.x - other.x, self.y - other.y) - - def rotate(self, theta): - # counter-clockwise; assuming subtracted reference point already - x = math.cos(theta) * self.x - math.sin(theta) * self.y - y = math.sin(theta) * self.x + math.cos(theta) * self.y - return Point(x, y) - - class CarControl: def __init__(self): self.drive = False @@ -65,19 +56,6 @@ def __str__(self): return "driving: %s, forward: %s, turning: %s" % (self.drive, self.forward, self.psi) -def drive_straight(point: Point, orientation, distance, forward: bool): - if forward: - return Point( - point.x - distance * math.sin(orientation), - point.y + distance * math.cos(orientation) - ) - else: - return Point( - point.x + distance * math.sin(orientation), - point.y - distance * math.cos(orientation) - ) - - class CarState: # everything in pixel def __init__(self): @@ -88,7 +66,7 @@ def __init__(self): self.x = 3 * offset + 1.5 * self._w self.y = offset + 3.5 * self._h - self.center = Point(self.x, self.y) + self.center = Point2D(self.x, self.y) self.v_in_meters = 2 * 1600 / 3600. # 5 miles per hour self.v = car_h / (golf_h / self.v_in_meters) @@ -96,10 +74,10 @@ def __init__(self): self.orientation = 0 # radius, left is positive # anchor positions, offsets are constant - self.bottom_left_offset = Point(- 0.5 * self._w, - 0.5 * self._h) - self.top_left_offset = Point(- 0.5 * self._w, + 0.5 * self._h) - self.top_right_offset = Point(+ 0.5 * self._w, + 0.5 * self._h) - self.bottom_right_offset = Point(0.5 * self._w, - 0.5 * self._h) + self.bottom_left_offset = Point2D(- 0.5 * self._w, - 0.5 * self._h) + self.top_left_offset = Point2D(- 0.5 * self._w, + 0.5 * self._h) + self.top_right_offset = Point2D(+ 0.5 * self._w, + 0.5 * self._h) + self.bottom_right_offset = Point2D(0.5 * self._w, - 0.5 * self._h) self.bottom_left = self.center + self.bottom_left_offset self.top_left = self.center + self.top_left_offset @@ -110,10 +88,10 @@ def __init__(self): self.turning_center = None def from_center(self): - self.bottom_left = self.bottom_left_offset.rotate(self.orientation) + self.center - self.top_left = self.top_left_offset.rotate(self.orientation) + self.center - self.top_right = self.top_right_offset.rotate(self.orientation) + self.center - self.bottom_right = self.bottom_right_offset.rotate(self.orientation) + self.center + self.bottom_left = Point2D.rotate(self.bottom_left_offset, self.orientation) + self.center + self.top_left = Point2D.rotate(self.top_left_offset, self.orientation) + self.center + self.top_right = Point2D.rotate(self.top_right_offset, self.orientation) + self.center + self.bottom_right = Point2D.rotate(self.bottom_right_offset, self.orientation) + self.center def update_vertices(self, control: CarControl, dt=None): @@ -132,8 +110,8 @@ def update_vertices(self, control: CarControl, dt=None): # follow orientation s = self.v * dt # print("orientation: ", self.orientation) - self.center = drive_straight(self.center, orientation=self.orientation, distance=s, - forward=control.forward) + self.center.move(orientation=self.orientation, distance=s, + forward=control.forward) self.from_center() # rotate according to control.psi @@ -159,12 +137,12 @@ def update_vertices(self, control: CarControl, dt=None): # center should only change when steering angle changed if control.psi != self.last_psi and control.psi != 0: if control.psi > 0: # left - self.turning_center = Point( + self.turning_center = Point2D( x=self.top_left.x - R * math.cos(radius), y=self.top_left.y - R * math.sin(radius) ) else: # right - self.turning_center = Point( + self.turning_center = Point2D( x=self.top_right.x - R * math.cos(radius), y=self.top_right.y - R * math.sin(radius) ) @@ -173,8 +151,7 @@ def update_vertices(self, control: CarControl, dt=None): # print(self.turning_center.x, self.turning_center.y) # only rotate the middle point - # works for left turn only - self.center = (self.center - self.turning_center).rotate(theta) + self.turning_center + self.center = Point2D.rotate(self.center - self.turning_center, theta) + self.turning_center self.from_center() l = [ @@ -253,8 +230,8 @@ def on_text_motion(motion): print("current control:", window.control) window.clear() r = window.control.psi / 180. * math.pi - p1 = Point(0, 30).rotate(r) + Point(400, 570) - p2 = Point(0, -30).rotate(r) + Point(400, 570) + p1 = Point2D.rotate(Point2D(0, 30), r) + Point2D(400, 570) + p2 = Point2D.rotate(Point2D(0, -30), r) + Point2D(400, 570) steering_vertices.vertices = [int(e) for e in [p1.x, p1.y, p2.x, p2.y]] batch.draw() diff --git a/tests/test_common.py b/tests/test_common.py index f63eb66..318905b 100644 --- a/tests/test_common.py +++ b/tests/test_common.py @@ -1,7 +1,5 @@ import unittest -import numpy as np - from common.policies import *