MobileRobot/mobile_robot_local_path_planning_dwa_en.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Table of contents**<a id='toc0_'></a>    \n",
    "- [Local Path Planning using Dynamic Window Approach (DWA)](#toc1_)    \n",
    "- [Starting pybullet](#toc2_)    \n",
    "- [Definition of LiDAR Class](#toc3_)    \n",
    "- [Definition of Dynamic Window Approach](#toc4_)    \n",
    "- [Initial Settings](#toc5_)    \n",
    "- [Setting Parameters for Mobile Robot](#toc6_)    \n",
    "- [Setting Parameters for LiDAR](#toc7_)    \n",
    "- [Setting Obstacles](#toc8_)    \n",
    "- [Setting Parameters for Dynamic Window Approach](#toc9_)    \n",
    "- [Execution of Dynamic Window Approach](#toc10_)    \n",
    "\n",
    "<!-- vscode-jupyter-toc-config\n",
    "\tnumbering=false\n",
    "\tanchor=true\n",
    "\tflat=false\n",
    "\tminLevel=1\n",
    "\tmaxLevel=6\n",
    "\t/vscode-jupyter-toc-config -->\n",
    "<!-- THIS CELL WILL BE REPLACED ON TOC UPDATE. DO NOT WRITE YOUR TEXT IN THIS CELL -->"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# <a id='toc1_'></a>[Local Path Planning using Dynamic Window Approach (DWA)](#toc0_)\n",
    "\n",
    "This notebook explains the dynamic window approach.\n",
    "\n",
    "(For a manual summarizing the functions available in pybullet, please refer to [here](https://github.com/bulletphysics/bullet3/blob/master/docs/pybullet_quickstartguide.pdf).)\n",
    "\n",
    "The dynamic window approach is one of the methods for \"local path planning,\" which is a technique for planning a path for a robot to reach a target point while avoiding obstacles.\n",
    "\n",
    "Typically, local path planning is not used alone but is often combined with \"global path planning\" methods like A*.\n",
    "\n",
    "<br>\n",
    "\n",
    "Specifically,\n",
    "\n",
    "1. Plan a global path from the initial point to the target point using global path planning.\n",
    "\n",
    "<img src=\"../images/MobileRobot/mobile_robot_local_path_planning_dwa/global_path_planning.png\" width=\"50%\">  \n",
    "\n",
    "<br>\n",
    "<br>\n",
    "\n",
    "2. Divide the globally planned path into several intermediate points and use local path planning to sequentially travel from \"Intermediate Point 1\" to \"Intermediate Point 2\" to \"Intermediate Point 3\" ... to the \"Target Point.\"\n",
    "\n",
    "<img src=\"../images/MobileRobot/mobile_robot_local_path_planning_dwa/local_path_planning.png\" width=\"50%\">\n",
    "\n",
    "In local path planning, the robot travels to the target point while considering the surrounding environment in real-time.  \n",
    "Therefore, even if obstacles move slightly, the robot can reach the target point while avoiding obstacles without re-planning the global path.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# <a id='toc2_'></a>[Starting pybullet](#toc0_)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "startThreads creating 1 threads.\n",
      "starting thread 0\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "pybullet build time: Nov 28 2023 23:45:17\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "started thread 0 \n",
      "argc=2\n",
      "argv[0] = --unused\n",
      "argv[1] = --start_demo_name=Physics Server\n",
      "ExampleBrowserThreadFunc started\n",
      "X11 functions dynamically loaded using dlopen/dlsym OK!\n",
      "X11 functions dynamically loaded using dlopen/dlsym OK!\n",
      "Creating context\n",
      "Created GL 3.3 context\n",
      "Direct GLX rendering context obtained\n",
      "Making context current\n",
      "GL_VENDOR=Mesa\n",
      "GL_RENDERER=llvmpipe (LLVM 15.0.7, 256 bits)\n",
      "GL_VERSION=4.5 (Core Profile) Mesa 23.2.1-1ubuntu3.1~22.04.2\n",
      "GL_SHADING_LANGUAGE_VERSION=4.50\n",
      "pthread_getconcurrency()=0\n",
      "Version = 4.5 (Core Profile) Mesa 23.2.1-1ubuntu3.1~22.04.2\n",
      "Vendor = Mesa\n",
      "Renderer = llvmpipe (LLVM 15.0.7, 256 bits)\n",
      "b3Printf: Selected demo: Physics Server\n",
      "startThreads creating 1 threads.\n",
      "starting thread 0\n",
      "started thread 0 \n",
      "MotionThreadFunc thread started\n"
     ]
    }
   ],
   "source": [
    "import pybullet\n",
    "import pybullet_data\n",
    "physics_client = pybullet.connect(pybullet.GUI) "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# <a id='toc3_'></a>[Definition of LiDAR Class](#toc0_)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "ven = Mesa\n"
     ]
    }
   ],
   "source": [
    "import math \n",
    "import numpy as np\n",
    "\n",
    "class Lidar():\n",
    "    def __init__(self):\n",
    "        self.replace_item_uniqueIds = None\n",
    "\n",
    "    # Rotation matrix representing the posture\n",
    "    def rotation_matrix(self, roll, pitch, yaw):\n",
    "        R_x = [[1, 0, 0],\n",
    "                [0, math.cos(roll), -math.sin(roll)],\n",
    "                [0, math.sin(roll), math.cos(roll)]]\n",
    "        \n",
    "        R_y = [[math.cos(pitch), 0, math.sin(pitch)],\n",
    "                [0, 1, 0],\n",
    "                [-math.sin(pitch), 0, math.cos(pitch)]]\n",
    "        \n",
    "        R_z = [[math.cos(yaw), -math.sin(yaw), 0],\n",
    "                [math.sin(yaw), math.cos(yaw), 0],\n",
    "                [0, 0, 1]]\n",
    "        \n",
    "        return np.dot(R_z, np.dot(R_y, R_x))\n",
    "\n",
    "    def CheckHits(self, ray_from_position, ray_orientation, ray_length, angle_resolution_rad, ray_direction_range_rad, offset, draw_debug_line=False):\n",
    "        \"\"\"\n",
    "        Raycast for obstacle detection reflecting posture\n",
    "        Parameters\n",
    "        ---\n",
    "        ray_from_position : list\n",
    "            List storing the position where the ray starts [x, y, z]\n",
    "        orientation : list\n",
    "            Posture of the ray origin [roll, pitch, yaw]\n",
    "        ray_length : float\n",
    "            Length of the ray\n",
    "        angle_resolution_rad : float\n",
    "            Angle resolution\n",
    "        ray_direction_range_rad : float\n",
    "            List storing the range to fire the ray [startRad, endRad]\n",
    "        offset : float\n",
    "            Offset from rayFromPosition\n",
    "        draw_debug_line : bool\n",
    "            Whether to draw the ray or not\n",
    "        \n",
    "        Returns\n",
    "        ---\n",
    "        hit_results: tuple\n",
    "            Results of the raycast\n",
    "        \"\"\"\n",
    "        \n",
    "        \n",
    "        R = self.rotation_matrix(ray_orientation[0], ray_orientation[1], ray_orientation[2])\n",
    "        \n",
    "        # Number of divisions\n",
    "        num_divisions = int((ray_direction_range_rad[1] - ray_direction_range_rad[0]) / angle_resolution_rad)\n",
    "\n",
    "        # Generate ray start/end points for numDivisions within the range of ray_direction_range_rad\n",
    "        ray_froms = []\n",
    "        ray_toes = []\n",
    "        for i in range(num_divisions):\n",
    "            angle = ray_direction_range_rad[0] + i * angle_resolution_rad\n",
    "            direction = [math.cos(angle), math.sin(angle), 0]\n",
    "            \n",
    "            # Apply rotation matrix to rotate the direction vector\n",
    "            rotated_direction = np.dot(R, direction)\n",
    "\n",
    "            # Ray start point\n",
    "            ray_from = ray_from_position + offset * rotated_direction\n",
    "            ray_froms.append(ray_from)\n",
    "\n",
    "            # Ray end point\n",
    "            ray_to = ray_from + ray_length * rotated_direction\n",
    "            ray_toes.append(ray_to)\n",
    "        \n",
    "        # Perform raycast for numDivisions\n",
    "        hit_results = pybullet.rayTestBatch(ray_froms, ray_toes)\n",
    "\n",
    "        # Draw the ray\n",
    "        if draw_debug_line:\n",
    "            if self.replace_item_uniqueIds is None:\n",
    "                self.replace_item_uniqueIds = []\n",
    "                \n",
    "                for i in range(num_divisions):\n",
    "                    isHit = hit_results[i][0]\n",
    "                    ray_from = ray_froms[i]\n",
    "                    \n",
    "                    if isHit != -1:\n",
    "                        rayHitPosition = hit_results[i][3]\n",
    "                        self.replace_item_uniqueIds.append(pybullet.addUserDebugLine(ray_from, rayHitPosition, lineColorRGB=[1,0,0]))\n",
    "                    else:\n",
    "                        rayToPosition = ray_toes[i]\n",
    "                        self.replace_item_uniqueIds.append(pybullet.addUserDebugLine(ray_from, rayToPosition, lineColorRGB=[0,1,0]))\n",
    "            \n",
    "            else:\n",
    "                for i in range(num_divisions):\n",
    "                    isHit = hit_results[i][0]\n",
    "                    ray_from = ray_froms[i]\n",
    "                    \n",
    "                    if isHit != -1:\n",
    "                        rayHitPosition = hit_results[i][3]\n",
    "                        pybullet.addUserDebugLine(ray_from, rayHitPosition, lineColorRGB=[1,0,0], replaceItemUniqueId=self.replace_item_uniqueIds[i])\n",
    "                    else:\n",
    "                        rayToPosition = ray_toes[i]\n",
    "                        pybullet.addUserDebugLine(ray_from, rayToPosition, lineColorRGB=[0,1,0], replaceItemUniqueId=self.replace_item_uniqueIds[i])\n",
    "        return hit_results\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# <a id='toc4_'></a>[Definition of Dynamic Window Approach](#toc0_)\n",
    "\n",
    "Dynamic Window Approach (DWA) is a method that evaluates combinations of a robot's linear and angular velocities to select the optimal ones.\n",
    "\n",
    "The general procedure is as follows:\n",
    "1. Initialization\n",
    "   - Initialization is performed in the `__init__` method.\n",
    "2. Calculation of Vr (Velocity Result)\n",
    "   - Vr is calculated in the `calc_vr` method.\n",
    "   - Vr is the intersection of Vs (Velocity Sample), Vd (Velocity dynamic window), and Va (Velocity admissible).\n",
    "     - Vs: Range of the robot's linear and angular velocities (determined by the robot's physical specifications)\n",
    "     - Vd: Range of linear and angular velocities the robot can take within a small time interval dt from its current velocities\n",
    "     - Va: Range of linear and angular velocities at which the robot does not collide with obstacles when operating at its current position\n",
    "   <img src=\"../images/MobileRobot/mobile_robot_local_path_planning_dwa/dynamic_window_approach_overview.png\" width=\"50%\">\n",
    "3. Selection of optimal linear and angular velocities\n",
    "   - The `calc_best_u` method selects the optimal linear and angular velocities from the range of Vr.\n",
    "     1. Calculation of predicted trajectory\n",
    "          - The `calc_predicted_trajectory` method calculates the predicted trajectory up to `predict_time` based on the linear and angular velocities.\n",
    "     2. Calculation of costs: The following costs are calculated for the predicted trajectory.\n",
    "          - `calc_to_goal_cost` method: Whether the end point of the trajectory is facing the goal direction\n",
    "          - `calc_speed_cost` method: Whether the trajectory allows the robot to move at high linear and angular velocities\n",
    "          - `calc_obstacle_cost` method: Whether the trajectory keeps a safe distance from obstacles\n",
    "     3. The linear and angular velocities that result in the trajectory with the lowest total cost are selected as the control inputs.\n",
    "4. Steps 2 and 3 are repeated until the goal is reached.\n",
    " "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "class Vs:\n",
    "    \"\"\"\n",
    "    Definition of Vs (velocity space).\n",
    "    Defines the range of velocities and angular velocities that the robot can take based on its specifications (the range is basically fixed as it depends on the specifications of the mobile robot).\n",
    "    \"\"\"\n",
    "    def __init__(self, linear_vel_min, linear_vel_max, angle_vel_min, angle_vel_max):\n",
    "        self.linear_vel_min = linear_vel_min\n",
    "        self.linear_vel_max = linear_vel_max\n",
    "        self.angle_vel_min = angle_vel_min\n",
    "        self.angle_vel_max = angle_vel_max\n",
    "\n",
    "class Vd:\n",
    "    \"\"\"\n",
    "    Definition of Vd (Velocity dynamic window).\n",
    "    Defines the range of velocities and angular velocities that the robot can take during a small time interval dt based on its current velocity and angular velocity.\n",
    "    \"\"\"\n",
    "    def __init__(self):\n",
    "        self.linear_vel_min = 0.0\n",
    "        self.linear_vel_max = 0.0\n",
    "        self.angle_vel_min = 0.0\n",
    "        self.angle_vel_max = 0.0\n",
    "\n",
    "    def calc_Vd(self, linear_vel_current, angle_vel_current, linear_acc_min, linear_acc_max, angle_acc_min, angle_acc_max, dt):\n",
    "        self.linear_vel_min = linear_vel_current + linear_acc_min * dt\n",
    "        self.linear_vel_max = linear_vel_current + linear_acc_max * dt\n",
    "        self.angle_vel_min = angle_vel_current + angle_acc_min * dt\n",
    "        self.angle_vel_max = angle_vel_current + angle_acc_max * dt\n",
    "\n",
    "class Vr:\n",
    "    \"\"\"\n",
    "    Definition of Vr (Velocity result).\n",
    "    Calculates the intersection of Vs and Vd (the overlapping range of Vs and Vd).\n",
    "    In the paper, Vr is defined as the intersection of Vs, Vd, and Va, but calculating the entire region of Va is computationally expensive. Therefore, here it is defined as the intersection of Vs and Vd.\n",
    "    Va is determined only on candidate trajectories in \"calc_obstacle_cost\" (since it is sufficient to know whether the candidate trajectory is in the Va region, it is not necessary to calculate the entire Va in advance).\n",
    "    ※ Va (velocity admissible): The range of velocities and angular velocities where the robot does not collide with obstacles when moving at a certain velocity and angular velocity from the current position.\n",
    "       Unlike Vs and Vd, collision detection is required to determine the region, making it computationally expensive.\n",
    "    \"\"\"\n",
    "    def __init__(self):\n",
    "        self.linear_vel_min = 0.0\n",
    "        self.linear_vel_max = 0.0\n",
    "        self.angle_vel_min = 0.0\n",
    "        self.angle_vel_max = 0.0\n",
    "\n",
    "    def calc_Vr(self, vs: Vs, vd: Vd):\n",
    "        self.linear_vel_min = max(vs.linear_vel_min, vd.linear_vel_min)\n",
    "        self.linear_vel_max = min(vs.linear_vel_max, vd.linear_vel_max)\n",
    "        self.angle_vel_min = max(vs.angle_vel_min, vd.angle_vel_min)\n",
    "        self.angle_vel_max = min(vs.angle_vel_max, vd.angle_vel_max)\n",
    "\n",
    "class DynamicWindowApproach:\n",
    "\n",
    "    def __init__(self, x_init,\n",
    "                       linear_vel_min, linear_vel_max, linear_acc_min, linear_acc_max,\n",
    "                       angle_vel_min, angular_vel_max, angle_acc_min, angle_acc_max,\n",
    "                       predict_time, dt,\n",
    "                       linear_vel_resolution, angle_vel_resolution,\n",
    "                       to_goal_cost_gain, speed_cost_gain, obstacle_cost_gain,\n",
    "                       lidar_angle_min, lidar_angle_resolution,\n",
    "                       robot_radius):\n",
    "        self.x = x_init\n",
    "        self.predict_time = predict_time\n",
    "        self.dt = dt\n",
    "        self.linear_vel_resolution = linear_vel_resolution\n",
    "        self.angle_vel_resolution = angle_vel_resolution\n",
    "        self.to_goal_cost_gain = to_goal_cost_gain\n",
    "        self.speed_cost_gain = speed_cost_gain\n",
    "        self.obstacle_cost_gain = obstacle_cost_gain\n",
    "        self.lidar_angle_min = lidar_angle_min\n",
    "        self.lidar_angle_resolution = lidar_angle_resolution\n",
    "        self.robot_radius = robot_radius\n",
    "\n",
    "        # Velocity space (the range of velocities and angular velocities that the robot can take)\n",
    "        self.vs = Vs(linear_vel_min, linear_vel_max, angle_vel_min, angular_vel_max)\n",
    "\n",
    "        self.linear_acc_min = linear_acc_min\n",
    "        self.linear_acc_max = linear_acc_max\n",
    "        self.angle_acc_min = angle_acc_min\n",
    "        self.angle_acc_max = angle_acc_max\n",
    "\n",
    "    def motion_model(self, x, u, dt):\n",
    "        \"\"\"\n",
    "        Calculates the next state x_next from the current state x and input u using the motion model of a two-wheeled robot on a 2D plane.\n",
    "        \n",
    "        Parameters\n",
    "        ----------\n",
    "        x : numpy.ndarray\n",
    "            Current state [x, y, theta]\n",
    "        u : numpy.ndarray\n",
    "            Input [v, w]\n",
    "        dt : float\n",
    "            Time step\n",
    "\n",
    "        Returns\n",
    "        -------\n",
    "        x_next : numpy.ndarray\n",
    "            Next state [x, y, theta]\n",
    "        \"\"\"\n",
    "        theta = x[2]\n",
    "        F = np.array([[1.0, 0.0, 0.0, 0.0, 0.0],\n",
    "                      [0.0, 1.0, 0.0, 0.0, 0.0],\n",
    "                      [0.0, 0.0, 1.0, 0.0, 0.0],\n",
    "                      [0.0, 0.0, 0.0, 0.0, 0.0],\n",
    "                      [0.0, 0.0, 0.0, 0.0, 0.0]])\n",
    "        B = np.array([[dt * np.cos(theta), 0.0],\n",
    "                      [dt * np.sin(theta), 0.0],\n",
    "                      [0.0, dt],\n",
    "                      [1.0, 0.0],\n",
    "                      [0.0, 1.0]])\n",
    "        x_next = F @ x + B @ u # Motion model without considering friction\n",
    "        return x_next\n",
    "\n",
    "\n",
    "    def calc_best_u(self, x, u, lidar_results, goal):\n",
    "        \"\"\"\n",
    "        Calculates the optimal velocity and angular velocity\n",
    "\n",
    "        Parameters\n",
    "        ----------\n",
    "        x : numpy.ndarray\n",
    "            Current state [x, y, theta]\n",
    "        u : numpy.ndarray\n",
    "            Current velocity and angular velocity [v, w]\n",
    "        lidar_results : list\n",
    "            Lidar results\n",
    "        goal : numpy.ndarray\n",
    "            Goal position [x, y]\n",
    "\n",
    "        Returns\n",
    "        -------\n",
    "        u_best : numpy.ndarray\n",
    "            Optimal velocity and angular velocity calculated by the dynamic window approach [v, w]\n",
    "        \"\"\"\n",
    "        # Calculate Vr\n",
    "        vd = Vd()\n",
    "        vd.calc_Vd(u[0], u[1], self.linear_acc_min, self.linear_acc_max, self.angle_acc_min, self.angle_acc_max, self.dt)\n",
    "        vr = Vr()\n",
    "        vr.calc_Vr(self.vs, vd)\n",
    "\n",
    "        # Search for the optimal velocity and angular velocity\n",
    "        u_best = np.array([0.0, 0.0])\n",
    "        min_cost = 1000000\n",
    "        for linear_vel_candidate in np.arange(vr.linear_vel_min, vr.linear_vel_max, self.linear_vel_resolution):\n",
    "            for angle_vel_candidate in np.arange(vr.angle_vel_min, vr.angle_vel_max, self.angle_vel_resolution):\n",
    "                # Calculate the predicted trajectory\n",
    "                u_candidate = np.array([linear_vel_candidate, angle_vel_candidate])\n",
    "                trajectory = self.calc_predict_trajectory(x, u_candidate, self.predict_time, self.dt)\n",
    "\n",
    "                # Calculate the cost\n",
    "                to_goal_cost = self.calc_to_goal_cost(trajectory, goal)\n",
    "                speed_cost = self.calc_speed_cost(trajectory, self.vs)\n",
    "                obstacle_cost = self.calc_obstacle_cost(trajectory, lidar_results, self.robot_radius)\n",
    "                total_cost = self.to_goal_cost_gain * to_goal_cost + self.speed_cost_gain * speed_cost + self.obstacle_cost_gain * obstacle_cost\n",
    "                \n",
    "                # Update the minimum cost\n",
    "                if total_cost < min_cost:\n",
    "                    min_cost = total_cost\n",
    "                    u_best = np.array([linear_vel_candidate, angle_vel_candidate])\n",
    "        return u_best\n",
    "\n",
    "    def calc_predict_trajectory(self, x, u, predict_time, dt):\n",
    "        \"\"\"\n",
    "        Calculates the trajectory up to predict_time from the current state x and control input u\n",
    "\n",
    "        Parameters\n",
    "        ----------\n",
    "        x : numpy.ndarray\n",
    "            Current state [x, y, theta]\n",
    "        u : numpy.ndarray\n",
    "            Control input [v, w]\n",
    "        dt : float\n",
    "            Time step\n",
    "\n",
    "        Returns\n",
    "        -------\n",
    "        trajectory : numpy.ndarray\n",
    "            Trajectory [x, y, theta, v, w]\n",
    "        \"\"\"\n",
    "        trajectory = np.array([x])\n",
    "        current_time = 0.0\n",
    "        while current_time <= predict_time:\n",
    "            x = self.motion_model(x, u, dt)\n",
    "            trajectory = np.vstack((trajectory, x))\n",
    "            current_time += dt\n",
    "        return trajectory            \n",
    "    \n",
    "    def calc_to_goal_cost(self, trajectory, goal):\n",
    "        \"\"\"\n",
    "        Calculates the cost to the goal\n",
    "        The cost is the difference in angle between the final position of the trajectory and the goal position (the more the trajectory is directed towards the goal, the lower the cost).\n",
    "\n",
    "        Parameters\n",
    "        ----------\n",
    "        trajectory : numpy.ndarray\n",
    "            Trajectory [x, y, theta, v, w]\n",
    "        goal : numpy.ndarray\n",
    "            Goal position [x, y]\n",
    "\n",
    "        Returns\n",
    "        -------\n",
    "        cost : float\n",
    "            Cost to the goal\n",
    "        \"\"\"\n",
    "        dx = goal[0] - trajectory[-1, 0]\n",
    "        dy = goal[1] - trajectory[-1, 1]\n",
    "        error_angle = math.atan2(dy, dx)\n",
    "        cost_angle = error_angle - trajectory[-1, 2]\n",
    "        cost = abs(math.atan2(math.sin(cost_angle), math.cos(cost_angle)))\n",
    "        return cost\n",
    "    \n",
    "    def calc_speed_cost(self, trajectory, vs: Vs):\n",
    "        \"\"\"\n",
    "        Calculates the cost related to speed\n",
    "        The cost is the difference from the current speed (the higher the speed, the lower the cost).\n",
    "\n",
    "        Parameters\n",
    "        ----------\n",
    "        trajectory : numpy.ndarray\n",
    "            Trajectory [x, y, theta, v, w]\n",
    "        vs : Vs\n",
    "            Velocity space\n",
    "\n",
    "        Returns\n",
    "        -------\n",
    "        cost : float\n",
    "            Cost related to speed\n",
    "        \"\"\"\n",
    "        cost = vs.linear_vel_max - trajectory[-1, 3]    \n",
    "        return cost\n",
    "\n",
    "    def calc_obstacle_cost(self, trajectory, lidar_results, robot_radius):\n",
    "        \"\"\"\n",
    "        Calculates the cost related to obstacles\n",
    "        The cost is the inverse of the distance to obstacles at each point on the trajectory (the farther from obstacles, the lower the cost).\n",
    "\n",
    "        Parameters\n",
    "        ----------\n",
    "        trajectory : numpy.ndarray\n",
    "            Trajectory [x, y, theta, v, w]\n",
    "        lidar_results : list\n",
    "\n",
    "        Returns\n",
    "        -------\n",
    "        cost : float\n",
    "            Cost related to obstacles\n",
    "        \"\"\"\n",
    "        min_dist = 100000\n",
    "        for trajectory_point_x, trajectory_point_y , _, _, _ in trajectory:\n",
    "            for lidar_result in lidar_results:\n",
    "                # Position where the ray hit (in world coordinates)\n",
    "                # In Pybullet, it is possible to directly obtain the position where the ray hit in the \"world coordinate system\",\n",
    "                # but in the case of an actual robot, the obtained value will be in the \"robot coordinate system\", so it is necessary to convert the obtained value to the world coordinate system.\n",
    "                hit_position = lidar_result[3] \n",
    "                # Skip if the lidar did not hit (if not hit, hit_point=[0, 0, 0])\n",
    "                if hit_position[0] == 0.0 and hit_position[1] == 0.0:\n",
    "                    continue\n",
    "                # The closer the distance between the \"point on the predicted trajectory\" and the \"position where the ray hit\", the higher the cost, as it is judged that there is an obstacle near the predicted trajectory.\n",
    "                dx = trajectory_point_x - hit_position[0]\n",
    "                dy = trajectory_point_y - hit_position[1]\n",
    "                dist = np.hypot(dx, dy) - robot_radius\n",
    "                if dist < min_dist:\n",
    "                    min_dist = dist\n",
    "        cost = 1.0 / (min_dist + 1e-6)\n",
    "        return cost\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# <a id='toc5_'></a>[Initial Settings](#toc0_)\n",
    "\n",
    "Set up the environment and generate the robot."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "ven = Mesa\n"
     ]
    }
   ],
   "source": [
    "pybullet.resetSimulation() # Reset the simulation space\n",
    "pybullet.setAdditionalSearchPath(pybullet_data.getDataPath()) # Add path to the data required by pybullet\n",
    "pybullet.setGravity(0.0, 0.0, -9.8) # Set gravity as on Earth\n",
    "time_step = 0.1\n",
    "pybullet.setTimeStep(time_step) # Set the elapsed time per step\n",
    "\n",
    "# Set the camera position for GUI mode\n",
    "camera_distance = 6.0\n",
    "camera_yaw = 180.0 # deg\n",
    "camera_pitch = -90.1 # deg\n",
    "camera_target_position = [0.0, 0.0, 0.0]\n",
    "pybullet.resetDebugVisualizerCamera(camera_distance, camera_yaw, camera_pitch, camera_target_position)\n",
    "\n",
    "# Load the floor\n",
    "plane_id = pybullet.loadURDF(\"plane.urdf\")\n",
    "\n",
    "# Load the robot\n",
    "car_start_pos = [0, 0, 0.1]  # Set the initial position (x, y, z)\n",
    "car_start_yaw = math.radians(0)\n",
    "car_start_orientation = pybullet.getQuaternionFromEuler([0, 0, car_start_yaw]) # Convert the initial orientation (roll, pitch, yaw) to quaternion\n",
    "car_id = pybullet.loadURDF(\"../urdf/simple_two_wheel_car.urdf\", car_start_pos, car_start_orientation)\n",
    "\n",
    "# Goal point sphere\n",
    "goal_visual_id = pybullet.createVisualShape(pybullet.GEOM_SPHERE, radius=0.1, rgbaColor=[0,1,0,1])\n",
    "goal_id = pybullet.createMultiBody(0, -1, goal_visual_id, [0.0, 0.0, 0.0], useMaximalCoordinates=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# <a id='toc6_'></a>[Setting Parameters for Mobile Robot](#toc0_)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Joint indices of the left and right wheels\n",
    "RIGHT_WHEEL_IDX = 0\n",
    "LEFT_WHEEL_IDX = 1\n",
    "WHEEL_THREAD = 0.325 # Distance between wheels (match the distance between wheels in \"simple_two_wheel_car.urdf\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# <a id='toc7_'></a>[Setting Parameters for LiDAR](#toc0_)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "LIDAR_LINK_IDX = 5 # Index of the link from which the ray is emitted\n",
    "lidar_ray_origin_offset = 0.1 # Offset from the origin of the lidar [m]\n",
    "lidar_ray_length = 2.0 # Length of the ray\n",
    "lidar_angle_min = math.radians(-180) # Minimum angle of the ray\n",
    "lidar_angle_max = math.radians(180) # Maximum angle of the ray\n",
    "lidar_angle_resolution = math.radians(5) # Angular resolution of the ray"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# <a id='toc8_'></a>[Setting Obstacles](#toc0_)\n",
    "\n",
    "Generate obstacles.\n",
    "\n",
    "**The control of the dynamic window approach changes depending on the position of the obstacles, so feel free to change or add obstacles to any position.**\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Add obstacle\n",
    "mass = 200 # kg\n",
    "box_size = [0.5, 0.5, 0.3]\n",
    "position = [2.0, 2.0, 0.3]\n",
    "orientation = [1, 0, 0, 0]\n",
    "box_collision_id = pybullet.createCollisionShape(pybullet.GEOM_BOX, halfExtents=box_size, physicsClientId=physics_client)\n",
    "box_visual_id = pybullet.createVisualShape(pybullet.GEOM_BOX, halfExtents=box_size, physicsClientId=physics_client, rgbaColor=[1,0,0,1]) \n",
    "box_body_id = pybullet.createMultiBody(mass, box_collision_id, box_visual_id, position, orientation, physicsClientId=physics_client)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# <a id='toc9_'></a>[Setting Parameters for Dynamic Window Approach](#toc0_)\n",
    "\n",
    "Set parameters related to the dynamic window approach.\n",
    "\n",
    "**Try changing each parameter to see how the robot behaves.**\n",
    "\n",
    "---\n",
    "\n",
    "Note: Depending on the position of obstacles and the target location, the dynamic window approach may fall into a local optimum and fail to reach the target point.\n",
    "\n",
    "Therefore, as mentioned at the beginning,\n",
    "- Find a global path using global path planning\n",
    "- → Divide the obtained path into several intermediate points\n",
    "- → \"Move towards Intermediate Point 1\" → \"Once reaching Intermediate Point 1, move towards Intermediate Point 2\" → ... Repeat until reaching the target point (dividing into intermediate points reduces the possibility of falling into a local optimum)\n",
    "\n",
    "Such a procedure is generally used. (In this notebook, global path planning is not performed.)\n",
    "\n",
    "---\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Parameters related to dynamic window\n",
    "linear_vel_min = -0.5 # Minimum speed of the robot [m/s]\n",
    "linear_vel_max = 1.0 # Maximum speed of the robot [m/s]\n",
    "linear_acc_min = -0.5 # Minimum acceleration of the robot [m/s^2]\n",
    "linear_acc_max = 0.5 # Maximum acceleration of the robot [m/s^2]\n",
    "angle_vel_min = math.radians(-80) # Minimum angular velocity of the robot [rad/s]\n",
    "angle_vel_max = math.radians(80) # Maximum angular velocity of the robot [rad/s]\n",
    "angle_acc_min = math.radians(-40) # Minimum angular acceleration of the robot [rad/s^2]\n",
    "angle_acc_max = math.radians(40) # Maximum angular acceleration of the robot [rad/s^2]\n",
    "predict_time = 2.0 # Prediction time [s] (how far ahead to calculate the predicted trajectory)\n",
    "linear_vel_resolution = 0.02 # Speed resolution [m/s]\n",
    "angle_vel_resolution = 0.02 # Angular velocity resolution [rad/s]\n",
    "robot_radius = 0.1 # Radius of the robot [m]\n",
    "\n",
    "# Parameters related to cost\n",
    "to_goal_cost_gain = 0.15 # Weight of the cost to the goal (the more the direction differs from the goal, the higher the cost)\n",
    "speed_cost_gain = 1.0 # Weight of the cost related to speed (the slower the speed, the higher the cost)\n",
    "obstacle_cost_gain = 0.7 # Weight of the cost related to obstacles (the closer to obstacles, the higher the cost)\n",
    "\n",
    "# Setting the goal position\n",
    "goal = np.array([1, 5, 0])\n",
    "# goal = np.array([4, 4, 0])\n",
    "# goal = np.array([3.5, 2, 0])\n",
    "\n",
    "# Place the spherical object indicating the goal position at the goal position\n",
    "pybullet.resetBasePositionAndOrientation(goal_id, goal, [0, 0, 0, 1])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# <a id='toc10_'></a>[Execution of Dynamic Window Approach](#toc0_)\n",
    "\n",
    "Execute the dynamic window approach based on the specified parameters."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      " v: 0.89 omega: 0.878       "
     ]
    }
   ],
   "source": [
    "import sys\n",
    "\n",
    "# Initialize debug drawing\n",
    "pybullet.removeAllUserDebugItems()\n",
    "\n",
    "# Set the robot to the initial position\n",
    "pybullet.resetBasePositionAndOrientation(car_id, car_start_pos, car_start_orientation)\n",
    "\n",
    "# Initialize lidar\n",
    "lidar = Lidar()\n",
    "\n",
    "# Initialize dynamic window approach\n",
    "dwa = DynamicWindowApproach(x_init=np.array(car_start_pos), \n",
    "                            linear_vel_min=linear_vel_min, linear_vel_max=linear_vel_max, linear_acc_min=linear_acc_min, linear_acc_max=linear_acc_max,\n",
    "                            angle_vel_min=angle_vel_min, angular_vel_max=angle_vel_max, angle_acc_min=angle_acc_min, angle_acc_max=angle_acc_max,\n",
    "                            predict_time=predict_time, dt=time_step,\n",
    "                            linear_vel_resolution=linear_vel_resolution, angle_vel_resolution=angle_vel_resolution,\n",
    "                            to_goal_cost_gain=to_goal_cost_gain, speed_cost_gain=speed_cost_gain, obstacle_cost_gain=obstacle_cost_gain,\n",
    "                            lidar_angle_min=lidar_angle_min, lidar_angle_resolution=lidar_angle_resolution,\n",
    "                            robot_radius=robot_radius)\n",
    "\n",
    "\n",
    "# Initial control input\n",
    "v_init = 0.0\n",
    "omega_init = math.radians(0.0)\n",
    "u = np.array([v_init, omega_init])\n",
    "\n",
    "# Initial state\n",
    "x = np.array([car_start_pos[0], car_start_pos[1], car_start_yaw, v_init, omega_init])\n",
    "\n",
    "while True:\n",
    "    # Get lidar information\n",
    "    lidar_position = pybullet.getLinkState(car_id, LIDAR_LINK_IDX) # Get information of lidar link\n",
    "    ray_from_position = [lidar_position[0][0], lidar_position[0][1], lidar_position[0][2]] # Set the starting point of the ray to the origin of the lidar link\n",
    "    lidar_orientation = list(pybullet.getEulerFromQuaternion(lidar_position[1])) \n",
    "    lidar_results = lidar.CheckHits(ray_from_position, lidar_orientation, lidar_ray_length,  lidar_angle_resolution, [lidar_angle_min, lidar_angle_max], lidar_ray_origin_offset, draw_debug_line=True)\n",
    "    \n",
    "    # Main process of dynamic window approach ========================================\n",
    "    u = dwa.calc_best_u(x, u, lidar_results, goal) # Calculate the optimal control input\n",
    "    # =========================================================================\n",
    "    v = u[0]\n",
    "    omega = u[1]\n",
    "    sys.stdout.write(f\"\\r v: {round(v, 3)} omega: {round(omega, 3)}       \")\n",
    "\n",
    "    # Calculate the command speed of the left and right wheels from the translational speed and rotational speed\n",
    "    right_wheel_velocity = v + omega * WHEEL_THREAD / 2\n",
    "    left_wheel_velocity = v - omega * WHEEL_THREAD / 2\n",
    "\n",
    "    # Set the speed\n",
    "    pybullet.setJointMotorControl2(car_id, RIGHT_WHEEL_IDX, pybullet.VELOCITY_CONTROL, targetVelocity=right_wheel_velocity)\n",
    "    pybullet.setJointMotorControl2(car_id, LEFT_WHEEL_IDX, pybullet.VELOCITY_CONTROL, targetVelocity=left_wheel_velocity)\n",
    "\n",
    "    # Advance the simulation by one step\n",
    "    pybullet.stepSimulation()\n",
    "\n",
    "    # Update the state (here, \"x, y, yaw\" are the true values obtained directly from the simulation. In practice, the values estimated using odometry, etc. are used)\n",
    "    car_position, car_orientation = pybullet.getBasePositionAndOrientation(car_id)\n",
    "    car_euler = pybullet.getEulerFromQuaternion(car_orientation)\n",
    "    car_yaw = car_euler[2]\n",
    "    x = np.array([car_position[0], car_position[1], car_yaw, v, omega], dtype=np.float64)\n",
    "\n",
    "    # End if the robot reaches near the goal\n",
    "    goal_dis = 0.3\n",
    "    if np.linalg.norm(goal[:2] - x[:2]) < goal_dis:\n",
    "        pybullet.addUserDebugText(\"GOAL!!\", [goal[0]-0.7, goal[1]-0.8, 0.3], [0, 0, 1], textSize=2)\n",
    "        break"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "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.10.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}