Merge branch 'main' of https://git.myitr.org/Jukebox/jukebox-software

inv_kin_testing.ipynb

@@ -9,7 +9,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 1,
+   "execution_count": 2,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -28,7 +28,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 2,
+   "execution_count": 3,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -52,15 +52,15 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 157,
+   "execution_count": 75,
    "metadata": {},
    "outputs": [
     {
      "name": "stdout",
      "output_type": "stream",
      "text": [
-      "[45.0, 90.70951422006029, 110.71500355039022, -20.005489330329944]\n",
-      "[45.0, -90.70951422006029, 110.71500355039022, -110.00548933032994]\n"
+      "flip\n",
+      "[62.18186071534606, -126.63734509021032, 142.6959710408549, -106.0586259506446]\n"
      ]
     }
    ],
@@ -77,41 +77,126 @@
     "    # Return angle\n",
     "    return normalized_theta\n",
     "\n",
-    "def get_joints_from_xyz_rel(x, y, z, initial_guess = (math.pi/2, math.pi/2, 0), limbs=(.422864, .359041, .092124)):\n",
-    "    # Get polar coordinates of x,y pair\n",
-    "    r, theta = cartesian_to_polar(x, y)\n",
-    "    \n",
-    "    # Get length of each limb\n",
-    "    l1, l2, l3 = limbs\n",
+    "def get_joints_from_xyz_rel(x, y, z, rx=0, ry=0, rz=0, tool_offset=(0.145, 0, 0.14), initial_guess = (math.pi/2, math.pi/2, 0)):\n",
+    "    # Get limbs and offsets\n",
+    "    offset_x, offset_y, offset_z = tool_offset\n",
+    "    l_bs, l1, l2, l3 = (0.14, .422864, .359041, .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",
+    "            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.atan2(y, 0)\n",
+    "        \n",
+    "        # Calculate tangent line y = mx + b\n",
+    "        m = (x*y - math.sqrt(x*x*y*y-(x*x-a*a)*(y*y-a*a)))/(x*x-a*a)\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(l_bs)\n",
+    "    r = math.sqrt((x-cx)**2 + (y-cy)**2) - 0.1\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",
+    "        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) - z,  # z\n",
+    "                l1*math.sin(a) + l2*math.sin(a-b) - 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, wrist = [normalize_degree(deg) for deg in [theta, *fsolve(inv_kin_r_z, initial_guess)]]\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, wrist\n",
+    "    return base, shoulder, elbow, wrist1, ry, rz\n",
     "\n",
     "def get_joints_from_xyz_abs(x, y, z):\n",
     "    joints = get_joints_from_xyz_rel(x, y, z)\n",
     "\n",
     "    # Joint offsets\n",
     "    # Base, Shoulder, Elbow, Wrist\n",
