Add setup-alpine-vm.sh

Dockerfile

@@ -3,11 +3,11 @@ FROM python:3.11-slim
 # Get runtime dependencies
 # glx for OpenCV, ghostscript for datasheet PDF rendering, zbar for barcode scanning, git for cloning repos
 RUN apt-get update && apt-get install -y libgl1-mesa-glx ghostscript libzbar0 git && apt-get clean && rm -rf /var/lib/apt/lists
-COPY *.py *.yml *.sh *.txt *.html static templates ./
+COPY requirements.txt ./
 #COPY config-server.yml config.yml
 RUN pip3 install -r requirements.txt
-
-CMD ["python3", "run.py"]
+COPY *.py *.yml *.sh *.txt *.html static templates ./
+CMD ["sh", "-c", "python3 run.py"]
 EXPOSE 5000
 EXPOSE 8000
 EXPOSE 9000

compose.yml

@@ -8,6 +8,23 @@ services:
       MEILI_NO_ANALYTICS: true
     volumes:
       - "meili_data:/meili_data"
+  
+  jukebox-software:
+    build: .
+    init: true
+    ports:
+      - "5000:5000"
+      - "8000:8000"
+      - "9000:9000"
+    environment:
+      - PYTHONUNBUFFERED=1
+    depends_on:
+      - meilisearch
+  
+  jukebox-web:
+    build: jukebox-web
+    ports:
+      - "3000:3000"
 
 volumes:
   meili_data:

config.yml

@@ -6,6 +6,17 @@ core:
 
 arm:
   ip: 192.168.1.145
+  tool:
+    offset_x: 0
+    offset_y: 0
+    offset_z: 0.14
+  limbs:
+    limb_base: 0.105
+    limb1: 0.425
+    limb2: 0.39225
+    limb3: 0.1
+    limb_wrist: 0.0997
+
 
 #cable_map:
 cameras:

get_specs.py

@@ -159,8 +159,8 @@ def touch(path):
 
 
 
-def get_multi(partnums, delay=0.25, dir="cables/", cache=True):
-    with alive_bar(len(partnums) * 2, dual_line=True, calibrate=30, bar="classic2", spinner="classic") as bar:
+def get_multi(partnums, delay=0.25, dir="cables/", cache=True, bar=None):
+    #with alive_bar(len(partnums) * 2, dual_line=True, calibrate=30, bar="classic2", spinner="classic", disable=True, file=sys.stdout) as bar:
         failed = list()
         actualpartnums = list()
         def _try_download_datasheet(partnum, output_dir, dstype): # Guess datasheet URL
@@ -188,7 +188,7 @@ def get_multi(partnums, delay=0.25, dir="cables/", cache=True):
                             # and set chunk_size parameter to None.
                             #if chunk: 
                             bartext = bartext + "."
-                            bar.text = bartext
+                            # bar.text = bartext
                             f.write(chunk)
                 #fprint("")
                 return output_dir + "/datasheet.pdf"
@@ -217,7 +217,7 @@ def get_multi(partnums, delay=0.25, dir="cables/", cache=True):
                             # and set chunk_size parameter to None.
                             #if chunk: 
                             bartext = bartext + "."
-                            bar.text = bartext
+                            # bar.text = bartext
                             f.write(chunk)
                 #fprint("")
                 return output_dir + "/datasheet.pdf"
@@ -244,7 +244,7 @@ def get_multi(partnums, delay=0.25, dir="cables/", cache=True):
                             # and set chunk_size parameter to None.
                             #if chunk: 
                             bartext = bartext + "."
-                            bar.text = bartext
+                            # bar.text = bartext
                             f.write(chunk)
                 #fprint("")
                 return output_dir + "/part-hires." + url.split(".")[-1]
@@ -255,29 +255,29 @@ def get_multi(partnums, delay=0.25, dir="cables/", cache=True):
 
         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)
+            # bar.text = "Using cached datasheet for " + partnum
+            # bar(skipped=True)
             if not os.path.exists(output_dir + "/parsed"):
                 
                 fprint("Parsing Datasheet contents of " + partnum)
-                bar.text = "Parsing Datasheet contents of " + partnum + ".pdf..."
+                # bar.text = "Parsing Datasheet contents of " + partnum + ".pdf..."
             
                 out = read_datasheet.parse(path, output_dir, partnum, dstype)
-                bar(skipped=False) 
+                # bar(skipped=False) 
                 return out
             else:
                 fprint("Datasheet already parsed for " + partnum)
-                bar.text = "Datasheet already parsed for " + partnum + ".pdf"
-                bar(skipped=True)
+                # bar.text = "Datasheet already parsed for " + partnum + ".pdf"
+                # bar(skipped=True)
 
         def __downloaded_datasheet(partnum, path, output_dir, dstype):
             fprint("Downloaded " + path)
-            bar.text = "Downloaded " + path
-            bar(skipped=False)
+            # bar.text = "Downloaded " + path
+            # bar(skipped=False)
             fprint("Parsing Datasheet contents of " + partnum)
-            bar.text = "Parsing Datasheet contents of " + partnum + ".pdf..."
+            # bar.text = "Parsing Datasheet contents of " + partnum + ".pdf..."
             out = read_datasheet.parse(path, output_dir, partnum, dstype)
-            bar(skipped=False)
+            # bar(skipped=False)
             return out
 
         def run_search(partnum):
@@ -290,7 +290,7 @@ def get_multi(partnums, delay=0.25, dir="cables/", cache=True):
             output_dir = dir + partnum
             path = output_dir + "/datasheet.pdf"
             bartext = "Downloading files for part " + partnum
-            bar.text = bartext
+            # bar.text = bartext
             partnum = oldpartnum.replace("_","/")
             returnval = [partnum, dstype, False, False]
             if (not os.path.exists(output_dir + "/found_part_hires")) or not (os.path.exists(path) and os.path.getsize(path) > 1) or not cache:
@@ -305,7 +305,7 @@ def get_multi(partnums, delay=0.25, dir="cables/", cache=True):
                     output_dir = dir + partnum
                     path = output_dir + "/datasheet.pdf"
                     bartext = "Downloading files for part " + partnum
-                    bar.text = bartext
+                    # bar.text = bartext
 
                     if not os.path.exists(output_dir + "/found_part_hires") or not cache:
                         if _download_image(search_result["image"], output_dir):
@@ -344,9 +344,13 @@ def get_multi(partnums, delay=0.25, dir="cables/", cache=True):
                 fprint("Using cached hi-res part image for " + partnum)
                 out = __use_cached_datasheet(partnum, path, output_dir, dstype)
                 returnval = [partnum, dstype, False, out]
+                actualpartnums.append(returnval)
                 return True
     
         for fullpartnum in partnums:
+            if fullpartnum is False:
+                actualpartnums.append(False)
+                continue
             if fullpartnum[0:2] == "BL": # catalog.belden.com entry
                 partnum = fullpartnum[2:]
                 dstype = "Belden"
@@ -373,19 +377,19 @@ def get_multi(partnums, delay=0.25, dir="cables/", cache=True):
                             time.sleep(delay)
                 if not success:
                     fprint("Failed to download datasheet for part " + partnum)
-                    bar.text = "Failed to download datasheet for part " + partnum
+                    # bar.text = "Failed to download datasheet for part " + partnum
                     failed.append((partnum, dstype))
-                    bar(skipped=True)
-                    bar(skipped=True)
+                    # bar(skipped=True)
+                    # bar(skipped=True)
             time.sleep(delay)
             
