kai/light_maymays
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Add aruco experiment code

Browse Source
This commit is contained in:
Kai Vogelgesang 2021-09-13 09:35:22 +02:00
parent d7ed085686
commit 95ae89cab7
Signed by: kai
GPG Key ID: 0A95D3B6E62C0879
10 changed files with 1185 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
aruco/.gitattributes vendored Normal file
View File

@ -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

2
aruco/.gitignore vendored Normal file
View File

@ -0,0 +1,2 @@
venv
.ipynb_checkpoints

119
aruco/angle_measure.py Normal file
View File

@ -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()

983
aruco/aruco.ipynb Normal file
View File

@ -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",
"<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
}

BIN
aruco/formula.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 193 KiB

65
aruco/requirements.txt Normal file
View File

@ -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

BIN
aruco/rotate_180.mp4 (Stored with Git LFS) Normal file
View File

Binary file not shown.

BIN
aruco/rotate_360.mp4 (Stored with Git LFS) Normal file
View File

Binary file not shown.

BIN
aruco/rotate_540.mp4 (Stored with Git LFS) Normal file
View File

Binary file not shown.

BIN
aruco/rotate_90.mp4 (Stored with Git LFS) Normal file
View File

Binary file not shown.