P4_trajectory_optimization.ipynb

{"nbformat":4,"nbformat_minor":0,"metadata":{"colab":{"provenance":[]},"kernelspec":{"name":"python3","display_name":"Python 3"},"language_info":{"name":"python"}},"cells":[{"cell_type":"markdown","source":["# HW1 AA274A Problem 4\n","\n","In this problem, you will write a trajectory optimization algorithm to generate a feasible trajectory subject to some constraints. Our setup includes the \"ego\" vehicle (meaning the agent we control), a static obstacle, and a goal position to be reached.\n","\n","In this setup, we will not make the assumption of differential flatness and instead solve the problem with a nonlinear optimizer!\n"],"metadata":{"id":"WshFeYmnY8sT"}},{"cell_type":"markdown","source":["# Problem Setup\n","We will first consider the case in which we are trying to generate a trajectory for a Turtlebot that must avoid an object. The agent has an initial position, a goal position, and a big obstacle in the center of the initial path. Our agent therefore must generate a trajectory that is dynamically feasible, does not collide with the obstacle, and reaches the final goal pose."],"metadata":{"id":"Z09yDOn0fSvM"}},{"cell_type":"code","source":["import numpy as np\n","import matplotlib.pyplot as plt\n","from scipy.optimize import minimize, Bounds\n","import typing as T"],"metadata":{"id":"8dsWwGaDcwdY"},"execution_count":null,"outputs":[]},{"cell_type":"code","source":["from utils import simulate_car_dyn, maybe_makedirs\n","from P2_trajectory_tracking import TrajectoryTracker"],"metadata":{"id":"jfWNEbKMhCio"},"execution_count":null,"outputs":[]},{"cell_type":"code","source":["# Initial conditions and parameters\n","EGO_START_POS, EGO_FINAL_GOAL_POS = (0.0,0.0), (5.0,5.0)\n","EGO_RADIUS, OBS_RADIUS = 0.1, 0.3\n","OBSTACLE_POS = (2.5, 2.5)"],"metadata":{"id":"aXBY0tKZY-QT"},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":["# Visualizing the Scene"],"metadata":{"id":"Lb1lyHx8fwKB"}},{"cell_type":"code","source":["def render_scene(traj=None, print_alpha=None):\n","  fig, ax = plt.subplots()\n","  ego_circle_start = plt.Circle(EGO_START_POS, radius=EGO_RADIUS, color='lime')\n","  ego_circle_end   = plt.Circle(EGO_FINAL_GOAL_POS, radius=EGO_RADIUS, color='red')\n","  obs_circle       = plt.Circle(OBSTACLE_POS, radius=OBS_RADIUS, color='blue')\n","  ax.add_patch(obs_circle)\n","  if traj is not None:\n","    for i in range(traj.shape[0]):\n","      x,y,theta = traj[i]\n","      ego_circle_current = plt.Circle((x, y), radius=EGO_RADIUS, color='cyan')\n","      ax.add_patch(ego_circle_current)\n","      ego_arrow_current = plt.arrow(x, y, dx=np.cos(theta)/2, dy=np.sin(theta)/2, head_width=0.1)\n","      ax.add_patch(ego_arrow_current)\n","  ax.add_patch(ego_circle_start)\n","  ax.add_patch(ego_circle_end)\n","  ax.set_xlim((-1.0, 6.0))\n","  ax.set_ylim((-1.0, 6.0))\n","  ax.set_aspect('equal')\n","  if print_alpha is not None:\n","    plt.title(\"Alpha: {:.2f}\".format(print_alpha))\n","  return plt"],"metadata":{"id":"3qsiPYMzchxf"},"execution_count":null,"outputs":[]},{"cell_type":"code","source":["# VIZUALIZE THE INITIAL SCENE\n","# RED circle is the obstacle\n","# GREEN is ego start position\n","# BLUE is obstacle position\n","plt = render_scene()\n","plt.show()"],"metadata":{"colab":{"base_uri":"https://localhost:8080/","height":435},"id":"sa-nhG64dMlw","outputId":"0c6b3ccf-bc22-4300-cd3a-121b6664a067","executionInfo":{"status":"ok","timestamp":1696018252736,"user_tz":420,"elapsed":274,"user":{"displayName":"Keiko Nagami","userId":"10093190229210908038"}}},"execution_count":null,"outputs":[{"output_type":"display_data","data":{"text/plain":["<Figure size 640x480 with 1 Axes>"],"image/png":"iVBORw0KGgoAAAANSUhEUgAAAagAAAGiCAYAAACyKVKmAAAAOXRFWHRTb2Z0d2FyZQBNYXRwbG90bGliIHZlcnNpb24zLjcuMSwgaHR0cHM6Ly9tYXRwbG90bGliLm9yZy/bCgiHAAAACXBIWXMAAA9hAAAPYQGoP6dpAAAb20lEQVR4nO3df3DU9Z3H8dcmMQtqsvww/IhsIlQxQgwqYIqxLUIqk3EQnDnlaKgxMp2DBhSpJ2Rupui0ZXEcO3YUkR8OeHcTwcoEWx0EhoFQKpz8GFqwHoi1k4j8UJHdJDe3yOZ7f+wRmxKEZPPd73uX56PznXGXb76f99YxT7673931OY7jCAAAYzK8HgAAgM4QKACASQQKAGASgQIAmESgAAAmESgAgEkECgBgEoECAJhEoAAAJhEoAIBJrgfq2LFjmjFjhvr376/evXvr1ltv1d69e91eFgCQ4rLcPPhXX32lsrIy3XPPPdq4caPy8vL00UcfqW/fvm4uCwBIAz43Pyx24cKF+uMf/6g//OEPbi0BAEhTrgZqxIgRmjRpkj799FM1NDTo+uuv109/+lP95Cc/6XT/aDSqaDTafrutrU2nT59W//795fP53BoTAOASx3HU3Nys/Px8ZWR08VUlx0V+v9/x+/1ObW2ts3//fmf58uVOr169nDVr1nS6/6JFixxJbGxsbGxptjU1NXW5Ia6eQWVnZ2vMmDF677332u977LHHtGfPHu3ateuC/f/xDCocDqugoEBNTU3Kzc11a0wAgEsikYiCwaDOnDmjQCDQpZ919SKJwYMHa8SIER3uu+WWW7R+/fpO9/f7/fL7/Rfcn5ubS6AAIIV152UaVy8zLysr0+HDhzvcd+TIERUWFrq5LAAgDbgaqCeeeEK7d+/W4sWLdfToUdXV1WnFihWqqalxc1kAQBpwNVBjx45VfX29Xn/9dRUXF+sXv/iFXnjhBVVWVrq5LAAgDbh6kUSiIpGIAoGAwuEwr0EBQApK5Pc4n8UHADCJQAEATCJQAACTCBQAwCQCBQAwiUABAEwiUAAAkwgUAMAkAgUAMIlAAQBMIlAAAJMIFADAJAIFADCJQAEATCJQAACTCBQAwCQCBQAwiUABAEwiUAAAkwgUAMAkAgUAMIlAAQBMIlAAAJMIFADAJAIFADCJQAEATCJQAACTCBQAwCQCBQAwiUABAEwiUAAAkwgUAMAkAgUAMIlAAQBMIlAAAJMIFADAJAIFADCJQAEATCJQAACTsrweAACueLGYtHmz9Kc/xW8XF0sVFVJmprdzeYxAAYCX6uqkhQulpqZ4kHw+6dw5KT9f+uUvpepqryf0DE/xAYBXli6VKivjcZLiZ1LnzsX/+bPPpEcflZ57zrv5POZqoJ5++mn5fL4OW1FRkZtLAkBqOHJEmjv30vs99dQ3T/1dYVw/gxo5cqSOHz/evu3cudPtJQHAvldekTIu41dwVpb08svuz2OQ669BZWVladCgQZe1bzQaVTQabb8diUTcGgsAvLV2bfwpvUs5d05at05avtz9mYxx/Qzqo48+Un5+voYNG6bKyko1NjZedN9QKKRAINC+BYNBt8cDAG905S/gzc3uzWGYz3Ecx62Db9y4US0tLbr55pt1/PhxPfPMMzp27JgOHTqknJycC/bv7AwqGAwqHA4rNzfXrTEBIPmGDJGOHbu8ffv2lU6fdncel0QiEQUCgW79Hnf1Kb6Kior2fy4pKVFpaakKCwv1xhtvaObMmRfs7/f75ff73RwJAGyorJSef/7ST/NlZUk/+lFyZjImqZeZ9+nTR8OHD9fRo0eTuSwA2DNrlnQ5T2DFYtLs2e7PY1BSA9XS0qKPP/5YgwcPTuayAGDP0KHSqlXxN+b6fBf++fn7XnxRGjkyubMZ4WqgnnzySTU0NOhvf/ub3nvvPT3wwAPKzMzU9OnT3VwWAFJDdbVUXy8NHx6//fexGjYsfvVeTY1383nM1degPv30U02fPl1ffvml8vLydPfdd2v37t3Ky8tzc1kASB1Tpkj33y/t3Cn9+c/xp/1GjpTGj+/8zOoK4upVfIlK5OoPAID3Evk9zmfxAQBMIlAAAJMIFADAJAIFADCJQAEATCJQAACTCBQAwCQCBQAwiUABAEwiUAAAkwgUAMAkAgUAMIlAAQBMIlAAAJMIFADAJAIFADCJQAEATCJQAACTCBQAwCQCBQAwiUABAEwiUAAAkwgUAMAkAgUAMIlAAQBMIlAAAJMIFADAJAIFADCJQAEATCJQAACTCBQAwCQCBQAwiUABAEwiUAAAkwgUAMAkAgUAMIlAAQBMIlAAAJMIFADAJAIFADCJQAEATCJQAACTkhaoJUuWyOfzad68eclaEgCQwpISqD179mj58uUqKSlJxnIAgDTgeqBaWlpUWVmplStXqm/fvt+6bzQaVSQS6bABAK5MrgeqpqZG9913n8rLyy+5bygUUiAQaN+CwaDb4wEAjHI1UGvXrtX+/fsVCoUua//a2lqFw+H2rampyc3xAACGZbl14KamJj3++OPasmWLevXqdVk/4/f75ff73RoJAJBCfI7jOG4ceMOGDXrggQeUmZnZfl8sFpPP51NGRoai0WiHP+tMJBJRIBBQOBxWbm6uG2MCAFyUyO9x186gJk6cqIMHD3a4r7q6WkVFRVqwYMEl4wQAuLK5FqicnBwVFxd3uO+aa65R//79L7gfAIB/xCdJAABMcu0MqjPbt29P5nIAgBTGGRQAwCQCBQAwiUABAEwiUAAAkwgUAMAkAgUAMIlAAQBMIlAAAJMIFADAJAIFADCJQAEATCJQAACTCBQAwCQCBQAwiUABAEwiUAAAkwgUAMAkAgUAMIlAAQBMIlAAAJMIFADAJAIFADCJQAEATCJQAACTCBQAwCQCBQAwiUABAEwiUAAAkwgUAMAkAgUAMIlAAQBMIlAAAJMIFADAJAIFADCJQAEATCJQAACTCBQAwCQCBQAwiUABAEwiUAAAkwgUAMAkVwO1bNkylZSUKDc3V7m5uRo3bpw2btzo5pIAgDThaqCGDBmiJUuWaN++fdq7d68mTJigKVOm6IMPPnBzWQBAGvA5juMkc8F+/frpueee08yZMy+5byQSUSAQUDgcVm5ubhKmAwD0pER+j2e5NNMFYrGYfvvb36q1tVXjxo3rdJ9oNKpoNNp+OxKJJGs8AIAxrl8kcfDgQV177bXy+/2aNWuW6uvrNWLEiE73DYVCCgQC7VswGHR7PACAUa4/xXf27Fk1NjYqHA7rzTff1KpVq9TQ0NBppDo7gwoGgzzFBwApKpGn+JL+GlR5ebm+853vaPny5Zfcl9egACC1JfJ7POnvg2pra+twlgQAQGdcvUiitrZWFRUVKigoUHNzs+rq6rR9+3Zt2rTJzWUBAGnA1UCdOnVKDz/8sI4fP65AIKCSkhJt2rRJP/zhD91cFgCQBlwN1Kuvvurm4QEAaYzP4gMAmESgAAAmJe2TJAB0FItJ//u/kuNIvXtLmZleTwTYQqCAJDl+XHrzTWnPHmn3bunjj6W2tvif+XzSsGHSd78rjRkj/dM/SUOGeDsv4LWkv1G3K3ijLtJBQ4P00ktSfX38bCkjQzp3rvN9MzPj+0jS5MnS3LnShAnxgAGpKKXeqAtcKb74Qpo2TRo/XtqwIf6UXlvbxeMkfbNPW5v0zjtSebk0dap04kSShgYMIVCAC95+Wxo+XFq/Pn7726J0Med/5p13pKKib44FXCkIFNDDVq+W7r9fOnMmfkaUqFhMikTir0stXZr48YBUQaCAHvT669LMmfHXkXry1d3zx5ozR1q1queOC1hGoIAecuiQ9PDDPRumzvzLv0j79rm7BmABgQJ6wLlz8Tglg88nzZgh8aUASHcECugBv/61dOBA9y6G6KpYTDp8WAqF3F8L8BKBAhJ09qz07LPuP7X39xwnHsX/+Z/krQkkG4ECErR+vXT6dPLXbW6OX5QBpCsCBSTo5Ze9+Ry9jAwuO0d6I1BAAs6ejX+uXk+836mr2trir3u1tCR/bSAZCBSQgEOHknNhxMU4TjxSQDoiUEAC9u/3dv2MDGnvXm9nANxCoIAEnDwpXXWVd+tnZkqnTnm3PuAmAgUk4OxZryfgDbtIXwQKSEB2dnLf/9QZv9/b9QG3ECggAYMHe3uRxLlz0qBB3q0PuIlAAQm44w5v13ec+FfEA+mIQAEJGDnS24skMjKk227zbn3ATQQKSMBVV0llZd59ksTo0dLVVyd/bSAZCBSQoJoa7z5JYs6c5K8LJAuBAhI0ZYqUl5f8dQMB6aGHkr8ukCwECkjQVVdJ//ZvyV3T55Oeekrq1Su56wLJRKCAHjBnjvTd70pZWe6vlZkp3Xqr9K//6v5agJcIFNADMjOl116LX7jg87m7ls8n/cd/eHv1IJAMBAroIcOHS2vXxgPiVqR8Punf/10qKXHn+IAlBAroQQ88EP+W24yM+NZTzp+ZrV4tTZ/ec8cFLCNQQA976CFp82ZpwICeeX9UZqbUr5/0zjtSVVXixwNSBYECXDBhgvTf//1NULpz8cT5n/nnf5YOH5YqKnpuPiAVECjAJYGA9Oqr0p490owZ8U8+l7794obzf5aVFQ/Trl3Sf/5n/AwKuNL4HMfrLwu4uEgkokAgoHA4rNzcXK/HARJy+rT01lvxb8D9r/+SPvgg/l1OjhN/P1NRUfxS9dGjvXvzL9DTEvk9TqAAD53/r8/tS9MBryTyezwJbysEcDGECbg4XoMCAJhEoAAAJhEoAIBJBAoAYJKrgQqFQho7dqxycnI0YMAATZ06VYcPH3ZzSQBAmnA1UA0NDaqpqdHu3bu1ZcsWff3117r33nvV2trq5rIAgDSQ1PdBff755xowYIAaGhr0/e9//5L78z4oAEhtKfM+qHA4LEnqd5HPbYlGo4pGo+23I5FIUuYCANiTtIsk2traNG/ePJWVlam4uLjTfUKhkAKBQPsWDAaTNR4AwJikPcU3e/Zsbdy4UTt37tSQIUM63aezM6hgMMhTfACQosw/xTdnzhy9/fbb2rFjx0XjJEl+v19+vz8ZIwEAjHM1UI7jaO7cuaqvr9f27ds1dOhQN5cDAKQRVwNVU1Ojuro6vfXWW8rJydGJEyckSYFAQL1793ZzaQBAinP1NSjfRT6qefXq1XrkkUcu+fNcZg4Aqc3sa1CGv2oKAGAcn8UHADCJQAEATCJQAACTCBQAwCQCBQAwiUABAEwiUAAAkwgUAMAkAgUAMIlAAQBMIlAAAJMIFADAJAIFADCJQAEATCJQAACTCBQAwCQCBQAwiUABAEwiUAAAkwgUAMAkAgUAMIlAAQBMIlAAAJMIFADAJAIFADCJQAEATCJQAACTCBQAwCQCBQAwiUABAEwiUAAAkwgUAMAkAgUAMIlAAQBMIlAAAJMIFADAJAIFADCJQAEATCJQAACTCBQAwCQCBQAwiUABAExyNVA7duzQ5MmTlZ+fL5/Ppw0bNri5HAAgjbgaqNbWVo0aNUpLly51cxkAQBrKcvPgFRUVqqiocHMJAECacjVQXRWNRhWNRttvRyIRD6cBAHjJ1EUSoVBIgUCgfQsGg16PBADwiKlA1dbWKhwOt29NTU1ejwQA8Iipp/j8fr/8fr/XYwAADDB1BgUAwHmunkG1tLTo6NGj7bc/+eQTHThwQP369VNBQYGbSwMAUpyrgdq7d6/uueee9tvz58+XJFVVVWnNmjVuLg0ASHGuBmr8+PFyHMfNJQAAaYrXoAAAJhEoAIBJBAoAYBKBAgCYRKAAACYRKACASQQKAGASgQIAmESgAAAmESgAgEkECgBgEoECAJhEoAAAJhEoAIBJBAoAYBKBAgCYRKAAACYRKACASQQKAGASgQIAmESgAAAmESgAgEkECgBgEoECAJhEoAAAJhEoAIBJBAoAYBKBAgCYRKAAACYRKACASQQKAGASgQIAmESgAAAmESgAgEkECgBgEoECAJhEoAAAJhEoAIBJBAoAYFKW1wNcKWKKabM260/6kySpWMWqUIUylenxZABgE4FKgjrVaaEWqklNylSmfPLpnM4pX/n6pX6palV7PSIAmMNTfC5bqqWqVKWa1CQpfiZ1TuckSZ/pMz2qR/WcnvNyRAAwKSmBWrp0qW644Qb16tVLpaWlev/995OxrOeO6Ijmau4l93tKT7U/9QcAiHM9UOvWrdP8+fO1aNEi7d+/X6NGjdKkSZN06tQpt5f23Ct6RRmX8X9xlrL0sl5OwkQAkDp8juM4bi5QWlqqsWPH6qWXXpIktbW1KRgMau7cuVq4cGGHfaPRqKLRaPvtSCSiYDCocDis3NxcN8d0Rb7ydVzHL2vfgAI6ozPuDgQASRaJRBQIBLr1e9zVM6izZ89q3759Ki8v/2bBjAyVl5dr165dF+wfCoUUCATat2Aw6OZ4rosoctn7NqvZxUkAIPW4GqgvvvhCsVhMAwcO7HD/wIEDdeLEiQv2r62tVTgcbt+amprcHM91fdTnsvcNKODeIACQgkxdxef3+5Wbm9thS2WVqrys9zllKUs/0o+SMBEApA5XA3XdddcpMzNTJ0+e7HD/yZMnNWjQIDeXNmGWZsnRpV/iiymm2ZqdhIkAIHW4Gqjs7GyNHj1aW7dubb+vra1NW7du1bhx49xc2oShGqpVWiXf///vH52/70W9qJEamezxAMA015/imz9/vlauXKnXXntNH374oWbPnq3W1lZVV18Zn55QrWrVq17DNVySOsRqmIZpndapRjVejggAJrn+UUfTpk3T559/rp///Oc6ceKEbrvtNr377rsXXDiRzqZoiu7X/dqpnfqz/ixHjkZqpMZrfKdnVgCAJLwPKhGJXD8PAPCe2fdBAQDQXQQKAGASgQIAmESgAAAmESgAgEkECgBgEoECAJhEoAAAJhEoAIBJBAoAYBKBAgCYRKAAACYRKACASQQKAGASgQIAmESgAAAmESgAgEkECgBgEoECAJhEoAAAJhEoAIBJBAoAYBKBAgCYRKAAACYRKACASQQKAGASgQIAmESgAAAmESgAgEkECgBgEoECAJhEoAAAJhEoAIBJBAoAYBKBAgCYRKAAACYRKACASQQKAGASgQIAmESgAAAmESgAgEkECgBgkmuB+tWvfqW77rpLV199tfr06ePWMgCANOVaoM6ePasHH3xQs2fPdmsJAEAay3LrwM8884wkac2aNZf9M9FoVNFotP12OByWJEUikR6dDQCQHOd/fzuO0+WfdS1Q3REKhdrD9veCwaAH0wAAesqXX36pQCDQpZ8xFaja2lrNnz+//faZM2dUWFioxsbGLj8wqyKRiILBoJqampSbm+v1OD2Cx2Rfuj0eiceUKsLhsAoKCtSvX78u/2yXArVw4UI9++yz37rPhx9+qKKioi4PIkl+v19+v/+C+wOBQNr8yzovNzeXx5QC0u0xpdvjkXhMqSIjo+uXPHQpUD/72c/0yCOPfOs+w4YN6/IQAAD8oy4FKi8vT3l5eW7NAgBAO9deg2psbNTp06fV2NioWCymAwcOSJJuvPFGXXvttZd1DL/fr0WLFnX6tF+q4jGlhnR7TOn2eCQeU6pI5DH5nO5c+3cZHnnkEb322msX3L9t2zaNHz/ejSUBAGnEtUABAJAIPosPAGASgQIAmESgAAAmESgAgEkpE6h0+fqOpUuX6oYbblCvXr1UWlqq999/3+uRum3Hjh2aPHmy8vPz5fP5tGHDBq9HSkgoFNLYsWOVk5OjAQMGaOrUqTp8+LDXYyVk2bJlKikpaf9kgnHjxmnjxo1ej9WjlixZIp/Pp3nz5nk9Src9/fTT8vl8HbbufiKPFceOHdOMGTPUv39/9e7dW7feeqv27t3bpWOkTKDS4es71q1bp/nz52vRokXav3+/Ro0apUmTJunUqVNej9Ytra2tGjVqlJYuXer1KD2ioaFBNTU12r17t7Zs2aKvv/5a9957r1pbW70erduGDBmiJUuWaN++fdq7d68mTJigKVOm6IMPPvB6tB6xZ88eLV++XCUlJV6PkrCRI0fq+PHj7dvOnTu9HqnbvvrqK5WVlemqq67Sxo0b9Ze//EXPP/+8+vbt27UDOSlm9erVTiAQ8HqMbrnzzjudmpqa9tuxWMzJz893QqGQh1P1DElOfX2912P0qFOnTjmSnIaGBq9H6VF9+/Z1Vq1a5fUYCWtubnZuuukmZ8uWLc4PfvAD5/HHH/d6pG5btGiRM2rUKK/H6DELFixw7r777oSPkzJnUKnu7Nmz2rdvn8rLy9vvy8jIUHl5uXbt2uXhZLiY899H1p1PYbYoFotp7dq1am1t1bhx47weJ2E1NTW67777Ovw3lco++ugj5efna9iwYaqsrFRjY6PXI3Xb7373O40ZM0YPPvigBgwYoNtvv10rV67s8nEIVJJ88cUXisViGjhwYIf7Bw4cqBMnTng0FS6mra1N8+bNU1lZmYqLi70eJyEHDx7UtddeK7/fr1mzZqm+vl4jRozweqyErF27Vvv371coFPJ6lB5RWlqqNWvW6N1339WyZcv0ySef6Hvf+56am5u9Hq1b/vrXv2rZsmW66aabtGnTJs2ePVuPPfZYp58u9G08/T4ot7++A+iumpoaHTp0KKVfBzjv5ptv1oEDBxQOh/Xmm2+qqqpKDQ0NKRuppqYmPf7449qyZYt69erl9Tg9oqKiov2fS0pKVFpaqsLCQr3xxhuaOXOmh5N1T1tbm8aMGaPFixdLkm6//XYdOnRIr7zyiqqqqi77OJ4G6kr6+o7rrrtOmZmZOnnyZIf7T548qUGDBnk0FTozZ84cvf3229qxY4eGDBni9TgJy87O1o033ihJGj16tPbs2aPf/OY3Wr58uceTdc++fft06tQp3XHHHe33xWIx7dixQy+99JKi0agyMzM9nDBxffr00fDhw3X06FGvR+mWwYMHX/AXoFtuuUXr16/v0nE8DdSV9PUd2dnZGj16tLZu3aqpU6dKiv8tY+vWrZozZ463w0GS5DiO5s6dq/r6em3fvl1Dhw71eiRXtLW1KRqNej1Gt02cOFEHDx7scF91dbWKioq0YMGClI+TJLW0tOjjjz/Wj3/8Y69H6ZaysrIL3qJx5MgRFRYWduk4pr7y/dv0xNd3eG3+/PmqqqrSmDFjdOedd+qFF15Qa2urqqurvR6tW1paWjr8De+TTz7RgQMH1K9fPxUUFHg4WffU1NSorq5Ob731lnJyctpfGwwEAurdu7fH03VPbW2tKioqVFBQoObmZtXV1Wn79u3atGmT16N1W05OzgWvC15zzTXq379/yr5e+OSTT2ry5MkqLCzUZ599pkWLFikzM1PTp0/3erRueeKJJ3TXXXdp8eLFeuihh/T+++9rxYoVWrFiRdcOlPgFhclRVVXlSLpg27Ztm9ejdcmLL77oFBQUONnZ2c6dd97p7N692+uRum3btm2d/jupqqryerRu6eyxSHJWr17t9Wjd9uijjzqFhYVOdna2k5eX50ycONHZvHmz12P1uFS/zHzatGnO4MGDnezsbOf66693pk2b5hw9etTrsRLy+9//3ikuLnb8fr9TVFTkrFixosvH4Os2AAAmcZk5AMAkAgUAMIlAAQBMIlAAAJMIFADAJAIFADCJQAEATCJQAACTCBQAwCQCBQAwiUABAEz6P+7xx9TB6SO8AAAAAElFTkSuQmCC\n"},"metadata":{}}]},{"cell_type":"markdown","source":["## Code Setup\n","\n","Here we will define some essential values for our problem (such as the initial state, final state, etc.). We have also provided two helper functions that may be useful for you!\n","\n","**TODO:** Fill in code for the initial and final state of the ego agent below. Some hints are provided in the code comments"],"metadata":{"id":"N2rPg41Ohu6E"}},{"cell_type":"code","source":["############################## Code starts here ##############################\n","# TODO: Define the initial and final state as numpy arrays for the ego agent here\n","# HINTS: Our state is a 3 dimensional vector [x, y, heading].\n","s_0 = None  # Initial state.\n","s_f = None  # Final state.\n","############################## Code ends here ##############################\n","\n","N = 50  # Number of time discretization nodes (0, 1, ... N).\n","s_dim = 3  # State dimension; 3 for (x, y, th).\n","u_dim = 2  # Control dimension; 2 for (V, om).\n","v_max = 0.5  # Maximum linear velocity.\n","om_max = 1.0  # Maximum angular velocity.\n","\n","def pack_decision_variables(t_f: float, s: np.ndarray, u: np.ndarray) -> np.ndarray:\n","    \"\"\"Packs decision variables (final time, states, controls) into a 1D vector.\n","\n","    Args:\n","        t_f: Final time, a scalar.\n","        s: States, an array of shape (N + 1, s_dim).\n","        u: Controls, an array of shape (N, u_dim).\n","\n","    Returns:\n","        An array `z` of shape (1 + (N + 1) * s_dim + N * u_dim,).\n","    \"\"\"\n","    return np.concatenate([[t_f], s.ravel(), u.ravel()])\n","\n","\n","def unpack_decision_variables(z: np.ndarray) -> T.Tuple[float, np.ndarray, np.ndarray]:\n","    \"\"\"Unpacks a 1D vector into decision variables (final time, states, controls).\n","\n","    Args:\n","        z: An array of shape (1 + (N + 1) * s_dim + N * u_dim,).\n","\n","    Returns:\n","        t_f: Final time, a scalar.\n","        s: States, an array of shape (N + 1, s_dim).\n","        u: Controls, an array of shape (N, u_dim).\n","    \"\"\"\n","    t_f = float(z[0])\n","    s = z[1:1 + (N + 1) * s_dim].reshape(N + 1, s_dim)\n","    u = z[-N * u_dim:].reshape(N, u_dim)\n","    return t_f, s, u"],"metadata":{"id":"pm850gRihuEZ"},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":["## Trajectory Optimization\n","\n","Next, we will define a function to set up and solve the trajectory optimization problem. Consider the following questions:\n","\n","\n","\n","1.   What is the cost for a given trajectory?\n","2.   What are the **constraints** that the trajectory must obey? Constraints here are *equality* constraints.\n","\n","We will use scipy.optimize.minimize to solve this nonlinear optimization problem. You can find some documentation for this function [here](https://docs.scipy.org/doc/scipy/reference/generated/scipy.optimize.minimize.html)\n","\n"],"metadata":{"id":"Tnq6CNk7g2A2"}},{"cell_type":"code","source":["def optimize_trajectory(\n","    time_weight: float = 1.0,\n","    verbose: bool = True\n","):\n","    \"\"\"Computes the optimal trajectory as a function of `time_weight`.\n","\n","    Args:\n","        time_weight: \\alpha in the HW writeup.\n","\n","    Returns:\n","        t_f_opt: Final time, a scalar.\n","        s_opt: States, an array of shape (N + 1, s_dim).\n","        u_opt: Controls, an array of shape (N, u_dim).\n","    \"\"\"\n","\n","    def cost(z):\n","        ############################## Code starts here ##############################\n","        # TODO: Define a cost function here\n","        # HINT: you may find `unpack_decision_variables` useful here. z is the packed 1D representation of t,s and u. Return the value of the cost.\n","\n","        ############################## Code ends here ##############################\n","\n","    # Initialize the trajectory with a straight line\n","    z_guess = pack_decision_variables(\n","        20, s_0 + np.linspace(0, 1, N + 1)[:, np.newaxis] * (s_f - s_0),\n","        np.ones(N * u_dim))\n","\n","    # Minimum and Maximum bounds on states and controls\n","    # This is because we would want to include safety limits\n","    # for omega (steering) and velocity (speed limit)\n","    bounds = Bounds(\n","        pack_decision_variables(\n","            0., -np.inf * np.ones((N + 1, s_dim)),\n","            np.array([0.01, -om_max]) * np.ones((N, u_dim))),\n","        pack_decision_variables(\n","            np.inf, np.inf * np.ones((N + 1, s_dim)),\n","            np.array([v_max, om_max]) * np.ones((N, u_dim)))\n","    )\n","\n","    # Define the equality constraints\n","    def eq_constraints(z):\n","        t_f, s, u = unpack_decision_variables(z)\n","        dt = t_f / N\n","        constraint_list = []\n","        for i in range(N):\n","            V, om = u[i]\n","            x, y, th = s[i]\n","            ############################## Code starts here ##############################\n","            # TODO: Append to `constraint_list` with dynanics constraints\n","\n","            ############################## Code ends here ##############################\n","\n","        ############################## Code starts here ##############################\n","        # TODO: Append to `constraint_list` with initial and final state constraints\n","\n","        ############################## Code ends here ##############################\n","        return np.concatenate(constraint_list)\n","\n","    # Define the inequality constraints\n","    def ineq_constraints(z):\n","      t_f, s, u = unpack_decision_variables(z)\n","      dt = t_f / N\n","      constraint_list = []\n","      for i in range(N):\n","          V, om = u[i]\n","          x, y, th = s[i]\n","          ############################## Code starts here ##############################\n","          # TODO: Append to `constraint_list` with collision avoidance constraint\n","\n","          ############################## Code ends here ################################\n","      return np.array(constraint_list)\n","\n","    result = minimize(cost,\n","                      z_guess,\n","                      bounds=bounds,\n","                      constraints=[{\n","                          'type': 'eq',\n","                          'fun': eq_constraints\n","                      },\n","                      {\n","                          'type': 'ineq',\n","                          'fun': ineq_constraints\n","                      }])\n","    if verbose:\n","        print(result)\n","\n","    return unpack_decision_variables(result.x)"],"metadata":{"id":"Ku5f2oEudUyJ"},"execution_count":null,"outputs":[]},{"cell_type":"code","source":["# Call the trajectory optimizer\n","t, s, u = optimize_trajectory()"],"metadata":{"id":"EfGNzPN8jHST"},"execution_count":null,"outputs":[]},{"cell_type":"code","source":["# Plot optimized trajectory\n","plt = render_scene(s)\n","plt.show()"],"metadata":{"id":"WX-R-89YXVbK"},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":["Let's write a small wrapper function which gives us a plot of the trajectory for a given value of alpha"],"metadata":{"id":"_JNEkklO1yTS"}},{"cell_type":"code","source":["def show_traj_plan(alpha=1.0):\n","  t, s, u = optimize_trajectory(time_weight=alpha)\n","  plt = render_scene(s, print_alpha=alpha)\n","  plt.show()"],"metadata":{"id":"ePmxqd0V16Gq"},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":["## Varying alpha\n","Let us now experiment with different values of alpha to see how the output changes. We have included three examples below, but feel free to experiment with other values as well!"],"metadata":{"id":"nvJ5yJ-V2IbY"}},{"cell_type":"code","source":["show_traj_plan(alpha=1.0)"],"metadata":{"id":"zycxbTK72hRL"},"execution_count":null,"outputs":[]},{"cell_type":"code","source":["show_traj_plan(alpha=20.0)"],"metadata":{"id":"De2z_OQB3SRr"},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":["## Trajectory Tracking"],"metadata":{"id":"9OIZXrnPF_Ef"}},{"cell_type":"code","source":["# Finite difference for derivatives\n","N = s.shape[0] - 1\n","traj_t = np.linspace(0,t,N+1)\n","dt = t / N\n","\n","traj_v = np.zeros((N+1, 2))\n","traj_a = np.zeros((N+1, 2))\n","\n","for i in range(N):\n","  traj_v[i+1,:] = (s[i+1, 0:2] - s[i, 0:2]) / dt\n","\n","for i in range(N):\n","  traj_a[i+1,:] = (traj_v[i+1, 0:2] - traj_v[i, 0:2]) / dt\n","\n","traj = np.column_stack((s, traj_v, traj_a))"],"metadata":{"id":"YivSBM_J2xiY"},"execution_count":null,"outputs":[]},{"cell_type":"code","source":["# Simulate noisy open-loop car dynamics\n","noise_scale = np.sqrt(0.2)\n","states_ol, ctrl_ol = simulate_car_dyn(s_0[0], s_0[1], s_0[2], traj_t, actions=u, noise_scale=noise_scale)"],"metadata":{"id":"QRK_TlndGWz3"},"execution_count":null,"outputs":[]},{"cell_type":"code","source":["# Define tracking controller\n","kpx = 2\n","kpy = 2\n","kdx = 2\n","kdy = 2\n","V_max = 0.75\n","om_max = 1.0\n","controller = TrajectoryTracker(kpx=kpx, kpy=kpy, kdx=kdx, kdy=kdy, V_max=V_max, om_max=om_max)\n","controller.load_traj(traj_t, traj)"],"metadata":{"id":"f_hg7tdTGY_y"},"execution_count":null,"outputs":[]},{"cell_type":"code","source":["# Apply tracking controller\n","tf_actual = traj_t[-1]\n","times_cl = np.arange(0, tf_actual, dt)\n","states_cl, ctrl_cl = simulate_car_dyn(s_0[0], s_0[1], s_0[2], times_cl, controller=controller, noise_scale=noise_scale)"],"metadata":{"id":"AFysTyWmGbzK"},"execution_count":null,"outputs":[]},{"cell_type":"code","source":["# Plots trajectories\n","plt.figure(figsize=[10,5])\n","plt.subplot(2,2,1)\n","plt.plot(times_cl, states_cl)\n","plt.plot(traj_t, traj[:,0], linestyle=':', color='C0', alpha=0.7)\n","plt.plot(traj_t, traj[:,1], linestyle=':', color='C1', alpha=0.7)\n","plt.plot(traj_t, traj[:,2], linestyle=':', color='C2', alpha=0.7)\n","plt.grid(True)\n","plt.xlabel('t')\n","plt.legend(labels=[\"x\",\"y\",\"th\",\"Nominal\"])\n","plt.subplot(2,2,3)\n","plt.plot(times_cl[:-1], ctrl_cl)\n","plt.plot(traj_t[:-1], u[:,0], color='C0', linestyle=':', alpha=0.7)\n","plt.plot(traj_t[:-1], u[:,1], color='C1', linestyle=':', alpha=0.7)\n","plt.grid(True)\n","plt.xlabel('t')\n","plt.legend(labels=[\"V\",\"om\", \"Nominal\"])\n","plt.subplot(1,2,2)\n","plt.plot(traj[:,0], traj[:,1], color=\"k\", linestyle=\":\", alpha=0.7)\n","plt.plot(states_ol[:,0], states_ol[:,1], alpha=0.7)\n","plt.plot(states_cl[:,0], states_cl[:,1])\n","plt.legend(labels=[\"Nominal\", \"Open Loop\", \"Closed Loop\"])\n","plt.xlabel('x')\n","plt.ylabel('y')\n","plt.grid(True)\n","plt.tight_layout()\n","plt.savefig(\"plots/trajopt_closedloop.pdf\")\n","plt.show()"],"metadata":{"id":"HajIyhYD3Uq2"},"execution_count":null,"outputs":[]}]}