RobotArm/robot_arm_eye_in_hand_estimate_obj_pose_en.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Table of contents**<a id='toc0_'></a>    \n",
    "- [Robot Arm eye in hand estimate obj pose](#toc1_)    \n",
    "- [Starting pybullet](#toc2_)    \n",
    "- [Initial Setup for pybullet](#toc3_)    \n",
    "- [Generating the Robot Arm](#toc4_)    \n",
    "- [Generating Colored Objects](#toc5_)    \n",
    "- [Function Definitions](#toc6_)    \n",
    "- [Camera Settings](#toc7_)    \n",
    "- [Setting the Initial Pose of the Arm](#toc8_)    \n",
    "- [Estimating the Position of the Colored Object in the Camera Coordinate System](#toc9_)    \n",
    "- [Estimating the Position of the Colored Object in the World Coordinate System](#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>[Robot Arm eye in hand estimate obj pose](#toc0_)\n",
    "\n",
    "In this notebook, we will generate a 6-axis robot arm and explain how to estimate the position of an object of a specified color using the \"camera at the end of the arm\".\n",
    "\n",
    "(For a manual summarizing the functions available in pybullet, 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": "stderr",
     "output_type": "stream",
     "text": [
      "pybullet build time: Nov 28 2023 23:45:17\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "startThreads creating 1 threads.\n",
      "starting thread 0\n",
      "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 paths 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 time elapsed per step\n",
    "\n",
    "# Load the floor\n",
    "plane_id = pybullet.loadURDF(\"plane.urdf\")\n",
    "\n",
    "# Set the camera position and other parameters in GUI mode\n",
    "camera_distance = 3.5\n",
    "camera_yaw = 180.0 # deg\n",
    "camera_pitch = -40 # deg\n",
    "camera_target_position = [0, 0.5, 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_)"
   ]
  },
  {
   "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.0]  # Set the initial position (x, y, z)\n",
    "arm_start_orientation = pybullet.getQuaternionFromEuler([0, 0, 0])  # Set the initial orientation (roll, pitch, yaw)\n",
    "arm_id = pybullet.loadURDF(\"../urdf/simple6d_arm_with_gripper.urdf\", arm_start_pos, arm_start_orientation, useFixedBase=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# <a id='toc5_'></a>[Generating Colored Objects](#toc0_)\n",
    "\n",
    "We will generate colored objects that we want to detect with the camera."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "# You can change this to get different results (make sure it fits within the field of view of the camera at the end of the arm) ####\n",
    "color_box_pos = [-0.5, 0.2, 0.05] # Set the initial position (x, y, z) of the colored object\n",
    "#################################################################################\n",
    "\n",
    "color_box_id = pybullet.loadURDF(\"../urdf/simple_box.urdf\", color_box_pos, pybullet.getQuaternionFromEuler([0.0, 0.0, 0.0]), globalScaling=0.1, useFixedBase=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# <a id='toc6_'></a>[Function Definitions](#toc0_)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "import cv2\n",
    "import numpy as np\n",
    "\n",
    "def detect_color_obj_pose(target_rgb, rgb_img, depth_img):\n",
    "    \"\"\"\n",
    "    Function to obtain the center position, depth, and orientation of the first detected colored object\n",
    "    \n",
    "    Parameters\n",
    "    ----------\n",
    "    target_rgb : list\n",
    "        RGB of the color to be detected\n",
    "    rgb_img : numpy.ndarray\n",
    "        Camera image (RGB)\n",
    "    depth_img : numpy.ndarray\n",
    "        Camera image (Depth)\n",
    "\n",
    "    Returns\n",
    "    -------\n",
    "    obj_pose : numpy.ndarray\n",
    "        Position and orientation of the colored object (x, y, z, roll, pitch, yaw)\n",
    "    \"\"\"\n",
    "\n",
    "    # Convert the camera image to HSV format\n",
    "    hsv_img = cv2.cvtColor(rgb_img, cv2.COLOR_RGB2HSV)\n",
    "\n",
    "    # Convert RGB to HSV\n",
    "    target_hsv = cv2.cvtColor(np.uint8([[target_rgb]]), cv2.COLOR_RGB2HSV)[0][0]\n",
    "\n",
    "    # Specify the range of the color to be detected\n",
    "    lower = np.array([target_hsv[0]-10, 50, 50])\n",
    "    upper = np.array([target_hsv[0]+10, 255, 255])\n",
    "\n",
    "    # Extract only the specified color\n",
    "    mask = cv2.inRange(hsv_img, lower, upper)\n",
    "\n",
    "    # Extract contours\n",
    "    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)\n",
    "\n",
    "    # Get the contour with the largest area\n",
    "    max_area = 0\n",
    "    max_area_contour = None\n",
    "    for contour in contours:\n",
    "        area = cv2.contourArea(contour)\n",
    "        if area > max_area:\n",
    "            max_area = area\n",
    "            max_area_contour = contour\n",
    "\n",
    "    # If no contour is found\n",
    "    if max_area_contour is None:\n",
    "        return None\n",
    "    \n",
    "    # Get the center position of the contour\n",
    "    M = cv2.moments(max_area_contour)\n",
    "    cx = int(M['m10']/M['m00'])\n",
    "    cy = int(M['m01']/M['m00'])\n",
    "\n",
    "    # Get the depth at the center position of the contour\n",
    "    depth = depth_img[cy, cx]\n",
    "\n",
    "    pos = [cx, cy, depth]\n",
    "    \n",
    "    return pos\n",
    "\n",
    "\n",
    "\n",
    "def Rx(theta):\n",
    "    \"\"\"\n",
    "    Calculate the rotation matrix around the x-axis\n",
    "\n",
    "    Parameters\n",
    "    ----------\n",
    "    theta : float\n",
    "        Rotation angle [rad]\n",
    "\n",
    "    Returns\n",
    "    -------\n",
    "    numpy.ndarray\n",
    "        Rotation matrix around the x-axis\n",
    "    \"\"\"\n",
    "    return np.array([[1, 0, 0],\n",
    "                     [0, np.cos(theta), -np.sin(theta)],\n",
    "                     [0, np.sin(theta), np.cos(theta)]])\n",
    "\n",
    "def Ry(theta):\n",
    "    \"\"\"\n",
    "    Calculate the rotation matrix around the y-axis\n",
    "\n",
    "    Parameters\n",
    "    ----------\n",
    "    theta : float\n",
    "        Rotation angle [rad]\n",
    "\n",
    "    Returns\n",
    "    -------\n",
    "    numpy.ndarray\n",
    "        Rotation matrix around the y-axis\n",
    "    \"\"\"\n",
    "    return np.array([[np.cos(theta), 0, np.sin(theta)],\n",
    "                     [0, 1, 0],\n",
    "                     [-np.sin(theta), 0, np.cos(theta)]])\n",
    "\n",
    "def Rz(theta):\n",
    "    \"\"\"\n",
    "    Calculate the rotation matrix around the z-axis\n",
    "\n",
    "    Parameters\n",
    "    ----------\n",
    "    theta : float\n",
    "        Rotation angle [rad]\n",
    "\n",
    "    Returns\n",
    "    -------\n",
    "    numpy.ndarray\n",
    "        Rotation matrix around the z-axis\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 Hx(theta):\n",
    "    \"\"\"\n",
    "    Calculate the homogeneous transformation matrix around the x-axis\n",
    "\n",
    "    Parameters\n",
    "    ----------\n",
    "    theta : float\n",
    "        Rotation angle [rad]\n",
    "\n",
    "    Returns\n",
    "    -------\n",
    "    numpy.ndarray\n",
    "        Homogeneous transformation matrix around the x-axis\n",
    "    \"\"\"\n",
    "    return np.array([[1, 0, 0, 0],\n",
    "                     [0, np.cos(theta), -np.sin(theta), 0],\n",
    "                     [0, np.sin(theta), np.cos(theta), 0],\n",
    "                     [0, 0, 0, 1]])\n",
    "\n",
    "def Hy(theta):\n",
    "    \"\"\"\n",
    "    Calculate the homogeneous transformation matrix around the y-axis\n",
    "\n",
    "    Parameters\n",
    "    ----------\n",
    "    theta : float\n",
    "        Rotation angle [rad]\n",
    "\n",
    "    Returns\n",
    "    -------\n",
    "    numpy.ndarray\n",
    "        Homogeneous transformation matrix around the y-axis\n",
    "    \"\"\"\n",
    "    return np.array([[np.cos(theta), 0, np.sin(theta), 0],\n",
    "                     [0, 1, 0, 0],\n",
    "                     [-np.sin(theta), 0, np.cos(theta), 0],\n",
    "                     [0, 0, 0, 1]])\n",
    "\n",
    "def Hz(theta):\n",
    "    \"\"\"\n",
    "    Calculate the homogeneous transformation matrix around the z-axis\n",
    "\n",
    "    Parameters\n",
    "    ----------\n",
    "    theta : float\n",
    "        Rotation angle [rad]\n",
    "\n",
    "    Returns\n",
    "    -------\n",
    "    numpy.ndarray\n",
    "        Homogeneous transformation matrix around the z-axis\n",
    "    \"\"\"\n",
    "    return np.array([[np.cos(theta), -np.sin(theta), 0, 0],\n",
    "                     [np.sin(theta), np.cos(theta), 0, 0],\n",
    "                     [0, 0, 1, 0],\n",
    "                     [0, 0, 0, 1]])\n",
    "\n",
    "def Hp(x, y, z):\n",
    "    \"\"\"\n",
    "    Calculate the homogeneous transformation matrix for translation\n",
    "\n",
    "    Parameters\n",
    "    ----------\n",
    "    x : float\n",
    "        Translation in the x direction\n",
    "    y : float\n",
    "        Translation in the y direction\n",
    "    z : float\n",
    "        Translation in the z direction\n",
    "\n",
    "    Returns\n",
    "    -------\n",
    "    numpy.ndarray\n",
    "        Homogeneous transformation matrix for translation\n",
    "    \"\"\"\n",
    "    return np.array([[1, 0, 0, x],\n",
    "                     [0, 1, 0, y],\n",
    "                     [0, 0, 1, z],\n",
    "                     [0, 0, 0, 1]])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# <a id='toc7_'></a>[Camera Settings](#toc0_)\n",
    "Define the camera settings (focal length, intrinsic parameters, etc.)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Camera settings\n",
    "fov = 60 # In Pybullet, specify the vertical field of view (fov)\n",
    "image_width = 224 # Image width\n",
    "image_height = 224 # Image height\n",
    "aspect = image_width / image_height # Aspect ratio\n",
    "near = 0.05 # Minimum distance of the camera\n",
    "far = 5 # Maximum distance of the camera\n",
    "projection_matrix = pybullet.computeProjectionMatrixFOV(fov, aspect, near, far) # Compute the projection matrix\n",
    "\n",
    "# Calculate the focal length\n",
    "fov_rad = np.deg2rad(fov)\n",
    "f = (image_height / 2) / np.tan(fov_rad / 2)\n",
    "\n",
    "# Camera intrinsic parameters\n",
    "camera_matrix = np.array([[f, 0, image_width//2],\n",
    "                         [0, f, image_height//2],\n",
    "                         [0, 0, 1]], dtype=np.float32)\n",
    "\n",
    "# Distortion coefficients (assuming no distortion here)\n",
    "dist_coeffs = np.array([0.0, 0.0, 0.0, 0.0, 0.0], dtype=np.float32)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# <a id='toc8_'></a>[Setting the Initial Pose of the Arm](#toc0_)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Indices of the robot's joints\n",
    "LINK1_JOINT_IDX = 0\n",
    "LINK2_JOINT_IDX = 1\n",
    "LINK3_JOINT_IDX = 2\n",
    "LINK4_JOINT_IDX = 3\n",
    "LINK5_JOINT_IDX = 4\n",
    "LINK6_JOINT_IDX = 5\n",
    "CAMERA_IDX = 6\n",
    "CAMERA_TARGET_IDX = 7\n",
    "\n",
    "# Lengths of each link\n",
    "ARM_ORIGIN_X_WORLD = 0.0 # x-coordinate of the arm's origin in the world coordinate system\n",
    "ARM_ORIGIN_Y_WORLD = 0.0 # y-coordinate of the arm's origin in the world coordinate system\n",
    "ARM_ORIGIN_Z_WORLD = 0.8 # z-coordinate of the arm's origin in the world coordinate system (length of base_link in \"simple6d_arm_with_gripper.urdf\")\n",
    "LINK1_LENGTH = 0.3 # Length of link1 (length of link1 in \"simple6d_arm_with_gripper.urdf\")\n",
    "LINK2_LENGTH = 0.5 # Length of link2 (length of link2 in \"simple6d_arm_with_gripper.urdf\")\n",
    "LINK3_LENGTH = 0.5 # Length of link3 (length of link3 in \"simple6d_arm_with_gripper.urdf\")\n",
    "LINK4_LENGTH = 0.1 # Length of link4 (length of link4 in \"simple6d_arm_with_gripper.urdf\")\n",
    "LINK5_LENGTH = 0.1 # Length of link5 (length of link5 in \"simple6d_arm_with_gripper.urdf\")\n",
    "LINK6_LENGTH = 0.15 # Length of link6 (length of link6 in \"simple6d_arm_with_gripper.urdf\")\n",
    "CAMERA_Z_SIZE = 0.01 # Length of the camera in the z-direction\n",
    "CAMERA_X_OFFSET = 0.08 # x-direction offset from the origin of the link6 coordinate system to the camera\n",
    "\n",
    "# You can change this to get different results (make sure the colorBox fits within the field of view of the camera at the end of the arm) ####\n",
    "# Angles of each joint\n",
    "joint1_angle = -15.0\n",
    "joint2_angle = -80.0\n",
    "joint3_angle = -100.0\n",
    "joint4_angle = 0.0\n",
    "joint5_angle = 0.0\n",
    "joint6_angle = 10.0\n",
    "########################################################################################\n",
    "\n",
    "# Convert the angles of each joint to radians\n",
    "q1 = np.deg2rad(joint1_angle)\n",
    "q2 = np.deg2rad(joint2_angle)\n",
    "q3 = np.deg2rad(joint3_angle)\n",
    "q4 = np.deg2rad(joint4_angle)\n",
    "q5 = np.deg2rad(joint5_angle)\n",
    "q6 = np.deg2rad(joint6_angle)\n",
    "\n",
    "# Set the angles of each joint\n",
    "pybullet.resetJointState(arm_id, LINK1_JOINT_IDX, q1)\n",
    "pybullet.resetJointState(arm_id, LINK2_JOINT_IDX, q2)\n",
    "pybullet.resetJointState(arm_id, LINK3_JOINT_IDX, q3)\n",
    "pybullet.resetJointState(arm_id, LINK4_JOINT_IDX, q4)\n",
    "pybullet.resetJointState(arm_id, LINK5_JOINT_IDX, q5)\n",
    "pybullet.resetJointState(arm_id, LINK6_JOINT_IDX, q6)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# <a id='toc9_'></a>[Estimating the Position of the Colored Object in the Camera Coordinate System](#toc0_)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Get the position of the camera\n",
    "camera_link_pose = pybullet.getLinkState(arm_id, CAMERA_IDX)[0] # Position of the camera link at the end of the arm\n",
    "camera_target_link_pose = pybullet.getLinkState(arm_id, CAMERA_TARGET_IDX)[0] # Position of a virtual link set just in front of the camera link\n",
    "camera_link_orientation = pybullet.getEulerFromQuaternion(pybullet.getLinkState(arm_id, CAMERA_IDX)[1]) # Orientation of the camera link at the end of the arm\n",
    "\n",
    "# Set the up vector of the camera according to the pose of the robot arm\n",
    "RW1 = Rz(theta=0) @ Ry(theta=0) @ Rx(theta=0) # Rotation matrix from world coordinate system to link1 coordinate system\n",
    "R12 = Rz(theta=q1) # Rotation matrix from link1 coordinate system to link2 coordinate system\n",
    "R23 = Ry(theta=q2) # Rotation matrix from link2 coordinate system to link3 coordinate system\n",
    "R34 = Ry(theta=q3) # Rotation matrix from link3 coordinate system to link4 coordinate system\n",
    "R45 = Rz(theta=q4) # Rotation matrix from link4 coordinate system to link5 coordinate system\n",
    "R56 = Ry(theta=q5) # Rotation matrix from link5 coordinate system to link6 coordinate system\n",
    "R6C = Rz(theta=q6 + np.deg2rad(-90.0)) # Rotation matrix from link6 coordinate system to camera coordinate system (rotating the camera by -90 degrees makes the captured image oriented correctly)\n",
    "R = RW1 @ R12 @ R23 @ R34 @ R45 @ R56 @ R6C # Rotation matrix from link1 coordinate system to camera coordinate system\n",
    "camera_up_vector = np.array([0, -1, 0]) # Default up vector of the camera\n",
    "rotate_camera_up_vector = R @ camera_up_vector # Up vector of the camera according to the pose of the robot arm\n",
    "\n",
    "# Compute the view matrix of the camera\n",
    "view_matrix = pybullet.computeViewMatrix(cameraEyePosition=[camera_link_pose[0], camera_link_pose[1], camera_link_pose[2]], cameraTargetPosition=[camera_target_link_pose[0], camera_target_link_pose[1], camera_target_link_pose[2]], cameraUpVector=[rotate_camera_up_vector[0], rotate_camera_up_vector[1], rotate_camera_up_vector[2]])\n",
    "\n",
    "# Get the camera image\n",
    "_, _, rgb_img, depth_img, _ = pybullet.getCameraImage(\n",
    "    width=image_width,\n",
    "    height=image_height,\n",
    "    viewMatrix=view_matrix,\n",
    "    projectionMatrix=projection_matrix,\n",
    "    renderer=pybullet.ER_BULLET_HARDWARE_OPENGL\n",
    ")\n",
    "\n",
    "# Get the position of the object of the specified color\n",
    "detect_color_rgb = [0, 0, 255] # Specify the color to be detected (blue in this case)\n",
    "pos = detect_color_obj_pose(detect_color_rgb, rgb_img, depth_img)\n",
    "pixel_x = pos[0] # x-coordinate (position on the image)\n",
    "pixel_y = pos[1] # y-coordinate (position on the image)\n",
    "normal_z = pos[2] # z-coordinate (depth normalized to 0-1)\n",
    "z = far * near / (far - (far - near) * normal_z) # Convert depth normalized to 0-1 to distance [m]\n",
    "x = (pixel_x - image_width // 2) * z / f # Convert pixelX to distance [m]\n",
    "y = (pixel_y - image_height // 2) * z / f # Convert pixelY to distance [m]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# <a id='toc10_'></a>[Estimating the Position of the Colored Object in the World Coordinate System](#toc0_)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "2"
      ]
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Calculate the position of the camera in the world coordinate system\n",
    "TW1 = Hz(theta=0) @ Hy(theta=0) @ Hx(theta=0) @ Hp(x=ARM_ORIGIN_X_WORLD, y=ARM_ORIGIN_Y_WORLD, z=ARM_ORIGIN_Z_WORLD) # Homogeneous transformation matrix from world coordinate system to link1 coordinate system\n",
    "T12 = Hz(theta=q1) @ Hp(x=0, y=0, z=LINK1_LENGTH) # Transformation matrix from link1 coordinate system to link2 coordinate system\n",
    "T23 = Hy(theta=q2) @ Hp(x=0, y=0, z=LINK2_LENGTH) # Transformation matrix from link2 coordinate system to link3 coordinate system\n",
    "T34 = Hy(theta=q3) @ Hp(x=0, y=0, z=LINK3_LENGTH) # Transformation matrix from link3 coordinate system to link4 coordinate system\n",
    "T45 = Hz(theta=q4) @ Hp(x=0, y=0, z=LINK4_LENGTH) # Transformation matrix from link4 coordinate system to link5 coordinate system\n",
    "T56 = Hy(theta=q5) @ Hp(x=0, y=0, z=LINK5_LENGTH) # Transformation matrix from link5 coordinate system to link6 coordinate system\n",
    "T6C = Hz(theta=q6) @ Hp(x=CAMERA_X_OFFSET, y=0, z=LINK6_LENGTH - CAMERA_Z_SIZE / 2) # Transformation matrix from link6 coordinate system to camera coordinate system\n",
    "origin_pos = np.array([0, 0, 0, 1]) # Origin position\n",
    "camera_pos_world = TW1 @ T12 @ T23 @ T34 @ T45 @ T56 @ T6C @ origin_pos # Position of the camera coordinate system origin in the world coordinate system\n",
    "camera_pos_world = camera_pos_world[:3] # Convert the coordinates obtained by the homogeneous transformation matrix from [x, y, z, 1] to [x, y, z]\n",
    "color_obj_pos_camera = np.array([x, y, z]) # Center coordinates of the \"colored object\" in the camera coordinate system\n",
    "color_obj_pos_world = camera_pos_world + R @ color_obj_pos_camera # Position of the \"colored object\" in the world coordinate system\n",
    "\n",
    "# Draw a red line on the Pybullet screen from the \"camera position\" to the \"estimated position of the colored object\" in the world coordinate system\n",
    "pybullet.addUserDebugLine(camera_pos_world, color_obj_pos_world, [1, 0, 0], lineWidth=2)\n",
    "\n",
    "# Display the \"true position\" and \"estimated position\" of the box\n",
    "boxPos, _ = pybullet.getBasePositionAndOrientation(color_box_id)\n",
    "pybullet.addUserDebugText(f\"true box pose ({boxPos[0]:.3f}, {boxPos[1]:.3f}, {boxPos[2]:.3f})\", [2.0, 0.5, 0], textColorRGB=[1, 0, 0], textSize=1.3)\n",
    "pybullet.addUserDebugText(f\"eye in hand, estimate box pose ({color_obj_pos_world[0]:.3f}, {color_obj_pos_world[1]:.3f}, {color_obj_pos_world[2]:.3f})\", [2.0, 1.0, 0.0], textColorRGB=[1, 0, 0], textSize=1.3)"
   ]
  }
 ],
 "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
}