984 lines
27 KiB
Plaintext
984 lines
27 KiB
Plaintext
{
|
|
"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",
|
|
"<img src=\"formula.png\">"
|
|
]
|
|
},
|
|
{
|
|
"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
|
|
}
|