{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Inverse Kinematics Simulation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "from scipy.optimize import fsolve\n",
    "import matplotlib.pyplot as plt\n",
    "import math\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Polar coordinate functions"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "def cartesian_to_polar(x, y):\n",
    "    r = np.sqrt(x**2 + y**2)\n",
    "    theta = np.arctan2(y, x)\n",
    "    return r, theta\n",
    "\n",
    "def polar_to_cartesian(r, theta):\n",
    "    x = r * np.cos(theta)\n",
    "    y = r * np.sin(theta)\n",
    "    return x, y"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Calculate end-joint values from xyz position"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 53,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[-243.619306486927, -123.2097721836616, 140.34764917140853, -107.13787698774695, -90.0, 90.0]\n"
     ]
    }
   ],
   "source": [
    "def normalize_degree(theta):\n",
    "    # Normalizes degree theta from -1.5pi to 1.5pi\n",
    "    multiplier = 1.5\n",
    "    normalized_theta = theta % (math.pi * multiplier)\n",
    "    \n",
    "    # Maintain the negative sign if the original angle is negative\n",
    "    if theta < 0:\n",
    "        normalized_theta -= math.pi * multiplier\n",
    "\n",
    "    # Return angle\n",
    "    return normalized_theta\n",
    "\n",
    "def get_joints_from_xyz_rel(x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_guess = (math.pi/2, math.pi/2, 0)):\n",
    "    # Get limbs and offsets\n",
    "    offset_x, offset_y, offset_z = (0, 0, 0.14)     # Tool offset\n",
    "    l_bs, l1, l2, l3, l_wt = (0.1333, .425, .39225, .1267, .0997)    # Limb lengths\n",
    "    #l3=0.15\n",
    "    \n",
    "    # Calculate base angle and r relative to shoulder joint\n",
    "    def calculate_theta(x, y, a):\n",
    "        # Calculate if we need the + or - in our equations\n",
    "        if (x>a and y>=0) or (x>-a and y<0):\n",
    "            flip = 1\n",
    "        elif (x<a and y>=0) or (x<-a and y<0):\n",
    "            flip = -1\n",
    "        else: \n",
    "            # Critical section (x=a, or x=-a). Infinite slope\n",
    "            # Return 0 or 180 depending on sign\n",
    "            return math.atan2(y, 0) - math.pi/2\n",
    "        \n",
    "        # Calculate tangent line y = mx + b\n",
    "        if abs(a) != abs(x): # If there is no division by 0\n",
    "            m = (x*y + math.sqrt(x*x*y*y-(x*x-a*a)*(y*y-a*a)))/(x*x-a*a)\n",
    "        else: # Deal with edge case when x^2=a^2\n",
    "            m = flip*(-a*a+y*y)/(a*y-flip*abs(a*y))\n",
    "        b = flip * a * math.sqrt(1+m*m)\n",
    "\n",
    "        # Calculate equivalent tangent point on circle\n",
    "        cx = (-flip*m*b)/(1+m*m)\n",
    "        cy = m*cx + flip*b\n",
    "\n",
    "        # Calculate base angle, make angle negative if flip=1\n",
    "        theta = math.atan2(cy, cx) + (-math.pi if flip==1 else 0)\n",
    "\n",
    "        return theta \n",
    "    \n",
    "    base_theta = calculate_theta(x, y, l_bs)\n",
    "    cx, cy = l_bs*math.cos(base_theta), l_bs*math.sin(base_theta)\n",
    "    r = math.sqrt((x-cx)**2 + (y-cy)**2) \n",
    "\n",
    "\n",
    "    # Formulas to find out joint positions for (r, z)\n",
    "    def inv_kin_r_z(p):\n",
    "        a, b, c = p            \n",
    "\n",
    "        return (l1*math.cos(a) + l2*math.cos(a-b) + l3*math.cos(a-b-c) - r, # r\n",
    "                l1*math.sin(a) + l2*math.sin(a-b) - l3*math.sin(a-b-c) - (l3*math.sin(a-b-c)) - (z + offset_z),  # z\n",
    "                a-b-c) # wrist angle\n",
    "\n",
    "\n",
    "    # Normalize angles\n",
    "    base, shoulder, elbow, wrist1 = [normalize_degree(deg) for deg in [base_theta, *fsolve(inv_kin_r_z, initial_guess)]]\n",
    "\n",
    "    # Return result\n",
    "    return base, shoulder, elbow, wrist1, ry, rz\n",
    "\n",
    "def get_joints_from_xyz_abs(x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2):\n",
    "    joints = get_joints_from_xyz_rel(x, y, z, rx, ry, rz)\n",
    "\n",
    "    # Joint offsets\n",
    "    # Base, Shoulder, Elbow, Wrist\n",
    "    inverse = [1, -1, 1, 1, 1, 1]\n",
    "    offsets = [-math.pi/2, 0, 0, -math.pi/2, 0, 0]\n",
    "\n",
    "    # Return adjusted joint positions\n",
    "    return [o+j*i for j, o, i in zip(joints, offsets, inverse)]\n",
    "\n",
    "# print([math.degrees(deg) for deg in get_joints_from_xyz_rel(0.3, 0.3, 0.3)])\n",
    "print([math.degrees(deg) for deg in get_joints_from_xyz_abs(0, -0.3, 0.1)])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 58,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "flip\n",
      "m: -0.9031146536634159\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "47.914345304219715"
      ]
     },
     "execution_count": 58,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "def calculate_theta(x, y, a):\n",
    "        # Calculate if we need the + or - in our equations\n",
    "        if (x>a and y>=0) or (x>-a and y<0):\n",
    "            print('no flip')\n",
    "            flip = 1\n",
    "        elif (x<a and y>=0) or (x<-a and y<0):\n",
    "            print('flip')\n",
    "            flip = -1\n",
    "        else: \n",
    "            print('critical')\n",
    "            # Critical section (x=a, or x=-a). Infinite slope\n",
    "            # Return 0 or 180 depending on sign\n",
    "            return math.degrees(math.atan2(y, 0)-math.pi/2)\n",
    "        \n",
    "        # Calculate tangent line y = mx + b\n",
    "        if abs(a) != abs(x): # If there is no division by 0\n",
    "            m = (x*y + math.sqrt(x*x*y*y-(x*x-a*a)*(y*y-a*a)))/(x*x-a*a)\n",
    "        else: # Deal with edge case when x^2=a^2\n",
    "            m = flip*(-a*a+y*y)/(a*y-flip*abs(a*y))\n",
    "        \n",
    "        print('m:',m)\n",
    "        b = flip * a * math.sqrt(1+m*m)\n",
    "\n",
    "        # Calculate equivalent tangent point on circle\n",
    "        cx = (-flip*m*b)/(1+m*m)\n",
    "        cy = m*cx + flip*b\n",
    "\n",
    "        # Calculate base angle\n",
    "        theta = math.atan2(cy, cx) + (-math.pi if flip==1 else 0)\n",
    "\n",
    "        return math.degrees(theta)\n",
    "\n",
    "calculate_theta(-0.1333,0.3,0.1333)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Simulate arm and joint angles"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Target position (x,y,z): 0.3 0.3 0.3\n",
      "R:  0.4243\n",
      "Angles (base, shoulder, elbow, wrist): [-26.688, 105.6932, 101.1834, 4.5098]\n",
      "Robot Angles: [-116.688, -105.6932, 101.1834, -85.4902, -90.0, 90.0]\n",
      "elbow (x,y): -0.114 0.407\n",
      "wrist (x,y): 0.244 0.435\n",
      "tool  (x,y): 0.394 0.435\n"
     ]
    },
    {
     "data": {
      "image/png": "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",
      "text/plain": [
       "<Figure size 640x480 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "def draw_arm(x, y, z):\n",
    "\n",
    "    # Get joint angles\n",
    "    l1, l2, l3 = (.422864, .359041, .15)\n",
    "    offset_x, offset_y, offset_z = (0.145, 0, 0.195)\n",
    "    r, theta = cartesian_to_polar(x, y)\n",
    "    base, shoulder, elbow, wrist, _, _ = get_joints_from_xyz_rel(x, y, z)\n",
    "\n",
    "    # Print angles\n",
    "    print('Target position (x,y,z):', x, y, z)\n",
    "    print('R: ', round(math.sqrt(x**2+y**2),4))\n",
    "    print('Angles (base, shoulder, elbow, wrist):', [round(math.degrees(i), 4) for i in [base, shoulder, elbow, wrist]])\n",
    "    print('Robot Angles:', [round(math.degrees(i), 4) for i in get_joints_from_xyz_abs(x, y, z)])\n",
    "\n",
    "    # Calculate each joint's endpoint position\n",
    "    x1, y1 = polar_to_cartesian(l1, shoulder)\n",
    "    x2, y2 = polar_to_cartesian(l2, shoulder-elbow)\n",
    "    x2 += x1\n",
    "    y2 += y1\n",
    "    x3, y3 = polar_to_cartesian(l3, shoulder-elbow-wrist)\n",
    "    x3 += x2\n",
    "    y3 += y2 \n",
    "    \n",
    "    tx = x3\n",
    "    ty = y3 - offset_z\n",
    "\n",
    "    # Print each joint's endpoint position\n",
    "    print('elbow (x,y):', round(x1,3), round(y1,3))\n",
    "    print('wrist (x,y):', round(x2,3), round(y2,3))\n",
    "    print('tool  (x,y):', round(x3,3), round(y3,3))\n",
    "\n",
    "    # Draw limbs\n",
    "    plt.plot([0, x1], [0, y1], color='cyan', linewidth=7)\n",
    "    plt.plot([x1, x2], [y1, y2], color='orange', linewidth=7)\n",
    "    plt.plot([x2, x2+l3], [y2, y2], color='red', linewidth=7)\n",
    "    plt.plot()\n",
    "\n",
    "    # Draw toolpoint\n",
    "    plt.plot([x3, tx], [y3, ty], color='black', linewidth=7)\n",
    "\n",
    "    # Display angles\n",
    "    plt.text(0, 0.02, f'{round(math.degrees(shoulder), 2)}°')\n",
    "    plt.text(x1, y1+0.02, f'{round(math.degrees(elbow), 2)}°')\n",
    "    plt.text(x2, y2+0.02, f'{round(math.degrees(wrist), 2)}°')\n",
    "\n",
    "    # Display r arrow\n",
    "    plt.annotate(f'', xy=(0, 0), xycoords='data', xytext=(x3, 0), textcoords='data', arrowprops={'arrowstyle': '<->'})\n",
    "    plt.annotate(f'r={round(r,4)}', xy=(x2/2, 0.01), xycoords='data', xytext=(x2/2, 0), textcoords='offset points')\n",
    "\n",
    "    # Display z arrow\n",
    "    plt.annotate(f'', xy=(x3, 0), xycoords='data', xytext=(tx, ty), textcoords='data', arrowprops={'arrowstyle': '<->'})\n",
    "    plt.annotate(f'z={round(ty,4)}', xy=(tx+0.01, ty/2), xycoords='data', xytext=(x3/2, 0), textcoords='offset points')\n",
    "   \n",
    "    # Display plot\n",
    "    ax = plt.subplot(111)\n",
    "    ax.spines[['right', 'top']].set_visible(False)\n",
    "    plt.axis('equal')\n",
    "    plt.show()\n",
    "\n",
    "draw_arm(0.3, 0.3, 0.3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "draw_arm(0.3, 0.3, 0.3)\n",
    "draw_arm(-0.3, -0.3, 0.7)\n",
    "draw_arm(-0.3, 0.4, 0.2)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Interactive Arm"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from ipywidgets import interact, FloatSlider\n",
    "\n",
    "# Interactive slider for z coordinate\n",
    "interact(draw_arm,  x=FloatSlider(min=0, max=1, step=0.01, value=0.3),\n",
    "                    y=FloatSlider(min=0, max=1, step=0.01, value=0.3),\n",
    "                    z=FloatSlider(min=0, max=1, step=0.01, value=0.3))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 66,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Angles: [153.31198894476785, 105.69315886537287, 101.18338602047919, 4.509772844893658, -90.0, 0.0]\n",
      "(cx, cy): (-0.11909893611698993, 0.0598693027836566)\n",
      "-0.06746836046451549 0.16257881075200548\n",
      "-0.24309496877200426 -0.18679793543865197\n",
      "-0.29999999999998095 -0.29999999999996196\n"
     ]
    },
    {
     "data": {
      "image/png": "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",
      "text/plain": [
       "<Figure size 640x480 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Target position (x,y,z): -0.3 -0.3 0.3\n",
      "R:  0.4243\n",
      "Angles (base, shoulder, elbow, wrist): [153.312, 105.6932, 101.1834, 4.5098]\n",
      "Robot Angles: [63.312, -105.6932, 101.1834, -85.4902, -90.0, 90.0]\n",
      "elbow (x,y): -0.114 0.407\n",
      "wrist (x,y): 0.244 0.435\n",
      "tool  (x,y): 0.394 0.435\n"
     ]
    },
    {
     "data": {
      "image/png": "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",
      "text/plain": [
       "<Figure size 640x480 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "\n",
    "def display_arm(x, y, z):\n",
    "    # Get joint and position information\n",
    "    angles = get_joints_from_xyz_rel(x, y, z)\n",
    "    print('Angles:', [math.degrees(angle) for angle in angles])\n",
    "\n",
    "    offset_x, offset_y, offset_z = (0, 0, 0.14)     # Tool offset\n",
    "    l_bs, l1, l2, l3, l_wt = (0.1333, .425, .39225, .1267, .0997)    # Limb lengths\n",
    "    cx, cy = l_bs*math.cos(angles[0]), l_bs*math.sin(angles[0]) # Base tangent point\n",
    "    line_angle = math.pi/2+angles[0]\n",
    "    print(f'(cx, cy): ({cx}, {cy})')\n",
    "\n",
    "\n",
    "    # Plot coordinate system\n",
    "    fig = plt.figure()\n",
    "    ax = fig.add_subplot(1, 1, 1)\n",
    "    ax.spines['left'].set_position('center')\n",
    "    ax.spines['bottom'].set_position('center')\n",
    "    ax.spines['right'].set_color('none')\n",
    "    ax.spines['top'].set_color('none')\n",
    "    ax.set_yticklabels([])\n",
    "    ax.set_xticklabels([])\n",
    "    ax.set_xticks([])\n",
    "    ax.set_yticks([])\n",
    "\n",
    "    # Target point\n",
    "    # plt.plot(x, y, 'go')\n",
    "\n",
    "    # Circle\n",
    "    circle = plt.Circle((0, 0), l_bs, color='b', fill=False, linewidth=1)\n",
    "    plt.plot([0, cx], [0, cy], color='b', linewidth=1)\n",
    "    ax.add_patch(circle)\n",
    "\n",
    "    # Draw limbs\n",
    "    # Shoulder\n",
    "    x1, y1 = polar_to_cartesian(l1*math.cos(angles[1]), line_angle)\n",
    "    x1, y1 = cx+x1, cy+y1\n",
    "    print(x1, y1)\n",
    "    plt.plot([cx, x1], [cy, y1], color='cyan', linewidth=3)\n",
    "\n",
    "    # Elbow\n",
    "    x2, y2 = polar_to_cartesian(l2*math.cos(angles[1]-angles[2]), line_angle)\n",
    "    x2 += x1\n",
    "    y2 += y1\n",
    "    print(x2, y2)\n",
    "    plt.plot([x1, x2], [y1, y2], color='orange', linewidth=2)\n",
    "\n",
    "    # Wrist\n",
    "    x3, y3 = polar_to_cartesian(l3*math.cos(angles[1]-angles[2]-angles[3]), line_angle)\n",
    "    x3 += x2\n",
    "    y3 += y2 \n",
    "    print(x3, y3)\n",
    "    plt.plot([x2, x3], [y2, y3], color='red', linewidth=2)\n",
    "\n",
    "    # Display angle\n",
    "    plt.text(0.01, -0.01-cy/abs(cy+0.00001)*0.02, f'{round(math.degrees(angles[0]), 2)}°', fontsize=7)\n",
    "\n",
    "    # Display x arrow\n",
    "    sign = x3/abs(x3)\n",
    "    plt.annotate(f'', xy=(0, y3), xycoords='data', xytext=(x3, y3), textcoords='data', arrowprops={'arrowstyle': '<->'})\n",
    "    plt.annotate(f'x={round(x3,3)}', xy=(-0.1-sign*0.13, y3-0.015), xycoords='data', xytext=(x2/2, 0), textcoords='offset points')\n",
    "\n",
    "    # Display y arrow\n",
    "    sign = y3/abs(y3)\n",
    "    plt.annotate(f'', xy=(x3, 0), xycoords='data', xytext=(x3, y3), textcoords='data', arrowprops={'arrowstyle': '<->'})\n",
    "    plt.annotate(f'y={round(y3,3)}', xy=((x3-0.1), -0.015-sign*0.03), xycoords='data', xytext=(x3/2, 0), textcoords='offset points')\n",
    "\n",
    "\n",
    "    # Set axis limits and labels\n",
    "    axis_limit = math.hypot(x, y)+.1\n",
    "    plt.axis('square')\n",
    "    plt.xlim(-axis_limit, axis_limit)\n",
    "    plt.ylim(-axis_limit, axis_limit)\n",
    "\n",
    "    # Adjust the position of axis labels\n",
    "    plt.xlabel('+x', horizontalalignment='right', x=1.05)\n",
    "    plt.ylabel('+y', verticalalignment='top', rotation=0, y=1.05)\n",
    "    plt.show()\n",
    "\n",
    "    draw_arm(x,y,z)\n",
    "\n",
    "display_arm(-0.3,-0.3,0.3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}