-    "    inverse = [1, -1, 1, 1]\n",
-    "    offsets = [0, 0, 0, -math.pi/2]\n",
+    "    inverse = [1, -1, 1, 1, 1, 1]\n",
+    "    offsets = [-math.pi/2, 0, 0, -math.pi/2, -math.pi/2, 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.3, 0.3, 0.3)])"
+    "# 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": 64,
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "no flip\n"
+     ]
+    },
+    {
+     "data": {
+      "text/plain": [
+       "-126.86989764584402"
+      ]
+     },
+     "execution_count": 64,
+     "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.atan2(y, 0)\n",
+    "        \n",
+    "        # Calculate tangent line y = mx + b\n",
+    "        m = (x*y + math.sqrt(x*x*y*y-(x*x-a*a)*(y*y-a*a)))/(x*x-a*a)\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 theta\n",
+    "\n",
+    "calculate_theta(5,-5,1)"
    ]
   },
   {
@@ -123,24 +208,26 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 164,
+   "execution_count": 70,
    "metadata": {},
    "outputs": [
     {
      "name": "stdout",
      "output_type": "stream",
      "text": [
+      "no flip\n",
       "Target position (x,y,z): 0.3 0.3 0.3\n",
-      "Angles (base, shoulder, elbow, wrist): [45.0, 90.7095, 110.715, -20.0055]\n",
-      "Robot Angles: [45.0, -90.7095, 110.715, -110.0055]\n",
-      "elbow (x,y): -0.005 0.423\n",
-      "wrist (x,y): 0.332 0.3\n",
-      "tool  (x,y): 0.424 0.3\n"
+      "Angles (base, shoulder, elbow, wrist): [-25.7322, 113.4225, 127.6133, -14.1908]\n",
+      "no flip\n",
+      "Robot Angles: [-115.7322, -113.4225, 127.6133, -104.1908]\n",
+      "elbow (x,y): -0.168 0.388\n",
+      "wrist (x,y): 0.18 0.3\n",
+      "tool  (x,y): 0.33 0.3\n"
      ]
     },
     {
      "data": {
-      "image/png": "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 truncated
+      "image/png": "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 truncated
       "text/plain": [
        "<Figure size 640x480 with 1 Axes>"
       ]
@@ -153,7 +240,8 @@
     "def draw_arm(x, y, z):\n",
     "\n",
     "    # Get joint angles\n",
-    "    l1, l2, l3 = (.422864, .359041, .092124)\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",
@@ -169,7 +257,10 @@
     "    y2 += y1\n",
     "    x3, y3 = polar_to_cartesian(l3, shoulder-elbow-wrist)\n",
     "    x3 += x2\n",
-    "    y3 += y2\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",
@@ -180,6 +271,10 @@
     "    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",
@@ -191,8 +286,8 @@
     "    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=(x3, y3), textcoords='data', arrowprops={'arrowstyle': '<->'})\n",
-    "    plt.annotate(f'z={round(y2,4)}', xy=(x3+0.01, y2/2), xycoords='data', xytext=(x3/2, 0), textcoords='offset points')\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=(x3+0.01, y2/2), xycoords='data', xytext=(x3/2, 0), textcoords='offset points')\n",
     "   \n",
     "    # Display plot\n",
     "    ax = plt.subplot(111)\n",
@@ -205,7 +300,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 165,
+   "execution_count": 18,
    "metadata": {},
    "outputs": [
     {
@@ -213,16 +308,16 @@
      "output_type": "stream",
      "text": [
       "Target position (x,y,z): 0.3 0.3 0.3\n",
-      "Angles (base, shoulder, elbow, wrist): [45.0, 90.7095, 110.715, -20.0055]\n",
-      "Robot Angles: [45.0, -90.7095, 110.715, -110.0055]\n",
-      "elbow (x,y): -0.005 0.423\n",
-      "wrist (x,y): 0.332 0.3\n",
-      "tool  (x,y): 0.424 0.3\n"
+      "Angles (base, shoulder, elbow, wrist): [33.9862, 85.2162, 73.3858, 11.8304]\n",
+      "Robot Angles: [33.9862, -85.2162, 73.3858, -78.1696]\n",
+      "elbow (x,y): 0.035 0.421\n",
+      "wrist (x,y): 0.387 0.495\n",
+      "tool  (x,y): 0.537 0.495\n"
      ]
     },
     {
      "data": {
-      "image/png": "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 truncated
+      "image/png": "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 truncated
       "text/plain": [
        "<Figure size 640x480 with 1 Axes>"
       ]
@@ -235,16 +330,25 @@
      "output_type": "stream",
      "text": [
       "Target position (x,y,z): -0.3 -0.3 0.7\n",
-      "Angles (base, shoulder, elbow, wrist): [-135.0, 71.7346, 15.5104, 56.2242]\n",
-      "Robot Angles: [-135.0, -71.7346, 15.5104, -33.7758]\n",
-      "elbow (x,y): 0.133 0.402\n",
-      "wrist (x,y): 0.332 0.7\n",
-      "tool  (x,y): 0.424 0.7\n"
+      "Angles (base, shoulder, elbow, wrist): [-117.3239, 78.0693, 0.0255, 79.0945]\n",
+      "Robot Angles: [-117.3239, -78.0693, 0.0255, -10.9055]\n",
+      "elbow (x,y): 0.087 0.414\n",
+      "wrist (x,y): 0.162 0.765\n",
+      "tool  (x,y): 0.312 0.762\n"
+     ]
+    },
+    {
+     "name": "stderr",
+     "output_type": "stream",
+     "text": [
+      "c:\\Users\\nadee\\AppData\\Local\\Programs\\Python\\Python310\\lib\\site-packages\\scipy\\optimize\\_minpack_py.py:178: RuntimeWarning: The iteration is not making good progress, as measured by the \n",
+      "  improvement from the last ten iterations.\n",
+      "  warnings.warn(msg, RuntimeWarning)\n"
      ]
     },
     {
      "data": {
-      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAiMAAAGhCAYAAACzurT/AAAAOXRFWHRTb2Z0d2FyZQBNYXRwbG90bGliIHZlcnNpb24zLjUuMSwgaHR0cHM6Ly9tYXRwbG90bGliLm9yZy/YYfK9AAAACXBIWXMAAA9hAAAPYQGoP6dpAABEuklEQVR4nO3deVxWZf7/8ReLgAugpGyGkhtqKRQKUblUuJRTWU2ZX0tFs8axpoaZqWxRKxOnHH9+Jy3LXJpp+mo2LU6pZeQyJi5hlJpJrmh5o2iAooHC+f1B3UaCnBs43Nv7+XicxwyHzzn3574HmLfXda5zfAzDMBARERFxEl9nNyAiIiLeTWFEREREnEphRERERJxKYUREREScSmFEREREnEphRERERJxKYUREREScSmFEREREnEphRERERJxKYUREREScSmFERKSRTJkyBR8fnypb165dq9RkZWVx3XXX0bx5c0JCQujbty+nT5+u8ZwZGRn07t2b4OBgwsPDGTp0KLt27bJ///jx4zz44IPExcXRtGlT2rVrxx/+8AeKioqqnGfZsmV06dKFuLg4Pvjgg4Z94yK18Hd2AyIi3uTSSy/lk08+sX/t73/uz3BWVhaDBw9m4sSJvPjii/j7+/Pll1/i61vzvxvXrl3LhAkT6N27N2fPnuXxxx9n4MCBfP311zRv3pzvv/+e77//nhkzZtC9e3cOHDjA7373O77//nvefvttAEpLS5kwYQILFy7EMAzGjBnDwIEDCQgIsO6DEPkFH3d4UJ5hGJw4cYLg4GB8fHyc3Y6ISJ1MmTKF9957j5ycnGq/f+WVVzJgwACeffbZOr/G0aNHCQ8PZ+3atfTt27famqVLl3L33XdTUlKCv78/xcXF9OzZk88//xyA3r1789VXXxEcHFznPkQc4RbTNCdOnCA0NJQTJ044uxURkXr59ttviY6OpkOHDowYMYK8vDwAjhw5wqZNmwgPD+eqq64iIiKCfv36sX79eofO//P0S1hY2AVrQkJC7KMyISEhpKWlERUVRXR0NOPHj1cQkUblFiMjxcXFhIaG2n+BRETc0YoVKzh58iRxcXEcPnyYp59+mu+++47t27ezY8cOUlJSCAsLY8aMGSQkJPCPf/yDl156ie3bt9O5c+daz19RUcHNN99MYWFhjSGmoKCAxMRE7r77bp577rkq3ysqKsLX11dBRBqdwoiIiJMUFhbSvn17Zs6cSbdu3bj66quZOHEi06ZNs9f07NmTIUOGkJGRUev5xo8fz4oVK1i/fj0XX3zxed8vLi5mwIABhIWFsWzZMpo0adKg70ekrtximkZExBO1bNmSLl26sHv3bqKiogDo3r17lZpu3brZp3Iu5IEHHuCDDz5g9erV1QaREydOMHjwYIKDg3n33XcVRMSlKIyIiDjJyZMn2bNnD1FRUcTGxhIdHV1lWS5Abm4u7du3r/EchmHwwAMP8O677/Lpp59yySWXnFdTXFxsXx2zbNkygoKCGvy9iNSHwoiISCP585//zNq1a9m/fz8bNmzg1ltvxc/Pj+HDh+Pj48Nf/vIX/v73v/P222+ze/dunnrqKb755hvGjh1rP8f111/P7Nmz7V9PmDCBN954gzfffJPg4GBsNhs2m81+b5Kfg0hJSQnz58+nuLjYXlNeXt7on4FIdXSfERGRRnLo0CGGDx/OsWPHaNOmDddccw0bN26kTZs2ADz88MP8+OOP/PGPf+T48ePEx8ezatUqOnbsaD/Hnj17KCgosH/98ssvA9C/f/8qr7Vw4UJGjx7N1q1b2bRpEwCdOnWqUrNv3z5iY2MteKcijtEFrCIiIuJUmqYRERERp1IYERGxUnkpnNENG0UuRGFERMRK26bA8h5gy3R2JyIuSxewiog0tOuug7y8ylGR04d+2pkKTUIg8CLc9t+B7drBp586uwvxQHUKI3PmzOGFF17AZrMRHx/Piy++SFJSUrW1/fv3Z+3ateftv/HGG/nwww/r8vIiIq4tLw/27KnmG8U/bSLySw7H8yVLlpCens7kyZPZunUr8fHxDBo0iCNHjlRb/84773D48GH7tn37dvz8/Ljjjjvq3byIiIi4P4fDyMyZMxk3bhxpaWl0796duXPn0qxZMxYsWFBtfVhYGJGRkfZt1apVNGvWTGFERDxX+Y/O7kDErTgURsrKysjOziY1NfXcCXx9SU1NJSsry9Q55s+fz1133UXz5s1rrCktLaW4uLjKJiLiFsp/hNLqR4pFpHoOhZGCggLKy8uJiIiosj8iIgKbzVbr8Zs3b2b79u3ce++9F6zLyMggNDTUvsXExDjSpoiI83w1CSrOOLsLEbfSqJd0z58/nx49etR4sevPJk6cSFFRkX07ePBgI3UoIlIPR7Ng5wxndyHidhxaTdO6dWv8/PzIz8+vsj8/P5/IyMgLHltSUsLixYt55plnan2dwMBAAgMDHWlNRMS5zp6GjaMBl3/ChojLcWhkJCAggMTERDIzz928p6KigszMTFJSUi547NKlSyktLeXuu++uW6ciIq7sq6fgRK6zuxBxSw7fZyQ9PZ1Ro0bRq1cvkpKSmDVrFiUlJaSlpQEwcuRI2rZtS0ZGRpXj5s+fz9ChQ7nooosapnMREVdxdAN8M/Pc17X9mQsIg4BWlrZUH7vKyrjyu+94PzKSvk2bnvtGu3bOa0o8msNhZNiwYRw9epRJkyZhs9lISEhg5cqV9ota8/Ly8PWtOuCya9cu1q9fz8cff9wwXYuIuIqzp86fnnniAvWtroBBG8G3icWN1d2zd99N4b/+xeQuXVi9erWz2xEv4GMYhstPcBYXFxMaGkpRUREhISHObkdE5JzsdNj1/8zV+jaBwdnQsoe1PdXDoUOH6NChA2fOVK4I+uqrr+jRw3X7Fc/gpg9IEBFxAUfWw65Z5usvm+zSQQRg+vTp9vtARUVFmVp0IFJfCiMiInVx9hRsTMP06pmwROj+qKUt1dehQ4eYN29elWsA3377bbZt2+bkzsTTKYyIiNTFl0/Ayd3man0D4MrXwde1H5Q+ffp0WrRoYV/1OGjQIC655BKNjojlFEZERBx15L+w63/N1/eYAi0vtaydhvLll18yZcoUWrRoAYC/vz/Tpk3TyIhYThewiog44mwJLI+Hk3vM1Yf1hoEbXH5UBCrvG+Xr60tubi5xcXGsW7eOPn362PeLWEU/XSIijsh53HwQ8Q2AlEVuEUSAGgOHgohYTT9hIiJmHVkHuX83X9/zGQjtbl0/Ih5CYURExIyzJT+tnjHpoiTo+ifr+hHxIAojIiJm5DwGJ/eaq/UNhCsXuc30jIizKYyIiNQmfw3kzjZf3/MZCO1mWTsinkZhRETkQs6chI1jzNdfdKWmZ0QcpDAiInIhOY9ByT5ztb6BcOVC8PWzticRD6MwIiJSk/zV8O0c8/XxUyG0q3X9iHgohRERkeo4Oj3TOgXi/mhdPyIeTGFERKQ6OY9AyX5ztX5Bmp4RqQeFERGRX7N9Ct++bL6+53MQEmddPyIeTmFEROSXzpyATQ5Mz7S5GuIesq4fES+gMCIi8ktf/AVKDpir9QuC5AWanhGpJ4UREZGf2T6B3a+Yr4/PgJAu1vUj4iUURkREAM4Uw8ax5uvbXANxf7CuHxEvojAiIgKV0zOn8szV+jWtXD3joz+hIg1Bv0kiIoc/ht2vmq+Pz4DgTtb1I+JlFEZExLuVFcGme83Xt+kDcQ9a14+IF1IYERHv9sWf4dRBc7V+zeDKBZqeEWlg+o0SEe/1/Uew5zXz9QnTNT0jYgGFERHxTmVFsNmB6ZnwftBlgnX9iHgxhRER8U5b0+HUIXO1mp4RsZR+s0TE+3y/AvYuMF9/+fPQooN1/Yh4OYUREfEuZYWwaZz5+vD+0Hm8Vd2ICAojIuJttqbD6e/M1fo31/SMSCPQb5iIeI/vlsPehebrE56HFpdY14+IAAojIuItyn6AzQ5Mz0RcC51/Z10/ImKnMCIi3iH7j3D6e3O1/i0gWdMzIo1Fv2ki4vm++xD2vW6+/vIXoEWsZe2ISFUKIyLi2RyenrkeOt1vXT8ich6FERHxbJ8/BKcPm6v1bwFXzgcfH2t7EpEqFEZExHMd+g/s/6f5+iv+Bs3bW9ePiFSrTmFkzpw5xMbGEhQURHJyMps3b75gfWFhIRMmTCAqKorAwEC6dOnC8uXL69SwiIgppcdh833m6yMHQEcHpnNEpMH4O3rAkiVLSE9PZ+7cuSQnJzNr1iwGDRrErl27CA8PP6++rKyMAQMGEB4ezttvv03btm05cOAALVu2bIj+RUSql/0Q/GgzV+sfDMmvaXpGxEkcDiMzZ85k3LhxpKWlATB37lw+/PBDFixYwGOPPXZe/YIFCzh+/DgbNmygSZMmAMTGxtavaxGRCzn0Pux/w3z9FX+D5u2s60dELsihaZqysjKys7NJTU09dwJfX1JTU8nKyqr2mGXLlpGSksKECROIiIjgsssuY9q0aZSXl9f4OqWlpRQXF1fZRERMKT0Gmx1YDRM5EDrea10/IlIrh8JIQUEB5eXlREREVNkfERGBzVb9cOjevXt5++23KS8vZ/ny5Tz11FP87W9/Y+rUqTW+TkZGBqGhofYtJibGkTZFxJt9/gf4Md9cbZMQTc+IuADLV9NUVFQQHh7Oq6++SmJiIsOGDeOJJ55g7ty5NR4zceJEioqK7NvBgwetblNEPMHB9+DAm+brr5gJzfWPHRFnc+iakdatW+Pn50d+ftV/deTn5xMZGVntMVFRUTRp0gQ/Pz/7vm7dumGz2SgrKyMgIOC8YwIDAwkMDHSkNRHxdqXHYIsDz5KJGgwdxljXj4iY5tDISEBAAImJiWRmZtr3VVRUkJmZSUpKSrXHXH311ezevZuKigr7vtzcXKKioqoNIiIidfL5gw5Oz8zT9IyIi3B4miY9PZ158+bx+uuvs3PnTsaPH09JSYl9dc3IkSOZOHGLine truncated
+      "image/png": "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 truncated
       "text/plain": [
        "<Figure size 640x480 with 1 Axes>"
       ]
@@ -257,16 +361,16 @@
      "output_type": "stream",
      "text": [
       "Target position (x,y,z): -0.3 0.4 0.2\n",
-      "Angles (base, shoulder, elbow, wrist): [126.8699, 74.2826, 109.5002, -35.2176]\n",
-      "Robot Angles: [126.8699, -74.2826, 109.5002, -125.2176]\n",
-      "elbow (x,y): 0.115 0.407\n",
-      "wrist (x,y): 0.408 0.2\n",
-      "tool  (x,y): 0.5 0.2\n"
+      "Angles (base, shoulder, elbow, wrist): [111.1813, 100.8341, 104.0795, -3.2455]\n",
+      "Robot Angles: [111.1813, -100.8341, 104.0795, -93.2455]\n",
+      "elbow (x,y): -0.079 0.415\n",
+      "wrist (x,y): 0.279 0.395\n",
+      "tool  (x,y): 0.429 0.395\n"
      ]
     },
     {
      "data": {
-      "image/png": "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 truncated
+      "image/png": "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 truncated
       "text/plain": [
        "<Figure size 640x480 with 1 Axes>"
       ]
@@ -290,13 +394,13 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 160,
+   "execution_count": 19,
    "metadata": {},
    "outputs": [
     {
      "data": {
       "application/vnd.jupyter.widget-view+json": {
-       "model_id": "5617bc9f737949c4bc84c787ff88ce16",
+       "model_id": "30dd696bcdc14aec9a1d4c17fb4787d4",
        "version_major": 2,
        "version_minor": 0
       },
@@ -313,7 +417,7 @@
        "<function __main__.draw_arm(x, y, z)>"
       ]
      },
-     "execution_count": 160,
+     "execution_count": 19,
      "metadata": {},
      "output_type": "execute_result"
     }

ur5_control.py

@@ -216,35 +216,65 @@ def normalize_degree(theta):
     # Return angle
     return normalized_theta
 
-def get_joints_from_xyz_rel(x, y, z, initial_guess = (math.pi/2, math.pi/2, 0), limbs=(.422864, .359041, .092124)):
-    # Get polar coordinates of x,y pair
-    r, theta = cartesian_to_polar(x, y)
+
+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)):
+    # Get limbs and offsets
+    offset_x, offset_y, offset_z = (0, 0, 0.14)     # Tool offset
+    l_bs, l1, l2, l3, l_wt = (0.1333, .425, .39225, .1267, .0997)    # Limb lengths
+    #l3=0.15
     
-    # Get length of each limb
-    l1, l2, l3 = limbs
+    # Calculate base angle and r relative to shoulder joint
+    def calculate_theta(x, y, a):
+        # Calculate if we need the + or - in our equations
+        if (x>-a and y>=0) or (x>a and y<0):
+            flip = 1
+        elif (x<-a and y>=0) or (x<a and y<0):
+            flip = -1
+        else: 
+            # Critical section (x=a, or x=-a). Infinite slope
+            # Return 0 or 180 depending on sign
+            return math.atan2(y, 0)
+        
+        # Calculate tangent line y = mx + b
+        m = (x*y - math.sqrt(x*x*y*y-(x*x-a*a)*(y*y-a*a)))/(x*x-a*a)
+        b = flip * a * math.sqrt(1+m*m)
+
+        # Calculate equivalent tangent point on circle
+        cx = (-flip*m*b)/(1+m*m)
+        cy = m*cx + flip*b
+
+        # Calculate base angle, make angle negative if flip=1
+        theta = math.atan2(cy, cx) + (-math.pi if flip==1 else 0)
+
+        return theta 
     
+    base_theta = calculate_theta(x, y, l_bs)
+    cx, cy = l_bs*math.cos(base_theta), l_bs*math.sin(l_bs)
+    r = math.sqrt((x-cx)**2 + (y-cy)**2) 
+
+
     # Formulas to find out joint positions for (r, z)
     def inv_kin_r_z(p):
-        a, b, c = p
+        a, b, c = p            
 
         return (l1*math.cos(a) + l2*math.cos(a-b) + l3*math.cos(a-b-c) - r, # r
-                l1*math.sin(a) + l2*math.sin(a-b) - l3*math.sin(a-b-c) - z,  # z
+                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
                 a-b-c) # wrist angle
 
 
     # Normalize angles
-    base, shoulder, elbow, wrist = [normalize_degree(deg) for deg in [theta, *fsolve(inv_kin_r_z, initial_guess)]]
+    base, shoulder, elbow, wrist1 = [normalize_degree(deg) for deg in [base_theta, *fsolve(inv_kin_r_z, initial_guess)]]
 
     # Return result
-    return base, shoulder, elbow, wrist
+    return base, shoulder, elbow, wrist1, ry, rz
 
-def get_joints_from_xyz_abs(x, y, z):
-    joints = get_joints_from_xyz_rel(x, y, z)
+def get_joints_from_xyz_abs(x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2):
+    joints = get_joints_from_xyz_rel(x, y, z, rx, ry, rz)
 
     # Joint offsets
     # Base, Shoulder, Elbow, Wrist
-    inverse = [1, -1, 1, 1]
-    offsets = [0, 0, 0, -math.pi/2]
+    inverse = [1, -1, 1, 1, 1, 1]
+    offsets = [-math.pi/2, 0, 0, -math.pi/2, 0, 0]
 
     # Return adjusted joint positions
     return [o+j*i for j, o, i in zip(joints, offsets, inverse)]
@@ -262,7 +292,7 @@ if __name__ == "__main__":
                0.40002172976662786, 
                0, 
                -3.14152741295329, 
-               0]
+               math.radians(62)]
 
     # time.sleep(.5)
 
@@ -290,14 +320,21 @@ if __name__ == "__main__":
     # set_pos_abs(*home_pose)
 
 
-    angles = get_joints_from_xyz_abs(0.3, 0.3, 0.3)
-    rob.movej([*angles, *rob.getj()[4:]], acc=1, vel=1)
     
-    angles = get_joints_from_xyz_abs(-0.3, -0.3, 0.7)
-    rob.movej([*angles, *rob.getj()[4:]], acc=1, vel=1)
+    angles = get_joints_from_xyz_abs(-0.7, 0, 0)
+    rob.movej(angles, acc=2, vel=2)
+
+    # joints = []
+    # for i in np.linspace(-0.3, -0.7, 50):
+    #     joints.append(get_joints_from_xyz_abs(i, 0, 0))
+    # rob.movejs(joints, acc=2, vel=2)
+
+    # time.sleep(5)
+
+
+    # angles = get_joints_from_xyz_abs(0, -0.6, 0)
+    # rob.movej(angles, acc=2, vel=2)
     
-    angles = get_joints_from_xyz_abs(-0.3, 0.4, 0.2)
-    rob.movej([*angles, *rob.getj()[4:]], acc=1, vel=1)
 
 
     # set_pos_abs(*p1)