-    if len(failed) > 0:
-        fprint("Failed to download:")
-        for partnum in failed:
-            fprint(partnum[1] + " " + partnum[0])
-        return False, actualpartnums # Go to manual review upload page
-    else:
-        return True, actualpartnums # All cables downloaded; we are good to go
+        if len(failed) > 0:
+            fprint("Failed to download:")
+            for partnum in failed:
+                fprint(partnum[1] + " " + partnum[0])
+            return False, actualpartnums # Go to manual review upload page
+        else:
+            return True, actualpartnums # All cables downloaded; we are good to go
 
 
 
@@ -408,7 +412,7 @@ if __name__ == "__main__":
     # ]
     partnums = [
     # Actual cables in Jukebox
-     
+    "BL3092A",
     "AW86104CY",
     "AW3050",
     "AW6714",
@@ -438,8 +442,9 @@ if __name__ == "__main__":
     "BL6300FE 009Q",
     "BLRA500P 006Q",
 
-
+    ]
     # Some ones I picked, including some invalid ones
+    a = [
     "BL10GXS12", 
     "BLRST%205L-RKT%205L-949",
     "BL10GXS13",
@@ -455,7 +460,7 @@ if __name__ == "__main__":
     "BLFISX006W0", # datasheet only
     "BLFISX00103", # invalid
     "BLC6D1100007" # invalid
-
+    
     ]
     #print(query_search("TT-SLG-024-HTNN", "Belden"))
     from label_generator import gen_label

inv_kin_testing.ipynb

@@ -9,7 +9,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 81,
+   "execution_count": 1,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -28,7 +28,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 82,
+   "execution_count": 2,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -52,21 +52,21 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 83,
+   "execution_count": 3,
    "metadata": {},
    "outputs": [
     {
      "data": {
       "text/plain": [
-       "[-243.619306486927,\n",
-       " -123.2097721836616,\n",
-       " 140.34764917140853,\n",
-       " -107.13787698774695,\n",
+       "[82.5301003420473,\n",
+       " -83.54918182551367,\n",
+       " 111.24503912126411,\n",
+       " -117.69585729575046,\n",
        " -90.0,\n",
        " 90.0]"
       ]
      },
-     "execution_count": 83,
+     "execution_count": 3,
      "metadata": {},
      "output_type": "execute_result"
     }
@@ -76,7 +76,6 @@
     "    # 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",
@@ -146,10 +145,12 @@
     "    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",
+    "    robot_angles = [o+j*i for j, o, i in zip(joints, offsets, inverse)]\n",
+    "\n",
+    "    return robot_angles\n",
     "\n",
     "# Print degree rotation for each joint (robot angles)\n",
-    "[math.degrees(deg) for deg in get_joints_from_xyz_abs(0, -0.3, 0.1)]"
+    "[math.degrees(deg) for deg in get_joints_from_xyz_abs(-0.2, -0.5, 0.1)]"
    ]
   },
   {
@@ -161,7 +162,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 84,
+   "execution_count": 4,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -229,7 +230,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 5,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -321,25 +322,23 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 89,
+   "execution_count": 6,
    "metadata": {},
    "outputs": [
     {
      "name": "stdout",
      "output_type": "stream",
      "text": [
-      "Target position (x,y,z): 0.5 0.5 0.3\n",
-      "R:  0.7071\n",
-      "Angles (base, shoulder, elbow, wrist): [-34.1339, 65.0453, 57.0325, 8.0128]\n",
-      "Robot Angles: [-124.1339, -65.0453, 57.0325, -81.9872, -90.0, 90.0]\n",
-      "elbow (x,y): 0.179 0.385\n",
-      "wrist (x,y): 0.568 0.44\n",
-      "tool  (x,y): 0.694 0.44\n"
+      "Angles: [-53.867, 89.583, 87.39, 2.193, -90.0, 0.0]\n",
+      "Circle position (cx, cy): (0.079, -0.108)\n",
+      "Shoulder (x, y, z): (0.081, -0.106, 0.425)\n",
+      "Elbow    (x, y, z): (0.398, 0.125, 0.44)\n",
+      "Wrist    (x, y, z): (0.5, 0.2, 0.44)\n"
      ]
     },
     {
      "data": {
-      "image/png": "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 truncated
+      "image/png": "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 truncated
       "text/plain": [
        "<Figure size 640x480 with 1 Axes>"
       ]
@@ -351,16 +350,18 @@
      "name": "stdout",
      "output_type": "stream",
      "text": [
-      "Angles: [-34.134, 65.045, 57.033, 8.013, -90.0, 0.0]\n",
-      "Circle position (cx, cy): (0.11, -0.075)\n",
-      "Shoulder (x, y, z): (0.211, 0.074, 0.385)\n",
-      "Elbow    (x, y, z): (0.429, 0.395, 0.44)\n",
-      "Wrist    (x, y, z): (0.5, 0.5, 0.44)\n"
+      "Target position (x,y,z): 0.5 0.2 0.3\n",
+      "R:  0.5385\n",
+      "Angles (base, shoulder, elbow, wrist): [-53.8671, 89.5827, 87.3895, 2.1932]\n",
+      "Robot Angles: [-143.8671, -89.5827, 87.3895, -87.8068, -90.0, 90.0]\n",
+      "elbow (x,y): 0.003 0.425\n",
+      "wrist (x,y): 0.395 0.44\n",
+      "tool  (x,y): 0.522 0.44\n"
      ]
     },
     {
      "data": {
-      "image/png": "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 truncated
+      "image/png": "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 truncated
       "text/plain": [
        "<Figure size 640x480 with 1 Axes>"
       ]
@@ -371,10 +372,74 @@
    ],
    "source": [
     "def draw_arm(x, y, z, details=False):\n",
-    "    draw_arm_side_view(x,y,z, details=details)\n",
     "    draw_arm_top_view(x,y,z, details=details)\n",
+    "    draw_arm_side_view(x,y,z, details=details)\n",
     "\n",
-    "draw_arm(0.5,0.5,0.3, details=True)"
+    "draw_arm(0.5,0.2,0.3, details=True)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 10,
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "text/plain": [
+       "{'elbow': (0.06441407671759522, -0.08297834710053949, 0.42498873000714044),\n",
+       " 'wrist': (0.3809831975755794, 0.14814654552767514, 0.44000000000002354),\n",
+       " 'wrist2': (0.4617483203289997, 0.20711259462904327, 0.44000000000002354),\n",
+       " 'tool': (0.4617483203289997, 0.20711259462904327, 0.5800000000000236)}"
+      ]
+     },
+     "execution_count": 10,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "def joint_positions(x, y, z):\n",
+    "    # Get joint and position information\n",
+    "    angles = get_joints_from_xyz_abs(x, y, z)\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",
+    "\n",
+    "    # Elbow\n",
+    "    x1, y1 = polar_to_cartesian(l1*math.cos(angles[1]), line_angle)\n",
+    "    x1, y1 = cx+x1, cy+y1\n",
+    "    z1 = l1*math.sin(angles[1])\n",
+    "\n",
+    "    # Wrist\n",
+    "    x2, y2 = polar_to_cartesian(l2*math.cos(angles[1]-angles[2]), line_angle)\n",
+    "    x2 += x1\n",
+    "    y2 += y1\n",
+    "    z2 = z1 + l2*math.sin(angles[1]-angles[2])\n",
+    "\n",
+    "    # Wrist 2\n",
+    "    x3, y3 = polar_to_cartesian(l3*math.cos(angles[1]-angles[2]-angles[3]), line_angle)\n",
+    "    x3 += x2\n",
+    "    y3 += y2\n",
+    "    z3 = z2 + l3*math.sin(angles[1]-angles[2]-angles[3]) \n",
+    "\n",
+    "    # Tool\n",
+    "    x4, y4 = polar_to_cartesian(offset_z*math.cos(angles[1]-angles[2]-angles[3]-angles[4]), line_angle)\n",
+    "    x4 += x3\n",
+    "    y4 += y3\n",
+    "    z4 = z3 + offset_z*math.sin(angles[1]-angles[2]-angles[3]-angles[4])\n",
+    "\n",
+    "\n",
+    "    positions = {\n",
+    "        'elbow': (x1, y1, z1),\n",
+    "        'wrist': (x2, y2, z2),\n",
+    "        'wrist2': (x3, y3, z3),\n",
+    "        'tool': (x4, y4, z4)\n",
+    "    }\n",
+    "\n",
+    "    return positions\n",
+    "\n",
+    "joint_positions(0.5,0.2,0.3)"
    ]
   }
  ],

read_datasheet.py

@@ -177,7 +177,7 @@ def parse(filename, output_dir, partnum, dstype):
             if dstype == "Alphawire" and table_name_2.find("\n") >= 0:
                 torename[table_name_2] = table_name_2[0:table_name_2.find("\n")]
 
-            if table_name_2.find(table.iloc[-1, 0]) >= 0:
+            if dstype == "Alphawire" and table_name_2.find(table.iloc[-1, 0]) >= 0:
                 # Name taken from table directly above - this table does not have a name
                 torename[table_name_2] = "Specs " + str(len(tables))
                 #table_list["Specs " + str(len(tables))] = table_list[table_name_2] # rename table to arbitrary altername name
@@ -251,9 +251,6 @@ def parse(filename, output_dir, partnum, dstype):
                 #fprint(table_name)
                 #fprint(previous_table)
                 
-                
-                
-                
                 main_key = previous_table
                 cont_key = table_name
                 #fprint(tables)
@@ -267,15 +264,21 @@ def parse(filename, output_dir, partnum, dstype):
                     del tables[table_name]
     
                 else:
+                    #print(tables)
+                    #print(main_key)
+                    #print(cont_key)
                     for key in tables[cont_key].keys():
                         tables[main_key][key] = tables[cont_key][key]
                     del tables[table_name]
-
-        previous_table = table_name
+            else:
+                previous_table = table_name
+        else:
+            previous_table = table_name
     
     # remove & rename tables
+    #print(torename)
     for table_name in torename.keys():
-        tables[torename[table_name]] = tables[table_name]
+        tables[torename[str(table_name)]] = tables[str(table_name)]
         del tables[table_name]
     # remove multi-line values that occasionally squeak through
     def replace_newlines_in_dict(d):
@@ -298,7 +301,7 @@ def parse(filename, output_dir, partnum, dstype):
     output_table["id"] = id
     #output_table["position"] = id
     #output_table["brand"] = brand
-    output_table["fullspecs"] = tables
+    output_table["fullspecs"] = {"partnum": partnum, "id": id, **tables}
     output_table["searchspecs"] = {"partnum": partnum, **flatten(tables)}
     
     output_table["searchspecs"]["id"] = id
@@ -313,9 +316,9 @@ def parse(filename, output_dir, partnum, dstype):
     for file_path in json_files:
         os.remove(file_path)
         #print(f"Deleted {file_path}")
-    with open(output_dir + "/search_" + output_table["searchspecs"]["id"] + ".json", 'w') as json_file:
+    with open(output_dir + "/search.json", 'w') as json_file:
         json.dump(output_table["searchspecs"], json_file)
-    with open(output_dir + "/specs_" + output_table["partnum"] + ".json", 'w') as json_file:
+    with open(output_dir + "/specs.json", 'w') as json_file:
         json.dump(output_table["fullspecs"], json_file)
 
     #print(json.dumps(output_table, indent=2))
@@ -346,12 +349,20 @@ def flatten(tables):
 
             fullkeyname = (table + ": " + keyname).replace(".","")
             if type(tables[table][key]) is not tuple:
-                out[fullkeyname] = convert_to_number(tables[table][key])
+                if len(tables[table][key]) > 0:
+                    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])
+                if len(tables[table][key][0]) > 0:
+                    out[fullkeyname] = convert_to_number(tables[table][key][0])
                 #print("\"" + keyname + "\":", "\"" + str(out[fullkeyname]) + "\",")
