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

.gitignore

@@ -15,7 +15,7 @@ output.mp4
 # log files
 output.log
 # images
-*.png
+map*.png
 # Built app
 build
 # Generated label images

belden-logo.svg

@@ -0,0 +1,41 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- Generator: Adobe Illustrator 25.2.1, SVG Export Plug-In . SVG Version: 6.00 Build 0)  -->
+<svg version="1.1" id="Layer_1" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" x="0px" y="0px"
+	 viewBox="0 0 446.6 151.4" style="enable-background:new 0 0 446.6 151.4;" xml:space="preserve">
+<style type="text/css">
+	.st0{fill:#004990;}
+</style>
+<g>
+	<g>
+		<path class="st0" d="M21.2,32.1h-1.4v87.2H55c20.3,0,32-9.1,32-24.9c0-12.3-5.6-19.7-15.8-22.1c5.1-3.6,7.8-9.1,7.8-16.9
+			c0-15.5-8.9-23.3-26.5-23.3H21.2z M44.7,51.6c7.4,0,11.4,1.1,11.4,6.8c0,4.8-3.1,6.8-10.5,6.8c0,0-0.6,0-1.1,0
+			c0-2.1,0-11.5,0-13.6C44.5,51.6,44.7,51.6,44.7,51.6z M45.4,84.4l1.8,0c4.6,0,10.3-0.1,13,2.6c1.2,1.2,1.8,2.9,1.8,5.2
+			c0,2-0.6,3.5-1.7,4.7c-3,3-9.2,2.9-13.8,2.9c0,0-1.4,0-2.1,0c0-2.1,0-13.3,0-15.4C44.9,84.4,45.4,84.4,45.4,84.4z"/>
+		<g>
+			<path class="st0" d="M139.8,32.1H90.4v87.2h50.8V98c0,0-23.7,0-26.1,0c0-2,0-9.8,0-11.8c2.4,0,24.8,0,24.8,0V64.8
+				c0,0-22.3,0-24.8,0c0-2,0-9.4,0-11.4c2.5,0,26.1,0,26.1,0V32.1H139.8z"/>
+		</g>
+		<g>
+			<path class="st0" d="M169.3,32.1H146v87.2h51V98c0,0-23.9,0-26.3,0c0-2.6,0-65.9,0-65.9H169.3z"/>
+		</g>
+		<g>
+			<path class="st0" d="M332.4,32.1H283v87.2h50.8V98c0,0-23.7,0-26.1,0c0-2,0-9.8,0-11.8c2.4,0,24.8,0,24.8,0V64.8
+				c0,0-22.3,0-24.8,0c0-2,0-9.4,0-11.4c2.5,0,26.1,0,26.1,0V32.1H332.4z"/>
+		</g>
+		<g>
+			<path class="st0" d="M424.6,32.1h-23.3c0,0,0,43,0,49.3c-4-5.1-38.4-49.3-38.4-49.3h-24v87.2h24.7c0,0,0-43.1,0-49.5
+				c4,5.1,38.4,49.5,38.4,49.5h24V32.1H424.6z"/>
+		</g>
+		<g>
+			<g>
+				<path class="st0" d="M233.8,32.1h-32.5v87.2h32.5c24.4,0,44.3-19.6,44.3-43.6C278.1,51.7,258.2,32.1,233.8,32.1z M226,53.5
+					c13.6,0.3,22,8.7,22,22.2c0,13.6-8.2,21.9-22,22.2V53.5z M231.5,101.3c12.5-3,20.7-10.7,20.8-25.7c-0.2-15-8.3-22.7-20.8-25.7
+					c14,2.1,25,9.5,25,25.6v0.1C256.5,91.8,245.5,99.2,231.5,101.3z M260.2,75.6c-0.2-18-10-29.8-24.9-33.3
+					c16.7,2.5,29.6,14,29.6,33.3c0,0,0,0,0,0.1h0c0,19.3-13,30.7-29.7,33.2C250.1,105.3,260,93.6,260.2,75.6z M240.3,115.7
+					c16.7-4.7,28.3-19.2,28.5-39.9v-0.3c-0.2-20.7-11.9-35.1-28.5-39.8c19,3.9,33.5,17.7,33.6,39.7v0.4
+					C273.9,97.9,259.4,111.8,240.3,115.7z"/>
+			</g>
+		</g>
+	</g>
+</g>
+</svg>

get_specs.py

@@ -70,6 +70,8 @@ def query_search(partnum, source):
                 if idx < 0:
                     fprint("Could not find part in API: " + partnum)
                     return False
+                
+                name = a["results"][idx]["title"]
                 #fprint("Search result found: result " + str(idx) + ", for ID " + name)
                 #urlname = a["results"][0]["raw"]["catalogitemurlname"]
                 img = a["results"][idx]["raw"]["catalogitemimageurl"]
@@ -279,6 +281,7 @@ def get_multi(partnums, delay=0.25, dir="cables/", cache=True):
             return out
 
         def run_search(partnum):
+            partnum = partnum.replace("%20", " ") # undo URL encoding
             oldpartnum = partnum
             if dstype == "Alphawire":
                 # For alphawire, sanitize the part number for only the final result check, because their API is very wierd
@@ -405,6 +408,7 @@ if __name__ == "__main__":
     # ]
     partnums = [
     # Actual cables in Jukebox
+     
     "AW86104CY",
     "AW3050",
     "AW6714",
@@ -437,7 +441,7 @@ if __name__ == "__main__":
 
     # Some ones I picked, including some invalid ones
     "BL10GXS12", 
-    "BLRST 5L-RKT 5L-949", 
+    "BLRST%205L-RKT%205L-949",
     "BL10GXS13",
     "BL10GXW12",
     "BL10GXW13",
@@ -448,13 +452,18 @@ if __name__ == "__main__":
     "BLFISD012R9",
     "BLFDSD012A9",
     "BLFSSL024NG",
-    "BLFISX006W0",
-    "BLFISX00103",
-    "BLC6D1100007"
+    "BLFISX006W0", # datasheet only
+    "BLFISX00103", # invalid
+    "BLC6D1100007" # invalid
 
     ]
-    print(query_search("74002", "Belden"))
-    #get_multi(partnums, 0.25)
+    #print(query_search("TT-SLG-024-HTNN", "Belden"))
+    from label_generator import gen_label
+    gen_label("BLTF-SD9-006-RI5")
+    gen_label("BLRA500P")
+    gen_label("AWFIT-221-1_4")
+    gen_label("BLRST 5L-RKT 5L-949")
+    get_multi(partnums, 0.25)
     #query_search("10GXS13", "Belden")
 
 

index.html

@@ -1 +0,0 @@
-<html>  <head>    <title>RGB Controller Configuration</title>    <style>      body { background-color: #cccccc; font-family: Arial, Helvetica, Sans-Serif; Color: #000088; }    </style>  </head>  <body>    <h1>RGB Controller Configuration</h1><br>    <h2>Set IP address</h2>    Needs reboot to apply<br>    Set to 0.0.0.0 for DHCP    <form method="post" enctype="application/x-www-form-urlencoded" action="/postform/">      <input type="text" name="ipa" value="0" size="3">.      <input type="text" name="ipb" value="0" size="3">.      <input type="text" name="ipc" value="0" size="3">.      <input type="text" name="ipd" value="0" size="3">      <input type="submit" value="Set">    </form><br>    <h2>Set Hostname</h2>    Needs reboot to apply<br>    Max 64 characters    <form method="post" enctype="application/x-www-form-urlencoded" action="/postform/">      <input type="text" name="hostname" value="RGBController" size="20">      <input type="submit" value="Set">    </form><br>    <h2>DMX512 Start Universe</h2>    Applies immediately<br>    Between (inclusive) 1-65000    <form method="post" enctype="application/x-www-form-urlencoded" action="/postform/">      <input type="text" name="universe" value="1" size="5">      <input type="submit" value="Set">    </form><br>    <form method="post" enctype="application/x-www-form-urlencoded" action="/postform/">      <input type="submit" name="reboot" value="Reboot">    </form><br>  </body></html>

label_generator.py

@@ -61,9 +61,15 @@ def input_cable():
             imgstr = "BL"
         elif output[1] == "Alphawire":
             imgstr = "AW"
-        img = generate_code(imgstr + output[0])
-        os.makedirs("labels", exist_ok=True)
-        img.save("labels/" + imgstr + output[0] + ".png")
+        gen_label(imgstr + output[0])
+        #img = generate_code(imgstr + output[0])
+        #os.makedirs("labels", exist_ok=True)
+        #img.save("labels/" + imgstr + output[0] + ".png")
+
+def gen_label(partnum, path="labels"):
+    img = generate_code(partnum)
+    os.makedirs(path, exist_ok=True)
+    img.save(path + "/" + partnum + ".png")
 
 def delete_folder(path):
     # Check if the path is a directory

label_image.py

@@ -3,8 +3,13 @@ from util import fprint
 
 from PIL import Image
 from PIL import ImageDraw
+from PIL import ImageFont
 #import cv2
 import numpy as np
+from util import find_data_file
+import segno
+import io
+#import cairosvg
 #import math
 
 
@@ -134,7 +139,8 @@ for charset in (CODE128A, CODE128B):
 
 def generate_code(data, show=False, check=False):
 
-    img = code128_image(data)
+    #img = code128_image(data)
+    img = qr_image(data)
     if show:
         img.show()
     #img.show()
@@ -198,6 +204,7 @@ def code128_format(data):
     return codes
 
 def code128_image(data, height=100, thickness=3, quiet_zone=False):
+    partnum = data
     if not data[-1] == CODE128B['Stop']:
         data = code128_format(data)
         
@@ -227,12 +234,86 @@ def code128_image(data, height=100, thickness=3, quiet_zone=False):
 
     #draw.arc(((width - width/5, width - width/5), (width*9 + width/5, width*9 + width/5)),0,360,fill='blue', width = int(width/8))
     draw.arc(((width+int(width / 1.4), width+int(width / 1.4)), (width*9-int(width / 1.4), width*9-int(width / 1.4))),0,360,fill='blue', width = int(width/8))
+    font_path = find_data_file("OCRAEXT.TTF") 
+    font_size = width/2
+    font = ImageFont.truetype(font_path, font_size)
+    text_width = font.getlength(partnum)
+    while text_width > width*4:
+        font_size -= 1
+        font = ImageFont.truetype(font_path, font_size)
+        text_width = font.getlength(partnum)
+
+    txtx = (int(width * 10) - text_width) / 2
+    txty = (int(width * 10)) / 2 + width / 2
+
+    draw.text((txtx,txty),partnum, "black", font)
+    return img
+
+def qr_image(data, width=600):
+    partnum = data
+    
+
+
+    # Monochrome Image
+    img  = Image.new('RGB', (int(width * 10), int(width * 10)), 'white')
+    draw = ImageDraw.Draw(img)
+    
+
+
+    #svg_path = find_data_file("belden-logo.svg")
+    #with open(svg_path, 'rb') as svg_file:
+    #    png_image = cairosvg.svg2png(file_obj=svg_file,dpi=width*30, scale=30, background_color="white")
+    #with open("output.png", 'wb') as file:
+    #    file.write(png_image)
+
+    png_image_io = "belden-logo-superhires.png"
+    png_image_pillow = Image.open(png_image_io)
+    png_width, png_height = png_image_pillow.size
+    png_image_pillow = png_image_pillow.resize((int(width*5.2), int(width*5.2/png_width*png_height)))
+    png_width, png_height = png_image_pillow.size
+    # paste belden logo first because it has a big border that would cover stuff up
+    img.paste(png_image_pillow, (int(width*5-png_width/2), int(width*4.25 - png_height/2)))
+
+    # draw circle border
+    #draw.arc(((width - width/5, width - width/5), (width*9 + width/5, width*9 + width/5)),0,360,fill='blue', width = int(width/8))
+    draw.arc(((width+int(width / 1.4), width+int(width / 1.4)), (width*9-int(width / 1.4), width*9-int(width / 1.4))),0,360,fill=(0, 73,144), width = int(width/8))
+    
+    font_path = find_data_file("GothamCond-Medium.otf") 
+    font_size = width/2
+    font = ImageFont.truetype(font_path, font_size)
+    text_width = font.getlength(partnum[2:])
+    # shrink font dynamically if it's too long of a name
+    while text_width > width*4:
+        font_size -= 1
+        font = ImageFont.truetype(font_path, font_size)
+        text_width = font.getlength(partnum[2:])
+
+    txtx = (int(width * 10) - text_width) / 2
+    txty = (int(width * 10)) / 2
+    # draw part number text
+    draw.text((txtx,txty),partnum[2:], "black", font)
+    
+    # Draw QR code
+    partnum = partnum.replace(" ", "%20")
+    qrcode = segno.make('HTTPS://BLDN.APP/' + partnum,micro=False,boost_error=False,error="L",mask=3)
+    out = io.BytesIO()
+    qrx, _ = qrcode.symbol_size(1,0)
+    qrcode.save(out, scale=width*2/qrx, kind="PNG", border=0)
+    qrimg = Image.open(out)
+    img.paste(qrimg, box=(int(width*4),int(width*5.75)))
+
+    
     return img
 
 if __name__ == "__main__":
     #print(generate_code("BL10GXS13"))
     #print(generate_code("BL10GXgd35j35S13"))
     #print(generate_code("BL10GX54hS13"))
-    print(generate_code("BL10Gj34qXS13", False, False))
+    #print(generate_code("BL10Gj34qXS13", False, False))
     #print(generate_code("BL104w5545dp7bfwp43643534/4563G-XS13"))
     #adjust_image(cv2.imread('test_skew.jpg'))
+    path = "labels"
+    img = generate_code("BL10GXS13")
+    import os
+    os.makedirs(path, exist_ok=True)
+    img.save(path + "/" + "BL10GXS13" + ".png")

requirements.txt

@@ -19,7 +19,7 @@ pandas
 pyarrow
 ghostscript
 pyzbar
-
+segno
 
 # Development
 matplotlib

run.py

@@ -17,7 +17,7 @@ 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
@@ -36,7 +36,7 @@ killme = None
 #pool = None
 serverproc = None
 camera = None
-
+ledsys = None
 to_server_queue = Queue()
 from_server_queue = Queue()
 
@@ -47,6 +47,8 @@ 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
@@ -233,7 +235,9 @@ def setup_server(pool):
     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 ledsys
+    ledsys = LEDSystem()
+    pool.apply_async(ledsys.init, callback=led_start_callback)
     #pool.apply_async(sensor_control.init, callback=sensor_start_callback)
     serverproc = Process(target=start_server_socket)
     serverproc.start()

setup-label-generator.py

@@ -10,7 +10,7 @@ import opcode
 import os
 import distutils
 #distutils_path = os.path.join(os.path.dirname(opcode.__file__), 'distutils')
-build_exe_options = {"include_msvcr": True, "packages": ["camelot", "setuptools"], "optimize": 0, "silent": True, "include_files": ["gs10030w64.exe"], "excludes": ["scipy", "torch"]}
+build_exe_options = {"include_msvcr": True, "packages": ["camelot", "setuptools", "segno"], "optimize": 0, "silent": True, "include_files": ["gs10030w64.exe", "GothamCond-Medium.otf", "belden-logo-superhires.png"], "excludes": ["scipy", "torch"]}
 
 # base="Win32GUI" should be used only for Windows GUI app
 base = "console"

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
@@ -14,7 +15,8 @@ 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
@@ -216,12 +218,12 @@ 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)):
+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):
     # Get limbs and offsets
-    offset_x, offset_y, offset_z = (0, 0, 0.14)     # Tool offset
-    l_bs, l1, l2, l3, l_wt = (0.1333, .425, .39225, .1267, .0997)    # Limb lengths
+    
     #l3=0.15
+    l_bs, l1, l2, l3, l_wt = (limb_base, limb1, limb2, limb3, limb_wrist)    # Limb lengths
+    l3 += l3offset # add wrist offset, used for gripper angle calculations
     
     # Calculate base angle and r relative to shoulder joint
     def calculate_theta(x, y, a):
@@ -238,19 +240,16 @@ def get_joints_from_xyz_rel(x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_guess =
         # Calculate tangent line y = mx + b
         m = (x*y - math.sqrt(x*x*y*y-(x*x-a*a)*(y*y-a*a)))/(x*x-a*a)
         b = flip * a * math.sqrt(1+m*m)
-
         # Calculate equivalent tangent point on circle
         cx = (-flip*m*b)/(1+m*m)
         cy = m*cx + flip*b
-
         # Calculate base angle, make angle negative if flip=1
         theta = math.atan2(cy, cx) + (-math.pi if flip==1 else 0)
-
         return theta 
 
     base_theta = calculate_theta(x, y, l_bs)
-    cx, cy = l_bs*math.cos(base_theta), l_bs*math.sin(l_bs)
-    r = math.sqrt((x-cx)**2 + (y-cy)**2) 
+    cx, cy = l_bs*math.cos(base_theta), l_bs*math.sin(base_theta)
+    r = math.sqrt((x+offset_x+cx)**2 + (y+offset_y+cy)**2) 
 
 
     # Formulas to find out joint positions for (r, z)
@@ -264,21 +263,128 @@ def get_joints_from_xyz_rel(x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_guess =
 
     # Normalize angles
     base, shoulder, elbow, wrist1 = [normalize_degree(deg) for deg in [base_theta, *fsolve(inv_kin_r_z, initial_guess)]]
-
+    wrist1 += rx
     # 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):
-    joints = get_joints_from_xyz_rel(x, y, z, rx, 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)
+
+    # 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 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)]
 
+def move_arc(x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2):
+    
+    global rob
+    start_joints = rob.getj()
+    end_joint = get_joints_from_xyz_abs(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(x, y, z, gripperangle=35, gripperlength=0.20+0.018, flip=False):
+    # gripper angle: from vertical
+    # gripper length: from joint to start of grip
+    # to flip, you can use flip=True or make gripper angle negative
+
+    # Determine tool rotation depending on gripper angle
+    if gripperangle < 0:
+        rz = - math.pi / 2
+    else:
+        rz = math.pi / 2
+
+    if flip:
+        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 + math.radians(180), l3offset=-gripperx, ry=math.pi/2, rz=-rz)
+
+    else:
+        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)
+    
+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"])   
+    
+    safe_move(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
+    #angles = get_joints_from_xyz_abs(joint["pos"][1]/1000, joint["pos"][0]/1000, 0.05, )
+
+def is_flipped():
+    global rob
+    wrist2 = rob.getj()[4]
+
+    if wrist2>0:
+        return True
+    else:
+        return False
+
+def flip():
+    global rob
+
+    # A list of safe positions to flip
+    safe_positions = [(-0.205, -0.108, 0.3),
+                      (0.205, -0.108, 0.3)]
+
+    # 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(*safe_pos, flip=is_flipped()), vel=2, acc=2) # Move to safe position
+    rob.movej(offset_gripper_angle(*safe_pos, flip=(not is_flipped())), vel=2, acc=2) # Flip gripper
+
+def safe_move(x, y, z):
+    flip_radius = 0.17 # 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()) or (r > flip_radius and not is_flipped()):
+        flip()
+
+    global rob
+    rob.movej(offset_gripper_angle(x, y, z, flip=is_flipped()), vel=2, acc=2)
+
+
+
 if __name__ == "__main__":
     
     #rob.movej((0, 0, 0, 0, 0, 0), 0.1, 0.2)
@@ -312,18 +418,72 @@ 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)
 
     
+    config = None
+    joints = []
+    for i in np.linspace(-0.2, -0.7, 10):
+         joints.append(get_joints_from_xyz_abs(i, 0, 0))
+    # rob.movejs(joints, acc=2, vel=2, radius=0.1)
 
-    angles = get_joints_from_xyz_abs(-0.7, 0, 0)
-    rob.movej(angles, acc=2, vel=2)
+    with open('config.yml', 'r') as fileread:
+        #global config
+        config = yaml.safe_load(fileread)
 
+    # move_arc(0, 0.3, 0.1)
+    # move_arc(0, -0.3, 0.3)
+
+
+    # for i in range(20):
+    #     goto_holder_index(i, 0.1)
+
+    flip()
+    flip()
+
+    #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)
+
+    #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)
+    #rob.movejs(joints, acc=2, vel=2)
     # joints = []
     # for i in np.linspace(-0.3, -0.7, 50):
     #     joints.append(get_joints_from_xyz_abs(i, 0, 0))