From 7ee77794615ef38cab498bb2e24d0edff3f40b9b Mon Sep 17 00:00:00 2001 From: Kai Vogelgesang Date: Mon, 25 Oct 2021 23:54:32 +0200 Subject: [PATCH] Add tilt experiment --- aruco/.gitattributes | 1 + aruco/angle_measure.py | 14 +- aruco/aruco.ipynb | 485 +++++++++++++++++++++++++++++++++++++---- aruco/tilt_180.mp4 | 3 + 4 files changed, 460 insertions(+), 43 deletions(-) create mode 100755 aruco/tilt_180.mp4 diff --git a/aruco/.gitattributes b/aruco/.gitattributes index a0478b0..75eb156 100644 --- a/aruco/.gitattributes +++ b/aruco/.gitattributes @@ -2,3 +2,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 +tilt_180.mp4 filter=lfs diff=lfs merge=lfs -text diff --git a/aruco/angle_measure.py b/aruco/angle_measure.py index 155235e..75c10e0 100644 --- a/aruco/angle_measure.py +++ b/aruco/angle_measure.py @@ -73,6 +73,11 @@ class MovingHead: if __name__ == "__main__": + lighting = MovingHead(43) + lighting.tilt = -0.5 * math.pi + lighting.rgbw = (0, 0, 0, 0xFF) + lighting.dimmer = 1 + head = MovingHead(1) head.rgbw = (0x00, 0x00, 0xFF, 0) head.tilt = -0.5 * math.pi @@ -94,8 +99,8 @@ if __name__ == "__main__": t0 = time.time() - left = -1.5 * math.pi - right = 1.5 * math.pi + left = -0.5 * math.pi + right = 0.5 * math.pi while True: @@ -103,13 +108,14 @@ if __name__ == "__main__": if int(now) % 10 < 5: - head.pan = left + head.tilt = left head.rgbw = (0xFF, 0x00, 0x00, 0) else: - head.pan = right + head.tilt = right head.rgbw = (0x00, 0xFF, 0x00, 0) head.render(dmx_data) + lighting.render(dmx_data) ser.write(dmx_data) ser.flush() diff --git a/aruco/aruco.ipynb b/aruco/aruco.ipynb index 07ed88e..274034a 100644 --- a/aruco/aruco.ipynb +++ b/aruco/aruco.ipynb @@ -2,7 +2,7 @@ "cells": [ { "cell_type": "markdown", - "id": "1f449a1b", + "id": "da2f6efd", "metadata": {}, "source": [ "# Fun with Aruco\n", @@ -12,7 +12,7 @@ }, { "cell_type": "markdown", - "id": "564b672c", + "id": "23bed7f2", "metadata": {}, "source": [ "## Imports" @@ -21,7 +21,7 @@ { "cell_type": "code", "execution_count": null, - "id": "a949b314", + "id": "91e61bf0", "metadata": {}, "outputs": [], "source": [ @@ -37,7 +37,7 @@ }, { "cell_type": "markdown", - "id": "dd01b516", + "id": "e1742fba", "metadata": {}, "source": [ "## Marker\n", @@ -48,7 +48,7 @@ { "cell_type": "code", "execution_count": null, - "id": "7a1ea892", + "id": "7337ad91", "metadata": {}, "outputs": [], "source": [ @@ -64,7 +64,7 @@ }, { "cell_type": "markdown", - "id": "1e9190d0", + "id": "e5a9c142", "metadata": {}, "source": [ "## Input\n", @@ -75,7 +75,7 @@ { "cell_type": "code", "execution_count": null, - "id": "49a9be85", + "id": "b574834e", "metadata": {}, "outputs": [], "source": [ @@ -138,7 +138,7 @@ }, { "cell_type": "markdown", - "id": "5feb4803", + "id": "e431d5af", "metadata": {}, "source": [ "The input contains several periods of the head moving back and forth.\n", @@ -148,7 +148,7 @@ { "cell_type": "code", "execution_count": null, - "id": "334b2d75", + "id": "b5d07cc1", "metadata": {}, "outputs": [], "source": [ @@ -176,7 +176,7 @@ }, { "cell_type": "markdown", - "id": "c7685829", + "id": "7c461964", "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." @@ -185,7 +185,7 @@ { "cell_type": "code", "execution_count": null, - "id": "8386785a", + "id": "c1c2def4", "metadata": {}, "outputs": [], "source": [ @@ -203,26 +203,67 @@ }, { "cell_type": "markdown", - "id": "6b3bda21", + "id": "91576c76", "metadata": {}, "source": [ - "# 360 Degrees" + "# Tilt 180 Degrees" ] }, { "cell_type": "code", "execution_count": null, - "id": "9888eedd", + "id": "62131fc1", "metadata": {}, "outputs": [], "source": [ - "global_t, global_phi = read_rotation_data(\"rotate_360.mp4\", (600, 1500), (1500, 2400))" + "cap = cv2.VideoCapture(\"tilt_180.mp4\")\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", + " break\n", + "\n", + "del cap\n", + "\n", + "fig = plt.figure(figsize=(15, 7))\n", + "ax = fig.add_subplot(1, 2, 1)\n", + "ax.imshow(frame)\n", + "\n", + "roi_topleft = (1400, 600)\n", + "roi_bottomright = (2300, 1500)\n", + "\n", + "(a, b) = roi_topleft\n", + "(c, d) = roi_bottomright\n", + "roi = frame[b:d, a:c]\n", + "\n", + "ax = fig.add_subplot(1, 2, 2)\n", + "ax.imshow(roi)" ] }, { "cell_type": "code", "execution_count": null, - "id": "0f4a9e22", + "id": "8b9e1d0d", + "metadata": {}, + "outputs": [], + "source": [ + "global_t, global_phi = read_rotation_data(\"tilt_180.mp4\", (1400, 600), (2300, 1500))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "a9b830fc", "metadata": {}, "outputs": [], "source": [ @@ -238,7 +279,231 @@ { "cell_type": "code", "execution_count": null, - "id": "861859fa", + "id": "91ac96c4", + "metadata": {}, + "outputs": [], + "source": [ + "# raw\n", + "\n", + "period = 5\n", + "t, phi = overlay(global_t, global_phi, 3.07, 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.tau / 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_tilt = np.linspace(0 + bucket_offset, period + bucket_offset, len(buckets))\n", + "pos_tilt = [sum(bucket) / len(bucket) for bucket in buckets]\n", + "\n", + "# velocity\n", + "\n", + "vel_tilt = np.diff(pos_tilt, 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_tilt_filtered = scipy.signal.lfilter(b,a,vel_tilt)\n", + "\n", + "acc_tilt = np.diff(vel_tilt_filtered, prepend=0) / dt" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "3df2b5ce", + "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_tilt, pos_tilt)\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_tilt, vel_tilt)\n", + "ax.plot(t_tilt, vel_tilt_filtered, color=\"gray\")\n", + "\n", + "acc_start_time = 0.65\n", + "acc_stop_time = 0.9\n", + "dec_start_time = 1.5\n", + "dec_stop_time = 1.85\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_tilt, acc_tilt)\n", + "\n", + "plt.show()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "89e17f29", + "metadata": {}, + "outputs": [], + "source": [ + "# approximated constant acceleration\n", + "\n", + "acc_tilt_approx = np.zeros(len(acc_tilt))\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 = pos_tilt[-1] # 0.5 * math.tau\n", + "\n", + "acc_start = np.searchsorted(t_tilt, acc_start_time)\n", + "acc_stop = np.searchsorted(t_tilt, 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_tilt, dec_start_time)\n", + "dec_stop = np.searchsorted(t_tilt, 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_tilt_approx[acc_start:acc_stop].fill(acc_value)\n", + "acc_tilt_approx[dec_start:dec_stop].fill(dec_value)\n", + "\n", + "# approximated velocity\n", + "\n", + "vel_tilt_approx = np.cumsum(acc_tilt_approx) * dt\n", + "\n", + "# approximated position\n", + "\n", + "pos_tilt_approx = np.cumsum(vel_tilt_approx) * dt" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "b0bc02f4", + "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_tilt, pos_tilt)\n", + "\n", + "ax.plot(t_tilt, pos_tilt_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_tilt, vel_tilt)\n", + "ax.plot(t_tilt, vel_tilt_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_tilt, vel_tilt_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_tilt, acc_tilt, 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_tilt, acc_tilt_approx, color=\"C1\")\n", + "\n", + "plt.show()" + ] + }, + { + "cell_type": "markdown", + "id": "163e4dca", + "metadata": {}, + "source": [ + "# 360 Degrees" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "e7c5bd8b", + "metadata": {}, + "outputs": [], + "source": [ + "global_t, global_phi = read_rotation_data(\"rotate_360.mp4\", (600, 1500), (1500, 2400))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "4be22807", + "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": "7d6f3694", "metadata": {}, "outputs": [], "source": [ @@ -278,7 +543,7 @@ { "cell_type": "code", "execution_count": null, - "id": "a52705aa", + "id": "f659cc9a", "metadata": {}, "outputs": [], "source": [ @@ -319,7 +584,7 @@ }, { "cell_type": "markdown", - "id": "9f0e11e1", + "id": "6e7da8e8", "metadata": {}, "source": [ "## Approximation\n", @@ -336,7 +601,7 @@ { "cell_type": "code", "execution_count": null, - "id": "8dc88112", + "id": "106d2335", "metadata": {}, "outputs": [], "source": [ @@ -383,7 +648,7 @@ { "cell_type": "code", "execution_count": null, - "id": "887a4959", + "id": "48716f50", "metadata": {}, "outputs": [], "source": [ @@ -433,7 +698,7 @@ }, { "cell_type": "markdown", - "id": "2699200d", + "id": "9b0adcee", "metadata": {}, "source": [ "# 540 Degrees" @@ -442,7 +707,7 @@ { "cell_type": "code", "execution_count": null, - "id": "267344a0", + "id": "8b75f733", "metadata": {}, "outputs": [], "source": [ @@ -452,7 +717,7 @@ { "cell_type": "code", "execution_count": null, - "id": "552dd8b3", + "id": "bef17bf1", "metadata": {}, "outputs": [], "source": [ @@ -468,7 +733,7 @@ { "cell_type": "code", "execution_count": null, - "id": "1c10cff1", + "id": "08df7358", "metadata": {}, "outputs": [], "source": [ @@ -520,7 +785,7 @@ { "cell_type": "code", "execution_count": null, - "id": "8261dffd", + "id": "4b226630", "metadata": {}, "outputs": [], "source": [ @@ -561,7 +826,7 @@ }, { "cell_type": "markdown", - "id": "b2bbd83f", + "id": "252158d8", "metadata": {}, "source": [ "## Approximation" @@ -570,7 +835,7 @@ { "cell_type": "code", "execution_count": null, - "id": "65cbe584", + "id": "245fc545", "metadata": {}, "outputs": [], "source": [ @@ -617,7 +882,7 @@ { "cell_type": "code", "execution_count": null, - "id": "d59c9632", + "id": "d2baff7c", "metadata": {}, "outputs": [], "source": [ @@ -667,7 +932,7 @@ }, { "cell_type": "markdown", - "id": "4eda53a3", + "id": "6dba7e18", "metadata": {}, "source": [ "# 180 degrees" @@ -676,7 +941,7 @@ { "cell_type": "code", "execution_count": null, - "id": "02da4e87", + "id": "1aa14192", "metadata": {}, "outputs": [], "source": [ @@ -686,7 +951,7 @@ { "cell_type": "code", "execution_count": null, - "id": "91a2fda0", + "id": "126c1611", "metadata": {}, "outputs": [], "source": [ @@ -702,7 +967,7 @@ { "cell_type": "code", "execution_count": null, - "id": "1b56893a", + "id": "8634be80", "metadata": {}, "outputs": [], "source": [ @@ -752,7 +1017,7 @@ { "cell_type": "code", "execution_count": null, - "id": "342d9551", + "id": "0e85ff00", "metadata": {}, "outputs": [], "source": [ @@ -798,7 +1063,7 @@ }, { "cell_type": "markdown", - "id": "18c3f7b1", + "id": "e9c0a9ef", "metadata": {}, "source": [ "## Approximation" @@ -807,7 +1072,7 @@ { "cell_type": "code", "execution_count": null, - "id": "84ece5d2", + "id": "d1a7564e", "metadata": {}, "outputs": [], "source": [ @@ -849,7 +1114,7 @@ { "cell_type": "code", "execution_count": null, - "id": "5fec98f9", + "id": "89c99381", "metadata": {}, "outputs": [], "source": [ @@ -899,7 +1164,7 @@ }, { "cell_type": "markdown", - "id": "c6811894", + "id": "82c5bfa6", "metadata": {}, "source": [ "## Comparison" @@ -908,7 +1173,7 @@ { "cell_type": "code", "execution_count": null, - "id": "faba1d94", + "id": "04772b01", "metadata": {}, "outputs": [], "source": [ @@ -957,9 +1222,151 @@ "\n", "plt.show()" ] + }, + { + "cell_type": "markdown", + "id": "374c818e", + "metadata": {}, + "source": [ + "# Model" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "f2790606", + "metadata": {}, + "outputs": [], + "source": [ + "v_max = 3.61 # rad / s\n", + "\n", + "a_acc = 4.51 # rad / s^2\n", + "a_dec = 3.83 # rad / s^2\n", + "\n", + "def time_taken(delta_phi, v_0):\n", + "\n", + " time_acc = (v_max - v_0) / a_acc\n", + " print(f\"{time_acc=}\")\n", + "\n", + " traveled_while_acc = (v_max ** 2 - v_0 ** 2) / (2 * a_acc)\n", + " print(f\"{traveled_while_acc=}\")\n", + "\n", + " time_dec = v_max / a_dec\n", + " print(f\"{time_dec=}\")\n", + "\n", + " traveled_while_dec = (v_max ** 2) / (2 * a_dec)\n", + " print(f\"{traveled_while_dec=}\")\n", + "\n", + " traveled_while_cruising = delta_phi - (traveled_while_acc + traveled_while_dec)\n", + " print(f\"{traveled_while_cruising=}\")\n", + "\n", + " time_cruise = traveled_while_cruising / v_max\n", + " print(f\"{time_cruise=}\")\n", + "\n", + " time_total = time_acc + time_cruise + time_dec\n", + " print(f\"{time_total=}\")\n", + " " + ] + }, + { + "cell_type": "markdown", + "id": "84b206bf", + "metadata": {}, + "source": [ + "## Comparison 180 degrees" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "b5e37028", + "metadata": {}, + "outputs": [], + "source": [ + "# model\n", + "print(\"Model:\")\n", + "time_taken(math.pi, 0)\n", + "\n", + "# actual\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", + "print(\"\\nActual:\")\n", + "print(f\"time acc: {acc_stop_time - acc_start_time}\")\n", + "print(f\"time dec: {dec_stop_time - dec_start_time}\")\n", + "print(f\"time cruising: {dec_start_time - acc_stop_time}\")\n", + "print(f\"time total: {dec_stop_time - acc_start_time}\")" + ] + }, + { + "cell_type": "markdown", + "id": "4b71b1c6", + "metadata": {}, + "source": [ + "## Comparison 360 degrees" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "a286ed66", + "metadata": {}, + "outputs": [], + "source": [ + "# model\n", + "print(\"Model:\")\n", + "time_taken(math.tau, 0)\n", + "\n", + "# actual\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", + "print(\"\\nActual:\")\n", + "print(f\"time acc: {acc_stop_time - acc_start_time}\")\n", + "print(f\"time dec: {dec_stop_time - dec_start_time}\")\n", + "print(f\"time cruising: {dec_start_time - acc_stop_time}\")\n", + "print(f\"time total: {dec_stop_time - acc_start_time}\")" + ] + }, + { + "cell_type": "markdown", + "id": "4e808bfe", + "metadata": {}, + "source": [ + "## Comparison 540 degrees" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "6b10a04f", + "metadata": {}, + "outputs": [], + "source": [ + "\n", + "# model\n", + "print(\"Model:\")\n", + "time_taken(3 * math.pi, 0)\n", + "\n", + "# actual\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", + "print(\"\\nActual:\")\n", + "print(f\"time acc: {acc_stop_time - acc_start_time}\")\n", + "print(f\"time dec: {dec_stop_time - dec_start_time}\")\n", + "print(f\"time cruising: {dec_start_time - acc_stop_time}\")\n", + "print(f\"time total: {dec_stop_time - acc_start_time}\")" + ] } ], "metadata": { + "interpreter": { + "hash": "fce917ee5987da4ee7f9f72fa808934308e0fb8739a0828f8cbf1cedb5699222" + }, "kernelspec": { "display_name": "Python 3 (ipykernel)", "language": "python", diff --git a/aruco/tilt_180.mp4 b/aruco/tilt_180.mp4 new file mode 100755 index 0000000..a7340d6 --- /dev/null +++ b/aruco/tilt_180.mp4 @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:135c9af0c672bdec22cd48a09a5a79ea3809ac23c081b6fcb63ec675eaac6537 +size 751445884