Skip to content

Commit

Permalink
add trajectory value show
Browse files Browse the repository at this point in the history
  • Loading branch information
@CHH3213
CHH3213 committed Aug 26, 2023
1 parent 8e26619 commit 50ef939
Showing 1 changed file with 31 additions and 12 deletions.
43 changes: 31 additions & 12 deletions PathPlanning/动态窗口法(DWA)/DynamicWindowApproaches.ipynb
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@
},
{
"cell_type": "code",
"execution_count": 43,
"execution_count": 223,
"metadata": {},
"outputs": [],
"source": [
Expand All @@ -32,7 +32,7 @@
},
{
"cell_type": "code",
"execution_count": 44,
"execution_count": 224,
"metadata": {},
"outputs": [],
"source": [
Expand Down Expand Up @@ -92,7 +92,7 @@
},
{
"cell_type": "code",
"execution_count": 45,
"execution_count": 225,
"metadata": {},
"outputs": [],
"source": [
Expand All @@ -118,7 +118,7 @@
},
{
"cell_type": "code",
"execution_count": 46,
"execution_count": 226,
"metadata": {},
"outputs": [],
"source": [
Expand Down Expand Up @@ -216,7 +216,7 @@
" \"\"\"\n",
" v_low=self.v_min\n",
" v_high = np.sqrt(2*self._dist(state,obstacle)*self.a_vmax)\n",
" w_low =self.w_min\n",
" w_low = self.w_min\n",
" w_high = np.sqrt(2*self._dist(state,obstacle)*self.a_wmax)\n",
" return [v_low,v_high,w_low,w_high]\n",
"\n",
Expand Down Expand Up @@ -369,7 +369,7 @@
},
{
"cell_type": "code",
"execution_count": 47,
"execution_count": 227,
"metadata": {},
"outputs": [],
"source": [
Expand Down Expand Up @@ -398,15 +398,29 @@
},
{
"cell_type": "code",
"execution_count": 48,
"execution_count": 228,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Goal!!\n",
"Done\n"
"ename": "KeyboardInterrupt",
"evalue": "",
"output_type": "error",
"traceback": [
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[1;31mKeyboardInterrupt\u001b[0m Traceback (most recent call last)",
"\u001b[1;32me:\\CHH3213_KING\\研究生\\导师\\就业规划\\自动驾驶\\chhRobotics\\PathPlanning\\动态窗口法(DWA)\\DynamicWindowApproaches.ipynb Cell 10\u001b[0m in \u001b[0;36m<cell line: 57>\u001b[1;34m()\u001b[0m\n\u001b[0;32m <a href='vscode-notebook-cell:/e%3A/CHH3213_KING/%E7%A0%94%E7%A9%B6%E7%94%9F/%E5%AF%BC%E5%B8%88/%E5%B0%B1%E4%B8%9A%E8%A7%84%E5%88%92/%E8%87%AA%E5%8A%A8%E9%A9%BE%E9%A9%B6/chhRobotics/PathPlanning/%E5%8A%A8%E6%80%81%E7%AA%97%E5%8F%A3%E6%B3%95%28DWA%29/DynamicWindowApproaches.ipynb#X36sZmlsZQ%3D%3D?line=50'>51</a>\u001b[0m \u001b[39m# camera.snap()\u001b[39;00m\n\u001b[0;32m <a href='vscode-notebook-cell:/e%3A/CHH3213_KING/%E7%A0%94%E7%A9%B6%E7%94%9F/%E5%AF%BC%E5%B8%88/%E5%B0%B1%E4%B8%9A%E8%A7%84%E5%88%92/%E8%87%AA%E5%8A%A8%E9%A9%BE%E9%A9%B6/chhRobotics/PathPlanning/%E5%8A%A8%E6%80%81%E7%AA%97%E5%8F%A3%E6%B3%95%28DWA%29/DynamicWindowApproaches.ipynb#X36sZmlsZQ%3D%3D?line=51'>52</a>\u001b[0m \u001b[39m# animation = camera.animate()\u001b[39;00m\n\u001b[0;32m <a href='vscode-notebook-cell:/e%3A/CHH3213_KING/%E7%A0%94%E7%A9%B6%E7%94%9F/%E5%AF%BC%E5%B8%88/%E5%B0%B1%E4%B8%9A%E8%A7%84%E5%88%92/%E8%87%AA%E5%8A%A8%E9%A9%BE%E9%A9%B6/chhRobotics/PathPlanning/%E5%8A%A8%E6%80%81%E7%AA%97%E5%8F%A3%E6%B3%95%28DWA%29/DynamicWindowApproaches.ipynb#X36sZmlsZQ%3D%3D?line=52'>53</a>\u001b[0m \u001b[39m# animation.save('trajectory.gif')\u001b[39;00m\n\u001b[0;32m <a href='vscode-notebook-cell:/e%3A/CHH3213_KING/%E7%A0%94%E7%A9%B6%E7%94%9F/%E5%AF%BC%E5%B8%88/%E5%B0%B1%E4%B8%9A%E8%A7%84%E5%88%92/%E8%87%AA%E5%8A%A8%E9%A9%BE%E9%A9%B6/chhRobotics/PathPlanning/%E5%8A%A8%E6%80%81%E7%AA%97%E5%8F%A3%E6%B3%95%28DWA%29/DynamicWindowApproaches.ipynb#X36sZmlsZQ%3D%3D?line=53'>54</a>\u001b[0m plt\u001b[39m.\u001b[39mshow()\n\u001b[1;32m---> <a href='vscode-notebook-cell:/e%3A/CHH3213_KING/%E7%A0%94%E7%A9%B6%E7%94%9F/%E5%AF%BC%E5%B8%88/%E5%B0%B1%E4%B8%9A%E8%A7%84%E5%88%92/%E8%87%AA%E5%8A%A8%E9%A9%BE%E9%A9%B6/chhRobotics/PathPlanning/%E5%8A%A8%E6%80%81%E7%AA%97%E5%8F%A3%E6%B3%95%28DWA%29/DynamicWindowApproaches.ipynb#X36sZmlsZQ%3D%3D?line=56'>57</a>\u001b[0m main(Config())\n",
"\u001b[1;32me:\\CHH3213_KING\\研究生\\导师\\就业规划\\自动驾驶\\chhRobotics\\PathPlanning\\动态窗口法(DWA)\\DynamicWindowApproaches.ipynb Cell 10\u001b[0m in \u001b[0;36mmain\u001b[1;34m(config)\u001b[0m\n\u001b[0;32m <a href='vscode-notebook-cell:/e%3A/CHH3213_KING/%E7%A0%94%E7%A9%B6%E7%94%9F/%E5%AF%BC%E5%B8%88/%E5%B0%B1%E4%B8%9A%E8%A7%84%E5%88%92/%E8%87%AA%E5%8A%A8%E9%A9%BE%E9%A9%B6/chhRobotics/PathPlanning/%E5%8A%A8%E6%80%81%E7%AA%97%E5%8F%A3%E6%B3%95%28DWA%29/DynamicWindowApproaches.ipynb#X36sZmlsZQ%3D%3D?line=16'>17</a>\u001b[0m x \u001b[39m=\u001b[39m KinematicModel(x, u, config\u001b[39m.\u001b[39mdt) \u001b[39m# simulate robot\u001b[39;00m\n\u001b[0;32m <a href='vscode-notebook-cell:/e%3A/CHH3213_KING/%E7%A0%94%E7%A9%B6%E7%94%9F/%E5%AF%BC%E5%B8%88/%E5%B0%B1%E4%B8%9A%E8%A7%84%E5%88%92/%E8%87%AA%E5%8A%A8%E9%A9%BE%E9%A9%B6/chhRobotics/PathPlanning/%E5%8A%A8%E6%80%81%E7%AA%97%E5%8F%A3%E6%B3%95%28DWA%29/DynamicWindowApproaches.ipynb#X36sZmlsZQ%3D%3D?line=17'>18</a>\u001b[0m trajectory \u001b[39m=\u001b[39m np\u001b[39m.\u001b[39mvstack((trajectory, x)) \u001b[39m# store state history\u001b[39;00m\n\u001b[1;32m---> <a href='vscode-notebook-cell:/e%3A/CHH3213_KING/%E7%A0%94%E7%A9%B6%E7%94%9F/%E5%AF%BC%E5%B8%88/%E5%B0%B1%E4%B8%9A%E8%A7%84%E5%88%92/%E8%87%AA%E5%8A%A8%E9%A9%BE%E9%A9%B6/chhRobotics/PathPlanning/%E5%8A%A8%E6%80%81%E7%AA%97%E5%8F%A3%E6%B3%95%28DWA%29/DynamicWindowApproaches.ipynb#X36sZmlsZQ%3D%3D?line=18'>19</a>\u001b[0m plt\u001b[39m.\u001b[39;49mcla()\n\u001b[0;32m <a href='vscode-notebook-cell:/e%3A/CHH3213_KING/%E7%A0%94%E7%A9%B6%E7%94%9F/%E5%AF%BC%E5%B8%88/%E5%B0%B1%E4%B8%9A%E8%A7%84%E5%88%92/%E8%87%AA%E5%8A%A8%E9%A9%BE%E9%A9%B6/chhRobotics/PathPlanning/%E5%8A%A8%E6%80%81%E7%AA%97%E5%8F%A3%E6%B3%95%28DWA%29/DynamicWindowApproaches.ipynb#X36sZmlsZQ%3D%3D?line=19'>20</a>\u001b[0m \u001b[39m# for stopping simulation with the esc key.\u001b[39;00m\n\u001b[0;32m <a href='vscode-notebook-cell:/e%3A/CHH3213_KING/%E7%A0%94%E7%A9%B6%E7%94%9F/%E5%AF%BC%E5%B8%88/%E5%B0%B1%E4%B8%9A%E8%A7%84%E5%88%92/%E8%87%AA%E5%8A%A8%E9%A9%BE%E9%A9%B6/chhRobotics/PathPlanning/%E5%8A%A8%E6%80%81%E7%AA%97%E5%8F%A3%E6%B3%95%28DWA%29/DynamicWindowApproaches.ipynb#X36sZmlsZQ%3D%3D?line=20'>21</a>\u001b[0m plt\u001b[39m.\u001b[39mgcf()\u001b[39m.\u001b[39mcanvas\u001b[39m.\u001b[39mmpl_connect(\n\u001b[0;32m <a href='vscode-notebook-cell:/e%3A/CHH3213_KING/%E7%A0%94%E7%A9%B6%E7%94%9F/%E5%AF%BC%E5%B8%88/%E5%B0%B1%E4%B8%9A%E8%A7%84%E5%88%92/%E8%87%AA%E5%8A%A8%E9%A9%BE%E9%A9%B6/chhRobotics/PathPlanning/%E5%8A%A8%E6%80%81%E7%AA%97%E5%8F%A3%E6%B3%95%28DWA%29/DynamicWindowApproaches.ipynb#X36sZmlsZQ%3D%3D?line=21'>22</a>\u001b[0m \u001b[39m'\u001b[39m\u001b[39mkey_release_event\u001b[39m\u001b[39m'\u001b[39m,\n\u001b[0;32m <a href='vscode-notebook-cell:/e%3A/CHH3213_KING/%E7%A0%94%E7%A9%B6%E7%94%9F/%E5%AF%BC%E5%B8%88/%E5%B0%B1%E4%B8%9A%E8%A7%84%E5%88%92/%E8%87%AA%E5%8A%A8%E9%A9%BE%E9%A9%B6/chhRobotics/PathPlanning/%E5%8A%A8%E6%80%81%E7%AA%97%E5%8F%A3%E6%B3%95%28DWA%29/DynamicWindowApproaches.ipynb#X36sZmlsZQ%3D%3D?line=22'>23</a>\u001b[0m \u001b[39mlambda\u001b[39;00m event: [exit(\u001b[39m0\u001b[39m) \u001b[39mif\u001b[39;00m event\u001b[39m.\u001b[39mkey \u001b[39m==\u001b[39m \u001b[39m'\u001b[39m\u001b[39mescape\u001b[39m\u001b[39m'\u001b[39m \u001b[39melse\u001b[39;00m \u001b[39mNone\u001b[39;00m])\n",
"File \u001b[1;32md:\\ProgramData\\Anaconda3\\envs\\gobigger\\lib\\site-packages\\matplotlib\\pyplot.py:1113\u001b[0m, in \u001b[0;36mcla\u001b[1;34m()\u001b[0m\n\u001b[0;32m 1111\u001b[0m \u001b[39m\"\"\"Clear the current axes.\"\"\"\u001b[39;00m\n\u001b[0;32m 1112\u001b[0m \u001b[39m# Not generated via boilerplate.py to allow a different docstring.\u001b[39;00m\n\u001b[1;32m-> 1113\u001b[0m \u001b[39mreturn\u001b[39;00m gca()\u001b[39m.\u001b[39;49mcla()\n",
"File \u001b[1;32md:\\ProgramData\\Anaconda3\\envs\\gobigger\\lib\\site-packages\\matplotlib\\axes\\_base.py:1301\u001b[0m, in \u001b[0;36m_AxesBase.cla\u001b[1;34m(self)\u001b[0m\n\u001b[0;32m 1297\u001b[0m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39mpatch\u001b[39m.\u001b[39mset_transform(\u001b[39mself\u001b[39m\u001b[39m.\u001b[39mtransAxes)\n\u001b[0;32m 1299\u001b[0m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39mset_axis_on()\n\u001b[1;32m-> 1301\u001b[0m \u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49mxaxis\u001b[39m.\u001b[39;49mset_clip_path(\u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49mpatch)\n\u001b[0;32m 1302\u001b[0m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39myaxis\u001b[39m.\u001b[39mset_clip_path(\u001b[39mself\u001b[39m\u001b[39m.\u001b[39mpatch)\n\u001b[0;32m 1304\u001b[0m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39m_shared_axes[\u001b[39m\"\u001b[39m\u001b[39mx\u001b[39m\u001b[39m\"\u001b[39m]\u001b[39m.\u001b[39mclean()\n",
"File \u001b[1;32md:\\ProgramData\\Anaconda3\\envs\\gobigger\\lib\\site-packages\\matplotlib\\axis.py:935\u001b[0m, in \u001b[0;36mAxis.set_clip_path\u001b[1;34m(self, clippath, transform)\u001b[0m\n\u001b[0;32m 934\u001b[0m \u001b[39mdef\u001b[39;00m \u001b[39mset_clip_path\u001b[39m(\u001b[39mself\u001b[39m, clippath, transform\u001b[39m=\u001b[39m\u001b[39mNone\u001b[39;00m):\n\u001b[1;32m--> 935\u001b[0m \u001b[39msuper\u001b[39;49m()\u001b[39m.\u001b[39;49mset_clip_path(clippath, transform)\n\u001b[0;32m 936\u001b[0m \u001b[39mfor\u001b[39;00m child \u001b[39min\u001b[39;00m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39mmajorTicks \u001b[39m+\u001b[39m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39mminorTicks:\n\u001b[0;32m 937\u001b[0m child\u001b[39m.\u001b[39mset_clip_path(clippath, transform)\n",
"File \u001b[1;32md:\\ProgramData\\Anaconda3\\envs\\gobigger\\lib\\site-packages\\matplotlib\\artist.py:790\u001b[0m, in \u001b[0;36mArtist.set_clip_path\u001b[1;34m(self, path, transform)\u001b[0m\n\u001b[0;32m 787\u001b[0m \u001b[39mif\u001b[39;00m transform \u001b[39mis\u001b[39;00m \u001b[39mNone\u001b[39;00m:\n\u001b[0;32m 788\u001b[0m \u001b[39mif\u001b[39;00m \u001b[39misinstance\u001b[39m(path, Rectangle):\n\u001b[0;32m 789\u001b[0m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39mclipbox \u001b[39m=\u001b[39m TransformedBbox(Bbox\u001b[39m.\u001b[39munit(),\n\u001b[1;32m--> 790\u001b[0m path\u001b[39m.\u001b[39;49mget_transform())\n\u001b[0;32m 791\u001b[0m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39m_clippath \u001b[39m=\u001b[39m \u001b[39mNone\u001b[39;00m\n\u001b[0;32m 792\u001b[0m success \u001b[39m=\u001b[39m \u001b[39mTrue\u001b[39;00m\n",
"File \u001b[1;32md:\\ProgramData\\Anaconda3\\envs\\gobigger\\lib\\site-packages\\matplotlib\\patches.py:278\u001b[0m, in \u001b[0;36mPatch.get_transform\u001b[1;34m(self)\u001b[0m\n\u001b[0;32m 276\u001b[0m \u001b[39mdef\u001b[39;00m \u001b[39mget_transform\u001b[39m(\u001b[39mself\u001b[39m):\n\u001b[0;32m 277\u001b[0m \u001b[39m\"\"\"Return the `~.transforms.Transform` applied to the `Patch`.\"\"\"\u001b[39;00m\n\u001b[1;32m--> 278\u001b[0m \u001b[39mreturn\u001b[39;00m \u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49mget_patch_transform() \u001b[39m+\u001b[39m artist\u001b[39m.\u001b[39mArtist\u001b[39m.\u001b[39mget_transform(\u001b[39mself\u001b[39m)\n",
"File \u001b[1;32md:\\ProgramData\\Anaconda3\\envs\\gobigger\\lib\\site-packages\\matplotlib\\patches.py:760\u001b[0m, in \u001b[0;36mRectangle.get_patch_transform\u001b[1;34m(self)\u001b[0m\n\u001b[0;32m 753\u001b[0m \u001b[39mdef\u001b[39;00m \u001b[39mget_patch_transform\u001b[39m(\u001b[39mself\u001b[39m):\n\u001b[0;32m 754\u001b[0m \u001b[39m# Note: This cannot be called until after this has been added to\u001b[39;00m\n\u001b[0;32m 755\u001b[0m \u001b[39m# an Axes, otherwise unit conversion will fail. This makes it very\u001b[39;00m\n\u001b[0;32m 756\u001b[0m \u001b[39m# important to call the accessor method and not directly access the\u001b[39;00m\n\u001b[0;32m 757\u001b[0m \u001b[39m# transformation member variable.\u001b[39;00m\n\u001b[0;32m 758\u001b[0m bbox \u001b[39m=\u001b[39m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39mget_bbox()\n\u001b[0;32m 759\u001b[0m \u001b[39mreturn\u001b[39;00m (transforms\u001b[39m.\u001b[39mBboxTransformTo(bbox)\n\u001b[1;32m--> 760\u001b[0m \u001b[39m+\u001b[39m transforms\u001b[39m.\u001b[39;49mAffine2D()\u001b[39m.\u001b[39;49mrotate_deg_around(\n\u001b[0;32m 761\u001b[0m bbox\u001b[39m.\u001b[39;49mx0, bbox\u001b[39m.\u001b[39;49my0, \u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49mangle))\n",
"File \u001b[1;32md:\\ProgramData\\Anaconda3\\envs\\gobigger\\lib\\site-packages\\matplotlib\\transforms.py:2042\u001b[0m, in \u001b[0;36mAffine2D.rotate_deg_around\u001b[1;34m(self, x, y, degrees)\u001b[0m\n\u001b[0;32m 2040\u001b[0m \u001b[39m# Cast to float to avoid wraparound issues with uint8's\u001b[39;00m\n\u001b[0;32m 2041\u001b[0m x, y \u001b[39m=\u001b[39m \u001b[39mfloat\u001b[39m(x), \u001b[39mfloat\u001b[39m(y)\n\u001b[1;32m-> 2042\u001b[0m \u001b[39mreturn\u001b[39;00m \u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49mtranslate(\u001b[39m-\u001b[39;49mx, \u001b[39m-\u001b[39;49my)\u001b[39m.\u001b[39;49mrotate_deg(degrees)\u001b[39m.\u001b[39mtranslate(x, y)\n",
"File \u001b[1;32md:\\ProgramData\\Anaconda3\\envs\\gobigger\\lib\\site-packages\\matplotlib\\transforms.py:2020\u001b[0m, in \u001b[0;36mAffine2D.rotate_deg\u001b[1;34m(self, degrees)\u001b[0m\n\u001b[0;32m 2012\u001b[0m \u001b[39mdef\u001b[39;00m \u001b[39mrotate_deg\u001b[39m(\u001b[39mself\u001b[39m, degrees):\n\u001b[0;32m 2013\u001b[0m \u001b[39m\"\"\"\u001b[39;00m\n\u001b[0;32m 2014\u001b[0m \u001b[39m Add a rotation (in degrees) to this transform in place.\u001b[39;00m\n\u001b[0;32m 2015\u001b[0m \n\u001b[1;32m (...)\u001b[0m\n\u001b[0;32m 2018\u001b[0m \u001b[39m and :meth:`scale`.\u001b[39;00m\n\u001b[0;32m 2019\u001b[0m \u001b[39m \"\"\"\u001b[39;00m\n\u001b[1;32m-> 2020\u001b[0m \u001b[39mreturn\u001b[39;00m \u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49mrotate(math\u001b[39m.\u001b[39;49mradians(degrees))\n",
"File \u001b[1;32md:\\ProgramData\\Anaconda3\\envs\\gobigger\\lib\\site-packages\\matplotlib\\transforms.py:2008\u001b[0m, in \u001b[0;36mAffine2D.rotate\u001b[1;34m(self, theta)\u001b[0m\n\u001b[0;32m 2005\u001b[0m b \u001b[39m=\u001b[39m math\u001b[39m.\u001b[39msin(theta)\n\u001b[0;32m 2006\u001b[0m rotate_mtx \u001b[39m=\u001b[39m np\u001b[39m.\u001b[39marray([[a, \u001b[39m-\u001b[39mb, \u001b[39m0.0\u001b[39m], [b, a, \u001b[39m0.0\u001b[39m], [\u001b[39m0.0\u001b[39m, \u001b[39m0.0\u001b[39m, \u001b[39m1.0\u001b[39m]],\n\u001b[0;32m 2007\u001b[0m \u001b[39mfloat\u001b[39m)\n\u001b[1;32m-> 2008\u001b[0m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39m_mtx \u001b[39m=\u001b[39m np\u001b[39m.\u001b[39;49mdot(rotate_mtx, \u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49m_mtx)\n\u001b[0;32m 2009\u001b[0m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39minvalidate()\n\u001b[0;32m 2010\u001b[0m \u001b[39mreturn\u001b[39;00m \u001b[39mself\u001b[39m\n",
"File \u001b[1;32m<__array_function__ internals>:180\u001b[0m, in \u001b[0;36mdot\u001b[1;34m(*args, **kwargs)\u001b[0m\n",
"\u001b[1;31mKeyboardInterrupt\u001b[0m: "
]
}
],
Expand Down Expand Up @@ -440,6 +454,11 @@
" plt.plot(ob[:, 0], ob[:, 1], \"ok\")\n",
" plot_robot(x[0], x[1], x[2], config)\n",
" plot_arrow(x[0], x[1], x[2])\n",
" \n",
" formatted_state = [f\"{value:.3f}\" for value in x]\n",
" annotation_text = f\"[x, y, yaw, v, w]:\\n{formatted_state}\"\n",
" plt.annotate(annotation_text, (x[0], x[1]), textcoords=\"offset points\", xytext=(0, 10), ha='center')\n",
" \n",
" plt.axis(\"equal\")\n",
" plt.grid(True)\n",
" plt.pause(0.001)\n",
Expand Down

0 comments on commit 50ef939

Please sign in to comment.