Compare commits
6
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 adeck
|
82a52dea5a | ||
|
adeck
|
77fdc43fce | ||
|
adeck
|
3de59f5985 | ||
 BlueOceanWave
|
6887fa943b | ||
|
BlueOceanWave
|
2ec7906ee4 | ||
|
adeck
|
069d2175d9 |
No files matched your search
+11
@@ -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:
|
||||
|
||||
+23
-22
@@ -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):
|
||||
@@ -373,10 +373,10 @@ 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:
|
||||
@@ -408,7 +408,7 @@ if __name__ == "__main__":
|
||||
# ]
|
||||
partnums = [
|
||||
# Actual cables in Jukebox
|
||||
|
||||
"BL3092A",
|
||||
"AW86104CY",
|
||||
"AW3050",
|
||||
"AW6714",
|
||||
@@ -438,8 +438,9 @@ if __name__ == "__main__":
|
||||
"BL6300FE 009Q",
|
||||
"BLRA500P 006Q",
|
||||
|
||||
|
||||
]
|
||||
# Some ones I picked, including some invalid ones
|
||||
a = [
|
||||
"BL10GXS12",
|
||||
"BLRST%205L-RKT%205L-949",
|
||||
"BL10GXS13",
|
||||
|
||||
+95
-30
@@ -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": "iVBORw0KGgoAAAANSUhEUgAAAZgAAAGYCAYAAAB/O/RVAAAAOXRFWHRTb2Z0d2FyZQBNYXRwbG90bGliIHZlcnNpb24zLjUuMSwgaHR0cHM6Ly9tYXRwbG90bGliLm9yZy/YYfK9AAAACXBIWXMAAA9hAAAPYQGoP6dpAAAoc0lEQVR4nO3dd3TUVd7H8U86IY2eAEqzAIYSESSCrpQosnrEVQE1NBVQhBWeR9xdHxUF14BlFVBZVARkUWQtlMUKCIiUEEVAcEFQECWhaCAEklCS+/xxITFSTDR3fpPk/TpnTs7cmfnNdwhnPrm/W34BxhgjAADKWKDXBQAAKiYCBgDgBAEDAHCCgAEAOEHAAACcIGAAAE4QMAAAJwgYAIATBAwAwAkCBgDgBAEDAHCCgEGF16lTJ02fPt3rMoBKh4BBpdWlSxcNGzasWNu+ffsUGhqqxYsXe1QVUHEQMKi0Bg4cqNdff11HjhwpbJs5c6bq16+vLl26eFgZUDEQMKi0brzxRknSvHnzCtumT5+uAQMGKCAgwKuygAojgOvBoKJJSUlRSkpK4f3c3FyFhIQoODi4sO2rr75SgwYNNHz4cG3ZskUffPCB1q5dq3bt2unbb79Vw4YNvSgdqFAIGFQ4mZmZyszMLLyfnJysm266qbDHIkmNGjVScHCwvvzySyUkJOi7777TE088oc2bN2vhwoVelA1UOMG//hSgfKlRo4Zq1KhReD88PFx16tTR+eeff8pzW7ZsqbZt2+rll1/W66+/rueff96XpQIVGgGDSm/gwIEaNmyYIiIi9Kc//cnrcoAKg0F+VHq33nqrgoODdeutt6pKlSpelwNUGIzBoNLbsWOHzjvvPKWlpalNmzZelwNUGAQMKq1jx47pp59+0siRI7V9+3atWLHC65KACoVTZKi0VqxYobp16yotLU2TJ0/2uhygwqEHAwBwgh4MAMAJAgYA4AQBA0gyxujgwYPijDFQdggYQFJ2drZiYmKUnZ3tdSlAhUHAAACcIGAAAE4QMAAAJwgYAIATBAwAwAkCBgDgBAEDAHCCgAEAOEHAAACcIGAAAE4QMAAAJwgYAIATBAwAwAkCBgDgBAEDAHCCgAEAOEHAAACcIGAAAE4QMAAAJwgYAIATBAwAwAkCBgDgBAEDAHCCgAEAOEHAAACcIGAAAE4QMAAAJwgYAIATBAwAwAkCBgDgBAEDAHCCgAEAOEHAAACcIGAAAE4QMAAAJwgYAIATBAwAwAkCBgDgBAEDAHCCgAEAOEHAAACcIGAAAE4QMAAAJwgYAIATBAwAwAkCBgDgBAEDAHCCgAEAOEHAAACcIGAAAE4QMAAAJwgYAIATBAwAwAkCBgDgBAEDAHCCgAEAOEHAAACcIGAAAE4QMAAAJwgYAIATBAwAwAkCBgDgBAEDAHCCgAEAOEHAAACcIGAAAE4QMAAAJwgYAIATBAwAwAkCBgDgBAEDAHCCgAEAOEHAAACcIGAAAE4QMAAAJwgYAIATBAwAwAkCBgDgBAEDAHCCgAEAOEHAAACcIGAAAE4QMAAAJwgYAIATBAwAwAkCBgDgBAEDAHCCgAEAOEHAAACcIGAAAE4QMAAAJwgYAIATBAwAwAkCBgDgBAEDAHCCgAEAOEHAAACcIGAAAE4QMAAAJwgYAIATBAwAwAkCBgDgBAEDAHCCgAEAOEHAAACcIGAAAE4QMAAAJwgYAIATBAwAwAkCBgDgBAEDAHCCgAEAOEHAAACcIGAAAE4QMAAAJwgYAIATBAwAwAkCBgDgBAEDAHCCgAEAOEHAAACcIGBQ5owxGjVqlOrWravw8HAlJSVp69atZ33No48+qoCAgGK3Zs2a+ahiAC4QMChzTz75pCZOnKjJkycrNTVVERER6tatm/Ly8s76uvj4eGVkZBTePv30Ux9VXHLZ2dkyxnhdBnyI3/lvR8BA+/btU1xcnFJSUgrbVq5cqdDQUC1evLhUxzLGaPz48XrooYfUo0cPtWrVSjNmzFB6errmzp171tcGBwcrLi6u8FarVq3f8nGc2Lx5s5KTkxUTE+OXwQc38vPz1bhxY7Vt21b/+c9/CJpSImCg2rVra+rUqXr00Uf12WefKTs7W3379tWwYcPUtWtXLV++XJGRkWe9vfbaa5Kk7du3a/fu3UpKSio8fkxMjNq3b69Vq1adtY6tW7eqXr16atKkiZKTk7Vz506nn7skTgbLRRddpOXLl+uFF15Qx44dvS4LPhIUFKQ5c+YoMjJS119//ZmDxhjpX/+SsrO9KdRPBRgiGScMHTpUixYtUtu2bfXll18qLS1NYWFhys3N1a5du8762tjYWEVFRWnlypXq2LGj0tPTVbdu3cLHe/XqpYCAAM2ePfu0r3///fd16NAhNW3aVBkZGRo9erR27dqljRs3Kioqqkw/5+kcPHhQMTExysrKUnR0tDZu3KhBgwYpNTVV1atX17XXXqvLL79cISEhzmuB/zHGaPPmzZo3b56+/vprNWzYUA899JAGDhwo7d8vDRokvf221LevNGOG1+X6DwOckJOTY5o0aWJCQkLMhg0bftMxVqxYYSSZ9PT0Yu09e/Y0vXr1KvFx9u/fb6Kjo82UKVN+Ux2llZWVZSSZrKwsY4wxAwYMMJK4cTvjLTY21pgVK4xp0MAYqeiWmuqT/7PlQbCAE7755hulp6eroKBAO3bsUMuWLSVJy5cvV/fu3c/62hdffFHJycmKi4uTJO3Zs6dYD2bPnj1KSEgocS3VqlXThRdeqG3btpX+g5SBqVOnqmfPnhozZoxSU1PVvn17jRo1SldffbUCAgI8qQne2bZtm8aOHauZM2eqTp06+svIkRqclSX94Q9Sfr59UvXq0tSp0qWXelusP/E64eAfjhw5Ylq3bm369+9vUlJSTJ06dcyePXuMMbZns3Xr1rPeDh48aIwxpqCgwMTFxZmnn3668NhZWVkmLCzMzJo1q8T1ZGdnm+rVq5sJEyaU7Qc9g1/2YE4qKCgwH3zwgUlMTDSSTGJiosnIyPBJTfBeQUGBGTZsmAkKCjJ169Y1EyZMMDnbthnTuXPxXssVVxizc6fX5fodAgbGGGNGjhxpGjVqZLKyskx+fr65/PLLzbXXXvubjjVu3DhTrVo1M2/ePLNhwwbTo0cP07hxY5Obm1v4nC5dupjnnnuu8P59991nli5darZv325WrFhhkpKSTK1atczevXt/92criTMFzEkng+bmm282X331lU9qgvfy8/NN3759bbDk5Bjzn/8YU7NmUbAEBhrz6KPGHDtmCgoKvC7X7xAwMEuWLDHBwcFm+fLlhW3bt2830dHRZtKkSaU+XkFBgXn44YdNbGysCQsLM127djVbtmwp9pyGDRuaRx55pPB+7969Td26dU1oaKipX7++6d27t9m2bdtv/kyl9WsBg0ouL8+Y4cOL91rq1zdm2TJjjDEfffSRCQwMNAsWLPC2Tj/DLDJAp84iAwp9/bV0yy3SF18UtfXoIb3yilSzpiSpc+fOWrp0qTp06KAVK1Z4VKj/YR0MAJyOMdKrr0pt2hSFS1iY9Pzz0pw5heGydu1aLV26VJJdoJyamupRwf6HgAGAX8rOtmtaBgyQDh+2bc2aSamp0tCh0s9mEo4ePVoNGjSQJDVu3FijR4/2oGD/RMAAwM999pl08cXSid0pJEl33mnbW7cu9tS1a9dq/vz5Gjx4sCRp4MCBev/99+nFnEDAAIAkFRRI//iH1KGD9M03ti06WnrjDWnKFCki4pSXjBkzRhdccIGuueYaSVLXrl110UUX0Ys5gYABgL17pWuvlUaOlI4ds23t29uxl969z/iysLAwPfvsswoOtmvWg4KC9Mwzzyg8PNwXVfs9ZpEBYhZZpbZokR1v2b27qO2vf5Uee0wq4d5z69evV0JCgtLS0tS2bVtHhZY/bBUDoHI6dkwaNUp64gk7Y0ySYmPtrshXXeVtbRUEAQOg8tm+Xbr1Vjsr7KRu3ey05NhY7+qqYBiDAVC5zJ4tJSQUhUtwsPT009J77xEuZYweDIDK4fBhafhwuwL/pPPOk2bNktq1866uCoyAAVDxbdhgZ4Nt3lzUlpwsTZpkpyLDCU6RAai4jJFeeMFeo+VkuERESNOn28F8wsUpejCo1I4dk3JzpcxMe//QISkqqthOICivMjPtCvy5c4vaLr7YnhJr2tSzsioTAgYVnjHSd9/ZMd3UVOm//5UyMqT0dOnHH4tmqEpS/fp2P8O6de3tnHPsXoft20tt29rwQTmwbJnUp4/0ww9FbcOH2ynJYWHe1VXJEDCokHbtsltJrVwprV4t7dlj2xs3llq1sruBnAyR6GjpyBGpXz/p5ZelnBwbQBkZ0o4dUkqK3fswMFCKj5cSE6Wrr7Y7tpdwHR585dgRaXg36cVPpIITfznUrGlPiV13naelVUYEDCoMY6QlS+y47dy59g/Vyy6zZ0kSE20vpE6d07/24EH7s1evU0/L5+fbXk9qqg2rVatsEMXFSYMHS4MG2Z4OPHZop9SptfT5gaK2Tp2kmTNt1xQ+R8Cg3DtwQJoxwwbLli3SRRdJEybY3T/KYgw3KEhq0cLe7rzTtm3cKP3zn9Izz0iPP257M/fcI3XpwviNJ76fK6XeIbU4IH0uO33pz3+U/jHf/gLhCWaRodzau1e6+277x+l999md1JcutV/+Q4e6nSDUooWdnLRrlzRxog22pCSpeXNp6tTi4zpwKD9PShsqLf+TdHS/dJWkayKleZOk8e8SLh4jYFDuGGN7LM2bS2+9Jf3tb9LOnXaB9pVX+rYHER1tey5ffmnHlVu2tL2cLl2kbdt8V0ellPVf6cNLpa2Titoa3CjN2yldN8S7ulCIgEG5smOHdM01Uv/+9ud//ys9/LAdrPdSQID0hz9Ib74pLVxoZ621bGknLR0/7m1tFY4x0rYp0geXSAe+tG1BVaR2/5Quf0sKre5tfShEwKBcOLlLine 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)"
|
||||
]
|
||||
}
|
||||
],
|
||||
|
||||
+266
-278
@@ -14,6 +14,9 @@ import cv2
|
||||
import numpy as np
|
||||
from uptime import uptime
|
||||
|
||||
|
||||
|
||||
class LEDSystem():
|
||||
sender = None
|
||||
debug = True
|
||||
config = None
|
||||
@@ -31,7 +34,12 @@ changecount = 0
|
||||
animation_time = 0
|
||||
start = uptime()
|
||||
|
||||
def ping(host):
|
||||
def __init__(self):
|
||||
self.start = uptime()
|
||||
#self.init()
|
||||
#return self
|
||||
|
||||
def ping(self, host):
|
||||
#Returns True if host (str) responds to a ping request.
|
||||
|
||||
# Option for the number of packets as a function of
|
||||
@@ -49,32 +57,23 @@ def ping(host):
|
||||
|
||||
return subprocess.call(command, stdout=subprocess.DEVNULL, stderr=subprocess.STDOUT) == 0
|
||||
|
||||
def map():
|
||||
global config
|
||||
global leds
|
||||
global leds_size
|
||||
global leds_normalized
|
||||
global controllers
|
||||
global rings
|
||||
global ringstatus
|
||||
global animation_time
|
||||
|
||||
def map(self):
|
||||
|
||||
with open('config.yml', 'r') as fileread:
|
||||
#global config
|
||||
config = yaml.safe_load(fileread)
|
||||
self.config = yaml.safe_load(fileread)
|
||||
|
||||
animation_time = config["animation_time"]
|
||||
leds = list()
|
||||
leds_size = list()
|
||||
controllers = list()
|
||||
rings = list(range(len(config["position_map"])))
|
||||
ringstatus = list(range(len(config["position_map"])))
|
||||
self.animation_time = self.config["animation_time"]
|
||||
self.leds = list()
|
||||
self.leds_size = list()
|
||||
self.controllers = list()
|
||||
self.rings = list(range(len(self.config["position_map"])))
|
||||
self.ringstatus = list(range(len(self.config["position_map"])))
|
||||
#print(rings)
|
||||
#fprint(config["led"]["map"])
|
||||
generate_map = False
|
||||
map = list()
|
||||
for shape in config["led"]["map"]:
|
||||
for shape in self.config["led"]["map"]:
|
||||
if shape["type"] == "circle":
|
||||
|
||||
if generate_map:
|
||||
@@ -84,22 +83,22 @@ def map():
|
||||
angle = 0
|
||||
radius = shape["diameter"] / 2
|
||||
lednum = shape["start"]
|
||||
for item in config['position_map']:
|
||||
for item in self.config['position_map']:
|
||||
# Check if the current item's position matches the target position
|
||||
#print(item['pos'],(shape["pos"][1],shape["pos"][0]))
|
||||
if tuple(item['pos']) == (shape["pos"][1],shape["pos"][0]):
|
||||
rings[item["index"]] = (shape["pos"][1],shape["pos"][0],lednum,lednum+shape["size"]) # rings[index] = x, y, startpos, endpos
|
||||
ringstatus[item["index"]] = [None, None]
|
||||
self.rings[item["index"]] = (shape["pos"][1],shape["pos"][0],lednum,lednum+shape["size"]) # rings[index] = x, y, startpos, endpos
|
||||
self.ringstatus[item["index"]] = [None, None]
|
||||
break
|
||||
if len(leds) < lednum + shape["size"]:
|
||||
for x in range(lednum + shape["size"] - len(leds)):
|
||||
leds.append(None)
|
||||
leds_size.append(None)
|
||||
if len(self.leds) < lednum + shape["size"]:
|
||||
for x in range(lednum + shape["size"] - len(self.leds)):
|
||||
self.leds.append(None)
|
||||
self.leds_size.append(None)
|
||||
while angle < 359.999:
|
||||
tmpangle = angle + shape["angle"]
|
||||
x = math.cos(tmpangle * (math.pi / 180.0)) * radius + shape["pos"][1] # flip by 90 degress when we changed layout
|
||||
y = math.sin(tmpangle * (math.pi / 180.0)) * radius + shape["pos"][0]
|
||||
leds[lednum] = (x,y)
|
||||
self.leds[lednum] = (x,y)
|
||||
lednum = lednum + 1
|
||||
angle = angle + anglediv
|
||||
|
||||
@@ -112,13 +111,13 @@ def map():
|
||||
xmov = math.cos(angle * (math.pi / 180.0)) * distdiv
|
||||
ymov = math.sin(angle * (math.pi / 180.0)) * distdiv
|
||||
pos = shape["pos"]
|
||||
if len(leds) < lednum + shape["size"]:
|
||||
for x in range(lednum + shape["size"] - len(leds)):
|
||||
leds.append(None)
|
||||
leds_size.append(None)
|
||||
if len(self.leds) < lednum + shape["size"]:
|
||||
for x in range(lednum + shape["size"] - len(self.leds)):
|
||||
self.leds.append(None)
|
||||
self.leds_size.append(None)
|
||||
|
||||
while dist < length:
|
||||
leds[lednum] = (pos[0], pos[1])
|
||||
self.leds[lednum] = (pos[0], pos[1])
|
||||
pos[0] += xmov
|
||||
pos[1] += ymov
|
||||
dist += distdiv
|
||||
@@ -140,276 +139,262 @@ def map():
|
||||
yaml_str = yaml.dump(data, default_flow_style=False)
|
||||
print(yaml_str)
|
||||
|
||||
print(rings)
|
||||
print(self.rings)
|
||||
flag = 0
|
||||
for x in leds:
|
||||
for x in self.leds:
|
||||
if x is None:
|
||||
flag = flag + 1
|
||||
if flag > 0:
|
||||
fprint("Warning: Imperfect LED map ordering. Hiding undefined lights.")
|
||||
for x in range(len(leds)):
|
||||
if leds[x] is None:
|
||||
leds[x] = (0, 0)
|
||||
for x in range(len(self.leds)):
|
||||
if self.leds[x] is None:
|
||||
self.leds[x] = (0, 0)
|
||||
|
||||
|
||||
#leds = tmpleds.reverse()
|
||||
#fprint(leds)
|
||||
|
||||
# controller mapping
|
||||
for ctrl in config["led"]["controllers"]:
|
||||
if len(controllers) < ctrl["universe"]+1:
|
||||
for x in range(ctrl["universe"]+1 - len(controllers)):
|
||||
controllers.append(None)
|
||||
for ctrl in self.config["led"]["controllers"]:
|
||||
if len(self.controllers) < ctrl["universe"]+1:
|
||||
for x in range(ctrl["universe"]+1 - len(self.controllers)):
|
||||
self.controllers.append(None)
|
||||
|
||||
controllers[ctrl["universe"]] = (ctrl["ledstart"],ctrl["ledend"]+1,ctrl["ip"])
|
||||
self.controllers[ctrl["universe"]] = (ctrl["ledstart"],ctrl["ledend"]+1,ctrl["ip"])
|
||||
for x in range(ctrl["ledstart"],ctrl["ledend"]+1):
|
||||
leds_size[x] = len(ctrl["mode"])
|
||||
self.leds_size[x] = len(ctrl["mode"])
|
||||
#fprint(controllers)
|
||||
|
||||
if(debug):
|
||||
if(self.debug):
|
||||
import matplotlib.pyplot as plt
|
||||
plt.axis('equal')
|
||||
for ctrl in controllers:
|
||||
plt.scatter(*zip(*leds[ctrl[0]:ctrl[1]]), s=2)
|
||||
for ctrl in self.controllers:
|
||||
plt.scatter(*zip(*self.leds[ctrl[0]:ctrl[1]]), s=2)
|
||||
#plt.scatter(*zip(*leds), s=3)
|
||||
plt.savefig("map.png", dpi=600, bbox_inches="tight")
|
||||
|
||||
leds_adj = [(x-min([led[0] for led in leds]), # push to zero start
|
||||
y-min([led[1] for led in leds]) )
|
||||
for x, y in leds]
|
||||
leds_adj = [(x-min([led[0] for led in self.leds]), # push to zero start
|
||||
y-min([led[1] for led in self.leds]) )
|
||||
for x, y in self.leds]
|
||||
|
||||
leds_normalized = [(x / max([led[0] for led in leds_adj]),
|
||||
self.leds_normalized = [(x / max([led[0] for led in leds_adj]),
|
||||
y / max([led[1] for led in leds_adj]))
|
||||
for x, y in leds_adj]
|
||||
#return leds, controllers
|
||||
|
||||
def init():
|
||||
map()
|
||||
global sender
|
||||
global config
|
||||
global leds
|
||||
global leds_size
|
||||
global controllers
|
||||
global data
|
||||
global exactdata
|
||||
sender = sacn.sACNsender(fps=config["led"]["fps"], universeDiscovery=False)
|
||||
sender.start() # start the sending thread
|
||||
"""for x in range(len(controllers)):
|
||||
print("Waiting for the controller at", controllers[x][2], "to be online...", end="")
|
||||
def init(self):
|
||||
self.map()
|
||||
self.sender = sacn.sACNsender(fps=self.config["led"]["fps"], universeDiscovery=False)
|
||||
self.sender.start() # start the sending thread
|
||||
"""for x in range(len(self.controllers)):
|
||||
print("Waiting for the controller at", self.controllers[x][2], "to be online...", end="")
|
||||
count = 0
|
||||
while not ping(controllers[x][2]):
|
||||
while not ping(self.controllers[x][2]):
|
||||
count = count + 1
|
||||
if count >= config["led"]["timeout"]:
|
||||
if count >= self.config["led"]["timeout"]:
|
||||
fprint(" ERROR: controller still offline after " + str(count) + " seconds, continuing...")
|
||||
break
|
||||
if count < config["led"]["timeout"]:
|
||||
if count < self.config["led"]["timeout"]:
|
||||
fprint(" done")"""
|
||||
for x in range(len(controllers)):
|
||||
print("Activating controller", x, "at", controllers[x][2], "with", controllers[x][1]-controllers[x][0], "LEDs.")
|
||||
sender.activate_output(x+1) # start sending out data
|
||||
sender[x+1].destination = controllers[x][2]
|
||||
sender.manual_flush = True
|
||||
for x in range(len(self.controllers)):
|
||||
print("Activating controller", x, "at", self.controllers[x][2], "with", self.controllers[x][1]-self.controllers[x][0], "LEDs.")
|
||||
self.sender.activate_output(x+1) # start sending out data
|
||||
self.sender[x+1].destination = self.controllers[x][2]
|
||||
self.sender.manual_flush = True
|
||||
|
||||
# initialize global pixel data list
|
||||
data = list()
|
||||
exactdata = list()
|
||||
for x in range(len(leds)):
|
||||
if leds_size[x] == 3:
|
||||
exactdata.append(None)
|
||||
data.append((20,20,127))
|
||||
elif leds_size[x] == 4:
|
||||
exactdata.append(None)
|
||||
data.append((50,50,255,0))
|
||||
self.data = list()
|
||||
self.exactdata = list()
|
||||
for x in range(len(self.leds)):
|
||||
if self.leds_size[x] == 3:
|
||||
self.exactdata.append(None)
|
||||
self.data.append((20,20,127))
|
||||
elif self.leds_size[x] == 4:
|
||||
self.exactdata.append(None)
|
||||
self.data.append((50,50,255,0))
|
||||
else:
|
||||
exactdata.append(None)
|
||||
data.append((0,0,0))
|
||||
sendall(data)
|
||||
self.exactdata.append(None)
|
||||
self.data.append((0,0,0))
|
||||
self.sendall(self.data)
|
||||
#time.sleep(50000)
|
||||
fprint("Running start-up test sequence...")
|
||||
for y in range(1):
|
||||
for x in range(len(leds)):
|
||||
setpixel(0,60,144,x)
|
||||
sendall(data)
|
||||
for x in range(len(self.leds)):
|
||||
self.setpixel(0,60,144,x)
|
||||
self.sendall(self.data)
|
||||
#time.sleep(2)
|
||||
alloffsmooth()
|
||||
self.alloffsmooth()
|
||||
|
||||
def sendall(datain):
|
||||
def sendall(self, datain):
|
||||
# send all LED data to all controllers
|
||||
# data must have all LED data in it as [(R,G,B,)] tuples in an array, 1 tuple per pixel
|
||||
global controllers
|
||||
global sender
|
||||
sender.manual_flush = True
|
||||
for x in range(len(controllers)):
|
||||
sender[x+1].dmx_data = list(sum(datain[controllers[x][0]:controllers[x][1]] , ())) # flatten the subsection of the data array
|
||||
self.sender.manual_flush = True
|
||||
for x in range(len(self.controllers)):
|
||||
self.sender[x+1].dmx_data = list(sum(datain[self.controllers[x][0]:self.controllers[x][1]] , ())) # flatten the subsection of the data array
|
||||
|
||||
sender.flush()
|
||||
self.sender.flush()
|
||||
time.sleep(0.002)
|
||||
#sender.flush() # 100% reliable with 2 flushes, often fails with 1
|
||||
#time.sleep(0.002)
|
||||
#sender.flush()
|
||||
|
||||
def fastsendall(datain):
|
||||
def fastsendall(self, datain):
|
||||
# send all LED data to all controllers
|
||||
# data must have all LED data in it as [(R,G,B,)] tuples in an array, 1 tuple per pixel
|
||||
global controllers
|
||||
global sender
|
||||
sender.manual_flush = False
|
||||
print(datain[controllers[0][0]:controllers[0][1]])
|
||||
for x in range(len(controllers)):
|
||||
sender[x+1].dmx_data = list(sum(datain[controllers[x][0]:controllers[x][1]] , ())) # flatten the subsection of the data array
|
||||
self.sender.manual_flush = False
|
||||
print(datain[self.controllers[0][0]:self.controllers[0][1]])
|
||||
for x in range(len(self.controllers)):
|
||||
self.sender[x+1].dmx_data = list(sum(datain[self.controllers[x][0]:self.controllers[x][1]] , ())) # flatten the subsection of the data array
|
||||
|
||||
sender.flush()
|
||||
self.sender.flush()
|
||||
|
||||
def senduniverse(datain, lednum):
|
||||
def senduniverse(self, datain, lednum):
|
||||
# send all LED data for 1 controller/universe
|
||||
# data must have all LED data in it as [(R,G,B,)] tuples in an array, 1 tuple per pixel
|
||||
global controllers
|
||||
global sender
|
||||
for x in range(len(controllers)):
|
||||
if lednum >= controllers[x][0] and lednum < controllers[x][1]:
|
||||
sender[x+1].dmx_data = list(sum(datain[controllers[x][0]:controllers[x][1]] , ())) # flatten the subsection of the data array
|
||||
for x in range(len(self.controllers)):
|
||||
if lednum >= self.controllers[x][0] and lednum < self.controllers[x][1]:
|
||||
self.sender[x+1].dmx_data = list(sum(datain[self.controllers[x][0]:self.controllers[x][1]] , ())) # flatten the subsection of the data array
|
||||
|
||||
sender.flush()
|
||||
self.sender.flush()
|
||||
time.sleep(0.004)
|
||||
#sender.flush() # 100% reliable with 2 flushes, often fails with 1
|
||||
#time.sleep(0.002)
|
||||
#sender.flush()
|
||||
|
||||
def alloff():
|
||||
def alloff(self):
|
||||
tmpdata = list()
|
||||
for x in range(len(leds)):
|
||||
if leds_size[x] == 3:
|
||||
for x in range(len(self.leds)):
|
||||
if self.leds_size[x] == 3:
|
||||
tmpdata.append((0,0,0))
|
||||
elif leds_size[x] == 4:
|
||||
elif self.leds_size[x] == 4:
|
||||
tmpdata.append((0,0,0,0))
|
||||
else:
|
||||
tmpdata.append((0,0,0))
|
||||
sendall(tmpdata)
|
||||
self.sendall(tmpdata)
|
||||
#sendall(tmpdata)
|
||||
#sendall(tmpdata) #definitely make sure it's off
|
||||
return self
|
||||
|
||||
def allon():
|
||||
global data
|
||||
sendall(data)
|
||||
def allon(self):
|
||||
self.sendall(self.data)
|
||||
return self
|
||||
|
||||
def alloffsmooth():
|
||||
tmpdata = data
|
||||
def alloffsmooth(self):
|
||||
tmpdata = self.data
|
||||
for x in range(256):
|
||||
for x in range(len(data)):
|
||||
setpixel(tmpdata[x][0]-1,tmpdata[x][1]-1,tmpdata[x][2]-1, x)
|
||||
sendall(tmpdata)
|
||||
for x in range(len(self.data)):
|
||||
self.setpixel(tmpdata[x][0]-1,tmpdata[x][1]-1,tmpdata[x][2]-1, x)
|
||||
self.sendall(tmpdata)
|
||||
|
||||
alloff()
|
||||
self.alloff()
|
||||
return self
|
||||
|
||||
def setpixelnow(r, g, b, num):
|
||||
def setpixelnow(self, r, g, b, num):
|
||||
# slight optimization: send only changed universe
|
||||
# unfortunately no way to manual flush data packets to only 1 controller with this sACN library
|
||||
global data
|
||||
setpixel(r,g,b,num)
|
||||
senduniverse(data, num)
|
||||
self.setpixel(r,g,b,num)
|
||||
self.senduniverse(self.data, num)
|
||||
return self
|
||||
|
||||
def setmode(stmode, r=0,g=0,b=0):
|
||||
global mode
|
||||
global firstrun
|
||||
def setmode(self, stmode, r=0,g=0,b=0):
|
||||
if stmode is not None:
|
||||
if mode != stmode:
|
||||
firstrun = True
|
||||
if self.mode != stmode:
|
||||
self.firstrun = True
|
||||
|
||||
mode = stmode
|
||||
self.mode = stmode
|
||||
return self
|
||||
|
||||
|
||||
def setring(r,g,b,idx):
|
||||
def setring(self, r,g,b,idx):
|
||||
|
||||
ring = rings[idx]
|
||||
ring = self.rings[idx]
|
||||
for pixel in range(ring[2],ring[3]):
|
||||
setpixel(r,g,b,pixel)
|
||||
self.setpixel(r,g,b,pixel)
|
||||
#global data
|
||||
#senduniverse(data, ring[2])
|
||||
return self
|
||||
|
||||
def runmodes(ring = -1, speed = 1):
|
||||
global mode
|
||||
global firstrun
|
||||
global changecount
|
||||
fprint("Mode: " + str(mode))
|
||||
if mode == "Startup":
|
||||
def runmodes(self, ring = -1, speed = 1):
|
||||
fprint("Mode: " + str(self.mode))
|
||||
if self.mode == "Startup":
|
||||
# loading animation. cable check
|
||||
if firstrun:
|
||||
changecount = animation_time * 3
|
||||
if self.firstrun:
|
||||
self.changecount = self.animation_time * 3
|
||||
firstrun = False
|
||||
for x in range(len(ringstatus)):
|
||||
ringstatus[x] = [True, animation_time]
|
||||
for x in range(len(self.ringstatus)):
|
||||
self.ringstatus[x] = [True, self.animation_time]
|
||||
|
||||
if changecount > 0:
|
||||
fprint(changecount)
|
||||
changecount = fadeorder(0,len(leds), changecount, 0,50,100)
|
||||
if self.changecount > 0:
|
||||
fprint(self.changecount)
|
||||
self.changecount = self.fadeorder(0,len(self.leds), self.changecount, 0,50,100)
|
||||
else:
|
||||
setmode("Startup2")
|
||||
self.setmode("Startup2")
|
||||
|
||||
|
||||
elif mode == "Startup2":
|
||||
if firstrun:
|
||||
firstrun = False
|
||||
elif self.mode == "Startup2":
|
||||
if self.firstrun:
|
||||
self.firstrun = False
|
||||
|
||||
else:
|
||||
for x in range(len(ringstatus)):
|
||||
if ringstatus[x][0]:
|
||||
setring(0, 50, 100, x)
|
||||
for x in range(len(self.ringstatus)):
|
||||
if self.ringstatus[x][0]:
|
||||
self.setring(0, 50, 100, x)
|
||||
else:
|
||||
ringstatus[x][1] = fadeall(rings[x][2],rings[x][3], ringstatus[x][1], 100,0,0) # not ready
|
||||
self.ringstatus[x][1] = self.fadeall(self.rings[x][2],self.rings[x][3], self.ringstatus[x][1], 100,0,0) # not ready
|
||||
|
||||
elif mode == "StartupCheck":
|
||||
if firstrun:
|
||||
firstrun = False
|
||||
for x in range(len(ringstatus)):
|
||||
ringstatus[x] = [False, animation_time]
|
||||
elif self.mode == "StartupCheck":
|
||||
if self.firstrun:
|
||||
self.firstrun = False
|
||||
for x in range(len(self.ringstatus)):
|
||||
self.ringstatus[x] = [False, self.animation_time]
|
||||
else:
|
||||
for x in range(len(ringstatus)):
|
||||
if ringstatus[x][0]:
|
||||
ringstatus[x][1] = fadeall(rings[x][2],rings[x][3], ringstatus[x][1], 0,50,100) # ready
|
||||
for x in range(len(self.ringstatus)):
|
||||
if self.ringstatus[x][0]:
|
||||
self.ringstatus[x][1] = self.fadeall(self.rings[x][2],self.rings[x][3], self.ringstatus[x][1], 0,50,100) # ready
|
||||
else:
|
||||
setring(100, 0, 0, x)
|
||||
self.setring(100, 0, 0, x)
|
||||
|
||||
elif mode == "GrabA":
|
||||
if firstrun:
|
||||
firstrun = False
|
||||
changecount = animation_time # 100hz
|
||||
if changecount > 0:
|
||||
changecount = fadeall(rings[ring][2],rings[ring][3], changecount, 100,0,0)
|
||||
elif self.mode == "GrabA":
|
||||
if self.firstrun:
|
||||
self.firstrun = False
|
||||
self.changecount = self.animation_time # 100hz
|
||||
if self.changecount > 0:
|
||||
self.changecount = self.fadeall(self.rings[ring][2],self.rings[ring][3], self.changecount, 100,0,0)
|
||||
else:
|
||||
setring(100,0,0,ring)
|
||||
setmode("GrabB")
|
||||
elif mode == "GrabB":
|
||||
if firstrun:
|
||||
firstrun = False
|
||||
changecount = animation_time # 100hz
|
||||
if changecount > 0:
|
||||
changecount = fadeorder(rings[ring][2],rings[ring][3], changecount, 0,100,0)
|
||||
self.setring(100,0,0,ring)
|
||||
self.setmode("GrabB")
|
||||
elif self.mode == "GrabB":
|
||||
if self.firstrun:
|
||||
self.firstrun = False
|
||||
self.changecount = self.animation_time # 100hz
|
||||
if self.changecount > 0:
|
||||
self.changecount = self.fadeorder(self.rings[ring][2],self.rings[ring][3], self.changecount, 0,100,0)
|
||||
else:
|
||||
setring(0,100,0,ring)
|
||||
setmode("idle")
|
||||
elif mode == "GrabC":
|
||||
if firstrun:
|
||||
firstrun = False
|
||||
changecount = animation_time # 100hz
|
||||
if changecount > 0:
|
||||
changecount = fadeall(rings[ring][2],rings[ring][3], changecount, 0,50,100)
|
||||
self.setring(0,100,0,ring)
|
||||
self.setmode("idle")
|
||||
elif self.mode == "GrabC":
|
||||
if self.firstrun:
|
||||
self.firstrun = False
|
||||
self.changecount = self.animation_time # 100hz
|
||||
if self.changecount > 0:
|
||||
self.changecount = self.fadeall(self.rings[ring][2],self.rings[ring][3], self.changecount, 0,50,100)
|
||||
else:
|
||||
setring(0,50,100,ring)
|
||||
setmode("idle")
|
||||
elif mode == "idle":
|
||||
self.setring(0,50,100,ring)
|
||||
self.setmode("idle")
|
||||
elif self.mode == "idle":
|
||||
time.sleep(0)
|
||||
|
||||
sendall(data)
|
||||
self.sendall(self.data)
|
||||
return self
|
||||
|
||||
def fadeall(idxa,idxb,sizerem,r,g,b):
|
||||
def fadeall(self, idxa,idxb,sizerem,r,g,b):
|
||||
if sizerem < 1:
|
||||
return 0
|
||||
global exactdata
|
||||
sum = 0
|
||||
for x in range(idxa,idxb):
|
||||
if exactdata[x] is None:
|
||||
exactdata[x] = data[x]
|
||||
old = exactdata[x]
|
||||
if self.exactdata[x] is None:
|
||||
self.exactdata[x] = self.data[x]
|
||||
old = self.exactdata[x]
|
||||
dr = (r - old[0])/sizerem
|
||||
sum += abs(dr)
|
||||
dr += old[0]
|
||||
@@ -419,27 +404,26 @@ def fadeall(idxa,idxb,sizerem,r,g,b):
|
||||
db = (b - old[2])/sizerem
|
||||
db += old[2]
|
||||
sum += abs(db)
|
||||
exactdata[x] = (dr, dg, db)
|
||||
self.exactdata[x] = (dr, dg, db)
|
||||
#print(new)
|
||||
setpixel(dr, dg, db, x)
|
||||
self.setpixel(dr, dg, db, x)
|
||||
if sizerem == 1:
|
||||
exactdata[x] = None
|
||||
self.exactdata[x] = None
|
||||
if sum == 0 and sizerem > 2:
|
||||
sizerem = 2
|
||||
return sizerem - 1
|
||||
|
||||
def fadeorder(idxa,idxb,sizerem,r,g,b):
|
||||
def fadeorder(self, idxa,idxb,sizerem,r,g,b):
|
||||
if sizerem < 1:
|
||||
return 0
|
||||
global exactdata
|
||||
drs = 0
|
||||
dgs = 0
|
||||
dbs = 0
|
||||
sum = 0
|
||||
for x in range(idxa,idxb):
|
||||
if exactdata[x] is None:
|
||||
exactdata[x] = data[x]
|
||||
old = exactdata[x]
|
||||
if self.exactdata[x] is None:
|
||||
self.exactdata[x] = self.data[x]
|
||||
old = self.exactdata[x]
|
||||
dr = (r - old[0])
|
||||
dg = (g - old[1])
|
||||
db = (b - old[2])
|
||||
@@ -453,7 +437,7 @@ def fadeorder(idxa,idxb,sizerem,r,g,b):
|
||||
sum += abs(drs) + abs(dgs) + abs(dbs)
|
||||
print(drs,dgs,dbs)
|
||||
for x in range(idxa,idxb):
|
||||
old = exactdata[x]
|
||||
old = self.exactdata[x]
|
||||
new = list(old)
|
||||
if drs > 0:
|
||||
if old[0] + drs > r:
|
||||
@@ -500,20 +484,18 @@ def fadeorder(idxa,idxb,sizerem,r,g,b):
|
||||
dbs = 0
|
||||
|
||||
if drs != 0 or dgs != 0 or dbs != 0:
|
||||
exactdata[x] = new
|
||||
setpixel(new[0],new[1],new[2],x)
|
||||
self.exactdata[x] = new
|
||||
self.setpixel(new[0],new[1],new[2],x)
|
||||
|
||||
if sizerem == 1:
|
||||
exactdata[x] = None
|
||||
self.exactdata[x] = None
|
||||
|
||||
if sum == 0 and sizerem > 2:
|
||||
sizerem = 2
|
||||
return sizerem - 1
|
||||
|
||||
|
||||
def setpixel(r, g, b, num):
|
||||
global data
|
||||
global leds_size
|
||||
def setpixel(self, r, g, b, num):
|
||||
# constrain values
|
||||
if r < 0:
|
||||
r = 0
|
||||
@@ -528,29 +510,28 @@ def setpixel(r, g, b, num):
|
||||
elif b > 255:
|
||||
b = 255
|
||||
|
||||
if leds_size[num] == 3:
|
||||
data[num] = (int(r), int(g), int(b))
|
||||
elif leds_size[num] == 4: # cut out matching white and turn on white pixel instead
|
||||
data[num] = (( int(r) - int(min(r,g,b)), int(g) - int(min(r,g,b)), int(b) - int(min(r,g,b)), int(min(r,g,b))) )
|
||||
if self.leds_size[num] == 3:
|
||||
self.data[num] = (int(r), int(g), int(b))
|
||||
elif self.leds_size[num] == 4: # cut out matching white and turn on white pixel instead
|
||||
self.data[num] = (( int(r) - int(min(r,g,b)), int(g) - int(min(r,g,b)), int(b) - int(min(r,g,b)), int(min(r,g,b))) )
|
||||
else:
|
||||
data[num] = (int(r), int(g), int(b))
|
||||
self.data[num] = (int(r), int(g), int(b))
|
||||
return self
|
||||
|
||||
|
||||
def close():
|
||||
global sender
|
||||
def close(self):
|
||||
time.sleep(0.5)
|
||||
sender.stop()
|
||||
self.sender.stop()
|
||||
return self
|
||||
|
||||
def mapimage(image, fps=90):
|
||||
global start
|
||||
while uptime() - start < 1/fps:
|
||||
def mapimage(self, image, fps=90):
|
||||
while uptime() - self.start < 1/fps:
|
||||
time.sleep(0.00001)
|
||||
fprint(1 / (uptime() - start))
|
||||
start = uptime()
|
||||
fprint(1 / (uptime() - self.start))
|
||||
self.start = uptime()
|
||||
minsize = min(image.shape[0:2])
|
||||
leds_normalized2 = [(x * minsize,
|
||||
y * minsize)
|
||||
for x, y in leds_normalized]
|
||||
for x, y in self.leds_normalized]
|
||||
|
||||
cv2.imshow("video", image)
|
||||
cv2.waitKey(1)
|
||||
@@ -567,39 +548,39 @@ def mapimage(image, fps=90):
|
||||
#avgx += x
|
||||
#avgy += y
|
||||
color = tuple(image[y, x])
|
||||
setpixel(color[2]/2,color[1]/2,color[0]/2,xx) # swap b & r
|
||||
self.setpixel(color[2]/2,color[1]/2,color[0]/2,xx) # swap b & r
|
||||
#print(color)
|
||||
else:
|
||||
#avgx += x
|
||||
#avgy += y
|
||||
setpixel(0,0,0,xx)
|
||||
self.setpixel(0,0,0,xx)
|
||||
#avgx /= len(leds)
|
||||
#avgy /= len(leds)
|
||||
#print((avgx,avgy, max([led[0] for led in leds_adj]), max([led[1] for led in leds_adj]) , min(image.shape[0:2]) ))
|
||||
global data
|
||||
fastsendall(data)
|
||||
self.fastsendall(self.data)
|
||||
return self
|
||||
|
||||
def mainloop(stmode, ring = -1, fps = 100, preview = False):
|
||||
global start
|
||||
while uptime() - start < 1/fps:
|
||||
def mainloop(self, stmode, ring = -1, fps = 100, preview = False):
|
||||
while uptime() - self.start < 1/fps:
|
||||
time.sleep(0.00001)
|
||||
fprint(1 / (uptime() - start))
|
||||
start = uptime()
|
||||
if mode is not None:
|
||||
setmode(stmode)
|
||||
runmodes(ring)
|
||||
fprint(1 / (uptime() - self.start))
|
||||
self.start = uptime()
|
||||
if self.mode is not None:
|
||||
self.setmode(stmode)
|
||||
self.runmodes(ring)
|
||||
if preview:
|
||||
drawdata()
|
||||
self.drawdata()
|
||||
return self
|
||||
|
||||
def drawdata():
|
||||
def drawdata(self):
|
||||
#tmp = list()
|
||||
#for x in len(leds):
|
||||
# led = leds[x]
|
||||
# tmp.append((led[0], led[1], data[x]))
|
||||
|
||||
x = [led[0] for led in leds]
|
||||
y = [led[1] for led in leds]
|
||||
colors = data
|
||||
x = [led[0] for led in self.leds]
|
||||
y = [led[1] for led in self.leds]
|
||||
colors = self.data
|
||||
colors_normalized = [(x[0]/255, x[1]/255, x[2]/255) for x in colors]
|
||||
# Plot the points
|
||||
plt.scatter(x, y, c=colors_normalized)
|
||||
@@ -612,42 +593,48 @@ def drawdata():
|
||||
plt.show()
|
||||
plt.savefig("map3.png", dpi=50, bbox_inches="tight")
|
||||
plt.clf()
|
||||
return self
|
||||
|
||||
def startup_animation(show):
|
||||
def startup_animation(self, show):
|
||||
|
||||
|
||||
stmode = "Startup"
|
||||
mainloop(stmode, preview=show)
|
||||
while mode == "Startup":
|
||||
mainloop(None, preview=show)
|
||||
self.mainloop(stmode, preview=show)
|
||||
while self.mode == "Startup":
|
||||
self.mainloop(None, preview=show)
|
||||
for x in range(54):
|
||||
ringstatus[x][0] = False
|
||||
mainloop(None, preview=show)
|
||||
self.ringstatus[x][0] = False
|
||||
self.mainloop(None, preview=show)
|
||||
|
||||
for x in range(animation_time):
|
||||
mainloop(None, preview=show)
|
||||
clear_animations()
|
||||
for x in range(self.animation_time):
|
||||
self.mainloop(None, preview=show)
|
||||
self.clear_animations()
|
||||
stmode = "StartupCheck"
|
||||
mainloop(stmode, preview=show)
|
||||
clear_animations()
|
||||
self.mainloop(stmode, preview=show)
|
||||
self.clear_animations()
|
||||
return self
|
||||
|
||||
def clear_animations():
|
||||
for x in range(len(leds)):
|
||||
exactdata[x] = None
|
||||
def clear_animations(self):
|
||||
for x in range(len(self.leds)):
|
||||
self.exactdata[x] = None
|
||||
return self
|
||||
|
||||
def do_animation(stmode, ring=-1):
|
||||
mainloop(stmode, ring, preview=show)
|
||||
wait_for_animation(ring)
|
||||
def do_animation(self, stmode, ring=-1):
|
||||
self.mainloop(stmode, ring, preview=show)
|
||||
self.wait_for_animation(ring)
|
||||
return self
|
||||
|
||||
def start_animation(stmode, ring=-1):
|
||||
mainloop(stmode, ring, preview=show)
|
||||
def start_animation(self, stmode, ring=-1):
|
||||
self.mainloop(stmode, ring, preview=show)
|
||||
return self
|
||||
|
||||
def wait_for_animation(ring=-1):
|
||||
while mode != "idle":
|
||||
mainloop(None, ring, preview=show)
|
||||
def wait_for_animation(self, ring=-1):
|
||||
while self.mode != "idle":
|
||||
self.mainloop(None, ring, preview=show)
|
||||
return self
|
||||
|
||||
if __name__ == "__main__":
|
||||
init()
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
"""cap = cv2.VideoCapture('badapple.mp4')
|
||||
while cap.isOpened():
|
||||
@@ -657,22 +644,23 @@ if __name__ == "__main__":
|
||||
mapimage(frame, fps=30)"""
|
||||
show = False
|
||||
ring = 1
|
||||
startup_animation(show)
|
||||
ledsys = LEDSystem()
|
||||
ledsys.startup_animation(show)
|
||||
for x in range(54):
|
||||
ringstatus[x][0] = True
|
||||
mainloop(None, preview=show)
|
||||
for x in range(animation_time):
|
||||
mainloop(None, preview=show)
|
||||
ledsys.ringstatus[x][0] = True
|
||||
ledsys.mainloop(None, preview=show)
|
||||
for x in range(ledsys.animation_time):
|
||||
ledsys.mainloop(None, preview=show)
|
||||
|
||||
do_animation("GrabA", 1)
|
||||
ledsys.do_animation("GrabA", 1)
|
||||
|
||||
do_animation("GrabA", 5)
|
||||
start_animation("GrabC", 1)
|
||||
ledsys.do_animation("GrabA", 5)
|
||||
ledsys.start_animation("GrabC", 1)
|
||||
|
||||
wait_for_animation(1)
|
||||
do_animation("GrabC", 5)
|
||||
ledsys.wait_for_animation(1)
|
||||
ledsys.do_animation("GrabC", 5)
|
||||
|
||||
close()
|
||||
ledsys.close()
|
||||
#sys.exit(0)
|
||||
|
||||
|
||||
|
||||
+20
-9
@@ -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]
|
||||
|
||||
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):
|
||||
@@ -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:
|
||||
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:
|
||||
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(",")))
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
from alive_progress import alive_bar
|
||||
import get_specs
|
||||
import traceback
|
||||
#import logging
|
||||
@@ -12,16 +13,19 @@ 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
|
||||
|
||||
|
||||
|
||||
@@ -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:
|
||||
@@ -231,13 +254,18 @@ 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)
|
||||
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 +281,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)
|
||||
|
||||
@@ -265,23 +293,156 @@ def setup_server(pool):
|
||||
fprint("Arm initialized.", sendqueue=to_server_queue)
|
||||
|
||||
|
||||
|
||||
|
||||
jbs = JukeboxSearch()
|
||||
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.find("bldn.app/") > -1:
|
||||
scan_value = scan_value[scan_value.find("bldn.app/")+9:]
|
||||
cable_list.append((counter, 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 = list()
|
||||
for cable in cable_list:
|
||||
tmp.append(cable[1])
|
||||
|
||||
tmp = [
|
||||
# Actual cables in Jukebox
|
||||
|
||||
"AW86104CY",
|
||||
"AW3050",
|
||||
"AW6714",
|
||||
"AW1172C",
|
||||
"AWFIT-221-1_4",
|
||||
|
||||
"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"
|
||||
]
|
||||
cable_list = tmp
|
||||
pool.apply_async(get_specs.get_multi, (tmp, 0.5), 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
|
||||
#partnums = list()
|
||||
# debug
|
||||
#success = True
|
||||
|
||||
#cable_list = list(range(len(partnums)))
|
||||
if success:
|
||||
# easy mode
|
||||
fprint("All cables inventoried and parsed.")
|
||||
for x in range(len(cable_list)):
|
||||
#cable_list[x] = (cable_list[x][0], partnums[x])
|
||||
cable_list[x] = (x, cable_list[x])
|
||||
fprint("Adding to database...")
|
||||
for idx,partnum in cable_list:
|
||||
with open("cables/" + partnum[2:] + "/search.json", "rb") as f:
|
||||
searchdata = json.load(f)
|
||||
searchdata["position"] = idx
|
||||
with open("cables/" + partnum[2:] + "/specs.json", "rb") as f:
|
||||
specs = json.load(f)
|
||||
searchdata["fullspecs"] = specs
|
||||
jbs.add_document(searchdata)
|
||||
|
||||
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():
|
||||
|
||||
|
||||
@@ -1,8 +1,10 @@
|
||||
#!/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_APIKEY = "fluffybunnyrabbit" # I WOULD RECOMMEND SOMETHING MORE SECURE
|
||||
@@ -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:
|
||||
|
||||
+145
-77
@@ -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,12 +14,25 @@ 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
|
||||
class Rob():
|
||||
robot = None
|
||||
#offset_x, offset_y, offset_z = (0, 0, 0.14) # Tool offset
|
||||
#
|
||||
def __init__(self, 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()
|
||||
|
||||
def init_arm(self):
|
||||
#sys.stdout = Logger()
|
||||
fprint("Starting UR5 power up...")
|
||||
|
||||
@@ -30,29 +44,29 @@ def init(ip):
|
||||
# trigger auto-initialize
|
||||
|
||||
# wait for auto-initialize
|
||||
|
||||
ip = self.ip
|
||||
# init urx
|
||||
fprint("Connecting to arm at " + ip)
|
||||
trying = True
|
||||
while trying:
|
||||
try:
|
||||
rob = urx.Robot(ip)
|
||||
self.robot = 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)
|
||||
rob.set_tcp((0, 0, 0.15, 0, 0, 0))
|
||||
self.robot.set_tcp((self.offset_x, self.offset_y, self.offset_z, 0, 0, 0))
|
||||
|
||||
# Set weight
|
||||
rob.set_payload(2, (0, 0, 0.1))
|
||||
self.robot.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,138 @@ 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):
|
||||
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)]
|
||||
|
||||
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=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
|
||||
def offset_gripper_angle(x, y, z, gripperangle=35, gripperlength=0.20+0.018, flip=False):
|
||||
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)
|
||||
|
||||
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)
|
||||
|
||||
|
||||
def goto_holder_index(idx, z=0.05, gripperangle=35, flip=False):
|
||||
def goto_holder_index(robot, 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)
|
||||
|
||||
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.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(robot, *safe_pos, flip=is_flipped()), vel=2, acc=2) # Move to safe position
|
||||
rob.movej(offset_gripper_angle(robot, *safe_pos, flip=(not is_flipped())), vel=2, acc=2) # Flip gripper
|
||||
|
||||
def safe_move(robot, x, y, z):
|
||||
rob = robot.robot
|
||||
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(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)
|
||||
|
||||
|
||||
|
||||
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
|
||||
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,40 +439,25 @@ 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)
|
||||
|
||||
|
||||
|
||||
# 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(robot)
|
||||
flip(robot)
|
||||
|
||||
#rob.movej(goto_holder_index(24, 0.2, 0), acc=2, vel=2)
|
||||
#joints = []
|
||||
@@ -398,11 +466,11 @@ if __name__ == "__main__":
|
||||
# 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)
|
||||
rob.movej(goto_holder_index(robot, 26, 0.1, angle), acc=2, vel=2)
|
||||
time.sleep(1)
|
||||
rob.movej(goto_holder_index(25, 0.1, angle), acc=2, vel=2)
|
||||
rob.movej(goto_holder_index(robot, 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(robot, 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)
|
||||
|
||||
Reference in new issue
Block a user