-
+            else:
+                tmp = []
+                for x in range(len(tables[table][key])):
+                    if len(tables[table][key][x]) > 0:
+                        tmp.append(tables[table][key][x].strip())
+                        #out[fullkeyname + " " + str(x+1)] = convert_to_number(tables[table][key][x])
+                out[fullkeyname] = tmp
             # if the item has at least two commas in it, split it
             if tables[table][key].count(',') > 0:
                 out[fullkeyname] = list(map(lambda x: x.strip(), tables[table][key].split(",")))

run.py

@@ -1,5 +1,6 @@
 #!/usr/bin/env python3
 
+from alive_progress import alive_bar
 import get_specs
 import traceback
 #import logging
@@ -12,17 +13,20 @@ from util import fprint
 from util import run_cmd
 import sys
 import ur5_control
+from ur5_control import Rob
 import os
 import signal
 import socket
 from flask import Flask, render_template, request
 import requests
-import led_control
+from led_control import LEDSystem
 import server
 import asyncio
 import json
 import process_video
-
+import search
+from search import JukeboxSearch
+#multiprocessing.set_start_method('spawn', True)
 
 
 config = None
@@ -32,13 +36,22 @@ led_ready = False
 camera_ready = False
 sensor_ready = False
 vm_ready = False
+cable_search_ready = False
 killme = None
 #pool = None
 serverproc = None
 camera = None
-
+ledsys = None
+arm = None
 to_server_queue = Queue()
 from_server_queue = Queue()
+mode = "Startup"
+counter = 0
+jbs = None
+scan_value = None
+arm_state = None
+cable_list = list()
+parse_res = None
 
 def arm_start_callback(res):
     global arm_ready
@@ -47,10 +60,14 @@ def arm_start_callback(res):
 def led_start_callback(res):
     global led_ready
     led_ready = True
+    global ledsys
+    ledsys = res
 
 def camera_start_callback(res):
     global camera_ready
     camera_ready = True
+    global scan_value
+    scan_value = res
     
 def sensor_start_callback(res):
     global sensor_ready
@@ -60,6 +77,12 @@ def vm_start_callback(res):
     global vm_ready
     vm_ready = True
 
+def cable_search_callback(res):
+    global cable_search_ready
+    cable_search_ready = True
+    global parse_res 
+    parse_res = res
+
 def wait_for(val, name):
     #global val
     if val is False:
@@ -67,7 +90,15 @@ def wait_for(val, name):
         while val is False:
             sleep(0.1)
 
+def send_data(type, call, data, client_id="*"):
+    out = dict()
+    out["type"] = type
+    out["call"] = call
+    out["data"] = data
+    to_server_queue.put((client_id, json.dumps(out)))
+
 def start_server_socket():
