Jukebox/jukebox-software
10
0 Fork 0
Code Issues 3 Pull Requests Packages Projects Releases 2 Wiki Activity

Basic move restrictions and flip routine

Browse Source
This commit is contained in:
BlueOceanWave committed 2024-03-23 15:47:10 -05:00
1 parent 218303e92b
commit 2ec7906ee4
2 files changed
+207 -84
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

inv_kin_testing.ipynb
+95 -30
View File
@@ -9,7 +9,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 81, "execution_count": 1,
"metadata": {}, "metadata": {},
"outputs": [], "outputs": [],
"source": [ "source": [
@@ -28,7 +28,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 82, "execution_count": 2,
"metadata": {}, "metadata": {},
"outputs": [], "outputs": [],
"source": [ "source": [
@@ -52,21 +52,21 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 83, "execution_count": 3,
"metadata": {}, "metadata": {},
"outputs": [ "outputs": [
{ {
"data": { "data": {
"text/plain": [ "text/plain": [
"[-243.619306486927,\n", "[82.5301003420473,\n",
" -123.2097721836616,\n", " -83.54918182551367,\n",
" 140.34764917140853,\n", " 111.24503912126411,\n",
" -107.13787698774695,\n", " -117.69585729575046,\n",
" -90.0,\n", " -90.0,\n",
" 90.0]" " 90.0]"
] ]
}, },
"execution_count": 83, "execution_count": 3,
"metadata": {}, "metadata": {},
"output_type": "execute_result" "output_type": "execute_result"
} }
@@ -76,7 +76,6 @@
" # Normalizes degree theta from -1.5pi to 1.5pi\n", " # Normalizes degree theta from -1.5pi to 1.5pi\n",
" multiplier = 1.5\n", " multiplier = 1.5\n",
" normalized_theta = theta % (math.pi * multiplier)\n", " normalized_theta = theta % (math.pi * multiplier)\n",
" \n",
" # Maintain the negative sign if the original angle is negative\n", " # Maintain the negative sign if the original angle is negative\n",
" if theta < 0:\n", " if theta < 0:\n",
" normalized_theta -= math.pi * multiplier\n", " normalized_theta -= math.pi * multiplier\n",
@@ -146,10 +145,12 @@
" offsets = [-math.pi/2, 0, 0, -math.pi/2, 0, 0]\n", " offsets = [-math.pi/2, 0, 0, -math.pi/2, 0, 0]\n",
"\n", "\n",
" # Return adjusted joint positions\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", "\n",
"# Print degree rotation for each joint (robot angles)\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", "cell_type": "code",
"execution_count": 84, "execution_count": 4,
"metadata": {}, "metadata": {},
"outputs": [], "outputs": [],
"source": [ "source": [
@@ -229,7 +230,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": null, "execution_count": 5,
"metadata": {}, "metadata": {},
"outputs": [], "outputs": [],
"source": [ "source": [
@@ -321,25 +322,23 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 89, "execution_count": 6,
"metadata": {}, "metadata": {},
"outputs": [ "outputs": [
{ {
"name": "stdout", "name": "stdout",
"output_type": "stream", "output_type": "stream",
"text": [ "text": [
"Target position (x,y,z): 0.5 0.5 0.3\n", "Angles: [-53.867, 89.583, 87.39, 2.193, -90.0, 0.0]\n",
"R: 0.7071\n", "Circle position (cx, cy): (0.079, -0.108)\n",
"Angles (base, shoulder, elbow, wrist): [-34.1339, 65.0453, 57.0325, 8.0128]\n", "Shoulder (x, y, z): (0.081, -0.106, 0.425)\n",
"Robot Angles: [-124.1339, -65.0453, 57.0325, -81.9872, -90.0, 90.0]\n", "Elbow (x, y, z): (0.398, 0.125, 0.44)\n",
"elbow (x,y): 0.179 0.385\n", "Wrist (x, y, z): (0.5, 0.2, 0.44)\n"
"wrist (x,y): 0.568 0.44\n",
"tool (x,y): 0.694 0.44\n"
] ]
}, },
{ {
"data": { "data": {
"image/png": "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 truncated "image/png": "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 truncated
"text/plain": [ "text/plain": [
"<Figure size 640x480 with 1 Axes>" "<Figure size 640x480 with 1 Axes>"
] ]
@@ -351,16 +350,18 @@
"name": "stdout", "name": "stdout",
"output_type": "stream", "output_type": "stream",
"text": [ "text": [
"Angles: [-34.134, 65.045, 57.033, 8.013, -90.0, 0.0]\n", "Target position (x,y,z): 0.5 0.2 0.3\n",
"Circle position (cx, cy): (0.11, -0.075)\n", "R: 0.5385\n",
"Shoulder (x, y, z): (0.211, 0.074, 0.385)\n", "Angles (base, shoulder, elbow, wrist): [-53.8671, 89.5827, 87.3895, 2.1932]\n",
"Elbow (x, y, z): (0.429, 0.395, 0.44)\n", "Robot Angles: [-143.8671, -89.5827, 87.3895, -87.8068, -90.0, 90.0]\n",
"Wrist (x, y, z): (0.5, 0.5, 0.44)\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": { "data": {
"image/png": "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 truncated "image/png": "iVBORw0KGgoAAAANSUhEUgAAAiMAAAGmCAYAAACuv4RHAAAAOXRFWHRTb2Z0d2FyZQBNYXRwbG90bGliIHZlcnNpb24zLjUuMSwgaHR0cHM6Ly9tYXRwbG90bGliLm9yZy/YYfK9AAAACXBIWXMAAA9hAAAPYQGoP6dpAAA2kElEQVR4nO3df1yV9d3H8Tc/5KeCGgaBCIrd4m8cJGG3P1akOZc/ZstaCyJnLbVl3DU1J1RmWJm5mdNmaaYZ1X1n7UdpRrHNopulUqZl6eb8MUHdChQa6DnX/Ye3J48C5zrIlwP6ej4e5zHPxfe6zve6bH7efK/r+z1+lmVZAgAA8BF/X3cAAABc3AgjAADApwgjAADApwgjAADApwgjAADApwgjAADApwgjAADApwgjAADApwgjAADApwgjAADApwgjAIAGFRQU6IorrlCHDh106aWXavz48dq1a1ej++zYsUMTJ05UYmKi/Pz8tHjx4nPaHDt2TDNmzFBCQoJCQ0M1ZMgQ/eUvf3FrU15ertGjRys2NlbTp0+X0+lszlNDK0IYAQA06I9//KOmTZumDz/8UJs2bdKJEyc0cuRIVVdXN7hPTU2NevTooQULFigmJqbeNj/5yU+0adMmrVmzRtu3b9fIkSOVmZmpgwcPutrMnTtXaWlpeuutt/TXv/5VhYWFzX5+aB38+KI8AIBdR44c0aWXXqo//vGPGjZsmMf2iYmJmjFjhmbMmOHa9s0336hDhw564403NGbMGNf21NRUjR49Wo888ogk6YYbbtCNN96oG264QT/72c/Up08fTZ06tdnPCb7HyAgAwLbKykpJUufOnZt8jJMnT8rhcCgkJMRte2hoqDZv3ux6P2vWLN19990KDg7W1q1blZWV1eTPROsW6OsOAADaBqfTqRkzZuiqq65Sv379mnycDh06KCMjQ/PmzVPv3r0VHR2tl156SSUlJerZs6erXVpamg4ePKijR482eLsHFwZGRgAAtkybNk2ffvppszy7sWbNGlmWpbi4OAUHB+tXv/qVbr75Zvn7u5elwMBAgshFgDACAPBo+vTp+v3vf6/33ntPXbt2Pe/jJSUl6Y9//KOOHz+u/fv3q7S0VCdOnFCPHj2aobdoa9pEGLEsS1VVVeJZWwBoWZZlafr06Vq/fr3effddde/evVmPHx4erssuu0xfffWVNm7cqHHjxjXr8dE2tIlnRo4dO6bIyEhVVlYqIiLC190BgIvGtGnTtG7dOr3xxhvq0KGDysvLJUmRkZEKDQ2VJGVlZSkuLk4FBQWSpLq6Ou3cudP154MHD6qsrEzt27d3PROyceNGWZalXr16affu3br//vuVnJysnJwcH5wlfK1NTO2tqqoijACAD/j5+dW7fdWqVbrtttskSSNGjFBiYqKef/55SdLevXvrHUEZPny4iouLJUmvvPKKZs+erQMHDqhz586aOHGi5s+fr8jISBOngVaOMAIAAHyqTTwzciFyOByaO3euunfvrtDQUCUlJWnevHluz8X4+fnV+3riiScaPO6yZcs0YMAARUREKCIiQhkZGXrrrbfc2uzZs0cTJkxQly5dFBERoRtvvFEVFRVubUpKSpSSkqLExEQ999xzzXvyAACcgTDiI4899piWLVump59+Wp999pkee+wxPf7441qyZImrzaFDh9xeK1eulJ+fnyZOnNjgcbt27aoFCxZoy5Yt+uijj3T11Vdr3Lhx2rFjhySpurpaI0eOlJ+fn9599129//77qqur0/XXX+/2vQ+TJ0/W3LlztW7dOhUUFGj//v3mLgYA4KLGbRof+f73v6/o6Gi3UYeJEycqNDRUa9eurXef8ePH69ixYyoqKvLqszp37qwnnnhCkydP1ttvv63Ro0frq6++cl3LyspKderUSW+//bYyMzMlSQkJCSouLtall16qESNGaPXq1erTp08TzxYAgIYxMuIjQ4YMUVFRkb744gtJ0scff6zNmzdr9OjR9bavqKjQH/7wB02ePNn2ZzgcDhUWFqq6uloZGRmSpNraWvn5+Sk4ONjVLiQkRP7+/m7LMOfl5al3796KjIzUlVdeSRABABjTJqb2XohmzZqlqqoqJScnKyAgQA6HQ/Pnz9ctt9xSb/vVq1erQ4cO+sEPfuDx2Nu3b1dGRob+/e9/q3379lq/fr0rTFx55ZUKDw/XzJkz9eijj8qyLM2aNUsOh0OHDh1yHWPy5Mm66aabVFdXp06dOjXPSQMAUA9GRnzklVde0Ysvvqh169Zp69atWr16tRYuXKjVq1fX237lypW65ZZbzvliqfr06tVLZWVl+t///V/dddddys7Ods3579Kli1599VX97ne/U/v27RUZGamvv/5a3/nOd85Zhjk8PJwgAgAwjmdGfCQ+Pl6zZs3StGnTXNseeeQRrV27Vp9//rlb2z//+c8aNmyYysrKNHDgQK8/KzMzU0lJSXrmmWfcth89elSBgYHq2LGjYmJi9F//9V+6//77m3ZCAAA0EbdpfKSmpuackYiAgAC3GS2nPffcc0pNTW1SEJFOfdNmbW3tOdujoqIkSe+++64OHz6ssWPHNun4AACcD8KIj1x//fWaP3++unXrpr59+2rbtm1atGiRbr/9drd2VVVVevXVV/Xkk0/We5xrrrlGEyZM0PTp0yVJs2fP1ujRo9WtWzcdO3ZM69atU3FxsTZu3OjaZ9WqVerdu7e6dOmikpIS3XPPPbr33nvVq1cvcycMoPVwnpBO1kiObyRHzak/X3+bdOCQJEuyLEnOs/7XkuQnBXHr1pSrDx7UvpMnG23TrX17vfvVVy3Uo5ZDGPGRJUuWaO7cuZo6daoOHz6s2NhY3XnnncrLy3NrV1hYKMuydPPNN9d7nD179ujo0aOu94cPH1ZWVpYOHTqkyMhIDRgwQBs3btS1117rarNr1y7Nnj1b//rXv5SYmKg5c+bo3nvvNXOiAOyxrFMh4XQ4ODMonLnN7X0D2042sO/pP1v1FLwvJVWcu/lc/2rmE8dp+yTt8dTo+PEW6EnL45kRAGiMZUnOOu8CQWNh4pxtZ4QHy+G788yVzTACU3rKcxhJCgzU7hMnWqI7LYqREQBtk2VJzloPowFNHDk4e5t17rNcAJoPYQRA87IsyfHvZhw5aGQkQq1+YBeADYQR4GJhOU+FhPMp/nbDAwB4gTAC+JrlPP9bCXbCg+Pfvj5TAKgXYQRoiNNxnsX/mzNuSTQSJpznrgEDABcTwgjaHufJ85ziaLOts87XZwoAFwXCCJpPfQspNcfIwdnbnBfetDbA5y6x2S6sq+Qf7LkdvPf3v0seFj1Tu3Yt05cWRhi50J25kFJzP5dgZyElAL7hFygFhkkBYd/+b0DoudsCQ0/977ozt4ee1eaMfdv3PLUPml/PntIeDyuNxMa2TF9aGGHEV9wWUrJ5K6Gp0yF9uZASAHf+7eov9I1tazRM1Lc99NTnAG0EYcSk6r+r5INbFeyoUdjJGoU5ahTi+EZhJ2sU6qhRAAspAa2Hf1A9Rf6MkQM74cHjSESo5M8/u8DZ+H+FSZZTGUf+7OteAG1bQMj5jRycva2+MBEQKvkH+PpMgYsWYcSkAO6r4gIWENr4bYMmjRycPRIRKvn5+/pMARhGGDEpMMzXPcDFyNvi7zEQ1BcwQggJAJoNYcQkRkbg4neexb+RgHFm24AQyc/P1ycLAF4hjBhk+bfTCf92CmJdjNbLz99D8W+m8OAfTEgAgAYQRgyyJNUEhCnIWenrrrQ9fv5SQLihGQ1nHM8/iJAAAD5GGDHIklQTGKaOJy6gMOIXIAWGN1MgaKStfztCAgBcJAgjBjl1amSkRTT3Qkqnt7GQEgDAMMKIQU5JXwd11FftOqomMEw1AWGqCQzTNwGhbu8vCQjTNef7XAILKQEA2igqmEFOSVdc95HHduMkXWO8NwAAtE4sFGCQ3cXe+UsAAFzMqIMGEUYAAPCMOmiQ3TDCnBEAwMWMMGIQIyMAAHhGHTTIstmOvwQAwMWMOmgQIyMAAHhGHTSIMAIAgGfUQYMIIwAAeEYdNIgwAgCAZ02qg0uXLlViYqJCQkKUnp6u0tJSW/sVFhbKz89P48ePb8rHtjlM7QUAwDOvw8jLL7+s3Nxc5efna+vWrRo4cKBGjRqlw4cPN7rf3r17dd9992no0KFN7mxbw8gIAACeeV0HFy1apClTpignJ0d9+vTR8uXLFRYWppUrVza4j8Ph0C233KKHHnpIPXr0OK8OtyVM7QUAwDOv6mBdXZ22bNmizMzMbw/g76/MzEyVlJQ0uN/DDz+sSy+9VJMnT7b1ObW1taqqqnJ7tUWMjAAA4JlXdfDo0aNyOByKjo522x4dHa3y8vJ699m8ebOee+45rVixwvbnFBQUKDIy0vWKj4/3pputBmEEAADPjNbBY8eO6dZbb9WKFSsUFRVle7/Zs2ersrLS9dq/f7/BXppDGAEAwLNAbxpHRUUpICBAFRUVbtsrKioUExNzTvs9e/Zo7969uv76613bnM5TJTowMFC7du1SUlLSOfsFBwcrODjYm661SoQRAAA886oOBgUFKTU1VUVFRa5tTqdTRUVFysjIOKd9cnKytm/frrKyMtdr7Nix+u53v6uysrI2e/vFLqb2AgDgmVcjI5KUm5ur7OxspaWlafDgwVq8eLGqq6uVk5MjScrKylJcXJwKCgoUEhKifv36ue3fsWNHSTpn+4WIkREAADzzOoxMmjRJR44cUV5ensrLy5WSkqINGza4Hmrdt2+f/P0prxJTewEAsMPPsiy7NdNnqqqqFBkZqcrKSkVERPi6O7Z9ImmgjXb3SXrCcF8AAK1bz549tWfPnkbbJCUlaffu3S3Uo5bDL+UGcZsGAADPqIMGEUYAAPCMOmgQYQQAAM+ogwYRRgAA8Iw6aBDrjAAA4BlhxCBGRgAA8Iw6aBDrjAAA4Bl10CBGRgAA8Iw6aBBhBAAAz6iLine truncated
"text/plain": [ "text/plain": [
"<Figure size 640x480 with 1 Axes>" "<Figure size 640x480 with 1 Axes>"
] ]
@@ -371,10 +372,74 @@
], ],
"source": [ "source": [
"def draw_arm(x, y, z, details=False):\n", "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_top_view(x,y,z, details=details)\n",
" draw_arm_side_view(x,y,z, details=details)\n",
"\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)"
] ]
} }
], ],
ur5_control.py
+112 -54
View File
@@ -6,6 +6,7 @@ import numpy as np
import time import time
import os import os
import logging import logging
import yaml
from urx.robotiq_two_finger_gripper import Robotiq_Two_Finger_Gripper from urx.robotiq_two_finger_gripper import Robotiq_Two_Finger_Gripper
import sys import sys
from util import fprint from util import fprint
@@ -191,11 +192,6 @@ def move_to_polar(start_pos, end_pos):
return rx_intermediate 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(): def move_to_home():
global rob global rob
@@ -222,8 +218,6 @@ def normalize_degree(theta):
# Return angle # Return angle
return normalized_theta 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(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 # Get limbs and offsets
@@ -276,37 +270,61 @@ def get_joints_from_xyz_rel(x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_guess =
def get_joints_from_xyz_abs(x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2, l3offset=0): 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) 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 # Joint offsets
# Base, Shoulder, Elbow, Wrist # Base, Shoulder, Elbow, Wrist
inverse = [1, -1, 1, 1, 1, 1] inverse = [1, -1, 1, 1, 1, 1]
offsets = [-math.pi/2, 0, 0, -math.pi/2, 0, 0] 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) print("CRASH! Shoulder at", joints[1] * 180/math.pi)
#else: #else:
#print("Shoulder at", joints[1] * 180/math.pi) #print("Shoulder at", joints[1] * 180/math.pi)
# Return adjusted joint positions # Return adjusted joint positions
return [o+j*i for j, o, i in zip(joints, offsets, inverse)] return [o+j*i for j, o, i in zip(joints, offsets, inverse)]
# gripper angle: from vertical def move_arc(x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2):
# gripper length: from joint to start of grip
# to flip, you can use flip=True or make gripper angle negative 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): 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: if gripperangle < 0:
rz = - math.pi / 2 rz = - math.pi / 2
else: else:
rz = math.pi / 2 rz = math.pi / 2
if flip: if flip:
gripperangle = -degtorad(gripperangle) gripperangle = -math.radians(gripperangle)
grippery = gripperlength - math.cos(gripperangle) * gripperlength grippery = gripperlength - math.cos(gripperangle) * gripperlength
grippery += math.sin(gripperangle) * limb3 grippery += math.sin(gripperangle) * limb3
gripperx = math.sin(gripperangle) * gripperlength + limb3 * 2 gripperx = math.sin(gripperangle) * gripperlength + limb3 * 2
gripperx -= (1-math.cos(gripperangle)) * limb3 gripperx -= (1-math.cos(gripperangle)) * limb3
rz = - math.pi / 2 rz = - math.pi / 2
# flip the whole wrist # 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(x, y, z-grippery, rx=gripperangle + math.radians(180), l3offset=-gripperx, ry=math.pi/2, rz=-rz)
else: else:
gripperangle = degtorad(gripperangle) gripperangle = math.radians(gripperangle)
grippery = gripperlength - math.cos(gripperangle) * gripperlength grippery = gripperlength - math.cos(gripperangle) * gripperlength
grippery -= math.sin(gripperangle) * limb3 grippery -= math.sin(gripperangle) * limb3
gripperx = math.sin(gripperangle) * gripperlength gripperx = math.sin(gripperangle) * gripperlength
@@ -314,15 +332,59 @@ def offset_gripper_angle(x, y, z, gripperangle=35, gripperlength=0.20+0.018, fli
return get_joints_from_xyz_abs(x, y, z-grippery, rx=gripperangle, l3offset=-gripperx, rz=rz) 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): def goto_holder_index(idx, z=0.05, gripperangle=35, flip=False):
joint = config["position_map"][idx] joint = config["position_map"][idx]
print("Going to cable holder index", joint["index"], "at position", joint["pos"]) 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(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) #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, ) #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__": if __name__ == "__main__":
#rob.movej((0, 0, 0, 0, 0, 0), 0.1, 0.2) #rob.movej((0, 0, 0, 0, 0, 0), 0.1, 0.2)
@@ -356,59 +418,55 @@ if __name__ == "__main__":
0.0510] 0.0510]
curr_pos = rob.getl() 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 config = None
joints = [] joints = []
for i in np.linspace(-0.2, -0.7, 50): for i in np.linspace(-0.2, -0.7, 10):
joints.append(get_joints_from_xyz_abs(i, 0, 0)) joints.append(get_joints_from_xyz_abs(i, 0, 0))
#rob.movejs(joints, acc=2, vel=2) # rob.movejs(joints, acc=2, vel=2, radius=0.1)
import yaml
with open('config.yml', 'r') as fileread: with open('config.yml', 'r') as fileread:
#global config #global config
config = yaml.safe_load(fileread) 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) #rob.movej(goto_holder_index(24, 0.2, 0), acc=2, vel=2)
#joints = [] #joints = []
#for i in np.linspace(0, 340, 340): #for i in np.linspace(0, 340, 340):
# joints.append(goto_holder_index(24, 0.5, i)) # joints.append(goto_holder_index(24, 0.5, i))
#rob.movejs(joints, acc=1, vel=3) #rob.movejs(joints, acc=1, vel=3)
angle = 30 # angle = 30
rob.movej(goto_holder_index(26, 0.1, angle), acc=2, vel=2) # rob.movej(goto_holder_index(26, 0.1, angle), acc=2, vel=2)
#rob.movej(goto_holder_index(32, 0.2, angle), 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(38, 0.2, angle), acc=2, vel=2)
rob.movej(goto_holder_index(26, 0.1, angle, flip=True), acc=2, vel=2) # rob.movej(goto_holder_index(26, 0.1, angle, flip=True), acc=2, vel=2)
# rob.movej(offset_gripper_angle(-0.3, -0.3, 0.4, flip=True), acc=2, vel=2)
rob.movej(goto_holder_index(25, 0.2, angle, flip=True), acc=2, vel=2) # safe_move(0.1, 0.1, 0.1)
rob.movej(goto_holder_index(24, 0.0, angle, flip=True), acc=2, vel=2) # safe_move(-0.3, -0.3, 0.1)
time.sleep(1)
rob.movej(goto_holder_index(25, 0.1, angle, flip=True), acc=2, vel=2) # flip()
rob.movej(goto_holder_index(49, 0.1, angle), acc=2, vel=2) # flip()
rob.movej(goto_holder_index(49, 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.0, 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(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(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(52, 0.1, angle, flip=True), acc=2, vel=2)