zenloger/autopilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: change coordinate system of simulator and autopilot, reduce approximation error

Browse Source
This commit is contained in:
russian_proger
2025-10-07 18:25:09 +03:00
parent 520a333812
commit f0ac60c8ef
5 changed files with 272 additions and 113 deletions
Download Patch File Download Diff File Expand all files Collapse all files

186
test_autopilot.ipynb
View File

@@ -40,7 +40,7 @@
"\n",
"vzm.set_target_points(points_coords)\n",
"autopilot = AutoPilot(points_coords, [], vzm)\n",
"imgs = [Image.open(Path('images') / f'photo_{i}.png') for i in range(10)]\n",
"imgs = [Image.open(Path('images') / f'photo_{i}.png') for i in range(30)]\n",
"autopilot.handle(imgs[0])\n",
"vzm.update_drone_trajectory(autopilot.x, autopilot.y)\n"
]
@@ -49,19 +49,54 @@
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"-84.81833939223279\n",
"[[[ 0.43529349 -148.93070048]]]\n",
"164.0668026014628 -13.040291124133018 90.0\n"
]
}
],
"source": [
"import cv2\n",
"import math\n",
"mat = np.array([[ 9.03878625e-02, 9.95998304e-01, -1.36910620e+01],\n",
" [-9.96729952e-01, 9.12780821e-02, 6.55573605e+00],\n",
" [ 1.28059260e-05, -4.64443066e-06, 1.00000000e+00]])\n",
"\n",
"x = 156.295617\n",
"y = 0\n",
"\n",
"print(np.atan2(mat[1, 0], mat[0, 0]) / math.pi * 180)\n",
"\n",
"print(cv2.perspectiveTransform(np.array([x, y]).reshape((1, 1, 2)), mat))\n",
"\n",
"nx, ny, na = autopilot.calc_position(np.linalg.inv(mat), x, y, math.pi / 2)\n",
"\n",
"print(nx, ny, na / math.pi * 180)"
]
},
{
"cell_type": "code",
"execution_count": 7,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\n",
" [Pilot] translate: -1.5380446518826545 49.21369409309772\n",
" [Pilot] translate: -2.2243520368114386e-14 2.083887107525814e-15\n",
" [Pilot] Drone Position: (47.97, -11.12)\n",
" [Pilot] Angle: -11.3°\n",
" [Pilot] Target Index: 0\n",
" [Pilot] Target Position: [0. 0.]\n",
" [Pilot] Distance: 0.0\n",
"0.004129499336505804 50.0\n",
" [Pilot] Target Index: 1\n",
" [Pilot] Target Position: [347.32359333 -69.46471867]\n",
" [Pilot] Distance: 304.9907527982671\n",
"0.004129499336505887 50.0\n",
"47.96512465732409 -11.122053958129657\n",
"\n",
" [Pilot] translate: -0.773373251191613 49.85522586610471\n",
" [Pilot] Drone Position: (96.75, -21.44)\n",
@@ -69,7 +104,8 @@
" [Pilot] Target Index: 1\n",
" [Pilot] Target Position: [347.32359333 -69.46471867]\n",
" [Pilot] Distance: 304.9907527982671\n",
"0.003525142621744054 50.0\n",
"0.003525142621744165 50.0\n",
"96.7474713468631 -21.438269533988773\n",
"\n",
" [Pilot] translate: -0.4238729464016157 49.716078726704744\n",
" [Pilot] Drone Position: (145.50, -31.21)\n",
@@ -77,7 +113,8 @@
" [Pilot] Target Index: 1\n",
" [Pilot] Target Position: [347.32359333 -69.46471867]\n",
" [Pilot] Distance: 255.13708614259195\n",
"0.0019957432639431116 50.0\n",
"0.0019957432639432504 50.0\n",
"145.49554314093962 -31.210354705238323\n",
"\n",
" [Pilot] translate: -0.5072011337859063 49.476309464169745\n",
" [Pilot] Drone Position: (194.03, -40.83)\n",
@@ -85,7 +122,8 @@
" [Pilot] Target Index: 1\n",
" [Pilot] Target Position: [347.32359333 -69.46471867]\n",
" [Pilot] Distance: 205.42141613269357\n",
"0.0008527889497844943 50.0\n",
"0.0008527889497846608 50.0\n",
"194.02953935562192 -40.83395396007064\n",
"\n",
" [Pilot] translate: 0.11201474583194077 50.52107985317721\n",
" [Pilot] Drone Position: (243.70, -50.06)\n",
@@ -93,7 +131,8 @@
" [Pilot] Target Index: 1\n",
" [Pilot] Target Position: [347.32359333 -69.46471867]\n",
" [Pilot] Distance: 155.9448225243591\n",
"0.0006948810462804877 50.0\n",
"0.0006948810462807098 50.0\n",
"243.70149729872674 -50.058304936308375\n",
"\n",
" [Pilot] translate: 0.0398933070509686 50.472489376060885\n",
" [Pilot] Drone Position: (293.34, -59.18)\n",
@@ -102,65 +141,154 @@
" [Pilot] Target Position: [347.32359333 -69.46471867]\n",
" [Pilot] Distance: 105.42365806674246\n",
"-0.005757684837339738 50.0\n",
"293.3429112368944 -59.17991649962423\n",
"\n",
" [Pilot] translate: -0.5779730654942618 49.87019784134602\n",
" [Pilot] Drone Position: (342.21, -69.12)\n",
" [Pilot] Angle: -10.8°\n",
" [Pilot] Target Index: 1\n",
" [Pilot] Target Position: [347.32359333 -69.46471867]\n",
" [Pilot] Distance: 54.951716943628604\n",
" [Pilot] Distance: 54.95171694362866\n",
"-0.6 50.0\n",
"342.21488384208834 -69.12481364723727\n",
"\n",
" [Pilot] translate: -0.3671006590822984 49.56271477490151\n",
" [Pilot] Drone Position: (376.83, -104.60)\n",
" [Pilot] Angle: -45.3°\n",
" [Pilot] Target Index: 2\n",
" [Pilot] Target Position: [ 295.22505433 -347.32359333]\n",
" [Pilot] Distance: 282.1393363134888\n",
" [Pilot] Distance: 282.13933631348874\n",
"-0.6 50.0\n",
" [Pilot] translate: 152.48531037725778 -11.363336940584135\n",
"376.82813934296445 -104.60043911576437\n",
"\n",
" [Pilot] translate: 0.814116178363097 49.83161557706345\n",
" [Pilot] Drone Position: (386.60, -153.47)\n",
" [Pilot] Angle: -79.6°\n",
" [Pilot] Target Index: 2\n",
" [Pilot] Target Position: [ 295.22505433 -347.32359333]\n",
" [Pilot] Distance: 256.07341344997275\n",
" [Pilot] Distance: 256.0734134499727\n",
"-0.6 50.0\n",
"\n"
]
},
{
"ename": "IndexError",
"evalue": "list index out of range",
"output_type": "error",
"traceback": [
"\u001b[31m---------------------------------------------------------------------------\u001b[39m",
"\u001b[31mIndexError\u001b[39m Traceback (most recent call last)",
"\u001b[36mCell\u001b[39m\u001b[36m \u001b[39m\u001b[32mIn[3]\u001b[39m\u001b[32m, line 3\u001b[39m\n\u001b[32m 1\u001b[39m \u001b[38;5;28;01mfor\u001b[39;00m i \u001b[38;5;129;01min\u001b[39;00m \u001b[38;5;28mrange\u001b[39m(\u001b[32m1\u001b[39m, \u001b[32m20\u001b[39m):\n\u001b[32m 2\u001b[39m \u001b[38;5;28mprint\u001b[39m()\n\u001b[32m----> \u001b[39m\u001b[32m3\u001b[39m command = autopilot.handle(\u001b[43mimgs\u001b[49m\u001b[43m[\u001b[49m\u001b[43mi\u001b[49m\u001b[43m]\u001b[49m)\n\u001b[32m 4\u001b[39m \u001b[38;5;28mprint\u001b[39m(command.dangle, command.velocity)\n\u001b[32m 5\u001b[39m vzm.set_target_index(autopilot.target_idx)\n",
"\u001b[31mIndexError\u001b[39m: list index out of range"
" [Pilot] translate: 223.2717832052611 -50.70818702916637\n",
"386.60059278853873 -153.47120593833623\n"
]
}
],
"source": [
"for i in range(1, 20):\n",
"for i in range(1, 10):\n",
" print()\n",
" command = autopilot.handle(imgs[i])\n",
" print(command.dangle, command.velocity)\n",
" vzm.set_target_index(autopilot.target_idx)\n",
" vzm.update_drone_trajectory(autopilot.x, autopilot.y)\n",
" pos = autopilot.get_position_by_chunk()\n",
" if pos is not None:\n",
" print(pos)\n",
" vzm.update_global_map(pos[0], pos[1])\n",
" print(autopilot.x, autopilot.y)\n",
" vzm.update_global_map(0, 0)\n",
" vzm.pause(2.5)\n"
]
},
{
"cell_type": "code",
"execution_count": 96,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"[[[ 0.18926711 -21.79121458]]]\n",
"0.40152777578126847\n"
]
}
],
"source": [
"mat = np.array([[ 9.18211825e-01, -3.88263543e-01, 4.35846954e-01],\n",
" [ 3.89868385e-01, 9.19587423e-01, -2.49399645e+01],\n",
" [-3.56258907e-06, 3.01183248e-06, 1.00000000e+00]])\n",
"\n",
"pos = np.array([1., 3.]).reshape((1, 1, 2))\n",
"print(cv2.perspectiveTransform(pos, mat))\n",
"\n",
"(mat) @ np.array([1., 3., 1.]).reshape((3))\n",
"print(np.atan2(mat[1, 0], mat[0, 0]))"
]
},
{
"cell_type": "code",
"execution_count": 95,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"[[-0.70710678 2.12132034]\n",
" [ 0.70710678 3.53553391]\n",
" [-2.12132034 3.53553391]\n",
" [ 0. 0. ]]\n",
"[[ 7.07106671e-01 -7.07106696e-01 -9.13243653e-17]\n",
" [ 7.07106528e-01 7.07106788e-01 3.65297461e-16]\n",
" [-7.16496367e-08 2.10734232e-09 1.00000000e+00]]\n",
"0.7853980622449934\n"
]
}
],
"source": [
"ang = np.radians(45)\n",
"rot = np.array([\n",
" [np.cos(ang), -np.sin(ang)],\n",
" [np.sin(ang), np.cos(ang)]\n",
"])\n",
"\n",
"pts = np.array([\n",
" [1., 2.],\n",
" [3., 2.],\n",
" [1., 4.],\n",
" [0., 0.]\n",
"])\n",
"\n",
"dst = pts @ rot.T\n",
"\n",
"mat, mask = cv2.findHomography(pts, pts @ rot.T)\n",
"# print(np.atan2(mat[0, 1], mat[0, 0]))\n",
"# pts @ rot.T\n",
"pts @ mat[0:2]\n",
"# print(cv2.perspectiveTransform(pts[0].reshape((-1, 1, 2)), mat))\n",
"print(dst)\n",
"print(mat)\n",
"\n",
"\n",
"mat[0:2, 0:2] @ pts.T\n",
"\n",
"print(np.atan2(mat[1, 0], mat[0, 0]))\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
},
{
"cell_type": "code",
"execution_count": 98,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"(1, 2, 3)\n"
]
}
],
"source": [
"def a(*args):\n",
" print(args)\n",
"\n",
"a(*(1, 2, 3))"
]
}
],
"metadata": {