joint_angle_control.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# **Dynamixel 7-DOF robot arm (Samsung Princeternship)**\n",
    "\n",
    "Authors: Ekin Gurgen, Ainil Norazman, Miguel Opena\n",
    "\n",
    "This project took place from Jan 6, 2020 to Jan 10, 2020 at Samsung AI Center (New York, NY). Goal: To program a robotic arm to hook a ring. We took several approaches to this goal, and these are outlined in the coming cells. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Take a look at:\n",
    "# http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/sample_code/python_read_write_protocol_2_0/#python-protocol-20\n",
    "import dynamixel_sdk\n",
    "import time\n",
    "import math\n",
    "import warnings\n",
    "import numpy as np\n",
    "\n",
    "import realsense as rs\n",
    "import apriltag\n",
    "import cv2\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "import PIL.Image\n",
    "import IPython.display\n",
    "\n",
    "import dynamixel\n",
    "from dynamixel import JOINT_1, JOINT_2, JOINT_3, JOINT_4, JOINT_5, JOINT_6, JOINT_7 "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## **Primitives for arm control**\n",
    "These are the basic commands: initialize a control object, set the arm's position, and lock/unlock joints as desired. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "JOINT_ALL = [JOINT_1, JOINT_2, JOINT_3, JOINT_4, JOINT_5, JOINT_6, JOINT_7]\n",
    "JOINTS_UNLOCKED = [JOINT_1, JOINT_2, JOINT_3, JOINT_5]\n",
    "JOINTS_LOCKED = [JOINT_4, JOINT_6, JOINT_7]\n",
    "\n",
    "# JOINT_LIMITS = {1: [100,3900], 2: [1000,2738], 3: [484,3192], 5: [914, 3200]}\n",
    "JOINT_LIMITS = {1: [1,3500], 2: [1005,2733], 3: [489,3187], 5: [919, 3195]}"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Initializes controller object\n",
    "ctl = dynamixel.DynamixelController()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Default starting position of arm\n",
    "def home():\n",
    "    ctl.set_position(JOINT_1, 1)\n",
    "    ctl.set_position(JOINT_2, 2050)\n",
    "    ctl.set_position(JOINT_3, 1850)\n",
    "    ctl.set_position(JOINT_4, 3050)\n",
    "    ctl.set_position(JOINT_5, 2070)\n",
    "    ctl.set_position(JOINT_6, 1000)\n",
    "    ctl.set_position(JOINT_7, 1200)\n",
    "\n",
    "# Default starting position of arm (looks cooler)\n",
    "def homeCool():\n",
    "    ctl.set_position(JOINT_1, 1000)\n",
    "    ctl.set_position(JOINT_2, 1800)\n",
    "    ctl.set_position(JOINT_3, 500)\n",
    "    ctl.set_position(JOINT_4, 3050)\n",
    "    ctl.set_position(JOINT_5, 2400)\n",
    "    ctl.set_position(JOINT_6, 1050)\n",
    "    ctl.set_position(JOINT_7, 1200)\n",
    "\n",
    "home()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Get position of all joints\n",
    "for joint in JOINT_ALL: \n",
    "    print(ctl.get_position(joint)) "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Get position of specific joint\n",
    "print(ctl.get_position(JOINT_1))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Unlock joints listed in JOINTS_UNLOCKED\n",
    "for joint in JOINTS_UNLOCKED:\n",
    "    ctl.unlock(joint)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Unlock a specific joint\n",
    "ctl.unlock(JOINT_5)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Lock a specific joint\n",
    "ctl.lock(JOINT_1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Unlock all joints\n",
    "for joint in JOINT_ALL:\n",
    "    ctl.unlock(joint)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Lock all joints\n",
    "for joint in JOINT_ALL:\n",
    "    ctl.lock(joint)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Deletes the control object and unlocks all joints\n",
    "del ctl"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## **Control Strategy No.1:** Trajectory mimicry\n",
    "The arm replays a trajectory based on how a human user guides it. \n",
    "\n",
    "Currently, the movement is jagged and slow. We could add velocity control to make it smoother. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "''' Returns a dictionary of poses over time (user guides the arm) '''\n",
    "def trajectoryCopy(num_iterations=50, delay=0.1): \n",
    "    traj = {}\n",
    "    for i in range(num_iterations):\n",
    "        print(f\"Iteration {i}\")\n",
    "        trajNow = {}\n",
    "        for joint in JOINTS_UNLOCKED:\n",
    "            pos = ctl.get_position(joint)\n",
    "            trajNow[joint] = pos\n",
    "        traj[i] = trajNow\n",
    "        time.sleep(delay)\n",
    "    return traj\n",
    "\n",
    "''' Iterates through a dictionary of poses and replays them with a slowdown factor '''\n",
    "def trajectoryMimic(num_iterations=50, delay=0.1, slowdown=4): \n",
    "    for i in range(num_iterations):\n",
    "        location = traj[i]\n",
    "        for joint in location:\n",
    "            ctl.set_position(joint, location[joint])\n",
    "        time.sleep(delay * slowdown)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Test code for trajectory mimicry\n",
    "# Note: unlock the arm to use this feature\n",
    "delay = 0.2\n",
    "num_iterations = 40\n",
    "traj = trajectoryCopy(num_iterations, delay)\n",
    "for i in range(num_iterations):\n",
    "    print(traj[i])\n",
    "trajectoryMimic(num_iterations, delay, slowdown=2)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## **Control Strategy No.2:** Inverse kinematics\n",
    "\n",
    "Implementing inverse kinematics for a 4-DOF model of the robotic arm. We are only using Joints 1, 2, 3 and 5; the other joints remain locked. \n",
    "\n",
    "To simplify things, we can treat the arm as a 3-DOF manipulator in the plane (Joints 2, 3, and 5). Then, we can have Joint 1 rotate this 3-DOF arm radially, so that it can rotate in 3D space.\n",
    "\n",
    "Suppose we have a 3-DOF model of a robotic arm with three segments. It is constrained to move in a plane. Let $(x,y)$ denote the end effector's position in 2D space. Let $\\phi$ denote the angle of said effector. We can describe the length of each segment as $(l_1,l_2,l_3)$ and the angle of each joint actuator as $(\\theta_1,\\theta_2,\\theta_3)$. Then, the direct kinematics of the system are\n",
    "\n",
    "\\begin{align}\n",
    "    x &= l_1 \\cos\\theta_1 + l_2 \\cos(\\theta_1 + \\theta_2) + l_3 \\cos(\\theta_1 + \\theta_2 + \\theta_3)\\\\\n",
    "    x &= l_1 \\sin\\theta_1 + l_2 \\sin(\\theta_1 + \\theta_2) + l_3 \\sin(\\theta_1 + \\theta_2 + \\theta_3)\\\\\n",
    "    \\phi &= \\theta_1 + \\theta_2 + \\theta_3\n",
    "\\end{align}\n",
    "\n",
    "For inverse kinematics, we start with $(x,y,\\theta)$ and arrive at $(\\theta_1,\\theta_2,\\theta_3)$. Due to the nature of the solution, we have to consider a parameter $\\sigma$. When we set $\\sigma=+1$, the robot moves into an ''elbow-up'' position. When we set $\\sigma=-1$, the robot moves into an ''elbow-down'' position. I do not want to type out all the equations. Refer to page 19-20 of the source below. \n",
    "\n",
    "Source: notes from [Vijay Kumar](https://www.seas.upenn.edu/~meam520/notes02/IntroRobotKinematics5.pdf)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "''' Variables for arm lengths '''\n",
    "h = 0.054 \n",
    "l1 = 0.170\n",
    "l2 = 0.115\n",
    "l3 = 0.23\n",
    "\n",
    "''' Converts Dynamixel motor position (0-4000) to radians (0-2pi) '''\n",
    "def toRad(position):\n",
    "    return (position-2000) * (math.pi/2000)\n",
    "\n",
    "''' Converts radians (0-2pi) to Dynamixel motor position (0-4000) '''\n",
    "def toPosition(angle):\n",
    "    return (angle*2000/math.pi) + 2000\n",
    "\n",
    "''' Executes forward kinematics for the Dynamixel robotic arm '''\n",
    "def forwardKinematics(theta1, theta2, theta3):\n",
    "    xcurrent = l1 * math.cos(theta1) + l2 * math.cos(theta1+theta2) + l3 * math.cos(theta1+theta2+theta3)            \n",
    "    ycurrent = l1 * math.sin(theta1) + l2 * math.sin(theta1+theta2) + l3 * math.sin(theta1+theta2+theta3)\n",
    "    return xcurrent, ycurrent\n",
    "\n",
    "''' Executes inverse kinematics for the Dynamixel robotic arm'''\n",
    "def inverseKinematics(xGoal, yGoal, phiGoal, sigma=1):\n",
    "    \n",
    "    x_prime = xGoal - l3*math.cos(phiGoal)\n",
    "    y_prime = yGoal - l3*math.sin(phiGoal)\n",
    "    A = x_prime**2 + y_prime**2\n",
    "    \n",
    "    gamma = math.atan2(-y_prime/math.sqrt(A), x_prime/math.sqrt(A))\n",
    "    \n",
    "    cosarg = -(A + l1**2 - l2**2)/(2*l1*math.sqrt(A))\n",
    "    if cosarg < -1: cosarg = -1\n",
    "    if cosarg > 1: cosarg = 1\n",
    "    theta1 = gamma + sigma*math.acos(cosarg)\n",
    "    theta1 = math.pi - theta1\n",
    "    theta2 = math.atan2((y_prime - l1*math.sin(theta1))/l2, (x_prime - l1*math.cos(theta1))/l2) - theta1\n",
    "    theta3 = phiGoal - theta1 - theta2\n",
    "\n",
    "    position1 = int(-toPosition(theta1 - math.pi/2) % 4000) # % 4000\n",
    "    position2 = int(toPosition(theta2) - 150) % 4000\n",
    "    position3 = int(toPosition(theta3)) % 4000\n",
    "    \n",
    "\n",
    "    if not JOINT_LIMITS[2][0] <= position1 <= JOINT_LIMITS[2][1]: \n",
    "        raise ValueError(f'Joint 2 position {position1} exceeds motor limits.')\n",
    "    if not JOINT_LIMITS[3][0] <= position2 <= JOINT_LIMITS[3][1]: \n",
    "        raise ValueError(f'Joint 3 position {position2} exceeds motor limits.')\n",
    "    if not JOINT_LIMITS[5][0] <= position3 <= JOINT_LIMITS[5][1]: \n",
    "        raise ValueError(f'Joint 5 position {position3} exceeds motor limits.')\n",
    "    \n",
    "    return theta1, theta2, theta3, position1, position2, position3"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Rotating the base joint\n",
    "\n",
    "If we model Joints 2, 3, and 5 as a planar 3-DOF manipulator, we can simply rotate Joint 1 to give the robot access to many more places in 3D space. Calculating where Joint 1 should rotate is a simple matter of trigonometry, once one accounts for the restricted domain of the arctan function. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "''' Rotates Joint 1 (base) of the robotic arm '''\n",
    "def getBasePosition(xGoal, yGoal, zGoal):\n",
    "    \n",
    "    offset = 8 * math.pi / 180 # angle offset to account for hook placement\n",
    "    \n",
    "    if xGoal == 0 and zGoal >= 0: \n",
    "        zeta = math.pi / 2\n",
    "    elif xGoal == 0 and zGoal < 0:\n",
    "        zeta = -math.pi / 2\n",
    "    else: zeta = math.atan(zGoal / xGoal)\n",
    "    \n",
    "    zeta += offset\n",
    "    \n",
    "    if xGoal < 0: zeta += math.pi\n",
    "        \n",
    "    # Converts angle to Dynamixel motor position\n",
    "    pos = int(toPosition(zeta) - 2000) % 4000\n",
    "    \n",
    "    if not JOINT_LIMITS[1][0] <= pos <= JOINT_LIMITS[1][1]: \n",
    "        raise ValueError(f'Joint 1 position {pos} exceeds motor limits.')\n",
    "    return zeta, pos"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Hooking the ring\n",
    "A small subroutine to hook a ring. We used a bit of trial-and-error to establish the overall motion (inch forward into the ring and then pull up), as well as some fine-tuned parameters. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "''' Code for hooking a ring '''\n",
    "def scoop(push_constant=100, pull_constant=800):\n",
    "    pos2 = ctl.get_position(JOINT_2)\n",
    "    pos3 = ctl.get_position(JOINT_3)\n",
    "    pos5 = ctl.get_position(JOINT_5)\n",
    "    \n",
    "    # Push forward into ring\n",
    "    for i in range(3):\n",
    "        ctl.set_position(JOINT_3, pos3 + (i+1)*push_constant)\n",
    "        ctl.set_position(JOINT_2, pos2 + (i+1)*push_constant)\n",
    "        ctl.set_position(JOINT_5, pos5 + (i+1)*push_constant // 2)\n",
    "        time.sleep(1)\n",
    "    \n",
    "    # Pulls ring upward and retracts arm\n",
    "    ctl.set_position(JOINT_5, pos5 + pull_constant)\n",
    "    time.sleep(1)\n",
    "    ctl.set_position(JOINT_2, pos2)\n",
    "    ctl.set_position(JOINT_3, pos3)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Full control routine (inverse kinematics only)\n",
    "Given a position in 3D XYZ coordinates, the robot can navigate to said position (as long as it is reachable) with centimeter accuracy. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "''' Code for executing the full arm routine '''\n",
    "def fullRoutine(goalPose, phiGoal, sigma=1, scoopParams=(70,500), doScoop=True):\n",
    "    # Retrieve coordinates\n",
    "    xGoal = goalPose[0]\n",
    "    yGoal = goalPose[1]\n",
    "    zGoal = goalPose[2]\n",
    "    \n",
    "    # Home robot to pre-defined start position\n",
    "    home()\n",
    "    time.sleep(1)\n",
    "\n",
    "    # Get data from inverse kinematics\n",
    "    flatDistance = math.sqrt(xGoal**2 + zGoal**2)\n",
    "    p1, p2, p3, position1, position2, position3 = inverseKinematics(flatDistance, yGoal, phiGoal, sigma)\n",
    "    zeta, position0 = getBasePosition(xGoal, yGoal, zGoal)\n",
    "\n",
    "    # Setting position of robot base from getBasePosition\n",
    "    ctl.set_position(JOINT_1, position0)\n",
    "    time.sleep(1)\n",
    "\n",
    "    # Setting position of robot arm from inverse kinematics\n",
    "    ctl.set_position(JOINT_2, position1)\n",
    "    ctl.set_position(JOINT_3, position2)\n",
    "    ctl.set_position(JOINT_5, position3)\n",
    "    time.sleep(1)\n",
    "    \n",
    "    # Setting position of robot arm from scooping subroutine\n",
    "    if doScoop: \n",
    "        scoop(scoopParams[0], scoopParams[1])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Miscellaneous testing code\n",
    "We used this code to test inverse kinematics, making sure that inverse and forward kinematics are inverse functions of each other. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "''' Code for testing the forward and inverse kinematics '''\n",
    "theta1 = 70*math.pi/180\n",
    "# theta2 < 0, theta3 > 0 <--> sigma = 1\n",
    "# theta2 > 0, theta3 < 0 <--> sigma = -1\n",
    "theta2 = -30*math.pi/180\n",
    "theta3 = 20*math.pi/180\n",
    "phiGoal = theta1 + theta2 + theta3\n",
    "print(theta1)\n",
    "print(theta2)\n",
    "print(theta3)\n",
    "\n",
    "xGoal, yGoal = forwardKinematics(theta1, theta2, theta3)\n",
    "print(xGoal, yGoal)\n",
    "p1, p2, p3, position1, position2, position3 = inverseKinematics(xGoal, yGoal, phiGoal)\n",
    "print(p1, p2, p3)\n",
    "print(p1 * 180 / math.pi, p2 * 180 / math.pi, p3 * 180 / math.pi)\n",
    "print(forwardKinematics(p1, p2, p3))\n",
    "\n",
    "print(position1)\n",
    "print(position2)\n",
    "print(position3)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## **Control Strategy No.3:** Vision (April tags)\n",
    "\n",
    "We attached an Intel Realsense D435 camera to the computer, mounting said camera on a tripod overlooking our test space. It locates the arm and the target using April tags. From these locations, we calculate the position in 3D space that the arm should navigate towards, then we use inverse kinematics to travel to the desired location. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Camera parameters w.r.t. camera image\n",
    "fx = 607.91638184\n",
    "fy = 609.396240230\n",
    "cx = 322.27319336\n",
    "cy = 240.48545837\n",
    "cam_params = [fx, fy, cx, cy]\n",
    "\n",
    "# Distortion parameters\n",
    "# Source: https://github.com/IntelRealSense/librealsense/issues/573\n",
    "coeffs = [-0.1494821, 0.01267229, -0.002873175, -0.002943313, -0.02257685]\n",
    "\n",
    "# Various offsets for arm\n",
    "ARM_HEIGHT = 0.385\n",
    "HOOK_LENGTH = 0.13\n",
    "RING_RADIUS = 0.07\n",
    "\n",
    "# Conversion (because America)\n",
    "INCH_TO_MM = 25.4\n",
    "INCH_TO_METERS = INCH_TO_MM / 1000\n",
    "\n",
    "# Size of the provided April tags\n",
    "tag_size = 33.4 / INCH_TO_MM"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "''' Function to initialize camera object that reads from Realsense '''\n",
    "def initCamera(): \n",
    "    serials = rs.RealSenseReal.connected_device_serial_numbers()\n",
    "\n",
    "    cam = rs.RealSenseReal(\n",
    "        serial_number=serials[0],\n",
    "        depth_sensor_preset=rs.RealSenseD400VisualPreset.DEFAULT,\n",
    "        temporal_filter_parameters=rs.TemporalFilterParameters(),\n",
    "        ignore_depth_scale_offset_calibration=True,\n",
    "    )\n",
    "    \n",
    "    return cam"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "''' Function to display images from camera '''\n",
    "def displayImage(image):\n",
    "    plt.imshow(image)\n",
    "    plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 321,
   "metadata": {},
   "outputs": [],
   "source": [
    "''' Function to display images from camera (uses IPython display) '''\n",
    "def showarray(a):\n",
    "    IPython.display.display(PIL.Image.fromarray(a[:, :, ::-1]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "''' Code for testing the full arm routine with VISION (April tags) '''\n",
    "''' Ingredients: Dynamixel 7-DOF robotic arm, April tag on top of arm, different April tag on top of tape roll '''\n",
    "def fullRoutineVisionApril(cam, phiGoal, sigma=1, scoopParams=(70,500), disp=False):\n",
    "    # Home robot to pre-defined start position\n",
    "    home()\n",
    "    time.sleep(2)\n",
    "    \n",
    "    # Capture image from camera object\n",
    "    image = cam.gray(norm=False)\n",
    "    if disp: displayImage(image)\n",
    "    # print(image)cam.gray(norm=False)\n",
    "    \n",
    "    # Detect the April tags in image\n",
    "    detector = apriltag.Detector()\n",
    "    detections, dimg = detector.detect(image, return_image=True)\n",
    "    result = detector.detect(image)\n",
    "\n",
    "    # check if tags are detected\n",
    "    if len(result) < 2: raise ValueError(f\"Error: missing tag. Recognized tag: {result[0].tag_id}\")\n",
    "    elif len(result) > 2: raise ValueError(\"too many tags in the image\")\n",
    "        \n",
    "    # Assumption: the arm has tag ID smaller than the object tag ID\n",
    "    arm = result[0]\n",
    "    # Refers to center of the arm tag w.r.t. camera image\n",
    "    arm_xPixel = int(round(arm.center[0]))\n",
    "    arm_yPixel = int(round(arm.center[1]))\n",
    "    \n",
    "    obj = result[1]\n",
    "    # Refers to center of the object tag w.r.t. camera image\n",
    "    obj_xPixel = int(round(obj.center[0]))\n",
    "    obj_yPixel = int(round(obj.center[1]))\n",
    "    \n",
    "    # Detect camera-frame poses of arm and object (rotation-translation matrices)\n",
    "    pose_arm, e0, e1 = detector.detection_pose(arm, cam_params, tag_size)\n",
    "    pose_obj, e2, e3 = detector.detection_pose(obj, cam_params, tag_size)\n",
    "    \n",
    "    # Convert poses to meters\n",
    "    arm_x = pose_arm[0][3] * INCH_TO_METERS\n",
    "    arm_y = pose_arm[1][3] * INCH_TO_METERS\n",
    "    arm_z = pose_arm[2][3] * INCH_TO_METERS\n",
    "\n",
    "    obj_x = pose_obj[0][3] * INCH_TO_METERS\n",
    "    obj_y = pose_obj[1][3] * INCH_TO_METERS\n",
    "    obj_z = pose_obj[2][3] * INCH_TO_METERS\n",
    "\n",
    "    # Gets camera-frame pose data as XYZ coordinates\n",
    "    arm_loc = np.array([arm_x, arm_y, arm_z])\n",
    "    obj_loc = np.array([obj_x, obj_y, obj_z])\n",
    "    \n",
    "    # Computes difference between arm and object locations\n",
    "    diff = obj_loc - arm_loc\n",
    "    diff = np.append(diff, [1])\n",
    "    \n",
    "    # Assumption: the April tag on the ARM lies flat with respect to the table surface\n",
    "    # Account for coordinate transformation: camera --> top of arm\n",
    "    rotation_matrix = np.array(pose_arm)\n",
    "    rotation_matrix[0][3] = 0\n",
    "    rotation_matrix[1][3] = 0\n",
    "    rotation_matrix[2][3] = 0\n",
    "    \n",
    "    # Uses formula from http://ogldev.atspace.co.uk/www/tutorial13/tutorial13.html\n",
    "    rot_inv = np.linalg.inv(rotation_matrix)\n",
    "    diff_real = np.matmul(rot_inv, diff)\n",
    "    \n",
    "    # Convert axes to our convention\n",
    "    diff_real_adjusted = np.array([diff_real[1], diff_real[2], diff_real[0]])\n",
    "    \n",
    "    # Account for vertical offset; ring radius\n",
    "    # Account for vector change: origin of arm --> center of ring\n",
    "    diff_real_adjusted[1] = ARM_HEIGHT - (diff_real_adjusted[1] + RING_RADIUS) + HOOK_LENGTH\n",
    "    diff_real_adjusted[0] = diff_real_adjusted[0] * 0.9\n",
    "    diff_real_adjusted[1] = diff_real_adjusted[1] * 0.8\n",
    "    diff_real_adjusted[2] = diff_real_adjusted[2] * 0.9\n",
    "    \n",
    "    fullRoutine(diff_real_adjusted, phiGoal, sigma=sigma, scoopParams=scoopParams)\n",
    "    \n",
    "    return diff_real_adjusted"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cam = initCamera()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "phiGoal = -5 * math.pi / 180 # try to make phi positive if possible\n",
    "fullRoutineVisionApril(cam, phiGoal, disp=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## **Control Strategy No.4**: Vision (color detection)\n",
    "\n",
    "We attached an Intel Realsense D435 camera to the computer, mounting said camera on a tripod overlooking our test space. It locates the arm using an April tag, but it uses color detection and a bounding box to locate a blue ring (roll of tape), which it targets and attempts to hook using our previous code on inverse kinematics."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Formula to deproject from 2D image space to 3D camera space\n",
    "x = (centerx - cx) / fx\n",
    "y = (centery - cy) / fy\n",
    "point = [pixelDepth * x, pixelDepth * y, pixelDepth]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "''' Code for testing the full arm routine with VISION (color) '''\n",
    "''' Ingredients: Dynamixel 7-DOF robotic arm, blue tape roll, April tag on top of arm '''\n",
    "def fullRoutineVisionColor(cam, phiGoal, sigma=1, scoopParams=(70,500), disp=False, lowerThreshold=250, ratio=3):\n",
    "    # Home robot to pre-defined start position\n",
    "    home()\n",
    "    time.sleep(2)\n",
    "\n",
    "    # Capture image from camera object\n",
    "    image = cam.gray(norm=False)\n",
    "    imageColor = cam.color(norm=False)\n",
    "    imageDepth = cam.depth(norm=False)\n",
    "    if disp: \n",
    "        displayImage(image)\n",
    "        displayImage(imageDepth)\n",
    "        # showarray(image)\n",
    "    # print(image)cam.gray(norm=False)\n",
    "\n",
    "    ''' Determine pose of robotic arm '''\n",
    "    # Detect the April tag in image (robotic arm only)\n",
    "    detector = apriltag.Detector()\n",
    "    detections, dimg = detector.detect(image, return_image=True)\n",
    "    result = detector.detect(image)\n",
    "\n",
    "    # check if tags are detected\n",
    "    if len(result) < 1: raise ValueError(f\"Error: missing tag on the robotic arm.\")\n",
    "    elif len(result) > 2: raise ValueError(\"too many tags in the image\")\n",
    "\n",
    "    # Assumption: the arm has tag ID smaller than the object tag ID\n",
    "    arm = result[0]\n",
    "    # Refers to center of the arm tag w.r.t. camera image\n",
    "    arm_xPixel = int(round(arm.center[0]))\n",
    "    arm_yPixel = int(round(arm.center[1]))\n",
    "\n",
    "    # Detect camera-frame poses of arm and object (rotation-translation matrices)\n",
    "    pose_arm, e0, e1 = detector.detection_pose(arm, cam_params, tag_size)\n",
    "\n",
    "    # Convert poses to meters\n",
    "    arm_x = pose_arm[0][3] * INCH_TO_METERS\n",
    "    arm_y = pose_arm[1][3] * INCH_TO_METERS\n",
    "    arm_z = pose_arm[2][3] * INCH_TO_METERS\n",
    "    \n",
    "    ''' Determine pose of ring '''\n",
    "    # Identify range of blues\n",
    "    lower = np.array([70, 0, 0], dtype=\"uint8\")\n",
    "    upper = np.array([255, 210, 50], dtype=\"uint8\")\n",
    "\n",
    "    # Apply a mask based on range of blues\n",
    "    mask = cv2.inRange(imageColor, lower, upper)\n",
    "    output = cv2.bitwise_and(imageColor, imageColor, mask=mask)\n",
    "\n",
    "    # Blur out some noise\n",
    "    output = cv2.medianBlur(output, 5)\n",
    "    \n",
    "    # Define the contours in the image to find the ring\n",
    "    edged = cv2.Canny(output, lowerThreshold, lowerThreshold * ratio)\n",
    "    contours, hierarchy = cv2.findContours(edged, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)\n",
    "    output = cv2.drawContours(output, contours, -1, (0, 255, 0), 3)\n",
    "    # if disp: print(contours)\n",
    "\n",
    "    # Produce a bounding box around the ring\n",
    "    maxCnt = None\n",
    "    maxBox = None\n",
    "    maxArea = 0\n",
    "    for cnt in contours: \n",
    "        rect = cv2.minAreaRect(cnt)\n",
    "        box = cv2.boxPoints(rect)\n",
    "        box = np.int0(box)\n",
    "        area = cv2.contourArea(cnt)\n",
    "        if area > maxArea:\n",
    "            maxCnt = cnt\n",
    "            maxArea = area\n",
    "            maxBox = box\n",
    "\n",
    "    output = cv2.drawContours(output, [maxBox], 0, (0, 0, 255), 2)\n",
    "    # print(maxBox)\n",
    "    if disp: showarray(output)\n",
    "        \n",
    "    # Source: https://docs.opencv.org/3.4/dd/d49/tutorial_py_contour_features.html\n",
    "    # centerx, centery refer to top of the bounding box\n",
    "    centerx = (maxBox[0][0] +maxBox[1][0] + maxBox[2][0] + maxBox[3][0]) // 4\n",
    "    centery = (maxBox[0][1] +maxBox[1][1] + maxBox[2][1] + maxBox[3][1]) // 4\n",
    "    pixelLoc = [centerx, centery]\n",
    "    pixelDepth = imageDepth[centery][centerx] / 1000\n",
    "    pixelDepth = pixelDepth * 1.01\n",
    "    \n",
    "    # Warn the user if pixelDepth is zero\n",
    "    if pixelDepth < 1e-2:\n",
    "        warnings.warn(\"Pixel depth registered as zero. Arm will not work.\")\n",
    "        \n",
    "    # Formula to deproject from 2D image space to 3D camera space\n",
    "    x = (centerx - cx) / fx\n",
    "    y = (centery - cy) / fy\n",
    "    point = [pixelDepth * x, pixelDepth * y, pixelDepth]\n",
    "    if disp: print(point)\n",
    "\n",
    "    ''' Consolidate arm and ring data into the same frame '''\n",
    "    # Gets camera-frame pose data as XYZ coordinates\n",
    "    arm_loc = np.array([arm_x, arm_y, arm_z])\n",
    "    obj_loc = point\n",
    "\n",
    "    # Computes difference between arm and object locations\n",
    "    diff = obj_loc - arm_loc\n",
    "    diff = np.append(diff, [1])\n",
    "\n",
    "    # Assumption: the April tag on the ARM lies flat with respect to the table surface\n",
    "    # Account for coordinate transformation: camera --> top of arm\n",
    "    rotation_matrix = np.array(pose_arm)\n",
    "    rotation_matrix[0][3] = 0\n",
    "    rotation_matrix[1][3] = 0\n",
    "    rotation_matrix[2][3] = 0\n",
    "\n",
    "    # Uses formula from http://ogldev.atspace.co.uk/www/tutorial13/tutorial13.html\n",
    "    rot_inv = np.linalg.inv(rotation_matrix)\n",
    "    diff_real = np.matmul(rot_inv, diff)\n",
    "\n",
    "    # Convert axes to our convention\n",
    "    diff_real_adjusted = np.array([diff_real[1], diff_real[2], diff_real[0]])\n",
    "\n",
    "    # Account for vertical offset; ring radius\n",
    "    # Account for vector change: origin of arm --> center of ring\n",
    "    diff_real_adjusted[1] = ARM_HEIGHT - diff_real_adjusted[1] + HOOK_LENGTH - 0.1\n",
    "    diff_real_adjusted[0] = diff_real_adjusted[0] * 0.9\n",
    "    diff_real_adjusted[1] = diff_real_adjusted[1] * 0.8\n",
    "    diff_real_adjusted[2] = diff_real_adjusted[2] * 0.9\n",
    "\n",
    "    fullRoutine(diff_real_adjusted, phiGoal, sigma=sigma, scoopParams=scoopParams, doScoop=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "home()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cam = initCamera()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "phiGoal = -10 * math.pi / 180 # try to make phi positive if possible\n",
    "fullRoutineVisionColor(cam, phiGoal, disp=True)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "dynamixel-arm-BmGiPNJM-py3.6",
   "language": "python",
   "name": "dynamixel-arm-bmgipnjm-py3.6"
  },
  "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.6.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}