RobotArm/robot_arm_jacobian_inverse_kinematics_en.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Table of contents**<a id='toc0_'></a>    \n",
    "- [Robot Arm](#toc1_)    \n",
    "- [Starting pybullet](#toc2_)    \n",
    "- [Initial Setup for pybullet](#toc3_)    \n",
    "- [Generating the Robot Arm](#toc4_)    \n",
    "- [Defining Functions](#toc5_)    \n",
    "  - [Defining Homogeneous Transformation Matrix](#toc5_1_)    \n",
    "  - [Defining the Jacobian Matrix](#toc5_2_)    \n",
    "  - [Defining the Pseudo-Inverse Matrix](#toc5_3_)    \n",
    "- [Running the Simulation](#toc6_)    \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>[Robot Arm](#toc0_)\n",
    "\n",
    "In this notebook, we will generate a 2-axis robot arm and explain how to solve inverse kinematics using numerical methods with the Jacobian matrix.\n",
    "\n",
    "![](../images/RobotArm/robot_arm_jacobian_inverse_kinemarics/inverse_kinematics_jacobian_overview_en.png)\n",
    "\n",
    "(For a manual summarizing the functions available in pybullet, please refer to [this link](https://github.com/bulletphysics/bullet3/blob/master/docs/pybullet_quickstartguide.pdf).)"
   ]
  },
  {
   "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>[Initial Setup for pybullet](#toc0_)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "ven = Mesa\n",
      "ven = Mesa\n"
     ]
    }
   ],
   "source": [
    "pybullet.resetSimulation() # Reset the simulation space\n",
    "pybullet.setAdditionalSearchPath(pybullet_data.getDataPath()) # Add path to necessary data for pybullet\n",
    "pybullet.setGravity(0.0, 0.0, -9.8) # Set gravity as on Earth\n",
    "time_step = 1./240.\n",
    "pybullet.setTimeStep(time_step) # Set the elapsed time per step\n",
    "\n",
    "# Load the floor\n",
    "plane_id = pybullet.loadURDF(\"plane.urdf\")\n",
    "\n",
    "# Set the camera position for GUI mode\n",
    "camera_distance = 2.0\n",
    "camera_yaw = 0.0 # deg\n",
    "camera_pitch = -20 # deg\n",
    "camera_target_position = [0.0, 0.0, 0.0]\n",
    "pybullet.resetDebugVisualizerCamera(camera_distance, camera_yaw, camera_pitch, camera_target_position)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# <a id='toc4_'></a>[Generating the Robot Arm](#toc0_)\n",
    "This time, we will generate a 2-axis robot arm `simple_2d_arm.urdf`.  \n",
    "The robot is configured as shown in the diagram below (sensors are explained in `robot_arm_sensor.ipynb`).  \n",
    "\n",
    "![](../images/RobotArm/2d_robot_arm.png)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "b3Printf: b3Warning[examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp,126]:\n",
      "\n",
      "b3Printf: No inertial data for link, using mass=1, localinertiadiagonal = 1,1,1, identity local inertial frame\n",
      "b3Printf: b3Warning[examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp,126]:\n",
      "\n",
      "b3Printf: target_position_vertual_link\n"
     ]
    }
   ],
   "source": [
    "# Load the robot\n",
    "arm_start_pos = [0, 0, 0.1]  # Set initial position (x, y, z)\n",
    "arm_start_orientation = pybullet.getQuaternionFromEuler([0, 0, 0])  # Set initial orientation (roll, pitch, yaw)\n",
    "arm_id = pybullet.loadURDF(\"../urdf/simple2d_arm.urdf\", arm_start_pos, arm_start_orientation, useFixedBase=True)  # Fix the root link with useFixedBase=True to prevent the robot from falling\n",
    "\n",
    "# Set the camera position for GUI mode\n",
    "camera_distance = 1.5\n",
    "camera_yaw = 180.0  # deg\n",
    "camera_pitch = -10  # deg\n",
    "camera_target_position = [0.0, 0.0, 1.0]\n",
    "pybullet.resetDebugVisualizerCamera(camera_distance, camera_yaw, camera_pitch, camera_target_position)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# <a id='toc5_'></a>[Defining Functions](#toc0_)\n",
    "\n",
    "## <a id='toc5_1_'></a>[Defining Homogeneous Transformation Matrix](#toc0_)\n",
    "We will define functions to create homogeneous transformation matrices.\n",
    "\n",
    "(For more details, please refer to [robot_arm_homogeneous_matrix_forward_kinematics](https://github.com/akinami3/PybulletRobotics/blob/main/RobotArm/robot_arm_homogeneous_matrix_forward_kinematics_en.ipynb).)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "def Hz(theta):\n",
    "    \"\"\"\n",
    "    Calculate the rotation matrix in a 2D plane (rotation around the z-axis in a 2D plane)\n",
    "\n",
    "    Parameters\n",
    "    ----------\n",
    "    theta : float\n",
    "        Rotation angle [rad]\n",
    "\n",
    "    Returns\n",
    "    -------\n",
    "    numpy.ndarray\n",
    "        Rotation matrix\n",
    "    \"\"\"\n",
    "    return np.array([[np.cos(theta), -np.sin(theta), 0], \n",
    "                     [np.sin(theta), np.cos(theta), 0], \n",
    "                     [0, 0, 1]])\n",
    "\n",
    "def Hp(x, y):\n",
    "    \"\"\"\n",
    "    Calculate the translation matrix in a 2D plane\n",
    "\n",
    "    Parameters\n",
    "    ----------\n",
    "    x : float\n",
    "        Translation amount in the x direction\n",
    "    y : float\n",
    "        Translation amount in the y direction\n",
    "\n",
    "    Returns\n",
    "    -------\n",
    "    numpy.ndarray\n",
    "        Translation matrix\n",
    "    \"\"\"\n",
    "    return np.array([[1, 0, x], \n",
    "                     [0, 1, y], \n",
    "                     [0, 0, 1]])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## <a id='toc5_2_'></a>[Defining the Jacobian Matrix](#toc0_)\n",
    "In [robot_arm_analytical_inverse_kinematics.ipynb](../RobotArm/robot_arm_analytical_inverse_kinematics_en.ipynb), we solved inverse kinematics analytically.\n",
    "\n",
    "Analytical solutions for inverse kinematics can provide instant solutions, but it can be challenging to solve analytically when the robot arm's configuration is complex.\n",
    "\n",
    "<br>\n",
    "\n",
    "On the other hand, numerical methods for inverse kinematics solve the problem by gradually moving each joint angle $\\vec{Q}$ to approach the \"target end-effector position $\\vec{P_{\\mathrm{goal}}}$\", making it possible to find solutions even for complex robot arm configurations.\n",
    "\n",
    "![](../images/RobotArm/robot_arm_jacobian_inverse_kinemarics/inverse_kinematics_jacobian_overview_en.png)\n",
    "\n",
    "<br>\n",
    "\n",
    "In this process, it is necessary to relate the \"small change in end-effector position $\\Delta \\vec{P}$\" to the \"small change in joint angles $\\Delta \\vec{Q}$\", and this is where the \"Jacobian matrix $J$\" comes into play.\n",
    "$$\n",
    "\\Delta \\vec{P} = J \\Delta \\vec{Q}\n",
    "$$\n",
    "![](../images/RobotArm/robot_arm_jacobian_inverse_kinemarics/delta_q_delta_p_jacpbian_en.png)\n",
    "\n",
    "<br>\n",
    "\n",
    "The Jacobian matrix is defined as the partial derivatives of the forward kinematics equations with respect to each joint angle.\n",
    "$$\n",
    "J = \\begin{bmatrix}\n",
    "\\frac{\\partial x}{\\partial \\theta_1} & \\frac{\\partial x}{\\partial \\theta_2} & \\cdots & \\frac{\\partial x}{\\partial \\theta_n} \\\\\n",
    "\\frac{\\partial y}{\\partial \\theta_1} & \\frac{\\partial y}{\\partial \\theta_2} & \\cdots & \\frac{\\partial y}{\\partial \\theta_n} \\\\\n",
    "\\frac{\\partial z}{\\partial \\theta_1} & \\frac{\\partial z}{\\partial \\theta_2} & \\cdots & \\frac{\\partial z}{\\partial \\theta_n} \\\\\n",
    "\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "<br>\n",
    "\n",
    "For the 2-axis robot arm in this case, the forward kinematics equations are\n",
    "$$\n",
    "x = l_1 \\cos(\\theta_1) + l_2 \\cos(\\theta_1 + \\theta_2) \\\\\n",
    "y = l_1 \\sin(\\theta_1) + l_2 \\sin(\\theta_1 + \\theta_2) \\\\\n",
    "$$\n",
    "Therefore, the Jacobian matrix is as follows:\n",
    "$$\n",
    "J = \\begin{bmatrix}\n",
    "\\frac{\\partial x}{\\partial \\theta_1} & \\frac{\\partial x}{\\partial \\theta_2} \\\\\n",
    "\\frac{\\partial y}{\\partial \\theta_1} & \\frac{\\partial y}{\\partial \\theta_2} \\\\\n",
    "\\end{bmatrix}\n",
    "= \\begin{bmatrix}\n",
    "-l_1 \\sin(\\theta_1) - l_2 \\sin(\\theta_1 + \\theta_2) & -l_2 \\sin(\\theta_1 + \\theta_2) \\\\\n",
    "l_1 \\cos(\\theta_1) + l_2 \\cos(\\theta_1 + \\theta_2) & l_2 \\cos(\\theta_1 + \\theta_2) \\\\\n",
    "\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "---\n",
    "\n",
    "- $l_1$: Length of link 1\n",
    "- $l_2$: Length of link 2\n",
    "- $\\theta_1$: Angle of joint 1\n",
    "- $\\theta_2$: Angle of joint 2\n",
    "\n",
    "---\n",
    "\n",
    "<br>\n",
    "\n",
    "Now that we have $J$, to solve\n",
    "$$\\Delta \\vec{P} = J \\Delta \\vec{Q} (revisited)$$\n",
    "we need to find either $\\Delta \\vec{P}$ or $\\Delta \\vec{Q}$.  \n",
    "$\\Delta \\vec{P}$ can be easily found as the difference between the \"target end-effector position $\\vec{P_{\\mathrm{goal}}}$\" and the \"current end-effector position $\\vec{P_{\\mathrm{current}}}$\", as shown below.\n",
    "($P_{\\mathrm{param}}$ is a parameter that determines how much to change the end-effector position.)\n",
    "$$\n",
    "\\Delta \\vec{P} = P_{\\mathrm{param}}(\\vec{P_{\\mathrm{goal}}} - \\vec{P_{\\mathrm{current}}})\n",
    "$$\n",
    "![](../images/RobotArm/robot_arm_jacobian_inverse_kinemarics/delta_p_en.png)\n",
    "* Here, $P_{\\mathrm{param}}$ is treated as a scalar value, but it can also be set as $P_{\\mathrm{param}} = \\begin{bmatrix} P_{\\mathrm{param_x}} & 0 \\\\ 0 & P_{\\mathrm{param_y}} \\end{bmatrix}$ with different values for the x and y components.\n",
    "\n",
    "<br>\n",
    "\n",
    "With the \"small change in end-effector position $\\Delta \\vec{P}$\" and the \"Jacobian matrix $J$\" determined, we can find the \"small change in joint angles $\\Delta \\vec{Q}$\" through the following transformation. (Here, $J^+$ represents the pseudo-inverse of the Jacobian matrix $J$.)\n",
    "$$\n",
    "\\Delta \\vec{P} = J \\Delta \\vec{Q} \\\\\n",
    "\\Delta \\vec{Q} = J^+ \\Delta \\vec{P}\n",
    "$$\n",
    "\n",
    "<br>\n",
    "\n",
    "Using the above equations, we can gradually move the joint angles $\\vec{Q}$ until the \"current end-effector position $\\vec{P_{\\mathrm{current}}}$\" converges to the \"target end-effector position $\\vec{P_{\\mathrm{goal}}}$\".\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "import math\n",
    "def make_jacobian_matrix(link1_length, theta1, link2_length, theta2):\n",
    "    \"\"\"\n",
    "    Calculate the Jacobian matrix of a 2-link arm in a 2D plane\n",
    "    J = [[∂x/∂θ1, ∂x/∂θ2],\n",
    "         [∂y/∂θ1, ∂y/∂θ2]]\n",
    "\n",
    "    Parameters\n",
    "    ----------\n",
    "    link1_length : float\n",
    "        Length of link 1\n",
    "    link2_length : float\n",
    "        Length of link 2\n",
    "    theta1 : float\n",
    "        Rotation angle of link 1 (rad)\n",
    "    theta2 : float\n",
    "        Rotation angle of link 2 (rad)\n",
    "\n",
    "    Returns\n",
    "    -------\n",
    "    J : numpy.ndarray\n",
    "        Jacobian matrix of a 2-link arm in a 2D plane\n",
    "    \"\"\"\n",
    "\n",
    "    J = np.array([[-link1_length*math.sin(theta1) - link2_length*math.sin(theta1+theta2), -link2_length*math.sin(theta1+theta2)],\n",
    "                  [ link1_length*math.cos(theta1) + link2_length*math.cos(theta1+theta2),  link2_length*math.cos(theta1+theta2)]])\n",
    "    return J\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## <a id='toc5_3_'></a>[Defining the Pseudo-Inverse Matrix](#toc0_)\n",
    "\n",
    "As mentioned above, when calculating $\\Delta \\vec{Q}$, the pseudo-inverse of the Jacobian matrix $J$, denoted as $J^+$, is required. Here, we will define a function to compute the pseudo-inverse matrix."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "def make_inverse_matrix(mat):\n",
    "     \"\"\"\n",
    "     Calculate the inverse of a 2x2 matrix (used when calculating the pseudo-inverse matrix)\n",
    "     (Although you can use numpy's linalg.inv function, here we implement it manually to demonstrate the algorithm)\n",
    "\n",
    "     Parameters\n",
    "     ----------\n",
    "     mat: numpy.ndarray\n",
    "           2x2 matrix\n",
    "\n",
    "     Returns\n",
    "     -------\n",
    "     inverse_mat : numpy.ndarray\n",
    "                      Inverse of the 2x2 matrix\n",
    "     \"\"\"\n",
    "     inverse_mat = 1/(mat[0,0]*mat[1,1] - mat[0,1]*mat[1,0]) * np.array([[mat[1,1], -mat[0,1]],\n",
    "                                                                                          [-mat[1,0], mat[0,0]]])\n",
    "     return inverse_mat\n",
    "\n",
    "def make_trans_matrix(mat):\n",
    "     \"\"\"\n",
    "     Calculate the transpose of a 2x2 matrix (used when calculating the pseudo-inverse matrix)\n",
    "     (Although you can use numpy's transpose function, here we implement it manually to demonstrate the algorithm)\n",
    "\n",
    "     Parameters\n",
    "     ----------\n",
    "     mat: numpy.ndarray\n",
    "           2x2 matrix\n",
    "\n",
    "     Returns\n",
    "     -------\n",
    "     trans_mat : numpy.ndarray\n",
    "                    Transpose of the 2x2 matrix\n",
    "     \"\"\"\n",
    "     mat_minus =  np.array([[mat[0,0], mat[1,0]],\n",
    "                                 [mat[0,1], mat[1,1]]])\n",
    "     return mat_minus\n",
    "\n",
    "def make_pseudo_inverse_matrix(mat):\n",
    "     \"\"\"\n",
    "     Calculate the pseudo-inverse matrix\n",
    "     (Although you can use numpy's linalg.pinv function, here we implement it manually to demonstrate the algorithm)\n",
    "\n",
    "     Parameters\n",
    "     ----------\n",
    "     mat: numpy.ndarray\n",
    "           Matrix\n",
    "\n",
    "     Returns\n",
    "     -------\n",
    "     pseudo_inverse_mat : numpy.ndarray\n",
    "                               Pseudo-inverse matrix\n",
    "     \"\"\"\n",
    "     trans_mat = make_trans_matrix(mat)\n",
    "     pseudo_inverse_mat = make_inverse_matrix(trans_mat @ mat) @ trans_mat\n",
    "     return pseudo_inverse_mat"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# <a id='toc6_'></a>[Running the Simulation](#toc0_)\n",
    "\n",
    "Summarizing the explanation so far, the steps for inverse kinematics using numerical methods with the Jacobian matrix are as follows:\n",
    "1. Set the \"target end-effector position $\\vec{P_{\\mathrm{goal}}}$\".\n",
    "2. Calculate the \"small change in end-effector position $\\Delta \\vec{P}$\" from the \"current end-effector position $\\vec{P_{\\mathrm{current}}}$\" towards the \"target end-effector position $\\vec{P_{\\mathrm{goal}}}$\".\n",
    "3. Compute the \"Jacobian matrix $J$\".\n",
    "4. Use the pseudo-inverse of the \"Jacobian matrix $J$\", denoted as $J^+$, to find the \"small change in joint angles $\\Delta \\vec{Q}$\".\n",
    "5. Move the \"current joint angles $\\vec{Q_{\\mathrm{current}}}$\" by the \"small change in joint angles $\\Delta \\vec{Q}$\" to get the \"updated joint angles $\\vec{Q_{\\mathrm{new}}}$\".\n",
    "6. Based on the \"updated joint angles $\\vec{Q_{\\mathrm{new}}}$\", use forward kinematics to find the \"updated end-effector position $\\vec{P_{\\mathrm{new}}}$\".\n",
    "7. Update the \"current end-effector position $\\vec{P_{\\mathrm{current}}}$\" to the \"updated end-effector position $\\vec{P_{\\mathrm{new}}}$\" and the \"current joint angles $\\vec{Q_{\\mathrm{current}}}$\" to the \"updated joint angles $\\vec{Q_{\\mathrm{new}}}$\".\n",
    "8. Repeat steps 2-7 until the \"current end-effector position $\\vec{P_{\\mathrm{current}}}$\" converges to the \"target end-effector position $\\vec{P_{\\mathrm{goal}}}$\".\n",
    "\n",
    "By running the following code, inverse kinematics will be executed according to the above steps."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "---\n",
    "\n",
    "Note: For clarity, the movement of the robot arm is visualized in pybullet. However, by commenting out lines 92-94 in the code below, you can omit the visualization part and perform only the inverse kinematics calculations.\n",
    "\n",
    "Relevant Code Section:\n",
    "```python\n",
    "pybullet.resetJointState(arm_id, LINK1_JOINT_IDX, Q_current[0][0]) # Update joint angle\n",
    "pybullet.resetJointState(arm_id, LINK2_JOINT_IDX, Q_current[1][0]) # Update joint angle\n",
    "time.sleep(time_step)\n",
    "```\n",
    "\n",
    "---\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[Final Result]                                                                                   , 0.79)        \n",
      "Number of loops: 955th, Target end-effector position P_goal: (0.00, 0.80), Current end-effector position P_current: (0.01, 0.79)\n"
     ]
    }
   ],
   "source": [
    "import time\n",
    "import sys\n",
    "\n",
    "# Add words to spell checker dictionary\n",
    "# spell-checker: ignore urdf pybullet\n",
    "\n",
    "# Length of the robot's links\n",
    "LINK1_LENGTH = 0.5   # Length of link1 (z-direction length of link1 in \"simple2d_arm.urdf\")\n",
    "LINK2_LENGTH = 0.55  # Length of link2 (z-direction length of link2+force_sensor_link in \"simple2d_arm.urdf\")\n",
    "\n",
    "# Indices of the robot's joints\n",
    "LINK1_JOINT_IDX = 0\n",
    "LINK2_JOINT_IDX = 1\n",
    "\n",
    "# Initial settings (changing this part will change the results) ========================\n",
    "# Maximum number of iterations\n",
    "max_loop_num = 1000\n",
    "\n",
    "# Initial angles of each link (be careful not to set to a singular posture)\n",
    "theta1_deg = 0.0\n",
    "theta2_deg = -1.0\n",
    "\n",
    "# [Step 1] Target end-effector position (be careful not to specify an unreachable position)\n",
    "xe_goal = 0.0\n",
    "ye_goal = 0.8\n",
    "P_goal = np.array([[xe_goal],\n",
    "                    [ye_goal]]) # (Initial) target end-effector position P_goal (do not change this part)\n",
    "\n",
    "# Constant that determines how much the end-effector position P moves in each loop (the larger the value, the faster it converges but becomes unstable; the smaller the value, the slower it converges but becomes stable)\n",
    "P_delta_param = 0.005\n",
    "\n",
    "# Threshold to determine if the target end-effector position is reached (how close P_current and P_goal need to be to be considered as reached)\n",
    "goal_dis = 0.01\n",
    "# ==========================================================================\n",
    "\n",
    "# Convert joint angles to radians\n",
    "link1_angle_rad  = np.deg2rad(theta1_deg) \n",
    "link2_angle_rad  = np.deg2rad(theta2_deg)\n",
    "\n",
    "# Set initial joint angles\n",
    "Q_current = np.array([[link1_angle_rad],\n",
    "                      [link2_angle_rad]]) # Current joint angles Q_current\n",
    "\n",
    "# Set the robot's joint angles to the initial joint angles\n",
    "pybullet.resetJointState(arm_id, LINK1_JOINT_IDX, Q_current[0][0]) # Update joint angle\n",
    "pybullet.resetJointState(arm_id, LINK2_JOINT_IDX, Q_current[1][0]) # Update joint angle\n",
    "\n",
    "# Calculate the initial end-effector position P_current using forward kinematics with homogeneous transformation matrices\n",
    "H12 = Hz(Q_current[0][0]) @ Hp(LINK1_LENGTH, 0) # H12: link \"1\" coordinate system -> link \"2\" coordinate system\n",
    "H2e = Hz(Q_current[1][0]) @ Hp(LINK2_LENGTH, 0) # H2e: link \"2\" coordinate system -> \"e\"nd effector coordinate system\n",
    "H1e = H12@H2e # H1e: link \"1\" coordinate system -> \"e\"nd effector coordinate system\n",
    "x1, y1 = 0, 0 # Origin of link1 coordinate system\n",
    "oe = H1e@np.array([x1, y1, 1])\n",
    "xe, ye = oe[0], oe[1]\n",
    "P_current = np.array([[xe],\n",
    "                      [ye]]) # Current end-effector position P_current\n",
    "\n",
    "# Perform inverse kinematics using numerical methods with the Jacobian matrix\n",
    "for i in range(max_loop_num):\n",
    "    # Vector from current end-effector position P_current to target end-effector position P_goal\n",
    "    P_current_to_P_goal = P_goal - P_current\n",
    "\n",
    "    # [Step 2]: Small amount ΔP in the direction from current end-effector position P_current to target end-effector position P_goal\n",
    "    P_delta = P_current_to_P_goal * P_delta_param\n",
    "\n",
    "    # [Step 3]: Calculate the Jacobian matrix\n",
    "    jacobian = make_jacobian_matrix(LINK1_LENGTH, Q_current[0][0], LINK2_LENGTH, Q_current[1][0]) # Jacobian matrix J\n",
    "    \n",
    "    # Calculate the pseudo-inverse of the Jacobian matrix\n",
    "    jacobian_inverse = make_pseudo_inverse_matrix(jacobian) \n",
    "\n",
    "    # [Step 4]: Calculate the small angles ΔQ required for each joint to move by ΔP\n",
    "    Q_delta = jacobian_inverse @ P_delta\n",
    "\n",
    "    # [Step 5]: Move each joint by ΔQ\n",
    "    Q_new = Q_current + Q_delta\n",
    "    Q_new = Q_new % (2*math.pi) # Ensure joint angles are within the range of 0 to 2π\n",
    "\n",
    "    # [Step 6]: Calculate the end-effector position P_new using the updated joint angles Q_new (forward kinematics)\n",
    "    H12 = Hz(Q_new[0][0]) @ Hp(LINK1_LENGTH, 0) # T12: link \"1\" coordinate system -> link \"2\" coordinate system\n",
    "    H2e = Hz(Q_new[1][0]) @ Hp(LINK2_LENGTH, 0) # T2e: link \"2\" coordinate system -> \"e\"nd effector coordinate system\n",
    "    H1e = H12@H2e # T1e: link \"1\" coordinate system -> \"e\"nd effector coordinate system\n",
    "    x1, y1 = 0, 0 # Origin of link1 coordinate system\n",
    "    oe = H1e@np.array([x1, y1, 1])\n",
    "    xe, ye = oe[0], oe[1]\n",
    "    P_new = np.array([[xe],\n",
    "                      [ye]]) # Updated end-effector position P_new\n",
    "    sys.stdout.write(\"\\rTarget end-effector position P_goal: ({:.2f}, {:.2f}), Current end-effector position P_current: ({:.2f}, {:.2f})       \".format(P_goal[0][0], P_goal[1][0], P_current[0][0], P_current[1][0]))\n",
    "\n",
    "    # [Step 7]: Update current joint angles Q_current and current end-effector position P_current\n",
    "    Q_current = Q_new\n",
    "    P_current = P_new\n",
    "\n",
    "    # Set the robot's joint angles to the current joint angles Q_current\n",
    "    pybullet.resetJointState(arm_id, LINK1_JOINT_IDX, Q_current[0][0]) # Update joint angle\n",
    "    pybullet.resetJointState(arm_id, LINK2_JOINT_IDX, Q_current[1][0]) # Update joint angle\n",
    "    time.sleep(time_step)\n",
    "\n",
    "    # [Step 8]: Terminate if the target end-effector position P_goal is reached\n",
    "    if (P_goal[0][0]-P_current[0][0])**2  + (P_goal[1][0]-P_current[1][0])**2 <  goal_dis**2: # Use the formula for the distance between two points (compare squared values to avoid the heavy sqrt function)\n",
    "        break\n",
    "\n",
    "# If the number of iterations equals the maximum number of loops, terminate as no solution was found\n",
    "if i == max_loop_num-1:\n",
    "    print(\"\\rCould not calculate the position (possible singularity or unreachable coordinates)                \")\n",
    "else:\n",
    "    print(\"\\r[Final Result]                                                                                   \")\n",
    "    print(\"Number of loops: {}th, Target end-effector position P_goal: ({:.2f}, {:.2f}), Current end-effector position P_current: ({:.2f}, {:.2f})\".format(i+1, P_goal[0][0], P_goal[1][0], P_current[0][0], P_current[1][0]))"
   ]
  }
 ],
 "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
}