{ "cells": [ { "cell_type": "markdown", "id": "da2f6efd", "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": "23bed7f2", "metadata": {}, "source": [ "## Imports" ] }, { "cell_type": "code", "execution_count": null, "id": "91e61bf0", "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": "e1742fba", "metadata": {}, "source": [ "## Marker\n", "\n", "The marker that should be printed (ours was drawn by hand ¯\\\\\\_(ツ)\\_/¯)" ] }, { "cell_type": "code", "execution_count": null, "id": "7337ad91", "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": "e5a9c142", "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": "b574834e", "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": "e431d5af", "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": "b5d07cc1", "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": "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." ] }, { "cell_type": "code", "execution_count": null, "id": "c1c2def4", "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": "91576c76", "metadata": {}, "source": [ "# Tilt 180 Degrees" ] }, { "cell_type": "code", "execution_count": null, "id": "62131fc1", "metadata": {}, "outputs": [], "source": [ "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": "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": [ "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": "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": [ "# 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": "f659cc9a", "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": "6e7da8e8", "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": "106d2335", "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": "48716f50", "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": "9b0adcee", "metadata": {}, "source": [ "# 540 Degrees" ] }, { "cell_type": "code", "execution_count": null, "id": "8b75f733", "metadata": {}, "outputs": [], "source": [ "global_t, global_phi = read_rotation_data(\"rotate_540.mp4\", (600, 1500), (1500, 2400))" ] }, { "cell_type": "code", "execution_count": null, "id": "bef17bf1", "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": "08df7358", "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": "4b226630", "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": "252158d8", "metadata": {}, "source": [ "## Approximation" ] }, { "cell_type": "code", "execution_count": null, "id": "245fc545", "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": "d2baff7c", "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": "6dba7e18", "metadata": {}, "source": [ "# 180 degrees" ] }, { "cell_type": "code", "execution_count": null, "id": "1aa14192", "metadata": {}, "outputs": [], "source": [ "global_t, global_phi = read_rotation_data(\"rotate_180.mp4\", (600, 1500), (1500, 2400))" ] }, { "cell_type": "code", "execution_count": null, "id": "126c1611", "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": "8634be80", "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": "0e85ff00", "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": "e9c0a9ef", "metadata": {}, "source": [ "## Approximation" ] }, { "cell_type": "code", "execution_count": null, "id": "d1a7564e", "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": "89c99381", "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": "82c5bfa6", "metadata": {}, "source": [ "## Comparison" ] }, { "cell_type": "code", "execution_count": null, "id": "04772b01", "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()" ] }, { "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", "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 }