+    global jbs
     """app = Flask(__name__)
 
     @app.route('/report_ip', methods=['POST'])
@@ -99,92 +130,99 @@ def start_server_socket():
             # Message handler
             try:
                 decoded = json.loads(message)
-                if "type" not in decoded:
-                    fprint("Missing \"type\" field.")
-                    continue
-                if "call" not in decoded:
-                    fprint("Missing \"call\" field.")
-                    continue
-                if "data" not in decoded:
-                    fprint("Missing \"data\" field.")
-                    continue
-
-                # if we get here, we have a "valid" data packet
-                data = decoded["data"]
-                call = decoded["call"]
-                match decoded["type"]:
-                    case "log":
-                        fprint("log message")
-                        if call == "send":
-                            fprint("webapp: " + str(data), sendqueue=to_server_queue)
-                        elif call == "request":
-                            fprint("")
-
-                    case "cable_map":
-                        fprint("cable_map message")
-                        if call == "send":
-                            fprint("")
-                        elif call == "request":
-                            fprint("")
-
-
-                    case "ping":
-                        fprint("Pong!!!")
-
-                    # Lucas' notes
-                    # Add a ping pong :) response/handler
-                    # Add a get cable response/handler
-                    #       this will tell the robot arm to move
-                    # Call for turning off everything
-
-                    # TODO Helper for converting Python Dictionaries to JSON
-                    # make function: pythonData --> { { "type": "...", "call": "...", "data": pythonData } }
-                            
-                    # to send: to_server_queue.put(("*", "JSON STRING HERE")) # replace * with UUID of client to send to one specific location
-                    
-                    case "cable_details":
-                        fprint("cable_details message")
-                        if call == "send":
-                            fprint("")
-                        elif call == "request":
-                            fprint("")
-
-                    case "cable_search":
-                        fprint("cable_search message")
-                        if call == "send":
-                            fprint("")
-                        elif call == "request":
-                            fprint("")
-
-                    case "keyboard":
-                        fprint("keyboard message")
-                        if call == "send":
-                            fprint("")
-                        elif call == "request":
-                            fprint("")
-                            if data["enabled"] == True:
-                                # todo : send this to client
-                                p = Process(target=run_cmd, args=("./keyboard-up.ps1",))
-                                p.start()
-                            elif data["enabled"] == False:
-                                p = Process(target=run_cmd, args=("./keyboard-down.ps1",))
-                                p.start()
-
-                    case "machine_settings":
-                        fprint("machine_settings message")
-                        if call == "send":
-                            fprint("")
-                        elif call == "request":
-                            fprint("")
-
-                    case _:
-                        fprint("Unknown/unimplemented data type: " + decoded["type"])
-
-
             except:
                 fprint("Non-JSON message recieved")
                 continue
 
+            if "type" not in decoded:
+                fprint("Missing \"type\" field.")
+                continue
+            if "call" not in decoded:
+                fprint("Missing \"call\" field.")
+                continue
+            if "data" not in decoded:
+                fprint("Missing \"data\" field.")
+                continue
+            # if we get here, we have a "valid" data packet
+            data = decoded["data"]
+            call = decoded["call"]
+            match decoded["type"]:
+                case "log":
+                    fprint("log message")
+                    if call == "send":
+                        fprint("webapp: " + str(data), sendqueue=to_server_queue)
+                    elif call == "request":
+                        fprint("")
+                case "cable_map":
+                    fprint("cable_map message")
+                    if call == "send":
+                        fprint("")
+                    elif call == "request":
+                        fprint("")
+                case "ping":
+                    fprint("Pong!!!")
+                # Lucas' notes
+                # Add a ping pong :) response/handler
+                # Add a get cable response/handler
+                #       this will tell the robot arm to move
+                # Call for turning off everything
+                # TODO Helper for converting Python Dictionaries to JSON
+                # make function: pythonData --> { { "type": "...", "call": "...", "data": pythonData } }
+                        
+                # to send: to_server_queue.put(("*", "JSON STRING HERE")) # replace * with UUID of client to send to one specific location
+                
+                case "cable_details":
+                    fprint("cable_details message")
+                    if call == "send":
+                        fprint("")
+                    elif call == "request":
+                        fprint("")
+                        dataout = dict()
+                        dataout["cables"] = list()
+                        print(data)
+                        if "part_number" in data:
+                            for part in data["part_number"]:
+                                #print(part)
+                                #print(jbs.get_partnum(part))
+                                dataout["cables"].append(jbs.get_partnum(part)["fullspecs"])
+                        if "position" in data:
+                            for pos in data["position"]:
+                                #print(pos)
+                                #print(jbs.get_position(str(pos)))
+                                dataout["cables"].append(jbs.get_position(str(pos))["fullspecs"])
+                        send_data(decoded["type"], "send", dataout, client_id)
+                            
+                case "cable_search":
+                    fprint("cable_search message")
+                    if call == "send":
+                        fprint("")
+                    elif call == "request":
+                        fprint("")
+                case "keyboard":
+                    fprint("keyboard message")
+                    if call == "send":
+                        fprint("")
+                    elif call == "request":
+                        fprint("")
+                        if data["enabled"] == True:
+                            # todo : send this to client
+                            p = Process(target=run_cmd, args=("./keyboard-up.ps1",))
+                            p.start()
+                        elif data["enabled"] == False:
+                            p = Process(target=run_cmd, args=("./keyboard-down.ps1",))
+                            p.start()
+                case "machine_settings":
+                    fprint("machine_settings message")
+                    if call == "send":
+                        fprint("")
+                    elif call == "request":
+                        fprint("")
+                case _:
+                    fprint("Unknown/unimplemented data type: " + decoded["type"])
+
+
+            
+
 
         sleep(0.001)  # Sleep to prevent tight loop
     
@@ -231,13 +269,19 @@ def setup_server(pool):
     global arm_ready
     global serverproc
     global camera
-
-    pool.apply_async(ur5_control.init, (config["arm"]["ip"],), callback=arm_start_callback)
-    pool.apply_async(led_control.init, callback=led_start_callback)
+    global arm
+    global jbs
+    arm = Rob(config)
+    pool.apply_async(arm.init_arm, callback=arm_start_callback)
+    global ledsys
+    ledsys = LEDSystem()
+    pool.apply_async(ledsys.init, callback=led_start_callback)
     #pool.apply_async(sensor_control.init, callback=sensor_start_callback)
+    jbs = JukeboxSearch()
     serverproc = Process(target=start_server_socket)
     serverproc.start()
-
+    
+    
     if led_ready is False:
         fprint("waiting for " + "LED controller initialization" + " to complete...", sendqueue=to_server_queue)
         while led_ready is False:
@@ -253,7 +297,7 @@ def setup_server(pool):
 
     if camera_ready is False:
         fprint("waiting for " + "Camera initilization" + " to complete...", sendqueue=to_server_queue)
-        # camera = process_video.qr_reader(config["cameras"]["banner"]["ip"], config["cameras"]["banner"]["port"])
+        camera = process_video.qr_reader(config["cameras"]["banner"]["ip"], config["cameras"]["banner"]["port"])
 
     fprint("Camera initialized.", sendqueue=to_server_queue)
 
@@ -263,25 +307,161 @@ def setup_server(pool):
         while arm_ready is False:
             sleep(0.1)
     fprint("Arm initialized.", sendqueue=to_server_queue)
+
+
     
-    
-    
-        
     return True
 
 
 
 def mainloop_server(pool):
+    # NON-blocking loop
     global config
     global counter
     global killme
+    global mode
+    global jbs
+    global arm
+    global ledsys
+    global camera
+    global arm_ready
+    global arm_state
+    global camera_ready
+    global cable_search_ready
+    global cable_list
 
     if killme.value > 0:
         killall()
-    counter = counter + 1
 
-    # fprint("Looking for QR code...")
-    # print(camera.read_qr(30))
+    if mode == "Startup":
+        counter = 54
+        if counter < 54:
+            # scanning cables
+            
+            if arm_state is None:
+                #pool.apply_async(arm.get cable to camera, callback=arm_start_callback)
+                #ur5_control.goto_holder_index(arm)
+                #ur5 get item
+                # ur5 bring to camera
+                fprint("Getting cable index " + str(counter) + " and scanning...")
+                arm_state = "GET"
+
+            elif arm_ready and arm_state == "GET":
+                fprint("Looking for QR code...")
+                pool.apply_async(camera.read_qr, (30,), callback=camera_start_callback)
+                arm_ready = False
+
+            elif camera_ready:
+                fprint("Adding cable to list...")
+                global scan_value
+                if scan_value is False:
+                    cable_list.append(scan_value)
+                elif scan_value.find("bldn.app/") > -1:
+                    scan_value = scan_value[scan_value.find("bldn.app/")+9:]
+                else:
+                    cable_list.append(scan_value)
+                fprint(scan_value)
+                #pool.apply_async(arm.return cable, callback=arm_start_callback)
+                arm_state = "RETURN"
+                camera_ready = False
+
+            elif arm_ready and arm_state == "RETURN":
+                counter += 1
+                arm_state = None
+            else:
+                # just wait til arm/camera is ready
+                pass
+        else:
+            # scanned everything
+            tmp = [
+                    # Actual cables in Jukebox
+                    "BLTF-1LF-006-RS5",
+                    "BLTF-SD9-006-RI5",
+                    "BLTT-SLG-024-HTN",
+                    "BLFISX012W0",
+                    "BLFI4X012W0",
+                    "BLSPE101",
+                    "BLSPE102",
+                    "BL7922A",
+                    "BL7958A",
+                    "BLIOP6U",
+                    "BL10GXW13",
+                    "BL10GXW53",
+                    "BL29501F",
+                    "BL29512",
+                    "BL3106A",
+                    "BL9841",
+                    "BL3105A",
+                    "BL3092A",
+                    "BL8760",
+                    "BL6300UE",
+                    "BL6300FE",
+                    "BLRA500P",
+                    "AW86104CY",
+                    "AW3050",
+                    "AW6714",
+                    "AW1172C",
+                    "AWFIT-221-1_4"
+                ]
+            while len(tmp) < 54:
+                tmp.append(False) # must have 54 entries
+            cable_list = tmp # remove for real demo
+            pool.apply_async(get_specs.get_multi, (tmp, 0.3), callback=cable_search_callback)
+            mode = "Parsing"
+            fprint("All cables scanned. Finding & parsing datasheets...")
+    if mode == "Parsing":
+            # waiting for search & parse to complete
+            #cable_search_ready = True
+            if cable_search_ready is False:
+                pass
+            else:
+                # done
+                global parse_res
+                success, partnums = parse_res
+                for idx in range(len(partnums)):
+                    if partnums[idx] is not False:
+                        cable_list[idx] = partnums[idx][0].replace("/", "_")
+                    else:
+                        cable_list[idx] = False
+                
+                print(partnums)
+                if success:
+                    # easy mode
+                    fprint("All cables inventoried and parsed.")
+                    fprint("Adding to database...")
+                    for idx in range(len(cable_list)):
+                        partnum = cable_list[idx]
+                        if partnum is not False:
+                            with open("cables/" + partnum + "/search.json", "rb") as f:
+                                searchdata = json.load(f)
+                                searchdata["position"] = idx
+                            with open("cables/" + partnum + "/specs.json", "rb") as f:
+                                specs = json.load(f)
+                            searchdata["fullspecs"] = specs
+                            searchdata["fullspecs"]["position"] = idx
+                            jbs.add_document(searchdata)
+                    #sleep(0.5)
+                    #print(jbs.get_position("1"))
+                    
+                    fprint("All cables added to database.")
+                    mode = "Idle"
+                else:
+                    # TODO: manual input
+                    pass
+
+            
+    if mode == "Idle":
+        # do nothing
+        if arm_ready is False:
+            pool.apply_async(ur5_control.move_to_home, (arm,), callback=arm_start_callback)
+            arm_ready = True
+
+        else:
+            # LED idle anim
+            pass
+
+            
+
 
 def run_loading_app():
     
@@ -401,7 +581,7 @@ if __name__ == "__main__":
     with open('config.yml', 'r') as fileread:
         #global config
         config = yaml.safe_load(fileread)
-
+    fprint("Config loaded.")
     with Manager() as manager:
         fprint("Spawning threads...")
         pool = LoggingPool(processes=10)
@@ -422,5 +602,12 @@ if __name__ == "__main__":
                 fprint("Entering main loop...")
                 while(keeprunning):
                     mainloop_server(pool)
+        else:
+            fprint("Mode unspecified - assuming server")
+            fprint("Starting in server mode.")
+            if setup_server(pool):
+                fprint("Entering main loop...")
+                while(keeprunning):
+                    mainloop_server(pool)
 
 

search.py

@@ -1,10 +1,12 @@
+#!/usr/bin/env python3
+
 """Interactions with the Meilisearch API for adding and searching cables."""
 from meilisearch import Client
 from meilisearch.task import TaskInfo
 from meilisearch.errors import MeilisearchApiError
-import json
+import time
 
-DEFAULT_URL = "http://localhost:7700"
+DEFAULT_URL = "http://meilisearch:7700"
 DEFAULT_APIKEY = "fluffybunnyrabbit" # I WOULD RECOMMEND SOMETHING MORE SECURE
 DEFAULT_INDEX = "cables"
 DEFAULT_FILTERABLE_ATTRS = ["partnum", "uuid", "position"] # default filterable attributes
@@ -34,12 +36,15 @@ class JukeboxSearch:
         # create the index if it does not exist already
         try:
             self.client.get_index(self.index)
+            self.client.delete_index(self.index)
+            self.client.create_index(self.index)
         except MeilisearchApiError as _:
             self.client.create_index(self.index)
         # make a variable to easily reference the index
         self.idxref = self.client.index(self.index)
-
+        time.sleep(0.05)
         # update filterable attributes if needed
+        self.idxref.update_distinct_attribute('partnum')
         self.update_filterables(filterable_attrs)
 
     def add_document(self, document: dict) -> TaskInfo:
@@ -65,11 +70,10 @@ class JukeboxSearch:
         
         :param filterables: List of all filterable attributes"""
 
