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/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 new file mode 100644 index 0000000..f702dae --- /dev/null +++ b/showcases/parallel_parking/pyglet_playground.py @@ -0,0 +1,255 @@ +import math + +import pyglet + +from .point import Point2D + +''' +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: +- [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 +- 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 +''' + +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_radius = 9 / 2 # adjust for things like we do not model the wheels exactly + +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 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) + + +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 = 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) + print("velocity: ", self.v_in_meters, self.v) + self.orientation = 0 # radius, left is positive + + # anchor positions, offsets are constant + 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 + 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 = 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): + + # 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.move(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_radius * pixel_to_meter: + if R > 0: + R = golf_turning_radius * pixel_to_meter + else: + R = -golf_turning_radius * 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 = Point2D( + x=self.top_left.x - R * math.cos(radius), + y=self.top_left.y - R * math.sin(radius) + ) + else: # right + self.turning_center = Point2D( + 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 + self.center = Point2D.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, 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], + ('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 + 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): + 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 + 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(52, window.control.psi + 10) + elif motion == pyglet.window.key.MOTION_RIGHT: + window.control.psi = max(-52, window.control.psi - 10) + + print("current control:", window.control) + window.clear() + r = window.control.psi / 180. * math.pi + 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() + + 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() + # print("current control:", window.control) + + pyglet.clock.schedule_interval(update, 0.05) + + pyglet.app.run() + + +if __name__ == '__main__': + run() 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 *