diff --git a/aruco/.gitattributes b/aruco/.gitattributes new file mode 100644 index 0000000..a0478b0 --- /dev/null +++ b/aruco/.gitattributes @@ -0,0 +1,4 @@ +rotate_180.mp4 filter=lfs diff=lfs merge=lfs -text +rotate_360.mp4 filter=lfs diff=lfs merge=lfs -text +rotate_540.mp4 filter=lfs diff=lfs merge=lfs -text +rotate_90.mp4 filter=lfs diff=lfs merge=lfs -text diff --git a/aruco/.gitignore b/aruco/.gitignore new file mode 100644 index 0000000..e1d7ba6 --- /dev/null +++ b/aruco/.gitignore @@ -0,0 +1,2 @@ +venv +.ipynb_checkpoints diff --git a/aruco/angle_measure.py b/aruco/angle_measure.py new file mode 100644 index 0000000..155235e --- /dev/null +++ b/aruco/angle_measure.py @@ -0,0 +1,119 @@ +import math +import time + +import serial + + +def clamp(x, ab): + (a, b) = ab + return max(a, min(b, x)) + + +def rescale(x, from_limits, to_limits): + (a, b) = from_limits + x_0_1 = (x - a) / (b - a) + + (c, d) = to_limits + return c + (d - c) * x_0_1 + + +class MovingHead: + def __init__(self, start_addr): + self.start_addr = start_addr + + self.pan = 0 # -3pi/2 to 3pi/2 + self.tilt = 0 # -pi/2 to pi/2 + self.speed = 0 + self.dimmer = 0 # 0 to 1 + self.rgbw = (0, 0, 0, 0) + + def __str__(self): + + return ( + f"MovingHead({self.start_addr}): pan={self.pan!r}, " + f"tilt={self.tilt!r}, speed={self.speed!r}, " + f"dimmer={self.dimmer!r}, rgbw={self.rgbw!r}" + ) + + def render(self, dst): + + pan = rescale(self.pan, (-1.5 * math.pi, 1.5 * math.pi), (255, 0)) + pan = clamp(int(pan), (0, 255)) + pan_fine = 0 + + tilt = rescale(self.tilt, (-0.5 * math.pi, 0.5 * math.pi), (0, 255)) + tilt = clamp(int(tilt), (0, 255)) + tilt_fine = 0 + + dimmer = clamp(7 + int(127 * self.dimmer), (7, 134)) + + (r, g, b, w) = self.rgbw + + channels = [ + pan, + pan_fine, + tilt, + tilt_fine, + self.speed, + dimmer, + r, + g, + b, + w, + 0, # color mode + 0, # auto jump speed + 0, # control mode + 0, # reset + ] + + offset = self.start_addr - 1 + + dst[offset : offset + len(channels)] = channels + + +if __name__ == "__main__": + + head = MovingHead(1) + head.rgbw = (0x00, 0x00, 0xFF, 0) + head.tilt = -0.5 * math.pi + head.dimmer = 1 + + dmx_data = bytearray(512) + + with serial.Serial("/dev/ttyUSB0", 500_000) as ser: + + def sync(): + # wait for sync + while True: + b = ser.readline() + if b.strip() == b"Sync.": + return + + print("syncing") + sync() + + t0 = time.time() + + left = -1.5 * math.pi + right = 1.5 * math.pi + + while True: + + now = time.time() - t0 + + + if int(now) % 10 < 5: + head.pan = left + head.rgbw = (0xFF, 0x00, 0x00, 0) + else: + head.pan = right + head.rgbw = (0x00, 0xFF, 0x00, 0) + + head.render(dmx_data) + + ser.write(dmx_data) + ser.flush() + response = ser.readline() + if response.strip() != b"Ack.": + print(f"received bad response: {response!r}") + sync() diff --git a/aruco/aruco.ipynb b/aruco/aruco.ipynb new file mode 100644 index 0000000..07ed88e --- /dev/null +++ b/aruco/aruco.ipynb @@ -0,0 +1,983 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "1f449a1b", + "metadata": {}, + "source": [ + "# Fun with Aruco\n", + "\n", + "We use Aruco markers to calculate the angular velocity of cheap-ish stage lighting" + ] + }, + { + "cell_type": "markdown", + "id": "564b672c", + "metadata": {}, + "source": [ + "## Imports" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "a949b314", + "metadata": {}, + "outputs": [], + "source": [ + "import math\n", + "\n", + "import numpy as np\n", + "import scipy.signal\n", + "import cv2, PIL\n", + "from cv2 import aruco\n", + "import matplotlib as mpl\n", + "import matplotlib.pyplot as plt" + ] + }, + { + "cell_type": "markdown", + "id": "dd01b516", + "metadata": {}, + "source": [ + "## Marker\n", + "\n", + "The marker that should be printed (ours was drawn by hand ¯\\\\\\_(ツ)\\_/¯)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "7a1ea892", + "metadata": {}, + "outputs": [], + "source": [ + "aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)\n", + "img = aruco.drawMarker(aruco_dict, 1, 700)\n", + "\n", + "fig, ax = plt.subplots()\n", + "ax.imshow(img, cmap = mpl.cm.gray, interpolation = \"nearest\")\n", + "ax.axis(\"off\")\n", + "\n", + "plt.show()" + ] + }, + { + "cell_type": "markdown", + "id": "1e9190d0", + "metadata": {}, + "source": [ + "## Input\n", + "\n", + "We read a captured video file and find the rotation of the marker." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "49a9be85", + "metadata": {}, + "outputs": [], + "source": [ + "def read_rotation_data(video_filename, roi_topleft, roi_bottomright):\n", + " cap = cv2.VideoCapture(video_filename)\n", + " fps = cap.get(cv2.CAP_PROP_FPS)\n", + "\n", + " phi = []\n", + " t = []\n", + "\n", + " frames_ok = 0\n", + " frames_err = 0\n", + " \n", + " report = int(fps + 0.5)\n", + " \n", + " while cap.isOpened():\n", + " # get frame data\n", + " frame_id = cap.get(1)\n", + " ret, frame = cap.read()\n", + " if not ret:\n", + " break\n", + " \n", + " # get region of interest\n", + " (a, b) = roi_topleft\n", + " (c, d) = roi_bottomright\n", + " roi = frame[b:d, a:c]\n", + "\n", + " # find aruco marker\n", + " corners, ids, rejectedImgPoints = aruco.detectMarkers(roi, aruco_dict)\n", + "\n", + " # calculate direction\n", + " try:\n", + " [[[a, b, c, d]]] = corners\n", + " p1 = (a + b) / 2\n", + " p2 = (d + c) / 2\n", + "\n", + " [x1, y1] = map(int, p1)\n", + " [x2, y2] = map(int, p2)\n", + "\n", + " [dx, dy] = p2 - p1\n", + " dy *= -1 # coordinates start in top left of image\n", + " # so y axis is flipped\n", + "\n", + " phi.append(math.atan2(dy, dx))\n", + " frames_ok += 1\n", + " except ValueError as e:\n", + " phi.append(None)\n", + " frames_err += 1\n", + " \n", + " # time\n", + " t.append(frame_id / fps)\n", + " \n", + " if frame_id % report == 0:\n", + " print(f\"\\r{frames_ok} frames ok, {frames_err} frames err\", end=\"\")\n", + " \n", + " print()\n", + " del cap\n", + " return t, phi" + ] + }, + { + "cell_type": "markdown", + "id": "5feb4803", + "metadata": {}, + "source": [ + "The input contains several periods of the head moving back and forth.\n", + "We overlay them on top of each other, such that we can average them later" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "334b2d75", + "metadata": {}, + "outputs": [], + "source": [ + "def overlay(global_t, global_phi, offset, period, flip_even=True):\n", + " \n", + " merged_t = []\n", + " phi = []\n", + " \n", + " for (t, v) in zip(global_t, global_phi):\n", + " (n, relative_t) = divmod(t + offset, period)\n", + " \n", + " # filter undetected markers\n", + " if not v:\n", + " continue\n", + " \n", + " # flip even periods\n", + " if flip_even and n % 2 == 0:\n", + " v *= -1\n", + " \n", + " phi.append(v)\n", + " merged_t.append(relative_t)\n", + " \n", + " return merged_t, phi" + ] + }, + { + "cell_type": "markdown", + "id": "c7685829", + "metadata": {}, + "source": [ + "Since our data is in the form `(timestamp, value)`, we need to group several data points into buckets before we can calculate the average." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "8386785a", + "metadata": {}, + "outputs": [], + "source": [ + "def get_buckets(t, phi, bucket_size):\n", + " t_max = max(t)\n", + " num_buckets = int(t_max // bucket_size + 1.5)\n", + " \n", + " buckets = [[] for _ in range(num_buckets)]\n", + "\n", + " for (now, v) in zip(t, phi):\n", + " buckets[int(now // bucket_size)].append(v)\n", + " \n", + " return buckets" + ] + }, + { + "cell_type": "markdown", + "id": "6b3bda21", + "metadata": {}, + "source": [ + "# 360 Degrees" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "9888eedd", + "metadata": {}, + "outputs": [], + "source": [ + "global_t, global_phi = read_rotation_data(\"rotate_360.mp4\", (600, 1500), (1500, 2400))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "0f4a9e22", + "metadata": {}, + "outputs": [], + "source": [ + "fig = plt.figure(figsize=(15, 7))\n", + "ax = fig.add_subplot()\n", + "ax.plot(global_t, global_phi)\n", + "ax.yaxis.set_major_locator(mpl.ticker.MultipleLocator(base=math.tau/2))\n", + "ax.set_xlabel(\"Time [s]\")\n", + "ax.set_ylabel(\"Angle [rad]\")\n", + "plt.show()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "861859fa", + "metadata": {}, + "outputs": [], + "source": [ + "# raw\n", + "\n", + "period = 5\n", + "t, phi = overlay(global_t, global_phi, 2.5, period)\n", + "\n", + "# fix discontinuity\n", + "for (i, x) in enumerate(t):\n", + " if phi[i] < 3 * x - 5:\n", + " phi[i] += math.tau\n", + "\n", + "# average\n", + "\n", + "bucket_size = dt = 0.025\n", + "buckets = get_buckets(t, phi, bucket_size)\n", + "bucket_offset = bucket_size / 2\n", + "\n", + "t_360 = np.linspace(0 + bucket_offset, period + bucket_offset, len(buckets))\n", + "pos_360 = [sum(bucket) / len(bucket) for bucket in buckets]\n", + "\n", + "# velocity\n", + "\n", + "vel_360 = np.diff(pos_360, prepend=0) / dt\n", + "\n", + "# acceleration\n", + "\n", + "n = 7 # the larger n is, the smoother curve will be\n", + "b = [1.0 / n] * n\n", + "a = 1\n", + "vel_360_filtered = scipy.signal.lfilter(b,a,vel_360)\n", + "\n", + "acc_360 = np.diff(vel_360_filtered, prepend=0) / dt" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "a52705aa", + "metadata": {}, + "outputs": [], + "source": [ + "fig = plt.figure(figsize=(15,15))\n", + "\n", + "# raw\n", + " \n", + "ax = fig.add_subplot(3,1,1)\n", + "ax.yaxis.set_major_locator(mpl.ticker.MultipleLocator(base=math.tau/2))\n", + "ax.set_ylabel(\"Angle [rad]\")\n", + "ax.scatter(t, phi, color=\"lightgray\")\n", + "\n", + "# average\n", + "\n", + "ax.plot(t_360, pos_360)\n", + "\n", + "# velocity\n", + "\n", + "ax = fig.add_subplot(3,1,2)\n", + "ax.yaxis.set_major_locator(mpl.ticker.MultipleLocator(base=math.tau/2))\n", + "ax.set_ylabel(\"Angular Velocity [rad / s]\")\n", + "ax.plot(t_360, vel_360)\n", + "ax.plot(t_360, vel_360_filtered, color=\"gray\")\n", + "\n", + "for x in [0.65, 1.45, 2.3, 3.25]:\n", + " ax.axvline(x, color='gray', linestyle='dotted')\n", + "\n", + "# acceleration\n", + "\n", + "ax = fig.add_subplot(3,1,3)\n", + "ax.yaxis.set_major_locator(mpl.ticker.MultipleLocator(base=math.tau/2))\n", + "ax.set_ylabel(\"Acceleration [rad / $s^2$]\")\n", + "ax.set_xlabel(\"Time [s]\")\n", + "ax.plot(t_360, acc_360)\n", + "\n", + "plt.show()" + ] + }, + { + "cell_type": "markdown", + "id": "9f0e11e1", + "metadata": {}, + "source": [ + "## Approximation\n", + "\n", + "From the velocity graph, it looks like the acceleration is fairly constant:\n", + "There are distinct phases where the head is accelerating and decelerating, and the velocity is constant everywhere else.\n", + "\n", + "Sadly, the noise in the acceleration graph is too strong to confirm this behavior.\n", + "Therefore, we take the start and stop times which we can see in the velocity graph, and calculate what constant acceleration would be necessary to produce these curves:\n", + "\n", + "" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "8dc88112", + "metadata": {}, + "outputs": [], + "source": [ + "# approximated constant acceleration\n", + "\n", + "acc_360_approx = np.zeros(len(acc_360))\n", + "\n", + "acc_start_time = 0.65\n", + "acc_stop_time = 1.45\n", + "dec_start_time = 2.3\n", + "dec_stop_time = 3.25\n", + "\n", + "t_0 = acc_stop_time - acc_start_time\n", + "t_1 = dec_start_time - acc_stop_time\n", + "t_2 = dec_stop_time - dec_start_time\n", + "h = math.tau\n", + "\n", + "acc_start = np.searchsorted(t_360, 0.65)\n", + "acc_stop = np.searchsorted(t_360, 1.45)\n", + "\n", + "acc_value = h / ((t_0 / 2 + t_1 + t_2 / 2) * t_0)\n", + "\n", + "print(f\"acceleration: {acc_value: .2f} ({acc_value / math.pi: .2f} pi)\")\n", + "\n", + "dec_start = np.searchsorted(t_360, 2.3)\n", + "dec_stop = np.searchsorted(t_360, 3.25)\n", + "\n", + "dec_value = -1 * acc_value * (acc_stop - acc_start) / (dec_stop - dec_start)\n", + "\n", + "print(f\"deceleration: {dec_value: .2f} ({dec_value / math.pi: .2f} pi)\")\n", + "\n", + "acc_360_approx[acc_start:acc_stop].fill(acc_value)\n", + "acc_360_approx[dec_start:dec_stop].fill(dec_value)\n", + "\n", + "# approximated velocity\n", + "\n", + "vel_360_approx = np.cumsum(acc_360_approx) * dt\n", + "\n", + "# approximated position\n", + "\n", + "pos_360_approx = np.cumsum(vel_360_approx) * dt" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "887a4959", + "metadata": {}, + "outputs": [], + "source": [ + "fig = plt.figure(figsize=(15,15))\n", + "\n", + "# raw\n", + " \n", + "ax = fig.add_subplot(3,1,1)\n", + "ax.yaxis.set_major_locator(mpl.ticker.MultipleLocator(base=math.tau/2))\n", + "ax.set_ylabel(\"Angle [rad]\")\n", + "ax.scatter(t, phi, color=\"lightgray\")\n", + "\n", + "# average\n", + "\n", + "ax.plot(t_360, pos_360)\n", + "\n", + "ax.plot(t_360, pos_360_approx)\n", + "\n", + "# velocity\n", + "\n", + "ax = fig.add_subplot(3,1,2)\n", + "ax.yaxis.set_major_locator(mpl.ticker.MultipleLocator(base=math.tau/2))\n", + "ax.set_ylabel(\"Angular Velocity [rad / s]\")\n", + "ax.plot(t_360, vel_360)\n", + "ax.plot(t_360, vel_360_filtered, color=\"gray\", linestyle=\"dotted\")\n", + "\n", + "for x in [0.65, 1.45, 2.3, 3.25]:\n", + " ax.axvline(x, color='gray', linestyle='dotted')\n", + " \n", + "ax.plot(t_360, vel_360_approx)\n", + "\n", + "# acceleration\n", + "\n", + "ax = fig.add_subplot(3,1,3)\n", + "ax.yaxis.set_major_locator(mpl.ticker.MultipleLocator(base=math.tau/2))\n", + "ax.set_ylabel(\"Acceleration [rad / $s^2$]\")\n", + "ax.set_xlabel(\"Time [s]\")\n", + "ax.plot(t_360, acc_360, color=\"lightgray\")\n", + "\n", + "for x in [0.65, 1.45, 2.3, 3.25]:\n", + " ax.axvline(x, color='gray', linestyle='dotted')\n", + " \n", + "ax.plot(t_360, acc_360_approx, color=\"C1\")\n", + "\n", + "plt.show()" + ] + }, + { + "cell_type": "markdown", + "id": "2699200d", + "metadata": {}, + "source": [ + "# 540 Degrees" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "267344a0", + "metadata": {}, + "outputs": [], + "source": [ + "global_t, global_phi = read_rotation_data(\"rotate_540.mp4\", (600, 1500), (1500, 2400))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "552dd8b3", + "metadata": {}, + "outputs": [], + "source": [ + "fig = plt.figure(figsize=(15, 7))\n", + "ax = fig.add_subplot()\n", + "ax.plot(global_t[:750], global_phi[:750])\n", + "ax.yaxis.set_major_locator(mpl.ticker.MultipleLocator(base=math.tau/2))\n", + "ax.set_xlabel(\"Time [s]\")\n", + "ax.set_ylabel(\"Angle [rad]\")\n", + "plt.show()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "1c10cff1", + "metadata": {}, + "outputs": [], + "source": [ + "# raw\n", + "\n", + "period = 5\n", + "\n", + "# overlay, keeping forward and back motion separate\n", + "t, phi = overlay(global_t, global_phi, 0.05, 2 * period, flip_even = False)\n", + "\n", + "# fix discontinuity and shift so 0 is vertical center\n", + "for (i, x) in enumerate(t):\n", + " if phi[i] >= max(-3 * x + 5, 3 * x - 23):\n", + " phi[i] -= math.tau\n", + " phi[i] += 1.5 * math.pi\n", + " \n", + "# overlay again to merge both motions\n", + "\n", + "t, phi = overlay(t, phi, 0, period)\n", + "\n", + "phi_0 = phi[np.argmin(t)]\n", + "\n", + "for i in range(len(phi)):\n", + " phi[i] -= phi_0\n", + " \n", + "# average\n", + "\n", + "bucket_size = dt = 0.025\n", + "buckets = get_buckets(t, phi, bucket_size)\n", + "bucket_offset = bucket_size / 2\n", + "\n", + "t_540 = np.linspace(0 + bucket_offset, period + bucket_offset, len(buckets))\n", + "pos_540 = [sum(bucket) / len(bucket) for bucket in buckets]\n", + "\n", + "# velocity\n", + "\n", + "vel_540 = np.diff(pos_540, prepend=0) / dt\n", + "\n", + "# acceleration\n", + "\n", + "n = 7 # the larger n is, the smoother curve will be\n", + "b = [1.0 / n] * n\n", + "a = 1\n", + "vel_540_filtered = scipy.signal.lfilter(b,a,vel_540)\n", + "\n", + "acc_540 = np.diff(vel_540_filtered, prepend=0) / dt" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "8261dffd", + "metadata": {}, + "outputs": [], + "source": [ + "fig = plt.figure(figsize=(15,15))\n", + "\n", + "# raw\n", + " \n", + "ax = fig.add_subplot(3,1,1)\n", + "ax.yaxis.set_major_locator(mpl.ticker.MultipleLocator(base=math.tau/2))\n", + "ax.set_ylabel(\"Angle [rad]\")\n", + "ax.scatter(t, phi, color=\"lightgray\")\n", + "\n", + "# average\n", + "\n", + "ax.plot(t_540, pos_540)\n", + "\n", + "# velocity\n", + "\n", + "ax = fig.add_subplot(3,1,2)\n", + "ax.yaxis.set_major_locator(mpl.ticker.MultipleLocator(base=math.tau/2))\n", + "ax.set_ylabel(\"Angular Velocity [rad / s]\")\n", + "ax.plot(t_540, vel_540)\n", + "ax.plot(t_540, vel_540_filtered, color=\"gray\")\n", + "\n", + "for x in [0.65, 1.45, 3.2, 4.15]:\n", + " ax.axvline(x, color='gray', linestyle='dotted')\n", + "\n", + "# acceleration\n", + "\n", + "ax = fig.add_subplot(3,1,3)\n", + "ax.yaxis.set_major_locator(mpl.ticker.MultipleLocator(base=math.tau/2))\n", + "ax.set_ylabel(\"Acceleration [rad / $s^2$]\")\n", + "ax.set_xlabel(\"Time [s]\")\n", + "ax.plot(t_540, acc_540)\n", + "\n", + "plt.show()" + ] + }, + { + "cell_type": "markdown", + "id": "b2bbd83f", + "metadata": {}, + "source": [ + "## Approximation" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "65cbe584", + "metadata": {}, + "outputs": [], + "source": [ + "# approximated constant acceleration\n", + "\n", + "acc_540_approx = np.zeros(len(acc_540))\n", + "\n", + "acc_start_time = 0.65\n", + "acc_stop_time = 1.45\n", + "dec_start_time = 3.2\n", + "dec_stop_time = 4.15\n", + "\n", + "t_0 = acc_stop_time - acc_start_time\n", + "t_1 = dec_start_time - acc_stop_time\n", + "t_2 = dec_stop_time - dec_start_time\n", + "h = 1.5 * math.tau\n", + "\n", + "acc_start = np.searchsorted(t_540, acc_start_time)\n", + "acc_stop = np.searchsorted(t_540, acc_stop_time)\n", + "\n", + "acc_value = h / ((t_0 / 2 + t_1 + t_2 / 2) * t_0)\n", + "\n", + "print(f\"acceleration: {acc_value: .2f} ({acc_value / math.pi: .2f} pi)\")\n", + "\n", + "dec_start = np.searchsorted(t_540, dec_start_time)\n", + "dec_stop = np.searchsorted(t_540, dec_stop_time)\n", + "\n", + "dec_value = -1 * acc_value * (acc_stop - acc_start) / (dec_stop - dec_start)\n", + "\n", + "print(f\"deceleration: {dec_value: .2f} ({dec_value / math.pi: .2f} pi)\")\n", + "\n", + "acc_540_approx[acc_start:acc_stop].fill(acc_value)\n", + "acc_540_approx[dec_start:dec_stop].fill(dec_value)\n", + "\n", + "# approximated velocity\n", + "\n", + "vel_540_approx = np.cumsum(acc_540_approx) * dt\n", + "\n", + "# approximated position\n", + "\n", + "pos_540_approx = np.cumsum(vel_540_approx) * dt" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "d59c9632", + "metadata": {}, + "outputs": [], + "source": [ + "fig = plt.figure(figsize=(15,15))\n", + "\n", + "# raw\n", + " \n", + "ax = fig.add_subplot(3,1,1)\n", + "ax.yaxis.set_major_locator(mpl.ticker.MultipleLocator(base=math.tau/2))\n", + "ax.set_ylabel(\"Angle [rad]\")\n", + "ax.scatter(t, phi, color=\"lightgray\")\n", + "\n", + "# average\n", + "\n", + "ax.plot(t_540, pos_540)\n", + "\n", + "ax.plot(t_540, pos_540_approx)\n", + "\n", + "# velocity\n", + "\n", + "ax = fig.add_subplot(3,1,2)\n", + "ax.yaxis.set_major_locator(mpl.ticker.MultipleLocator(base=math.tau/2))\n", + "ax.set_ylabel(\"Angular Velocity [rad / s]\")\n", + "ax.plot(t_540, vel_540)\n", + "ax.plot(t_540, vel_540_filtered, color=\"gray\", linestyle=\"dotted\")\n", + "\n", + "for x in [acc_start_time, acc_stop_time, dec_start_time, dec_stop_time]:\n", + " ax.axvline(x, color='gray', linestyle='dotted')\n", + " \n", + "ax.plot(t_540, vel_540_approx)\n", + "\n", + "# acceleration\n", + "\n", + "ax = fig.add_subplot(3,1,3)\n", + "ax.yaxis.set_major_locator(mpl.ticker.MultipleLocator(base=math.tau/2))\n", + "ax.set_ylabel(\"Acceleration [rad / $s^2$]\")\n", + "ax.set_xlabel(\"Time [s]\")\n", + "ax.plot(t_540, acc_540, color=\"lightgray\")\n", + "\n", + "for x in [acc_start_time, acc_stop_time, dec_start_time, dec_stop_time]:\n", + " ax.axvline(x, color='gray', linestyle='dotted')\n", + " \n", + "ax.plot(t_540, acc_540_approx, color=\"C1\")\n", + "\n", + "plt.show()" + ] + }, + { + "cell_type": "markdown", + "id": "4eda53a3", + "metadata": {}, + "source": [ + "# 180 degrees" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "02da4e87", + "metadata": {}, + "outputs": [], + "source": [ + "global_t, global_phi = read_rotation_data(\"rotate_180.mp4\", (600, 1500), (1500, 2400))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "91a2fda0", + "metadata": {}, + "outputs": [], + "source": [ + "fig = plt.figure(figsize=(15, 7))\n", + "ax = fig.add_subplot()\n", + "ax.plot(global_t[:750], global_phi[:750])\n", + "ax.yaxis.set_major_locator(mpl.ticker.MultipleLocator(base=math.tau/2))\n", + "ax.set_xlabel(\"Time [s]\")\n", + "ax.set_ylabel(\"Angle [rad]\")\n", + "plt.show()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "1b56893a", + "metadata": {}, + "outputs": [], + "source": [ + "# raw\n", + "\n", + "period = 5\n", + "t, phi = overlay(global_t, global_phi, 6.6, 2 * period, flip_even = False)\n", + "\n", + "# fix discontinuity and shift so 0 is vertical center\n", + "for (i, x) in enumerate(t):\n", + " if phi[i] < -1:\n", + " phi[i] += math.tau\n", + " phi[i] -= math.pi / 2\n", + " \n", + "# overlay again to merge both motions\n", + "\n", + "t, phi = overlay(t, phi, 0, period)\n", + "\n", + "phi_0 = phi[np.argmin(t)]\n", + "\n", + "for i in range(len(phi)):\n", + " phi[i] -= phi_0\n", + "\n", + "# average\n", + "\n", + "bucket_size = dt = 0.025\n", + "buckets = get_buckets(t, phi, bucket_size)\n", + "bucket_offset = bucket_size / 2\n", + "\n", + "t_180 = np.linspace(0 + bucket_offset, period + bucket_offset, len(buckets))\n", + "pos_180 = [sum(bucket) / len(bucket) for bucket in buckets]\n", + "\n", + "# velocity\n", + "\n", + "vel_180 = np.diff(pos_180, prepend=0) / dt\n", + "\n", + "# acceleration\n", + "\n", + "n = 7 # the larger n is, the smoother curve will be\n", + "b = [1.0 / n] * n\n", + "a = 1\n", + "vel_180_filtered = scipy.signal.lfilter(b,a,vel_180)\n", + "\n", + "acc_180 = np.diff(vel_180_filtered, prepend=0) / dt" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "342d9551", + "metadata": {}, + "outputs": [], + "source": [ + "fig = plt.figure(figsize=(15,15))\n", + "\n", + "# raw\n", + " \n", + "ax = fig.add_subplot(3,1,1)\n", + "ax.yaxis.set_major_locator(mpl.ticker.MultipleLocator(base=math.tau/2))\n", + "ax.set_ylabel(\"Angle [rad]\")\n", + "ax.scatter(t, phi, color=\"lightgray\")\n", + "\n", + "# average\n", + "\n", + "ax.plot(t_180, pos_180)\n", + "\n", + "# velocity\n", + "\n", + "ax = fig.add_subplot(3,1,2)\n", + "ax.yaxis.set_major_locator(mpl.ticker.MultipleLocator(base=math.tau/2))\n", + "ax.set_ylabel(\"Angular Velocity [rad / s]\")\n", + "ax.plot(t_180, vel_180)\n", + "ax.plot(t_180, vel_180_filtered, color=\"gray\")\n", + "\n", + "acc_start_time = 0.65\n", + "acc_stop_time = 1.45\n", + "dec_start_time = 1.45\n", + "dec_stop_time = 2.4\n", + "\n", + "for x in [acc_start_time, acc_stop_time, dec_start_time, dec_stop_time]:\n", + " ax.axvline(x, color='gray', linestyle='dotted')\n", + "\n", + "# acceleration\n", + "\n", + "ax = fig.add_subplot(3,1,3)\n", + "ax.yaxis.set_major_locator(mpl.ticker.MultipleLocator(base=math.tau/2))\n", + "ax.set_ylabel(\"Acceleration [rad / $s^2$]\")\n", + "ax.set_xlabel(\"Time [s]\")\n", + "ax.plot(t_180, acc_180)\n", + "\n", + "plt.show()" + ] + }, + { + "cell_type": "markdown", + "id": "18c3f7b1", + "metadata": {}, + "source": [ + "## Approximation" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "84ece5d2", + "metadata": {}, + "outputs": [], + "source": [ + "# approximated constant acceleration\n", + "\n", + "acc_180_approx = np.zeros(len(acc_540))\n", + "\n", + "t_0 = acc_stop_time - acc_start_time\n", + "t_1 = dec_start_time - acc_stop_time\n", + "t_2 = dec_stop_time - dec_start_time\n", + "h = 0.5 * math.tau\n", + "\n", + "acc_start = np.searchsorted(t_180, acc_start_time)\n", + "acc_stop = np.searchsorted(t_180, acc_stop_time)\n", + "\n", + "acc_value = h / ((t_0 / 2 + t_1 + t_2 / 2) * t_0)\n", + "\n", + "print(f\"acceleration: {acc_value: .2f} ({acc_value / math.pi: .2f} pi)\")\n", + "\n", + "dec_start = np.searchsorted(t_180, dec_start_time)\n", + "dec_stop = np.searchsorted(t_180, dec_stop_time)\n", + "\n", + "dec_value = -1 * acc_value * (acc_stop - acc_start) / (dec_stop - dec_start)\n", + "\n", + "print(f\"deceleration: {dec_value: .2f} ({dec_value / math.pi: .2f} pi)\")\n", + "\n", + "acc_180_approx[acc_start:acc_stop].fill(acc_value)\n", + "acc_180_approx[dec_start:dec_stop].fill(dec_value)\n", + "\n", + "# approximated velocity\n", + "\n", + "vel_180_approx = np.cumsum(acc_180_approx) * dt\n", + "\n", + "# approximated position\n", + "\n", + "pos_180_approx = np.cumsum(vel_180_approx) * dt" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "5fec98f9", + "metadata": {}, + "outputs": [], + "source": [ + "fig = plt.figure(figsize=(15,15))\n", + "\n", + "# raw\n", + " \n", + "ax = fig.add_subplot(3,1,1)\n", + "ax.yaxis.set_major_locator(mpl.ticker.MultipleLocator(base=math.tau/2))\n", + "ax.set_ylabel(\"Angle [rad]\")\n", + "ax.scatter(t, phi, color=\"lightgray\")\n", + "\n", + "# average\n", + "\n", + "ax.plot(t_180, pos_180)\n", + "\n", + "ax.plot(t_180, pos_180_approx)\n", + "\n", + "# velocity\n", + "\n", + "ax = fig.add_subplot(3,1,2)\n", + "ax.yaxis.set_major_locator(mpl.ticker.MultipleLocator(base=math.tau/2))\n", + "ax.set_ylabel(\"Angular Velocity [rad / s]\")\n", + "ax.plot(t_180, vel_180)\n", + "ax.plot(t_180, vel_180_filtered, color=\"gray\", linestyle=\"dotted\")\n", + "\n", + "for x in [acc_start_time, acc_stop_time, dec_start_time, dec_stop_time]:\n", + " ax.axvline(x, color='gray', linestyle='dotted')\n", + " \n", + "ax.plot(t_180, vel_180_approx)\n", + "\n", + "# acceleration\n", + "\n", + "ax = fig.add_subplot(3,1,3)\n", + "ax.yaxis.set_major_locator(mpl.ticker.MultipleLocator(base=math.tau/2))\n", + "ax.set_ylabel(\"Acceleration [rad / $s^2$]\")\n", + "ax.set_xlabel(\"Time [s]\")\n", + "ax.plot(t_180, acc_180, color=\"lightgray\")\n", + "\n", + "for x in [acc_start_time, acc_stop_time, dec_start_time, dec_stop_time]:\n", + " ax.axvline(x, color='gray', linestyle='dotted')\n", + " \n", + "ax.plot(t_180, acc_180_approx, color=\"C1\")\n", + "\n", + "plt.show()" + ] + }, + { + "cell_type": "markdown", + "id": "c6811894", + "metadata": {}, + "source": [ + "## Comparison" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "faba1d94", + "metadata": {}, + "outputs": [], + "source": [ + "fig = plt.figure(figsize=(15,15))\n", + "\n", + "# position\n", + " \n", + "ax = fig.add_subplot(3,1,1)\n", + "ax.yaxis.set_major_locator(mpl.ticker.MultipleLocator(base=math.tau/2))\n", + "ax.set_ylabel(\"Angle [rad]\")\n", + "\n", + "ax.plot(t_180, pos_180, color=\"lightgray\", linewidth=7)\n", + "ax.plot(t_180, pos_180_approx)\n", + "\n", + "ax.plot(t_360, pos_360, color=\"lightgray\", linewidth=7)\n", + "ax.plot(t_360, pos_360_approx)\n", + "\n", + "ax.plot(t_540, pos_540, color=\"lightgray\", linewidth=7)\n", + "ax.plot(t_540, pos_540)\n", + "\n", + "# velocity\n", + "\n", + "ax = fig.add_subplot(3,1,2)\n", + "ax.yaxis.set_major_locator(mpl.ticker.MultipleLocator(base=math.tau/2))\n", + "ax.set_ylabel(\"Angular Velocity [rad / s]\")\n", + "\n", + "ax.plot(t_180, vel_180, color=\"lightgray\")\n", + "ax.plot(t_180, vel_180_approx)\n", + "\n", + "ax.plot(t_360, vel_360, color=\"lightgray\")\n", + "ax.plot(t_360, vel_360_approx)\n", + "\n", + "ax.plot(t_540, vel_540, color=\"lightgray\")\n", + "ax.plot(t_540, vel_540_approx)\n", + "\n", + "# acceleration\n", + "\n", + "ax = fig.add_subplot(3,1,3)\n", + "ax.yaxis.set_major_locator(mpl.ticker.MultipleLocator(base=math.tau/2))\n", + "ax.set_ylabel(\"Acceleration [rad / $s^2$]\")\n", + "ax.set_xlabel(\"Time [s]\")\n", + "\n", + "ax.plot(t_180, acc_180_approx)\n", + "ax.plot(t_360, acc_360_approx)\n", + "ax.plot(t_540, acc_540_approx)\n", + "\n", + "plt.show()" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3 (ipykernel)", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.9.5" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/aruco/formula.png b/aruco/formula.png new file mode 100644 index 0000000..ecb3387 Binary files /dev/null and b/aruco/formula.png differ diff --git a/aruco/requirements.txt b/aruco/requirements.txt new file mode 100644 index 0000000..faa062e --- /dev/null +++ b/aruco/requirements.txt @@ -0,0 +1,65 @@ +argon2-cffi==21.1.0 +attrs==21.2.0 +backcall==0.2.0 +bleach==4.1.0 +cffi==1.14.6 +cycler==0.10.0 +debugpy==1.4.3 +decorator==5.1.0 +defusedxml==0.7.1 +entrypoints==0.3 +ipykernel==6.4.1 +ipython==7.27.0 +ipython-genutils==0.2.0 +ipywidgets==7.6.4 +jedi==0.18.0 +Jinja2==3.0.1 +jsonschema==3.2.0 +jupyter==1.0.0 +jupyter-client==7.0.2 +jupyter-console==6.4.0 +jupyter-core==4.7.1 +jupyterlab-pygments==0.1.2 +jupyterlab-widgets==1.0.1 +kiwisolver==1.3.2 +MarkupSafe==2.0.1 +matplotlib==3.4.3 +matplotlib-inline==0.1.3 +mistune==0.8.4 +nbclient==0.5.4 +nbconvert==6.1.0 +nbformat==5.1.3 +nest-asyncio==1.5.1 +notebook==6.4.3 +numpy==1.21.2 +opencv-contrib-python==4.5.3.56 +opencv-python==4.5.3.56 +packaging==21.0 +pandocfilters==1.4.3 +parso==0.8.2 +pexpect==4.8.0 +pickleshare==0.7.5 +Pillow==8.3.2 +prometheus-client==0.11.0 +prompt-toolkit==3.0.20 +ptyprocess==0.7.0 +pycparser==2.20 +Pygments==2.10.0 +pyparsing==2.4.7 +pyrsistent==0.18.0 +pyserial==3.5 +pytesseract==0.3.8 +python-dateutil==2.8.2 +pyzmq==22.2.1 +qtconsole==5.1.1 +QtPy==1.11.0 +scipy==1.7.1 +Send2Trash==1.8.0 +six==1.16.0 +terminado==0.12.1 +testpath==0.5.0 +tornado==6.1 +traitlets==5.1.0 +wcwidth==0.2.5 +webencodings==0.5.1 +widgetsnbextension==3.5.1 diff --git a/aruco/rotate_180.mp4 b/aruco/rotate_180.mp4 new file mode 100644 index 0000000..41dc177 --- /dev/null +++ b/aruco/rotate_180.mp4 @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f205a2c136eb1fcef07df6428cca52355bf7b09811a7cd256affdac71621bc29 +size 796218379 diff --git a/aruco/rotate_360.mp4 b/aruco/rotate_360.mp4 new file mode 100644 index 0000000..119c938 --- /dev/null +++ b/aruco/rotate_360.mp4 @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:571c036a2b9bf510cf19dba8ccb47b74acf91d110e97f590dc52533999040546 +size 795840422 diff --git a/aruco/rotate_540.mp4 b/aruco/rotate_540.mp4 new file mode 100644 index 0000000..f18351e --- /dev/null +++ b/aruco/rotate_540.mp4 @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:642becebc3ead0103feee21e23dfa5db9cd2b1dced464fbcf4c86cf078b8869c +size 781806942 diff --git a/aruco/rotate_90.mp4 b/aruco/rotate_90.mp4 new file mode 100644 index 0000000..d634635 --- /dev/null +++ b/aruco/rotate_90.mp4 @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:28b79bf93fcd257763a410d980f711eba8f16c66b0a5ed06d02fb7ebecebc72d +size 44884406