-        existing_filterables = self.idxref.get_filterable_attributes()
-        if len(set(existing_filterables).difference(set(filterables))) > 0:
-            taskref = self.idxref.update_filterable_attributes(filterables)
-
-            self.client.wait_for_task(taskref.index_uid)
+        #existing_filterables = self.idxref.get_filterable_attributes()
+        #if len(set(existing_filterables).difference(set(filterables))) > 0:
+        taskref = self.idxref.update_filterable_attributes(filterables)
+        #self.client.wait_for_task(taskref.index_uid)
 
     def search(self, query: str, filters: str = None):
         """Execute a search query on the Meilisearch index.
@@ -90,7 +94,7 @@ class JukeboxSearch:
         :returns: A dict containing the results; If no results found, an empty dict."""
         q = self.search("", filter)
         if q["estimatedTotalHits"] != 0:
-            return ["hits"][0]
+            return q["hits"][0]
         else:
             return dict()
 
@@ -114,4 +118,4 @@ class JukeboxSearch:
 
 # entrypoint
 if __name__ == "__main__":
-    jbs = JukeboxSearch()
+    jbs = JukeboxSearch()

setup-alpine-vm.sh

@@ -0,0 +1,17 @@
+#!/bin/sh
+
+# This script must run as root!
+
+echo "https://dl-cdn.alpinelinux.org/alpine/latest-stable/main
+https://dl-cdn.alpinelinux.org/alpine/latest-stable/community" > /etc/apk/repositories
+
+apk upgrade
+apk add git docker docker-cli-compose
+
+rc-update add docker
+service docker start
+
+git clone https://git.myitr.org/Jukebox/jukebox-software
+cd jukebox-software
+git submodule init
+git submodule update

