Commented jupytr notebook

Dockerfile

@@ -0,0 +1,11 @@
+FROM python:latest
+
+RUN apt-get update && apt-get install -y libgl1-mesa-glx ghostscript && apt-get clean && rm -rf /var/lib/apt/lists
+COPY . .
+#COPY config-server.yml config.yml
+RUN pip3 install -r requirements.txt
+
+CMD ["python3", "run.py"]
+EXPOSE 5000
+EXPOSE 8000
+EXPOSE 9000

config.yml

@@ -32,13 +32,13 @@ led:
       ledstart: 288
       ledend: 431
       mode: rgb
-    - universe: 1
-      ip: 192.168.68.130
+    - universe: 4
+      ip: 192.168.5.40
       ledstart: 432
       ledend: 575
       mode: rgb
-    - universe: 4
-      ip: 192.168.68.131
+    - universe: 1
+      ip: 192.168.5.4
       ledstart: 576
       ledend: 719
       mode: rgb

get_specs.py

@@ -26,7 +26,7 @@ def check_internet(url='https://belden.com', timeout=5):
     
 
 
-def query_search(partnum):
+def query_search(partnum, source):
     """token_url = "https://www.belden.com/coveo/rest/token?t=" + str(int(time.time()))
     with requests.get(token_url) as r:
         out = json.loads(r.content)
@@ -49,15 +49,50 @@ def query_search(partnum):
     # Bash script uses some crazy json formatting that I could not figure out
     # Despite the fact that I wrote it
     # So I'll just leave it, becuase it works.
+    if source == "Belden":
+        command = ["./query-search.sh", partnum]
+        result = subprocess.run(command, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True)
+        if result.returncode != 0: # error
+            fprint("No results found in search database for " + partnum + ". No hi-res part image available.", result.stderr)
+            return False
+        else:
+            data_out = json.loads(result.stdout)
+            return data_out
+    elif source == "Alphawire":
+        alphaurl = "https://www.alphawire.com//sxa/search/results/?l=en&s={4A774076-6068-460C-9CC6-A2D8E85E407F}&itemid={BF82F58C-EFD9-4D8B-AE3E-097DD12CF7DA}&sig=&autoFireSearch=true&productpartnumber=*" + partnum + "*&v={B22CD56D-AB95-4048-8AA1-5BBDF2F2D17F}&p=10&e=0&o=ProductPartNumber%2CAscending"
+        r = requests.get(url=alphaurl)
+        data = r.json()
+        output = dict()
+        #print(data)
+        try:
+            if data["Count"] > 0:
+                print(data["Results"][0]["Url"])
+                result = data["Results"][0]
+                if result["Url"].split("/")[-1] == partnum:
+                    #print(partnum)
+                    print(result["Html"])
+                    try:
+                        imgidx = result["Html"].index("<img src=") + 10
+                        imgidx2 = result["Html"].index("?", imgidx)
+                        output["image"] = result["Html"][imgidx:imgidx2]
+                        if output["image"].index("http") != 0:
+                            output["image"] = ""
+                            print("No cable image found.")
+                    except:
+                        print("No cable image found.")
 
-    command = ["./query-search.sh", partnum]
-    result = subprocess.run(command, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True)
-    if result.returncode != 0: # error
-        fprint("No results found in search database for " + partnum + ". No hi-res part image available.", result.stderr)
+                    dsidx = result["Html"].index("<a href=\"/disteAPI/") + 9
+                    dsidx2 = result["Html"].index(partnum, dsidx) + len(partnum)
+                    output["datasheet"] = "https://www.alphawire.com" + result["Html"][dsidx:dsidx2]
+                    #"test".index()
+                    print(output)
+                    return output
+
+
+        except:
+            return False
         return False
-    else:
-        data_out = json.loads(result.stdout)
-        return data_out
+
 
 def touch(path):
     with open(path, 'a'):
@@ -126,7 +161,7 @@ def get_multi(partnums):
                 sys.exit()
 
 
-        def _download_image(url, output_dir): # Download datasheet with known URL
+        def _download_image(url, output_dir): # Download image with known URL
             global bartext
 
             #fprint(url)
@@ -151,25 +186,31 @@ def get_multi(partnums):
                 os.remove(partnum + "/datasheet.pdf")
                 sys.exit()
 
-        def __use_cached_datasheet(partnum, path, output_dir):
+        def __use_cached_datasheet(partnum, path, output_dir, dstype):
             fprint("Using cached datasheet for " + partnum)
             bar.text = "Using cached datasheet for " + partnum
             bar(skipped=True)
             fprint("Parsing Datasheet contents of " + partnum)
             bar.text = "Parsing Datasheet contents of " + partnum + ".pdf..."
-            read_datasheet.parse(path, output_dir)
+            read_datasheet.parse(path, output_dir, partnum, dstype)
             bar(skipped=False)
 
-        def __downloaded_datasheet(partnum, path, output_dir):
+        def __downloaded_datasheet(partnum, path, output_dir, dstype):
             fprint("Downloaded " + path)
             bar.text = "Downloaded " + path
             bar(skipped=False)
             fprint("Parsing Datasheet contents of " + partnum)
             bar.text = "Parsing Datasheet contents of " + partnum + ".pdf..."
-            read_datasheet.parse(path, output_dir)
+            read_datasheet.parse(path, output_dir, partnum, dstype)
             bar(skipped=False)
 
-        for partnum in partnums:
+        for fullpartnum in partnums:
+            if fullpartnum[0:2] == "BL": # catalog.belden.com entry\
+                partnum = fullpartnum[2:]
+                dstype = "Belden"
+            elif fullpartnum[0:2] == "AW":
+                partnum = fullpartnum[2:]
+                dstype = "Alphawire"
             output_dir = "cables/" + partnum
             path = output_dir + "/datasheet.pdf"
             bartext = "Downloading files for part " + partnum
@@ -177,7 +218,7 @@ def get_multi(partnums):
             #
             if (not os.path.exists(output_dir + "/found_part_hires")) or not (os.path.exists(path) and os.path.getsize(path) > 1):
                 # Use query
-                search_result = query_search(partnum.replace(" ", ""))
+                search_result = query_search(partnum.replace(" ", ""), dstype)
                 # Try to use belden.com search
                 if search_result is not False:
                     # Download high resolution part image if available and needed
@@ -190,17 +231,17 @@ def get_multi(partnums):
 
                     # Download datasheet from provided URL if needed
                     if os.path.exists(path) and os.path.getsize(path) > 1:
-                        __use_cached_datasheet(partnum, path, output_dir)
+                        __use_cached_datasheet(partnum, path, output_dir, dstype)
 
                     elif _download_datasheet(search_result["datasheet"], output_dir) is not False:
-                        __downloaded_datasheet(partnum, path, output_dir)
+                        __downloaded_datasheet(partnum, path, output_dir, dstype)
                 
                 elif os.path.exists(path) and os.path.getsize(path) > 1:
-                    __use_cached_datasheet(partnum, path, output_dir)
+                    __use_cached_datasheet(partnum, path, output_dir, dstype)
                 
                 # If search fails, and we don't already have the datasheet, guess datasheet URL and skip the hires image download
                 elif _try_download_datasheet(partnum, output_dir) is not False:
-                    __downloaded_datasheet(partnum, path, output_dir)
+                    __downloaded_datasheet(partnum, path, output_dir, dstype)
 
                 # Failed to download with search or guess :(
                 else: 
@@ -213,7 +254,7 @@ def get_multi(partnums):
             # We already have a hi-res image and the datasheet - perfect!
             else:
                 fprint("Using cached hi-res part image for " + partnum)
-                __use_cached_datasheet(partnum, path, output_dir)
+                __use_cached_datasheet(partnum, path, output_dir, dstype)
     
     if len(failed) > 0:
         fprint("Failed to download:")
@@ -227,21 +268,22 @@ def get_multi(partnums):
 
 
 if __name__ == "__main__":
-    partnums = ["10GXS12", "RST 5L-RKT 5L-949", 
-"10GXS13",
-"10GXW12",
-"10GXW13",
-"2412",
-"2413",
-"OSP6AU",
-"FI4D024P9",
-"FISD012R9",
-"FDSD012A9",
-"FSSL024NG",
-"FISX006W0",
-"FISX00103",
-"C6D1100007"
+    partnums = ["BL7958A", "BL10GXS12", "BLRST 5L-RKT 5L-949", 
+"BL10GXS13",
+"BL10GXW12",
+"BL10GXW13",
+"BL2412",
+"BL2413",
+"BLOSP6AU",
+"BLFI4D024P9",
+"BLFISD012R9",
+"BLFDSD012A9",
+"BLFSSL024NG",
+"BLFISX006W0",
+"BLFISX00103",
+"BLC6D1100007"
     ]
     get_multi(partnums)
+    #query_search("3248", "Alphawire")
 
 

inv_kin_testing.ipynb

@@ -0,0 +1,326 @@
+{
+ "cells": [
+  {
+   "cell_type": "code",
+   "execution_count": 1,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "from scipy.optimize import fsolve\n",
+    "import matplotlib.pyplot as plt\n",
+    "import math\n",
+    "import numpy as np"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 2,
+   "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": "code",
+   "execution_count": 141,
+   "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"
+     ]
+    }
+   ],
+   "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, 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",
+    "    \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) - 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",
+    "\n",
+    "    # Return result\n",
+    "    return base, shoulder, elbow, wrist\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",
+    "\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)])"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 142,
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stdout",
+     "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"
+     ]
+    },
+    {
+     "data": {
+      "image/png": "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 truncated
+      "text/plain": [
+       "<Figure size 640x480 with 1 Axes>"
+      ]
+     },
+     "metadata": {},
+     "output_type": "display_data"
+    }
+   ],
+   "source": [
+    "def draw_arm(x, y, z):\n",
+    "    #(.422864, .359041, .092124)\n",
+    "    l1, l2, l3 = (.422864, .359041, .092124)\n",
+    "    r, theta = cartesian_to_polar(x, y)\n",
+    "    base, shoulder, elbow, wrist = get_joints_from_xyz_rel(x, y, z, limbs=(l1, l2, l3))\n",
+    "    print('Target position (x,y,z):', x, y, z)\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",
+    "    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",
+    "    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",
+    "\n",
+    "    # 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",
+    "    # 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/4, 0.01), xycoords='data', xytext=(x2/2, 0), textcoords='offset points')\n",
+    "\n",
+    "    # 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",
+    "   \n",
+    "    # Display\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": 144,
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stdout",
+     "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"
+     ]
+    },
+    {
+     "data": {
+      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAiMAAAGnCAYAAABl41fiAAAAOXRFWHRTb2Z0d2FyZQBNYXRwbG90bGliIHZlcnNpb24zLjUuMSwgaHR0cHM6Ly9tYXRwbG90bGliLm9yZy/YYfK9AAAACXBIWXMAAA9hAAAPYQGoP6dpAABD4UlEQVR4nO3deVyVZf7/8dcBZFEBxQXUUEwd0UpRUMNvbhMu5aSWTmaOOmTWTNoy1G/MaiSrCcecssWynLTSTFvM1tGMtLQYNdTJfdIg3MAdFBMQ7t8fx06SIOcgZ7lv3s/H4zwew32uc1+fc8vY2+u+ruu2GYZhICIiIuIlft4uQERERGo3hRERERHxKoURERER8SqFEREREfEqhRERERHxKoURERER8SqFEREREfEqhRERERHxKoURERER8SqFEREREfEqhZFa4KuvvuKGG26gefPm2Gw2li1bVu79pUuXMmDAABo1aoTNZmPz5s0XnOPMmTNMnDiRRo0aUb9+fYYPH05eXt5F+7XZbBW+nnrqKQCys7MZP348rVu3JiQkhDZt2pCamkpxcXG588ydO5dWrVrRpUsX1q1bd0nXQkREfI/CSC1QWFhI586dmT17dqXvX3PNNfzjH/+o9Bx/+ctf+Oijj3jnnXf48ssvOXDgADfddNNF+z148GC517x587DZbAwfPhyAnTt3UlZWxssvv8y2bdt45plnmDNnDg899JDjHDk5OcyYMYPFixfz8MMPk5ycXI0rICIivsymB+XVLjabjffff59hw4Zd8F52djatW7dm06ZNxMXFOY7n5+fTpEkTFi1axIgRIwB7kOjQoQMZGRlcffXVTvU9bNgwTp48SXp6eqVtnnrqKV566SV++OEHALZu3UpycjKrV6/m0KFD/Pa3vyUrK8v5LywiIj4vwNsFiO/LzMykpKSEpKQkx7HY2FhatmzpdBjJy8vjk08+4fXXX79ou/z8fCIiIhw/X3nllXTq1Inw8HACAwOZO3du9b+IiIj4JN2mkSrl5uYSGBhIgwYNyh2PjIwkNzfXqXO8/vrrhIaGXvTWzu7du3n++ee58847yx1/9dVXycvL4+jRo4wePdrl+kVExLdpZEQ8Yt68eYwePZrg4OAK39+/fz+DBg3i97//PRMmTLjg/UaNGrm7RBER8RKNjEiVoqKiKC4u5sSJE+WO5+XlERUVVeXn16xZw65du7j99tsrfP/AgQP069ePnj178sorr9REySIiYiIKI1Kl+Ph46tSpU27i6a5du8jJySExMbHKz7/66qvEx8fTuXPnC97bv38/ffv2JT4+nvnz5+Pnp19JEZHaxhS3aQzD4OTJk4SGhmKz2bxdjumcOnWK3bt3O37Oyspi8+bNRERE0LJlS44dO0ZOTg4HDhwA7EED7CMiUVFRhIeHM378eFJSUoiIiCAsLIy7776bxMTEcpNXY2NjSUtL48Ybb3QcKygo4J133uGf//znBXX9HERatWrFzJkzOXz4sOM9Z0ZcRETEIgwTyM/PNwAjPz/f26WY0qpVqwzggte4ceMMwzCM+fPnV/h+amqq4xw//fSTcddddxkNGzY06tata9x4443GwYMHy/UDGPPnzy937OWXXzZCQkKMEydOXFBXZf2a5NdSRERqiCn2GSkoKCA8PJz8/HzCwsK8XY6IiIjUIN2gFxEREa9SGBERERGvUhgRERERr1IYEREREa9SGBERERGvUhgRERERr1IYEREREa9SGBERERGvUhixkrISWP8nOPgZlJV6uxoRERGnmOLZNFK15UDIwc/os/tl2P0yhSHN2RNzKztjxnC4YSdKgXrAeC/XKSIi8msaGbGIvwAHsxY4fq730wE67ZjJzf/uzDWfdiZnx0xeOn3AewWKiIhUQs+msYiri/NZ9X4UIaVnKm1TavPDPzIJWo+B6BshoJ4HKxQREamYbtNYxKC97140iAD4G2WQ+5n9taEeRA+3B5Om/cDP30OVioiIlKfbNBbxu/Nu0TjlbCFkvQFf9IcPWsKmv8KJLe4pTkRE5CJ0m8YKCn+ED2Jq5lwN4yBmDMTcCiFRNXNOERGRi9DIiBVkv1lz5zq+GTbdD8tawKpBkPWmfRRFRETETTQyYnaGAZ90gIJd7usjoP5580v6an6JiIjUKI2MmF3paWicyOmA+u7r4+wpyHodvkiCD1rBpslwYqv7+hMRkVpFIyMWcf3ZQhrs+4AxWQsYkPuZfeWMuzXsYh8taTVK80tERKTaFEYsoj/w+bn/HfXTQUZlv8WY7AV0Ob7Z/Z3b/CGqP7QeC5cNhYC67u9TREQsQ2HEIgYBKyo4fsWJrYzJWsDo7De57Kf97i8kIBRaDrevyInsCzbdCRQRkYtTGLGI3wGfXOR9v7JS+h1axYqsBfjvfc8zK2TqRkPMaPutnPCO7u9PRERMSWHEIoYCHzrR7jQQcrYQ9i6zb3qW9zl4ZH5J1/Pml0S6vz8RETENhRGLuAl434l2J4Fy625+OgjZiyBrAZz4r1tqK8fmD1EDzptfEuL+PkVExKcpjFjE74F3nWh3Agiv7M3j30H2Avsmaj8drKnSKhcQCi1HnNu/pI/ml4iI1FIKIxZxC7DEiXZHgYiqGpWVQt4X9tGSfUs9NL+k5XnzSzq4vz8REfEZCiMWMRpY5ES7PKCpKycuOQX7lp2bX5LumfklEfH22zitboFgl6oVERETUhixiHHAG060OwA0q24npw/Ajz/PL/muumdxns0fmg2yj5a0GKL5JSIiFqUwYhG3AfOdaLcXuKwmOjz+X3so+XGRZ+aX1AmD6BH2EZOmvTS/RETEQqr1N/rs2bOJiYkhODiYHj16sH79eqc+t3jxYmw2G8OGDatOt3IRzj66rrSmOmzYGbrOhKF7od8KiPkD+Ltx59WSAvhhHqT3hQ9aw38fhvyd7utPREQ8xuUwsmTJElJSUkhNTWXjxo107tyZgQMHcujQoYt+Ljs7mwceeIBevXpVu1ipnMfDyM/8/KHZAOi5AG7Kg6tfh6gkwFbTPf3idA5se9L+tOLl3WDX83DmsPv6ExERt3L5Nk2PHj3o1q0bL7zwAgBlZWVER0dz99138+CDD1b4mdLSUnr37s1tt93GmjVrOHHiBMuWLXO6T92mqdpE4EUn2v0PaOfmWgA4vf/c/iVvQL4HnvBrC/hlfsllQ8A/2P19iohIjXBpZKS4uJjMzEySkpJ+OYGfH0lJSWRkZFT6uccee4ymTZsyfvx4p/opKiqioKCg3EsuztmRkbNureI8dVtAx/8H138H122C2BQIduOTfY2zcOBj+HokLI2EdRPg0FeeWf0jIiKXxKUwcuTIEUpLS4mMLL+dd2RkJLm5uRV+Zu3atbz66qvMnTvX6X7S0tIIDw93vKKjo10ps1YKcLJdjd+mqYrNBg3joOs/Ydhe6Lvcvp+IvxtXxpQUwJ5/wed94MPL4b+PQMEu9/UnIiKXxK1LEk6ePMmYMWOYO3cujRs3dvpzU6ZMIT8/3/Hau3evG6u0Bq/NGXGFXwA0Hwg9F56bX/IaRF6LW+eXFP4I2/4OH8fCih6w6wU4c8R9/YmIiMuc/Qc1AI0bN8bf35+8vLxyx/Py8oiKunAIfs+ePWRnZ3PDDTc4jpWV2YfNAwIC2LVrF23atLngc0FBQQQFBblSWq1nijByvjqhcPk4++v0PvsW9FkLIH+b+/o8ut7+2vgXaH7duf1LbtD8EhERL3NpZCQwMJD4+HjS09Mdx8rKykhPTycxMfGC9rGxsWzZsoXNmzc7XkOGDKFfv35s3rxZt19qkOnCyPnqXgYdJ8P1W2DQRmj/Fwh245N9jbOw/yNYezMsjYJ1d8ChNZpfIiLiJS6NjACkpKQwbtw4EhIS6N69O7NmzaKwsJDk5GQAxo4dS4sWLUhLSyM4OJgrr7yy3OcbNGgAcMFxuTSmDiM/s9kgoov91WUG5K4893ycZVD6k3v6LMmHPXPtr3ox9v1SWo+BsN+4pz8REbmAy2Fk5MiRHD58mKlTp5Kbm0tcXBzLly93TGrNycnBz0+7Y3qaz62muVR+AfZbKc2vs09I3bvUHkzyVgFu2jS4MBu2PWF/NephDyUtR0Kw8/OdRETEddoO3iL+DjziRLvVQB/3luJehXvt80uyF0D+dvf3ZwuAFoMhZgy0+B34ay6TiEhN0xCGRVjiNo0z6kXDFQ/C9VthUCa0v8+9T/Y1zsK+D2DtCPv8kvV3wqG14PsZXkTENBRGLKLWhJGf2WwQ0RXin4Fh+6HPJ9DqFveujCk5Abtfgc97wUdt4btUOLnbff2JiNQSCiMWUevCyPn8AqDF9fB/b9n3L+kxD5r2dW+fp36ArY/BR+1gRSL870UoOurePkVELEphxCIsN4G1uuqEQZtkSFoFQ3+Ezk9CWAf39nn0P/DtRHi/GXx1o32ybWmRe/sUEbEQhRGLqNUjI5Wp1xKumAKDt8HADfCbeyCoifv6KyuxL0NeM9weTNb/CQ5/o/klIiJV0Goai5gD/NmJdu8BN7m5Fp9WVgIHP7MvE97/AZSecX+f9dv8sn9J6IU7DouI1HYaGbEIjYw4ya+OfanuNYvhxlzo8S9o6ubFzqf2wNZp9kmvn/WE7+dA0TH39ikiYiIKIxahMFINgeHQZjwkrYah2dD57xAW694+j2TAhj+fm19yE+x9X/NLRKTWUxixCIWRS1SvFVzxEAzeDgPXw2/uhiA37rxaVgz73oc1N8H7zWHDXXA4Q/NLRKRWUhixCK2mqSE2GzTqBgnPwY0HoM9H0PL34OfGnVeLj8H3L8HKnvDRb2DLNDi5x339iYj4GIURi3D2IUMaGXGBXx37FvDXvA035UL3udC0t3v7PLUbtjxqn1+y8hr4/mUoPu7ePkVEvExhxCJ0m8bNAhtA29sh6UsYkgWdHodQNz/Z9/DXsOFP9m3o1wyHvcugtNi9fYqIeIHCiEUojHhQ/Ri48hHLine truncated
+      "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.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"
+     ]
+    },
+    {
+     "data": {
+      "image/png": "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 truncated
+      "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.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"
+     ]
+    },
+    {
+     "data": {
+      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAjQAAAGoCAYAAABRx3+mAAAAOXRFWHRTb2Z0d2FyZQBNYXRwbG90bGliIHZlcnNpb24zLjUuMSwgaHR0cHM6Ly9tYXRwbG90bGliLm9yZy/YYfK9AAAACXBIWXMAAA9hAAAPYQGoP6dpAABOZElEQVR4nO3de3QU9f3/8ecmkIRrEozkQqMBo6BcEgFJg4CogUCtwu9bLdgqiFYs3mrjDaoSEGsQUakFpQWpeIXqUbRWAxiBqsQghMhVBApy3XCpJCRIAtn5/RGzuJKQ2WUne3s9zpkjO/vez75nzCFv5vOZedsMwzAQERERCWBhvk5ARERE5GypoBEREZGAp4JGREREAp4KGhEREQl4KmhEREQk4KmgERERkYCngkZEREQCngoaERERCXgqaERERCTgqaARERGRgKeCRkLKf/7zH6699lqSkpKw2WwsWrTI5X3DMJg4cSKJiYm0aNGCrKwstm7d6hJTXFzMoEGDiImJ4ZxzzmHs2LFUVFSc8XtvueUWbDabyzZkyBCXmMLCQtLT00lJSeGll17yyvGKiIQKFTQSUiorK0lLS2PWrFn1vj9t2jSef/55Zs+eTVFREa1atSI7O5vjx48DsG/fPrKyskhNTaWoqIj8/Hw2btzILbfc0uh3DxkyhP379zu3N9980+X92267jccee4w33niDvLw8du/efdbHKyISKpr5OgGRpjR06FCGDh1a73uGYTBjxgweffRRhg0bBsArr7xCfHw8ixYtYuTIkXzwwQc0b96cWbNmERZW+++B2bNn06NHD7Zt20ZqamqD3x0ZGUlCQkKD71dWVtKzZ0/at29PbGwsR48ePYsjFREJLbpCI/KDHTt2YLfbycrKcu6Ljo4mIyODwsJCAKqqqoiIiHAWMwAtWrQA4LPPPjvj+MuXL6d9+/Z07tyZcePGcfjwYZf3J06cyMUXX0x0dDQ///nPueSSS7x1aCIiQU8FjcgP7HY7APHx8S774+Pjne9dddVV2O12nn76aaqrq/nuu+8YP348APv3729w7CFDhvDKK69QUFDAU089xYoVKxg6dCg1NTXOmNtuu43Dhw9z8OBB/vrXv3r78EREgpoKGhE3dO3alfnz5/PMM8/QsmVLEhIS6NixI/Hx8S5XbX5q5MiRXHfddXTv3p3hw4fzwQcf8OWXX7J8+XKXuFatWhEbG2vxUYiIBB8VNCI/qFvfUlpa6rK/tLTUZe3Lb37zG+x2O3v37uXw4cNMmjSJgwcP0qlTJ9Pf1alTJ+Li4ti2bZt3khcRCXEqaER+0LFjRxISEigoKHDuKy8vp6ioiMzMzNPi4+Pjad26NQsXLiQqKopBgwaZ/q49e/Zw+PBhEhMTvZK7iEioU0EjIaWiooKSkhJKSkqA2oXAJSUl7Nq1C5vNxn333ccTTzzB+++/z/r16xk1ahRJSUkMHz7cOcbMmTMpLi7mm2++YdasWdx9993k5eURExPjjOnSpQvvvvuu8zsffPBBvvjiC3bu3ElBQQHDhg0jNTWV7OzsJjx6EZHgFRS3bRuGwdGjR2nTpg02m83X6YgfW716NVdeeaXzdU5ODgCjR4/m5Zdf5qGHHqKyspKxY8dy5MgR+vXrR35+PlFRUc7PrFq1itzcXCoqKujSpQt/+9vfuPnmm12+Z8uWLZSVlQEQHh7OunXrmD9/PkeOHCEpKYnBgwczZcoUIiMjm+CoRUSCn80wDMPXSZyt8vJyoqOjKSsro23btr5OR0RERJqYppxEREQk4KmgERERkYCngkZEREQCngoaERERCXgqaERERCTgqaARERGRgKeCRkRERAKeChoREREJeCpoJPh9b/d1BiIiYjEVNBK03gI2VezE+KAzrPo9nKz0dUoiImIRFTQSlI4DtzpqOFx4M7YT5bDtb5R/1JOyw6t9nZqIiFhABY0Epc+AezdNpf/Bz5z72h79hpZLMpm98UmmOGpYBdT4LEMREfEmFTQSlDYe/pJJ6yedtr+5cZLff/UIVxRcya8rdvJI06cmIiIWUEEjwedkJdetvInmxskGQwYc/JSvPkrjtzteg8BvOC8iEvJU0EjQqSjOoePRbxqNiz5RTvfCm2Hlb6D6iPWJiYiIZVTQSHDZ8x6tt/3dvc98uwA+7AGlyy1JSURErKeCRoKH4yQU53j22WO7oeAqWPsw1FR7Ny8REbGcRwXNrFmzSElJISoqioyMDFatWmXqcwsWLMBmszF8+HCX/YZhMHHiRBITE2nRogVZWVls3brVk9QklIU148RVS/kirq+HAxiweRos+TmUbfZqaiIiYi23C5qFCxeSk5NDbm4uxcXFpKWlkZ2dzYEDB874uZ07d/LAAw/Qv3//096bNm0azz//PLNnz6aoqIhWrVqRnZ3N8ePH3U1PQlxR6070y1rBY90f56Qt3LNBvlsL+T3hm1laMCwiEiBshuHe39gZGRlcdtllzJw5EwCHw0FycjL33HMP48ePr/czNTU1DBgwgFtvvZVPP/2UI0eOsGjRIqD26kxSUhL3338/DzzwAABlZWXEx8fz8ssvM3LkyEZzKi8vJzo6mrKyMtq2bevO4UiQeRT48w9/7nOoiNdW3sSFFds8HzDpF5AxD1rEeyM9ERGxiFtXaKqrq1mzZg1ZWVmnBggLIysri8LCwgY/9/jjj9O+fXtuu+22097bsWMHdrvdZczo6GgyMjIaHLOqqory8nKXTQQg/0d/XhWXwaVD1zLngt95PuC+D+HD7rDnX2edm4iIWMetgubQoUPU1NQQH+/6r9X4+Hjs9vobAH722We89NJLzJkzp9736z7nzph5eXlER0c7t+TkZHcOQ4LUAWDNT/ZVNm/N2Iw5DO//Lociz/Fs4KqD8J/r1A9KRMSPWXqX09GjR7n55puZM2cOcXFxXht3woQJlJWVObfdu3d7bWwJXEvO8N57ycPp/ov15Cdme/4F2/4GH/UE9YMSEfE7zdwJjouLIzw8nNLSUpf9paWlJCQknBa/fft2du7cybXXXuvc53A4ar+4WTO2bNni/FxpaSmJiYkuY6anp9ebR2RkJJGRke6kLiHgo0bet7dI5BcDP2TpNzO5eu1D4Khy/0uOfgNLMqHHZLj4YQjzcOGxiIh4lVtXaCIiIujVqxcFBQXOfQ6Hg4KCAjIzM0+L79KlC+vXr6ekpMS5XXfddVx55ZWUlJSQnJxMx44dSUhIcBmzvLycoqKiescUqU8NsNhEnGELo33ne2HIGojp4dmXGSfhq0eg4Eqo2OnZGCIi4lVuXaEByMnJYfTo0fTu3Zs+ffowY8YMKisrGTNmDACjRo2iQ4cO5OXlERUVRbdu3Vw+HxMTA+Cy/7777uOJJ57gwgsvpGPHjjz22GMkJSWd9rwakYYUA4dNxCUB3QBiukL2qtrC5OtnPPvSg5/CR2nQexak/BZsNs/GERGRs+Z2QTNixAgOHjzIxIkTsdvtpKenk5+f71zUu2vXLsLC3Fua89BDD1FZWcnYsWM5cuQI/fr1Iz8/n6ioKHfTkxCV33gIAEMAZ9kRHgk9p0PSUCgcDd/vdf+LT5RD4c2w799w2YsQEeP+GCIictbcfg6NP9JzaORyYKWJuH8CN9T3RtX/4Mvfw663PE+iZTJkvgLxAz0fQ0REPKJeThLwvgO+MBEXBmQ19GZkO7h8Ifz8ZWjW2rNE1A9KRMRnVNBIwPsYcJiI+zkQe6YAmw06jYZffAXqByUiElBU0EjAc2f9jCmtO0HWCuj+OKgflIhIQFBBIwHNwIKCBiCsGXR/DAZ9Dq1T3U8MoOY4rL4bVvwSvi9tPF5ERDymgkYC2gZgn4m4OKCXJ18QlwFD14L6QYmI+DUVNBLQzF6dGcxZ/LA3bw0Zc6D/u6B+UCIifkkFjQQ0S6abGpI8HH6xHtQPSkTE76igkYBVAXxqMnawt760RSIM/BB6/QXCPOwnVtcPauOT4KjxVmYiIiFNBY0ErGXACRNxPYF4b36xLQzUD0pExK+ooJGA1aTTTfWp6wfV5X7Px6jrB7XjNd3eLSJyFlTQSEAygI9MxlpW0MCpflBXfQwtOng2Rl0/qJW/geojXk1PRCRUqKCRgLQN2GEiri21Twi2XMLV8It1cF69naLM+XYBfNgDSpd7LS0RkVChgkYCktnppiyguZWJ/Jj6QYmI+IwKGglIPl8/0xD1gxIR8QkVNBJwjlN7h5MZZ/HEmLOjflAiIk1KBY0EnE+B703EXQKcZ3EuZ6R+UCIiTUYFjQQcv51uaoj6QYmIWE4FjQQcs7drD7U0CzepH5SIiKVU0EhA+RYws0y2JdDP4lw8on5QIiKWUEEjAWWxybgrgSgrEzkb6gclIuJ1KmgkoATc+pmGqB+UiIhXqaCRgHEC+NhkrN8XNHXUD0pExCtU0EjAKASOmoi7APDwJmnfUD8oEZGzpoJGAkbQTDc1RP2gREQ8poJGAkbQFzSgflAiIh6yGUbgT7qXl5cTHR1NWVkZbdu29XU6YgE7kGgiLgI4DHhYCviXiv/Cypvh0ErPx4i9FPq+DtEXey8vERE/pCs0EhCWmIzrT5AUM6B+UCIiblBBIwEhJKab6qN+UCIipqigEb9Xg/krNEFX0NRRPygRkTNSQSN+bw2162Ia0wHoanEuPqV+UCIiDfKooJk1axYpKSlERUWRkZHBqlWrGox955136N27NzExMbRq1Yr09HReffVVl5hbbrkFm83msg0ZErT/1hY3uTPdZLMyEX+hflAiIqdxu6BZuHAhOTk55ObmUlxcTFpaGtnZ2Rw4cKDe+Hbt2vHII49QWFjIunXrGDNmDGPGjGHxYteuPEOGDGH//v3O7c033/TsiCTohOz6mTNRPygRERdu37adkZHBZZddxsyZMwFwOBwkJydzzz33MH78eFNj9OzZk2uuuYYpU6YAtVdojhw5wqJFi9zL/ge6bTt4/Q84F3A0EhcOHAJLine truncated
+      "text/plain": [
+       "<Figure size 640x480 with 1 Axes>"
+      ]
+     },
+     "metadata": {},
+     "output_type": "display_data"
+    }
+   ],
+   "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": 150,
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "application/vnd.jupyter.widget-view+json": {
+       "model_id": "846c528ad814490d90a400348b6ae20c",
+       "version_major": 2,
+       "version_minor": 0
+      },
+      "text/plain": [
+       "interactive(children=(FloatSlider(value=0.3, description='x', max=1.0, step=0.01), FloatSlider(value=0.3, desc…"
+      ]
+     },
+     "metadata": {},
+     "output_type": "display_data"
+    },
+    {
+     "data": {
+      "text/plain": [
+       "<function __main__.draw_arm(x, y, z)>"
+      ]
+     },
+     "execution_count": 150,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "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": 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
+}

led_control.py

@@ -178,9 +178,9 @@ def init():
     sendall(data)
     #time.sleep(50000)    
     fprint("Running start-up test sequence...")
-    for y in range(1):
+    for y in range(100):
         for x in range(len(leds)):
-            setpixel(5,5,5,x)
+            setpixel(0,0,150,x)
         sendall(data)
         #time.sleep(2)
         #alloffsmooth()
@@ -290,7 +290,7 @@ def close():
     time.sleep(0.5)
     sender.stop()
 
-def mapimage(image, fps=60):
+def mapimage(image, fps=90):
     global start
     while uptime() - start < 1/fps:
         time.sleep(0.00001)

read_datasheet.py

@@ -9,8 +9,10 @@ from PIL import Image
 import io
 import json
 from util import fprint
+import uuid
+from util import run_cmd
 
-def parse(filename, output_dir):
+def parse(filename, output_dir, partnum, dstype):
 
     # Extract table data
 
@@ -22,6 +24,7 @@ def parse(filename, output_dir):
     page = reader.pages[0]
     table_list = {}
     for table in tables:
+        table.df.infer_objects(copy=False)
         table.df.replace('', np.nan, inplace=True)
         table.df.dropna(inplace=True, how="all")
         table.df.dropna(inplace=True, axis="columns", how="all")
@@ -137,44 +140,104 @@ def parse(filename, output_dir):
 
 
         # multi-page table check
-        if table_name.isdigit() and len(tables) > 1:
-            fprint(table_name)
-            fprint(previous_table)
-            
-            
-            
-            
-            main_key = previous_table
-            cont_key = table_name
-            fprint(tables)
-            if vertical == False:
-                main_keys = list(tables[main_key].keys())
-                for i, (cont_key, cont_values) in enumerate(tables[cont_key].items()):
-                    if i < len(main_keys):
-                        fprint(tables[main_key][main_keys[i]])
-                        tables[main_key][main_keys[i]] = (tables[main_key][main_keys[i]] + (cont_key,) + cont_values)
-
-                del tables[table_name]
-
-            else:
-                for key in tables[cont_key].keys():
-                    tables[main_key][key] = tables[cont_key][key]
-                del tables[table_name]
+        if dstype == "Belden":
+            if table_name.isdigit() and len(tables) > 1:
+                fprint(table_name)
+                fprint(previous_table)
+                
+                
+                
+                
+                main_key = previous_table
+                cont_key = table_name
+                fprint(tables)
+                if vertical == False:
+                    main_keys = list(tables[main_key].keys())
+                    for i, (cont_key, cont_values) in enumerate(tables[cont_key].items()):
+                        if i < len(main_keys):
+                            fprint(tables[main_key][main_keys[i]])
+                            tables[main_key][main_keys[i]] = (tables[main_key][main_keys[i]] + (cont_key,) + cont_values)
+    
+                    del tables[table_name]
+    
+                else:
+                    for key in tables[cont_key].keys():
+                        tables[main_key][key] = tables[cont_key][key]
+                    del tables[table_name]
 
         previous_table = table_name
     
+    # remove multi-line values that occasionally squeak through
+    def replace_newlines_in_dict(d):
+        for key, value in d.items():
+            if isinstance(value, str):
+                # Replace \n with " " if the value is a string
+                d[key] = value.replace('\n', ' ')
+            elif isinstance(value, dict):
+                # Recursively call the function if the value is another dictionary
+                replace_newlines_in_dict(value)
+        return d
+    
+    tables = replace_newlines_in_dict(tables)
 
-    fprint(tables)
-    with open(output_dir + "/tables.json", 'w') as json_file:
-        json.dump(tables, json_file)
+    # summary
+
+    output_table = dict()
+    output_table["partnum"] = partnum
+    id = str(uuid.uuid4())
+    output_table["id"] = id
+    #output_table["position"] = id
+    #output_table["brand"] = brand
+    output_table["fullspecs"] = tables
+    output_table["searchspecs"] = {"partnum": partnum, **flatten(tables)}
+    
+    output_table["searchspecs"]["id"] = id
+    
 
 
+    print(output_table)
+
+    run_cmd("rm " + output_dir + "/*.json") # not reliable!
+    with open(output_dir + "/" + output_table["searchspecs"]["id"] + ".json", 'w') as json_file:
+        json.dump(output_table["searchspecs"], json_file)
+
+    return output_table
 
 
+def flatten(tables):
+    def convert_to_number(s):
+        try:
+            # First, try converting to an integer.
+            return int(s)
+        except ValueError:
+            # If that fails, try converting to a float.
+            try:
+                return float(s)
+            except ValueError:
+                # If it fails again, return the original string.
+                return s
+    out = dict()
+    print("{")
+    for table in tables.keys():
+        for key in tables[table].keys():
+            if len(key) < 64:
+                keyname = key
+            else:
+                keyname = key[0:64]
 
-    return tables
+            fullkeyname = (table + ": " + keyname).replace(".","")
+            if type(tables[table][key]) is not tuple:
+                out[fullkeyname] = convert_to_number(tables[table][key])
+                print("\"" + keyname + "\":", "\"" + str(out[fullkeyname]) + "\",")
+            elif len(tables[table][key]) == 1:
+                out[fullkeyname] = convert_to_number(tables[table][key][0])
+                
+                print("\"" + keyname + "\":", "\"" + str(out[fullkeyname]) + "\",")
+
+    print("}")
+    return out
 
     
 
 if __name__ == "__main__":
-    parse("test2.pdf", "10GXS13")
+    parse("test2.pdf", "cables/10GXS13", "10GXS13")

requirements.txt

@@ -5,13 +5,15 @@ pypdf2==2.12.1
 alive-progress
 requests
 git+https://github.com/Byeongdulee/python-urx.git
-psycopg2
+psycopg2-binary
 pyyaml
 Flask
 selenium
 sacn
 uptime
 websockets
+numpy
+scipy
 
 # Development
 matplotlib

ur5_control.py

@@ -1,6 +1,8 @@
 import urx
 import math3d as m3d
+from scipy.optimize import fsolve
 import math
+import numpy as np
 import time
 import os
 import logging
@@ -8,6 +10,9 @@ from urx.robotiq_two_finger_gripper import Robotiq_Two_Finger_Gripper
 import sys
 from util import fprint
 
+
+
+
 rob = None
 
 
@@ -45,7 +50,7 @@ def init(ip):
     time.sleep(0.2)
     fprint("UR5 ready.")
 
-def set_pos_abs(x, y, z, xb, yb, zb):
+def set_pos_abs(x, y, z, xb, yb, zb, threshold=None):
     global rob
     new_orientation = m3d.Transform()
     new_orientation.orient.rotate_xb(xb)  # Replace rx with the desired rotation around X-axis
@@ -60,7 +65,7 @@ def set_pos_abs(x, y, z, xb, yb, zb):
     new_trans.pos.y = y
     new_trans.pos.z = z
     #rob.speedj(0.2, 0.5, 99999)
-    rob.set_pose(new_trans, acc=2, vel=2, command="movej")  # apply the new pose
+    rob.set_pose(new_trans, acc=2, vel=2, command="movej", threshold=threshold)  # apply the new pose
 
 def set_pos_rel_rot_abs(x, y, z, xb, yb, zb):
     global rob
@@ -80,21 +85,241 @@ def set_pos_rel_rot_abs(x, y, z, xb, yb, zb):
     #rob.speedj(0.2, 0.5, 99999)
     rob.set_pose(new_trans, acc=0.1, vel=0.4, command="movej")  # apply the new pose
 
+def set_pos_abs_rot_rel(x, y, z, xb, yb, zb):
+    global rob
+    new_orientation = m3d.Transform()
+    new_orientation.orient.rotate_xb(xb)  # Replace rx with the desired rotation around X-axis
+    new_orientation.orient.rotate_yb(yb)  # Replace ry with the desired rotation around Y-axis
+    new_orientation.orient.rotate_zb(zb)  # Replace rz with the desired rotation around Z-axis
+
+    # Get the current pose
+    trans = rob.getl()
+    
+    # Apply the new orientation while keeping the current position
+    new_trans = m3d.Transform(new_orientation.orient, m3d.Vector(trans[0:3]))
+    new_trans.pos.x = x
+    new_trans.pos.y = y
+    new_trans.pos.z = z
+    #rob.speedj(0.2, 0.5, 99999)
+    rob.set_pose(new_trans, acc=0.1, vel=0.4, command="movej")  # apply the new pose
 
 
+def is_safe_move(start_pose, end_pose, r=0.25):
+    start_x, start_y = (start_pose[0], start_pose[1])
+    end_x, end_y = (end_pose[0], end_pose[1])
+
+    try:
+        m = (end_y-start_y)/(end_x-start_x)
+        b = start_y - m*start_x
+        # print('m = y/x =', m)
+        # print('b =', b)
+    except:
+        m = (end_x-start_x)/(end_y-start_y)
+        b = start_x - m*start_y
+        # print('m = x/y =', m)
+        # print('b =', b)
+    
+    return r**2 - b**2 + m**2 * r**2 < 0
+
+def cartesian_to_polar(x, y):
+    r = np.sqrt(x**2 + y**2)
+    theta = np.arctan2(y, x)
+    return r, theta
+
+def polar_to_cartesian(r, theta):
+    x = r * np.cos(theta)
+    y = r * np.sin(theta)
+    return x, y
+
+
+def move_to_polar(start_pos, end_pos):
+    global rob
+
+    # Convert to polar coordinates
+    start_r, start_theta = cartesian_to_polar(start_pos[0], start_pos[1])
+    end_r, end_theta = cartesian_to_polar(end_pos[0], end_pos[1])
+
+    # Interpolate for xy (spiral arc)
+    n_points = 30
+    r_intermediate = np.linspace(start_r, end_r, n_points)
+    theta_intermediate = np.linspace(start_theta, end_theta, n_points)
+
+    # Interpolate for z (height)
+    start_z = start_pos[2]
+    end_z = end_pos[2]
+
+    z_intermediate = np.linspace(start_z, end_z, n_points)
+
+    # Interpolate for rz (keep tool rotation fixed relative to robot)
+
+
+    curr_rot = rob.getl()
+
+    theta_delta = theta_intermediate[1]-theta_intermediate[0]
+    
+    rx_intermediate = [curr_rot[5] + theta_delta*i for i in range(n_points)]
+
+    # curr_rot = rob.getj()
+    # start_rz = curr_rot[5]
+    # rot = end_theta - start_theta
+    # end_base_joint = curr_rot[0]-start_theta + rot
+
+    # end_rz = curr_rot[0] + rot
+    # # rob.movel([*polar_to_cartesian(end_r, end_theta), *rob.getl()[2:]], acc=2, vel=2)
+
+    # print('start_theta = ', math.degrees(start_theta))
+    # print('end_theta = ', math.degrees(curr_rot[0]-start_theta+rot))
+    # print('start_rz =', math.degrees(start_rz))
+    # print('rot =', math.degrees(rot))
+    # print('end_rz =', math.degrees(end_rz))
+
+    # rz_intermediate = np.linspace(start_rz, end_rz, n_points)
+
+    # Convert back to cartesian coordinates
+    curr_pos = rob.getl()
+
+    intermediate_points = [[*polar_to_cartesian(r, theta), z, *curr_pos[3:]]
+                            for r, theta, z, rx in zip(r_intermediate, 
+                                                       theta_intermediate, 
+                                                       z_intermediate, 
+                                                       rx_intermediate)]
+
+    # Move robot
+    rob.movels(intermediate_points, acc=2, vel=2, radius=0.1)
+    
+
+    return rx_intermediate
+
+def move_to_home():
+    global rob
+
+    # Home position in degrees
+    home_pos = [0.10421807948612624, 
+                -2.206111555015423, 
+                1.710679229503537, 
+                -1.075834511928354, 
+                -1.569301366430687, 
+                1.675098295930943]
+
+    # Move robot
+    rob.movej(home_pos, acc=2, vel=2)
+
+def normalize_degree(theta):
+    # Normalizes degree theta from -1.5pi to 1.5pi
+    multiplier = 1
+    normalized_theta = theta % (math.pi * multiplier)
+    
+    # Maintain the negative sign if the original angle is negative
+    if theta < 0:
+        normalized_theta -= math.pi * multiplier
+
+    # 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)
+    
+    # Get length of each limb
+    l1, l2, l3 = limbs
+    
+    # Formulas to find out joint positions for (r, z)
+    def inv_kin_r_z(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
+                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)]]
+
+    # Return result
+    return base, shoulder, elbow, wrist
+
+def get_joints_from_xyz_abs(x, y, z):
+    joints = get_joints_from_xyz_rel(x, y, z)
+
+    # Joint offsets
+    # Base, Shoulder, Elbow, Wrist
+    inverse = [1, -1, 1, 1]
+    offsets = [0, 0, 0, -math.pi/2]
+
+    # Return adjusted joint positions
+    return [o+j*i for j, o, i in zip(joints, offsets, inverse)]
 
 if __name__ == "__main__":
     
     #rob.movej((0, 0, 0, 0, 0, 0), 0.1, 0.2)
     #rob.movel((x, y, z, rx, ry, rz), a, v)
     init("192.168.1.145")
-    fprint("Current tool pose is: ",  rob.getl())
-    #set_pos_rel_rot_abs(0, 0, -0.2, math.pi, 0, -math.pi)
-    set_pos_abs(0.3, -0.2, 0.5, math.pi, 0, -math.pi)
-    set_pos_abs(0, 0.2, 0.6, math.pi, 0, -math.pi)
-    set_pos_abs(-0.5, -0.2, 0.4, math.pi, 0, -math.pi)
-    #set_pos_rel_rot_abs(0, 0, 0, math.pi, 0, -math.pi)
-    fprint("Current tool pose is: ",  rob.getl())
+    print("Current tool pose is: ",  rob.getl())
+    move_to_home()
+
+    home_pose = [-0.4999999077032916, 
+               -0.2000072960336574, 
+               0.40002172976662786, 
+               0, 
+               -3.14152741295329, 
+               0]
+
+    # time.sleep(.5)
+
+    p1 = [0,
+
+        0.6,
+        .4,
+        0.2226,
+        3.1126,
+        0.0510]
+    
+    p2 = [0.171,
+        -0.115,
+        0.2,
+        0.2226,
+        3.1126,
+        0.0510]
+
+    curr_pos = rob.getl()
+    # up/down, 
+    # tool rotation
+    # tool angle (shouldn't need)
+    # rob.set_pos(p1[0:3], acc=0.5, vel=0.5)
+
+    # 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.3, 0.4, 0.2)
+    rob.movej([*angles, *rob.getj()[4:]], acc=1, vel=1)
+
+
+    # set_pos_abs(*p1)
+    # move = move_to_polar(p1, p2)
+
+
+    # for p in move:
+    #     print(math.degrees(p))
+    # print("Safe? :", is_safe_move(p1, p2))
+
+
+    # #set_pos_rel_rot_abs(0, 0, -0.2, math.pi, 0, -math.pi)
+    # set_pos_abs(0.3, -0.2, 0.5, math.pi, 0, -math.pi)
+    # set_pos_abs(0, 0.2, 0.6, math.pi, 0, -math.pi)
+    # set_pos_abs(-0.5, -0.2, 0.4, math.pi, 0, -math.pi)
+    # #set_pos_rel_rot_abs(0, 0, 0, math.pi, 0, -math.pi)
+
+    # print("Current tool pose is: ",  rob.getl())
+    # print("getj(): ",  rob.getj())
+
+    # move_to_home()
+
     rob.stop()
     os.kill(os.getpid(), 9) # dirty kill of self
     sys.exit(0)

util.py

@@ -123,7 +123,7 @@ class Logger(object):
         self.terminal = sys.stdout
 
     def write(self, message):
-        self.log.write(message)
+        #self.log.write(message)
         #close(filename)
         #self.log = open(filename, "a")
         try: