"
+
+ def __add__(self, other):
+ self.leds.extend(other)
+ return self
+
+ def __bool__(self):
+ return self.dirty
+
+ def __getitem__(self, index):
+ return self.leds[index]
+
+ def __setitem__(self, index, value):
+ ivalue = self.leds[index]
+ if ivalue != value:
+ self.dirty = True
+ self.leds[index] = value
+
+ def __getattr__(self, name):
+ import word2num
+ name = int(word2num.word2num(name))
+ print(name)
+ if 0 <= name < len(self.leds):
+ return self.leds[name]
+
+
+
+a = Ring()
+print(a)
+b = Ring()
+b.leds[2] = 3
+
+print(a + b)
+
+b.active = True
+
+if b:
+ print("Bexist")
+
+c = [a, b, b, a, a]
+
+d = list(filter(lambda x: bool(x), c))
+
+print(d)
+
+for i, ring in enumerate(c):
+ ring[0] = i
+ print(ring)
+
+print(a, b)
+
+print(f"\u001b[32m{a}")
+print(f"\u001b[37ma")
+
+print(getattr(a, "twenty two"))
+
+# eval(f"getattr(a,\"{input()}\")")
+
+
+# a = r"wow this string is cursed; for example \n"
+
+
+# SEARCHDATA=r"""{ "q": "{QUERY}", "sortCriteria": "relevancy", "numberOfResults": "250", "sortCriteria": "@catalogitemwebdisplaypriority ascending", "searchHub": "products-only-search", "pipeline": "Site Search", "maximumAge": "900000", "tab": "products-search", "locale": "en", "aq": "(NOT @z95xtemplate==(ADB6CA4F03EF4F47B9AC9CE2BA53FF97,FE5DD82648C6436DB87A7C4210C7413B)) ((@syssource==\"website_001002_catalog_index-rg-nc-prod-sitecore-prod\" @catalogitemprimarycategorypublished==true)) ((@catalogitemregionavailable=Global) (@z95xlanguage==en))", "cq": "((@z95xlanguage==en) (@z95xlatestversion==1) (@source==\"Coveo_web_index - rg-nc-prod-sitecore-prod\")) OR (@source==(\"website_001002_catalog_index-rg-nc-prod-sitecore-prod\",\"website_001002_Category_index-rg-nc-prod-sitecore-prod\"))", "firstResult": "0" }, "categoryFacets": "[{\"field\":\"@catalogitemcategories\",\"path\":[],\"injectionDepth\":1000,\"maximumNumberOfValues\":6,\"delimitingCharacter\":\"|\"}]", "facetOptions": "{}", "groupBy": " [{\"field\":\"@contenttype\",\"maximumNumberOfValues\":6,\"sortCriteria\":\"occurrences\",\"injectionDepth\":1000,\"completeFacetWithStandardValues\":true,\"allowedValues\":[\"Products\"],\"queryOverride\":\"{QUERY}\",\"advancedQueryOverride\":\"(NOT @z95xtemplate==(ADB6CA4F03EF4F47B9AC9CE2BA53FF97,FE5DD82648C6436DB87A7C4210C7413B)) ((((((((@z95xpath=3324AF2D58F64C0FB725521052F679D2 @z95xid<>3324AF2D58F64C0FB725521052F679D2) ((@z95xpath=C292F3A37B3A4E6BAB345DF87ADDE516 @z95xid<>C292F3A37B3A4E6BAB345DF87ADDE516) @z95xtemplate==E4EFEB787BDC4B1A908EFC64D56CB2A4)) OR ((@z95xpath=723501A864754FEEB8AE377E4C710271 @z95xid<>723501A864754FEEB8AE377E4C710271) ((@z95xpath=600114EAB0E5407A84AAA9F0985B6575 @z95xid<>600114EAB0E5407A84AAA9F0985B6575) @z95xtemplate==2BE4FD6B3B2C49EBBD9E1F6C92238B05))) OR (@syssource==\\"website_001002_catalog_index-rg-nc-prod-sitecore-prod\\" @catalogitemprimarycategorypublished==true)) OR ((@z95xpath=3324AF2D58F64C0FB725521052F679D2 @z95xid<>3324AF2D58F64C0FB725521052F679D2) @z95xpath<>C292F3A37B3A4E6BAB345DF87ADDE516)) OR @syssource==\\"website_001002_Category_index-rg-nc-prod-sitecore-prod\\") NOT @z95xtemplate==(ADB6CA4F03EF4F47B9AC9CE2BA53FF97,FE5DD82648C6436DB87A7C4210C7413B))) ((@catalogitemregionavailable=Global) (@z95xlanguage==en) OR (@contenttype=(Blogs,Resources,Other)) (NOT @ez120xcludefromcoveo==1))\",\"constantQueryOverride\":\"((@z95xlanguage==en) (@z95xlatestversion==1) (@source==\\"Coveo_web_index - rg-nc-prod-sitecore-prod\\")) OR (@source==(\\"website_001002_catalog_index-rg-nc-prod-sitecore-prod\\",\\"website_001002_Category_index-rg-nc-prod-sitecore-prod\\"))\"},{\"field\":\"@catalogitembrand\",\"maximumNumberOfValues\":6,\"sortCriteria\":\"occurrences\",\"injectionDepth\":1000,\"completeFacetWithStandardValues\":true,\"allowedValues\":[]},{\"field\":\"@catalogitemenvironment\",\"maximumNumberOfValues\":6,\"sortCriteria\":\"occurrences\",\"injectionDepth\":1000,\"completeFacetWithStandardValues\":true,\"allowedValues\":[]},{\"field\":\"@catalogitemregionalavailability\",\"maximumNumberOfValues\":6,\"sortCriteria\":\"occurrences\",\"injectionDepth\":1000,\"completeFacetWithStandardValues\":true,\"allowedValues\":[]},{\"field\":\"@prez45xtez120xt\",\"maximumNumberOfValues\":5,\"sortCriteria\":\"occurrences\",\"injectionDepth\":1000,\"completeFacetWithStandardValues\":true,\"allowedValues\":[]},{\"field\":\"@tags\",\"maximumNumberOfValues\":4,\"sortCriteria\":\"occurrences\",\"injectionDepth\":1000,\"completeFacetWithStandardValues\":true,\"allowedValues\":[]},{\"field\":\"@facetassettype\",\"maximumNumberOfValues\":3,\"sortCriteria\":\"occurrences\",\"injectionDepth\":1000,\"completeFacetWithStandardValues\":true,\"allowedValues\":[]},{\"field\":\"@facetbrand\",\"maximumNumberOfValues\":3,\"sortCriteria\":\"occurrences\",\"injectionDepth\":1000,\"completeFacetWithStandardValues\":true,\"allowedValues\":[]},{\"field\":\"@facetmarket\",\"maximumNumberOfValues\":6,\"sortCriteria\":\"occurrences\",\"injectionDepth\":1000,\"completeFacetWithStandardValues\":true,\"allowedValues\":[]},{\"field\":\"@facetsolution\",\"maximumNumberOfValues\":6,\"sortCriteria\":\"occurrences\",\"injectionDepth\":1000,\"completeFacetWithStandardValues\":true,\"allowedValues\":[]},{\"field\":\"@facetsearchcontentpagetype\",\"maximumNumberOfValues\":6,\"sortCriteria\":\"occurrences\",\"injectionDepth\":1000,\"completeFacetWithStandardValues\":true,\"allowedValues\":[]}]" }"""
+
+# QUERY = "AAAAAAAAAAAA"
+
+# b = SEARCHDATA.replace(r"{QUERY}", QUERY)
+
+q = [i * 2 for i in range(10)]
+
+d = {a : b for a,b in enumerate(q)}
+
+print(q)
+
+print(d)
+
+def stalin_sort(a):
+ b = sum(a)
+ b /= len(a)
+ return [b for _ in range(len(a))]
+
+def mao_sort(a):
+ i = 0
+ while i < len(a) - 1:
+ if a[i+1] < a[i]:
+ del a[i]
+ else:
+ i += 1
+ return a
+
+print(stalin_sort(list(range(10))))
+print(mao_sort([1, 3, 2, 4, 5, 8, 7, 6, 9]))
+
+# i l
\ No newline at end of file
diff --git a/ur5_control.py b/ur5_control.py
index 2030e60..724e500 100755
--- a/ur5_control.py
+++ b/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)
diff --git a/venv-setup.sh b/venv-setup.sh
index ad297b7..1232298 100755
--- a/venv-setup.sh
+++ b/venv-setup.sh
@@ -1,6 +1,7 @@
#!/bin/sh
+# change this to #!/bin/bash for windows
-python -m venv ./venv
+python3 -m venv ./venv
source ./venv/bin/activate
pip install --upgrade pip
\ No newline at end of file
diff --git a/websocket_test.html b/websocket_test.html
index 4cdca7a..c6489bc 100644
--- a/websocket_test.html
+++ b/websocket_test.html
@@ -1,15 +1,61 @@
+
WebSocket Test
+
+
+
+
+
WebSocket Test
-
+
+ Example JSON
+ { "type": "cable_map", "call": "request", "data": { } }
+ { "type": "log", "call": "send", "data": "123123" }
+
+ Messages/Logs
+
+
+
+
+
-
+
+