ur5_control.py

@@ -6,6 +6,7 @@ import numpy as np
 import time
 import os
 import logging
+import yaml
 from urx.robotiq_two_finger_gripper import Robotiq_Two_Finger_Gripper
 import sys
 from util import fprint
@@ -13,46 +14,59 @@ from util import fprint
 
 
 
-rob = None
-offset_x, offset_y, offset_z = (0, 0, 0.14)     # Tool offset
-limb_base, limb1, limb2, limb3, limb_wrist = (0.105, .425, .39225, .1, .0997)    # Limb lengths
 
-def init(ip):
-    global rob
-    #sys.stdout = Logger()
-    fprint("Starting UR5 power up...")
-    
-    # power up robot here
+class Rob():
+    robot = None
+    #offset_x, offset_y, offset_z = (0, 0, 0.14)     # Tool offset
+    #
+    def __init__(self, config):
+        self.config = config
+        armc = config["arm"]
+        self.ip = armc["ip"]
+        tool = armc["tool"]
+        limbs = armc["limbs"]
+        self.offset_x, self.offset_y, self.offset_z = (tool["offset_x"], tool["offset_y"], tool["offset_z"])
+        self.limb_base = limbs["limb_base"]
+        self.limb1 = limbs["limb1"]
+        self.limb2 = limbs["limb2"]
+        self.limb3 = limbs["limb3"]
+        self.limb_wrist = limbs["limb_wrist"]
+        #self.init_arm()
 
-    # wait for power up (this function runs async)
+    def init_arm(self):
+        #sys.stdout = Logger()
+        fprint("Starting UR5 power up...")
+
+        # power up robot here
+
+        # wait for power up (this function runs async)
 
 
-    # trigger auto-initialize
+        # trigger auto-initialize
 
-    # wait for auto-initialize
+        # wait for auto-initialize
+        ip = self.ip
+        # init urx
+        fprint("Connecting to arm at " + ip)
+        trying = True
+        while trying:
+            try:
+                self.robot = urx.Robot(ip)
+                trying = False
+            except:
+                time.sleep(1)
 
-    # init urx
-    fprint("Connecting to arm at " + ip)
-    trying = True
-    while trying:
-        try:
-            rob = urx.Robot(ip)
-            trying = False
-        except:
-            time.sleep(1)
-    robotiqgrip = Robotiq_Two_Finger_Gripper(rob)
+        # Sets robot arm endpoint offset (x,y,z,rx,ry,rz)
+        self.robot.set_tcp((self.offset_x, self.offset_y, self.offset_z, 0, 0, 0))
 
-    # Sets robot arm endpoint offset (x,y,z,rx,ry,rz)
-    rob.set_tcp((0, 0, 0.15, 0, 0, 0))
+        # Set weight
+        self.robot.set_payload(2, (0, 0, 0.1))
+        #rob.set_payload(2, (0, 0, 0.1))
+        time.sleep(0.2)
+        fprint("UR5 ready.")
 
-    # Set weight
-    rob.set_payload(2, (0, 0, 0.1))
-    #rob.set_payload(2, (0, 0, 0.1))
-    time.sleep(0.2)
-    fprint("UR5 ready.")
-
-def set_pos_abs(x, y, z, xb, yb, zb, threshold=None):
-    global rob
+def set_pos_abs(robot, x, y, z, xb, yb, zb, threshold=None):
+    rob = robot.robot
     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
@@ -68,8 +82,8 @@ def set_pos_abs(x, y, z, xb, yb, zb, threshold=None):
     #rob.speedj(0.2, 0.5, 99999)
     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
+def set_pos_rel_rot_abs(robot, x, y, z, xb, yb, zb):
+    rob = robot.robot
     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
@@ -86,8 +100,8 @@ 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
+def set_pos_abs_rot_rel(robot, x, y, z, xb, yb, zb):
+    rob = robot.robot
     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
@@ -133,8 +147,8 @@ def polar_to_cartesian(r, theta):
     return x, y
 
 
-def move_to_polar(start_pos, end_pos):
-    global rob
+def move_to_polar(robot, start_pos, end_pos):
+    rob = robot.robot
 
     # Convert to polar coordinates
     start_r, start_theta = cartesian_to_polar(start_pos[0], start_pos[1])
@@ -191,13 +205,8 @@ def move_to_polar(start_pos, end_pos):
 
     return rx_intermediate
 
-def degtorad(angle):
-        return angle/180.0 * math.pi
-def radtodeg(angle):
-        return angle*180.0 / math.pi
-
-def move_to_home():
-    global rob
+def move_to_home(robot):
+    rob = robot.robot
 
     # Home position in degrees
     home_pos = [0.10421807948612624, 
@@ -222,15 +231,14 @@ def normalize_degree(theta):
     # Return angle
     return normalized_theta
 
-
-
-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), l3offset=0):
+def get_joints_from_xyz_rel(robot, x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_guess = (math.pi/2, math.pi/2, 0), l3offset=0):
     # Get limbs and offsets
-    
     #l3=0.15
-    l_bs, l1, l2, l3, l_wt = (limb_base, limb1, limb2, limb3, limb_wrist)    # Limb lengths
+    l_bs, l1, l2, l3, l_wt = (robot.limb_base, robot.limb1, robot.limb2, robot.limb3, robot.limb_wrist)    # Limb lengths
     l3 += l3offset # add wrist offset, used for gripper angle calculations
-    
+    offset_x = robot.offset_x
+    offset_y = robot.offset_y
+    offset_z = robot.offset_z
     # Calculate base angle and r relative to shoulder joint
     def calculate_theta(x, y, a):
         # Calculate if we need the + or - in our equations
@@ -273,63 +281,197 @@ def get_joints_from_xyz_rel(x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_guess =
     # Return result
     return base, shoulder, elbow, wrist1, ry, rz
 
-def get_joints_from_xyz_abs(x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2, l3offset=0):
-    joints = get_joints_from_xyz_rel(x, y, z, rx, ry, rz, l3offset=l3offset)
+def get_joints_from_xyz_abs(robot, x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2, l3offset=0, use_closest_path=True):
+    rob = robot.robot
+    joints = get_joints_from_xyz_rel(robot, x, y, z, rx, ry, rz, l3offset=l3offset)
+
+    # Return current positions if coordinates don't make sense
+    if z<0:
+        return rob.getj()
 
     # Joint offsets
     # Base, Shoulder, Elbow, Wrist
     inverse = [1, -1, 1, 1, 1, 1]
     offsets = [-math.pi/2, 0, 0, -math.pi/2, 0, 0]
-    if radtodeg(joints[1]) > 137:
+
+    if math.degrees(joints[1]) > 137:
         print("CRASH! Shoulder at", joints[1] * 180/math.pi)
     #else:
         #print("Shoulder at", joints[1] * 180/math.pi)
-    # Return adjusted joint positions
-    return [o+j*i for j, o, i in zip(joints, offsets, inverse)]
 
-# gripper angle: from vertical
-# gripper length: from joint to start of grip
-# to flip, you can use flip=True or make gripper angle negative
-def offset_gripper_angle(x, y, z, gripperangle=35, gripperlength=0.20+0.018, flip=False):
+    # Get adjusted joint positions
+    adjusted_joints =  [o+j*i for j, o, i in zip(joints, offsets, inverse)]
+
+    curr_joints = rob.getj()
+    def get_complimentary_angle(joint_angle):
+        if joint_angle<0:
+            new_angle = joint_angle + 2*math.pi
+        else:
+            new_angle = joint_angle - 2*math.pi
+        
+        if abs(new_angle) > math.radians(350):
+            return joint_angle
+        else:
+            return new_angle
+        
+    # Use closest path (potentially going beyond 180 degrees)
+    if use_closest_path:
+        if abs(get_complimentary_angle(adjusted_joints[0])-curr_joints[0]) < abs(adjusted_joints[0]-curr_joints[0]):
+            adjusted_joints[0] = get_complimentary_angle(adjusted_joints[0])
+
+    # final_joint_positions = []
+    # for curr_joint, adjusted_joint in zip(curr_joints, adjusted_joints):
+    #     if abs(curr_joint - adjusted_joint) < abs(curr_joint - get_complimentary_angle(adjusted_joint)):
+    #         final_joint_positions.append(adjusted_joint)
+    #     else:
+    #         final_joint_positions.append(get_complimentary_angle(adjusted_joint))
+
+    # return final_joint_positions
+        
+    return adjusted_joints
+
+
+def move_arc(robot, x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2):
+    rob = robot.robot
+    
+    start_joints = rob.getj()
+    end_joint = get_joints_from_xyz_abs(robot, x, y, z, rx, ry, rz)
+
+    n_points = 50
+    intermediate_joints = []   
+    for i in range(0, 6):
+        intermediate_joints.append(np.linspace(start_joints[i], end_joint[i], n_points))
+
+    joints = [joint_position for joint_position in zip(*intermediate_joints)]
+
+    rob.movejs(joints, acc=2, vel=2, radius=0.1)
+
+def offset_gripper_angle(robot, x, y, z, gripperangle=30, gripperlength=0.20+0.018, flip=False, use_closest_path=True):
+    # gripper angle: from vertical
+    # gripper length: from joint to start of grip
+    # to flip, you can use flip=True or make gripper angle negative
+    limb3 = robot.limb3
+    # Determine tool rotation depending on gripper angle
     if gripperangle < 0:
         rz = - math.pi / 2
     else:
         rz = math.pi / 2
 
     if flip:
-        gripperangle = -degtorad(gripperangle)
+        gripperangle = -math.radians(gripperangle)
         grippery = gripperlength - math.cos(gripperangle) * gripperlength
         grippery += math.sin(gripperangle) * limb3
         gripperx = math.sin(gripperangle) * gripperlength + limb3 * 2
         gripperx -= (1-math.cos(gripperangle)) * limb3
         rz = math.pi / 2
         # flip the whole wrist
-        return get_joints_from_xyz_abs(x, y, z-grippery, rx=gripperangle + degtorad(180), l3offset=-gripperx, ry=math.pi/2, rz=rz)
+        return get_joints_from_xyz_abs(robot, x, y, z-grippery, rx=gripperangle + math.radians(180), l3offset=-gripperx, ry=math.pi/2, rz=rz, use_closest_path=use_closest_path)
+
     else:
-        gripperangle = degtorad(gripperangle)
+        gripperangle = math.radians(gripperangle)
         grippery = gripperlength - math.cos(gripperangle) * gripperlength
         grippery -= math.sin(gripperangle) * limb3
         gripperx = math.sin(gripperangle) * gripperlength
         gripperx += (1-math.cos(gripperangle)) * limb3
 
-        return get_joints_from_xyz_abs(x, y, z-grippery, rx=gripperangle, l3offset=-gripperx, rz=rz)
+        return get_joints_from_xyz_abs(robot, x, y, z-grippery, rx=gripperangle, l3offset=-gripperx, rz=rz, use_closest_path=use_closest_path)
     
-
-def goto_holder_index(idx, z=0.05, gripperangle=35, flip=False):
-    joint = config["position_map"][idx]
-    print("Going to cable holder index", joint["index"], "at position", joint["pos"])   
-    angles = offset_gripper_angle(joint["pos"][1]/1000, joint["pos"][0]/1000, z, gripperangle=gripperangle, flip=flip)
+def goto_holder_index(robot, idx, z=0.05, gripperangle=30, flip=False, verbose=False):
+    joint = robot.config["position_map"][idx]
+    
+    if verbose:
+        print("Going to cable holder index", joint["index"], "at position", joint["pos"])   
+    
+    safe_move(robot, joint["pos"][1]/1000, joint["pos"][0]/1000, z)
+    #angles = offset_gripper_angle(joint["pos"][1]/1000, joint["pos"][0]/1000, z, gripperangle=gripperangle, flip=flip)
     #rob.movej(angles, acc=2, vel=2)
-    return angles
+    #return angles
     #angles = get_joints_from_xyz_abs(joint["pos"][1]/1000, joint["pos"][0]/1000, 0.05, )
 
+def is_flipped(robot):
+    rob = robot.robot
+    wrist2 = rob.getj()[4]
+
+    if wrist2>0:
+        return True
+    else:
+        return False
+
+def flip(robot):
+    rob = robot.robot
+
+    # A list of safe positions to flip
+    safe_positions = [(-0.18, -0.108, 0.35),
+                      (0.18, -0.108, 0.35)]
+
+    # Find the closest safe position
+    curr_pos = rob.getl()[:3]
+    def dist_from_robot(pos):
+        x, y, z = pos
+        rx, ry, rz = curr_pos
+        return math.sqrt((rx-x)**2+(ry-y)**2+(rz-z)**2)
+
+    pos_dist_pairs = zip(safe_positions, [dist_from_robot(pos) for pos in safe_positions])
+    safe_pos = min(pos_dist_pairs, key=lambda x:x[1])[0]
+
+    # Flip at safe position
+    rob.movej(offset_gripper_angle(robot, *safe_pos, flip=is_flipped(robot)), vel=2, acc=2) # Move to safe position
+    rob.movej(offset_gripper_angle(robot, *safe_pos, flip=(not is_flipped(robot))), vel=2, acc=2) # Flip gripper
+
+def safe_move(robot, x, y, z):
+    rob = robot.robot
+    flip_radius = 0.22 # Min radius on which to flip
+    r = math.sqrt(x**2 + y**2) # Get position radius
+
+    # Flip gripper if needed
+    if (r <= flip_radius and is_flipped(robot)) or (r > flip_radius and not is_flipped(robot)):
+        flip(robot)
+
+    rob.movej(offset_gripper_angle(robot, x, y, z, flip=is_flipped(robot)), vel=2, acc=2)
+
+def pick_up_routine(robot, holder_index, verbose=False):
+    rob = robot.robot
+    
+    if verbose:
+        print('Pickup routine for index', holder_index)
+
+    goto_holder_index(robot, holder_index, 0.2)
+    curr_pos = rob.getl()
+    new_pos = curr_pos
+    new_pos[2] = 0
+    rob.movel(new_pos, vel=0.3, acc=1)
+    # goto_holder_index(robot, holder_index, 0.0)
+
+    # Close Gripper code
+    time.sleep(0.5)
+
+    new_pos[2] = 0.2
+    rob.movel(new_pos, vel=0.3, acc=1)
+    was_flipped = is_flipped(robot)
+
+    # Tray position 1
+    rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, flip=is_flipped(robot), use_closest_path=False), vel=2, acc=2)
+    rob.movej(get_joints_from_xyz_abs(robot, -0.35, -0.15, 0.0, math.pi/2, 0.1), vel=2, acc=2)
+    time.sleep(0.5)
+
+    # Back to safe position
+    rob.movej(offset_gripper_angle(robot, -0.205, -0.108, 0.3, flip=True), vel=2, acc=2)
+
+
 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")
+    #init("192.168.1.145")
+    with open('config.yml', 'r') as fileread:
+        #global config
+        config = yaml.safe_load(fileread)
+    robot = Rob(config) # robot of type Rob is the custom class above
+    robot.init_arm()
+    rob = robot.robot # rob is robot.robot is the urx robot class, what we've been using previously
+    
     print("Current tool pose is: ",  rob.getl())
-    move_to_home()
+    move_to_home(robot)
 
     home_pose = [-0.4999999077032916, 
                -0.2000072960336574, 
@@ -356,81 +498,45 @@ if __name__ == "__main__":
         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.2, 0, 0)
-    # rob.movej(angles, acc=2, vel=2)
-    # angles = get_joints_from_xyz_abs(-0.2, -0.2, 0)
-    # rob.movej(angles, acc=2, vel=2)
-    # angles = get_joints_from_xyz_abs(0, -0.6, 0)
-    # rob.movej(angles, acc=2, vel=2)
-    # angles = get_joints_from_xyz_abs(0, -0.5, 0)
-    # rob.movej(angles, acc=2, vel=2)
-    # angles = get_joints_from_xyz_abs(0, -0.4, 0)
-    # rob.movej(angles, acc=2, vel=2)
-    # angles = get_joints_from_xyz_abs(0, -0.3, 0)
-    # rob.movej(angles, acc=2, vel=2)
-    # angles = get_joints_from_xyz_abs(0, -0.2, 0)
-    # rob.movej(angles, acc=2, vel=2)
-    # angles = get_joints_from_xyz_abs(0, -0.13, 0)
-    # rob.movej(angles, acc=2, vel=2)
     config = None
     joints = []
-    for i in np.linspace(-0.2, -0.7, 50):
-         joints.append(get_joints_from_xyz_abs(i, 0, 0))
-    #rob.movejs(joints, acc=2, vel=2)
-    import yaml
-    with open('config.yml', 'r') as fileread:
-        #global config
-        config = yaml.safe_load(fileread)
+    for i in np.linspace(-0.2, -0.7, 10):
+         joints.append(get_joints_from_xyz_abs(robot, i, 0, 0))
+    # rob.movejs(joints, acc=2, vel=2, radius=0.1)
 
-    #rob.movej(goto_holder_index(24, 0.2, 0), acc=2, vel=2)
+    # rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, gripperangle=90, flip=is_flipped(robot)), vel=2, acc=2)
+
+    # move_arc(0, 0.3, 0.1)
+    # move_arc(0, -0.3, 0.3)
+
+    for i in range(20, 25):
+        pick_up_routine(robot, i, verbose=True)
+    
+    
+
+    # goto_holder_index(robot, 7, 0.0)
+
+    # pick_up_routine(robot, 8)
+         
+         
+    # rob.movej(get_joints_from_xyz_abs(robot, -0.35, -0.15, 0.0, math.pi/2, 0.1), vel=2, acc=2)
+
+
+    #rob.movej(goto_holder_index(24, 0.2, 0), acc=2, vel=2).
     #joints = []
     
-    #for i in np.linspace(0, 340, 340):
-    #    joints.append(goto_holder_index(24, 0.5, i))
-    #rob.movejs(joints, acc=1, vel=3)
-    angle = 30
-    rob.movej(goto_holder_index(26, 0.1, angle), acc=2, vel=2)
-    time.sleep(1)
-    rob.movej(goto_holder_index(25, 0.1, angle), acc=2, vel=2)
-    time.sleep(1)
-    rob.movej(goto_holder_index(24, 0.1, angle, flip=True), acc=2, vel=2)
-    #rob.movej(goto_holder_index(32, 0.2, angle), acc=2, vel=2)
-    #rob.movej(goto_holder_index(38, 0.2, angle), acc=2, vel=2)
-    #rob.movej(goto_holder_index(25, 0.1, angle, flip=True), acc=2, vel=2)
+        
+    # angle = 30
+    # goto_holder_index(robot, 26, 0.1, angle)
+    # time.sleep(1)
+    # goto_holder_index(robot, 25, 0.1, angle)
+    # time.sleep(1)
+    # goto_holder_index(robot, 24, 0.1, angle)
+        
+
 
-    #rob.movej(goto_holder_index(25, 0.2, angle, flip=True), acc=2, vel=2)
-    #rob.movej(goto_holder_index(24, 0.1, angle, flip=True), acc=2, vel=2)
-    #time.sleep(1)
-    #rob.movej(goto_holder_index(25, 0.1, angle, flip=True), acc=2, vel=2)
-    #rob.movej(goto_holder_index(49, 0.1, angle), acc=2, vel=2)
-    #rob.movej(goto_holder_index(49, 0.1, angle, flip=True), acc=2, vel=2)
-    # rob.movej(goto_holder_index(50, 0.1, angle, flip=True), acc=2, vel=2)
-    # rob.movej(goto_holder_index(51, 0.1, angle, flip=True), acc=2, vel=2)
-    # rob.movej(goto_holder_index(52, 0.1, angle, flip=True), acc=2, vel=2)
-    # rob.movej(goto_holder_index(53, 0.1, angle, flip=True), acc=2, vel=2)
-    #time.sleep(2)
-    #rob.movej(goto_holder_index(24, 0.15, 35, flip=True), acc=2, vel=2)
-    #time.sleep(10)
-    # time.sleep(4)
-    # goto_holder_index(26, 0.1, 20)
-    # time.sleep(4)
-    # goto_holder_index(26, 0.1, 30)
-    # time.sleep(4)
-    # goto_holder_index(26, 0.1, 40)
-    # for joint in config["position_map"]:
-    #joint = config["position_map"][26]
-    #print("Going to cable holder index", joint["index"], "at position", joint["pos"])   
-    #angles = get_joints_from_xyz_abs(joint["pos"][1]/1000, joint["pos"][0]/1000, 0.05, )# rx=math.pi / 5)
-    #joints.append(angles)
 
     #rob.movej(angles, acc=2, vel=2)
     #time.sleep(10)
@@ -443,25 +549,6 @@ if __name__ == "__main__":
     # time.sleep(5)
 
 
-    # angles = get_joints_from_xyz_abs(0, -0.6, 0)
-    # rob.movej(angles, acc=2, vel=2)
-    
-
-
-    # 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())

websocket_test.html

@@ -81,7 +81,7 @@
                 socket.send(message);
                 console.log('Message sent', message);
             }
-            setInterval(ping, 1500);
+            //setInterval(ping, 1500);
 
             // setInterval(() => {
             //     updateServiceStatus('serviceA', 'down');