468 lines
48 KiB
Plaintext
468 lines
48 KiB
Plaintext
{
|
|
"cells": [
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"# Inverse Kinematics Simulation"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 1,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"from scipy.optimize import fsolve\n",
|
|
"import matplotlib.pyplot as plt\n",
|
|
"import math\n",
|
|
"import numpy as np"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"### Polar coordinate functions"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 2,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"def cartesian_to_polar(x, y):\n",
|
|
" r = np.sqrt(x**2 + y**2)\n",
|
|
" theta = np.arctan2(y, x)\n",
|
|
" return r, theta\n",
|
|
"\n",
|
|
"def polar_to_cartesian(r, theta):\n",
|
|
" x = r * np.cos(theta)\n",
|
|
" y = r * np.sin(theta)\n",
|
|
" return x, y"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"### Calculate end-joint values from xyz position"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 3,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"text/plain": [
|
|
"[82.5301003420473,\n",
|
|
" -83.54918182551367,\n",
|
|
" 111.24503912126411,\n",
|
|
" -117.69585729575046,\n",
|
|
" -90.0,\n",
|
|
" 90.0]"
|
|
]
|
|
},
|
|
"execution_count": 3,
|
|
"metadata": {},
|
|
"output_type": "execute_result"
|
|
}
|
|
],
|
|
"source": [
|
|
"def normalize_degree(theta):\n",
|
|
" # Normalizes degree theta from -1.5pi to 1.5pi\n",
|
|
" multiplier = 1.5\n",
|
|
" normalized_theta = theta % (math.pi * multiplier)\n",
|
|
" # Maintain the negative sign if the original angle is negative\n",
|
|
" if theta < 0:\n",
|
|
" normalized_theta -= math.pi * multiplier\n",
|
|
"\n",
|
|
" # Return angle\n",
|
|
" return normalized_theta\n",
|
|
"\n",
|
|
"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)):\n",
|
|
" # Get limbs and offsets\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",
|
|
" \n",
|
|
" # Calculate base angle and r relative to shoulder joint\n",
|
|
" def calculate_theta(x, y, a):\n",
|
|
" # Calculate if we need the + or - in our equations\n",
|
|
" if (x>a and y>=0) or (x>-a and y<0):\n",
|
|
" flip = 1\n",
|
|
" elif (x<a and y>=0) or (x<-a and y<0):\n",
|
|
" flip = -1\n",
|
|
" else: \n",
|
|
" # Critical section (x=a, or x=-a). Infinite slope\n",
|
|
" # Return 0 or 180 depending on sign\n",
|
|
" return math.atan2(y, 0) - math.pi/2\n",
|
|
" \n",
|
|
" # Calculate tangent line y = mx + b\n",
|
|
" if abs(a) != abs(x): # If there is no division by 0\n",
|
|
" m = (x*y + math.sqrt(x*x*y*y-(x*x-a*a)*(y*y-a*a)))/(x*x-a*a)\n",
|
|
" else: # Deal with edge case when x^2=a^2\n",
|
|
" m = flip*(-a*a+y*y)/(a*y-flip*abs(a*y))\n",
|
|
" b = flip * a * math.sqrt(1+m*m)\n",
|
|
"\n",
|
|
" # Calculate equivalent tangent point on circle\n",
|
|
" cx = (-flip*m*b)/(1+m*m)\n",
|
|
" cy = m*cx + flip*b\n",
|
|
"\n",
|
|
" # Calculate base angle, make angle negative if flip=1\n",
|
|
" theta = math.atan2(cy, cx) + (-math.pi if flip==1 else 0)\n",
|
|
"\n",
|
|
" return theta \n",
|
|
" \n",
|
|
" base_theta = calculate_theta(x, y, l_bs)\n",
|
|
" cx, cy = l_bs*math.cos(base_theta), l_bs*math.sin(base_theta)\n",
|
|
" r = math.sqrt((x-cx)**2 + (y-cy)**2) \n",
|
|
"\n",
|
|
"\n",
|
|
" # Formulas to find out joint positions for (r, z)\n",
|
|
" def inv_kin_r_z(p):\n",
|
|
" a, b, c = p \n",
|
|
"\n",
|
|
" return (l1*math.cos(a) + l2*math.cos(a-b) + l3*math.cos(a-b-c) - r, # r\n",
|
|
" l1*math.sin(a) + l2*math.sin(a-b) - l3*math.sin(a-b-c) - (l3*math.sin(a-b-c)) - (z + offset_z), # z\n",
|
|
" a-b-c) # wrist angle\n",
|
|
"\n",
|
|
"\n",
|
|
" # Normalize angles\n",
|
|
" base, shoulder, elbow, wrist1 = [normalize_degree(deg) for deg in [base_theta, *fsolve(inv_kin_r_z, initial_guess)]]\n",
|
|
"\n",
|
|
" # Return result\n",
|
|
" return base, shoulder, elbow, wrist1, ry, rz\n",
|
|
"\n",
|
|
"def get_joints_from_xyz_abs(x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2):\n",
|
|
" joints = get_joints_from_xyz_rel(x, y, z, rx, ry, rz)\n",
|
|
"\n",
|
|
" # Joint offsets\n",
|
|
" # Base, Shoulder, Elbow, Wrist\n",
|
|
" inverse = [1, -1, 1, 1, 1, 1]\n",
|
|
" offsets = [-math.pi/2, 0, 0, -math.pi/2, 0, 0]\n",
|
|
"\n",
|
|
" # Return adjusted joint positions\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.2, -0.5, 0.1)]"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"### Simulate Arm and Joint Angles"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 4,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"def draw_arm_side_view(x, y, z, details=False):\n",
|
|
"\n",
|
|
" # Get joint angles\n",
|
|
" l1, l2, l3 = .425, .39225, .1267\n",
|
|
" offset_x, offset_y, offset_z = (0, 0, 0.14)\n",
|
|
" r, theta = cartesian_to_polar(x, y)\n",
|
|
" base, shoulder, elbow, wrist, _, _ = get_joints_from_xyz_rel(x, y, z)\n",
|
|
"\n",
|
|
" # Print angles\n",
|
|
" if details:\n",
|
|
" print('Target position (x,y,z):', x, y, z)\n",
|
|
" print('R: ', round(math.sqrt(x**2+y**2),4))\n",
|
|
" print('Angles (base, shoulder, elbow, wrist):', [round(math.degrees(i), 4) for i in [base, shoulder, elbow, wrist]])\n",
|
|
" print('Robot Angles:', [round(math.degrees(i), 4) for i in get_joints_from_xyz_abs(x, y, z)])\n",
|
|
"\n",
|
|
" # Calculate each joint's endpoint position\n",
|
|
" x1, y1 = polar_to_cartesian(l1, shoulder)\n",
|
|
" x2, y2 = polar_to_cartesian(l2, shoulder-elbow)\n",
|
|
" x2 += x1\n",
|
|
" y2 += y1\n",
|
|
" x3, y3 = polar_to_cartesian(l3, shoulder-elbow-wrist)\n",
|
|
" x3 += x2\n",
|
|
" y3 += y2 \n",
|
|
" \n",
|
|
" tx = x3\n",
|
|
" ty = y3 - offset_z\n",
|
|
"\n",
|
|
" # Print each joint's endpoint position\n",
|
|
" if details:\n",
|
|
" print('elbow (x,y):', round(x1,3), round(y1,3))\n",
|
|
" print('wrist (x,y):', round(x2,3), round(y2,3))\n",
|
|
" print('tool (x,y):', round(x3,3), round(y3,3))\n",
|
|
"\n",
|
|
" # Draw limbs\n",
|
|
" plt.plot([0, x1], [0, y1], color='cyan', linewidth=7)\n",
|
|
" plt.plot([x1, x2], [y1, y2], color='orange', linewidth=7)\n",
|
|
" plt.plot([x2, x2+l3], [y2, y2], color='red', linewidth=7)\n",
|
|
" plt.plot()\n",
|
|
"\n",
|
|
" # Draw toolpoint\n",
|
|
" plt.plot([x3, tx], [y3, ty], color='black', linewidth=7)\n",
|
|
"\n",
|
|
" # Display angles\n",
|
|
" plt.text(0, 0.02, f'{round(math.degrees(shoulder), 2)}°')\n",
|
|
" plt.text(x1, y1+0.02, f'{round(math.degrees(elbow), 2)}°')\n",
|
|
" plt.text(x2, y2+0.02, f'{round(math.degrees(wrist), 2)}°')\n",
|
|
"\n",
|
|
" # Display r arrow\n",
|
|
" plt.annotate(f'', xy=(0, 0), xycoords='data', xytext=(x3, 0), textcoords='data', arrowprops={'arrowstyle': '<->'})\n",
|
|
" plt.annotate(f'r={round(r,4)}', xy=(x2/2, 0.01), xycoords='data', xytext=(x2/2, 0), textcoords='offset points')\n",
|
|
"\n",
|
|
" # Display z arrow\n",
|
|
" plt.annotate(f'', xy=(x3, 0), xycoords='data', xytext=(tx, ty), textcoords='data', arrowprops={'arrowstyle': '<->'})\n",
|
|
" plt.annotate(f'z={round(ty,4)}', xy=(tx+0.01, ty/2), xycoords='data', xytext=(x3/2, 0), textcoords='offset points')\n",
|
|
" \n",
|
|
" # Display plot\n",
|
|
" ax = plt.subplot(111)\n",
|
|
" ax.spines[['right', 'top']].set_visible(False)\n",
|
|
" plt.axis('equal')\n",
|
|
" plt.show()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 5,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"def draw_arm_top_view(x, y, z, details=False):\n",
|
|
" # Get joint and position information\n",
|
|
" angles = get_joints_from_xyz_rel(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",
|
|
" if details:\n",
|
|
" print('Angles:', [round(math.degrees(angle),3) for angle in angles])\n",
|
|
" print(f'Circle position (cx, cy): ({round(cx, 3)}, {round(cy, 3)})')\n",
|
|
"\n",
|
|
"\n",
|
|
" # Plot coordinate system\n",
|
|
" fig = plt.figure()\n",
|
|
" ax = fig.add_subplot(1, 1, 1)\n",
|
|
" ax.spines['left'].set_position('center')\n",
|
|
" ax.spines['bottom'].set_position('center')\n",
|
|
" ax.spines['right'].set_color('none')\n",
|
|
" ax.spines['top'].set_color('none')\n",
|
|
" ax.set_yticklabels([])\n",
|
|
" ax.set_xticklabels([])\n",
|
|
" ax.set_xticks([])\n",
|
|
" ax.set_yticks([])\n",
|
|
"\n",
|
|
" # Target point\n",
|
|
" # plt.plot(x, y, 'go')\n",
|
|
"\n",
|
|
" # Circle\n",
|
|
" circle = plt.Circle((0, 0), l_bs, color='b', fill=False, linewidth=1)\n",
|
|
" plt.plot([0, cx], [0, cy], color='b', linewidth=1)\n",
|
|
" ax.add_patch(circle)\n",
|
|
"\n",
|
|
" # Draw limbs\n",
|
|
" # Shoulder\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",
|
|
" plt.plot([cx, x1], [cy, y1], color='cyan', linewidth=3)\n",
|
|
"\n",
|
|
" # Elbow\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",
|
|
" plt.plot([x1, x2], [y1, y2], color='orange', linewidth=2)\n",
|
|
"\n",
|
|
" # Wrist\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",
|
|
" plt.plot([x2, x3], [y2, y3], color='red', linewidth=2)\n",
|
|
"\n",
|
|
" # Print joint positions\n",
|
|
" if details:\n",
|
|
" print(f'Shoulder (x, y, z): ({round(x1,3)}, {round(y1,3)}, {round(z1,3)})')\n",
|
|
" print(f'Elbow (x, y, z): ({round(x2,3)}, {round(y2,3)}, {round(z2,3)})')\n",
|
|
" print(f'Wrist (x, y, z): ({round(x3,3)}, {round(y3,3)}, {round(z3,3)})')\n",
|
|
"\n",
|
|
" # Display angle\n",
|
|
" plt.text(0.01, -0.01-cy/abs(cy+0.00001)*0.02, f'{round(math.degrees(angles[0]), 2)}°', fontsize=7)\n",
|
|
"\n",
|
|
" # Display x arrow\n",
|
|
" sign = x3/abs(x3)\n",
|
|
" plt.annotate(f'', xy=(0, y3), xycoords='data', xytext=(x3, y3), textcoords='data', arrowprops={'arrowstyle': '<->'})\n",
|
|
" plt.annotate(f'x={round(x3,3)}', xy=(-0.1-sign*0.13, y3-0.015), xycoords='data', xytext=(x2/2, 0), textcoords='offset points')\n",
|
|
"\n",
|
|
" # Display y arrow\n",
|
|
" sign = y3/abs(y3)\n",
|
|
" plt.annotate(f'', xy=(x3, 0), xycoords='data', xytext=(x3, y3), textcoords='data', arrowprops={'arrowstyle': '<->'})\n",
|
|
" plt.annotate(f'y={round(y3,3)}', xy=((x3-0.1), -0.015-sign*0.03), xycoords='data', xytext=(x3/2, 0), textcoords='offset points')\n",
|
|
"\n",
|
|
"\n",
|
|
" # Set axis limits and labels\n",
|
|
" axis_limit = math.hypot(x, y)+.1\n",
|
|
" plt.axis('square')\n",
|
|
" plt.xlim(-axis_limit, axis_limit)\n",
|
|
" plt.ylim(-axis_limit, axis_limit)\n",
|
|
"\n",
|
|
" # Adjust the position of axis labels\n",
|
|
" plt.xlabel('+x', horizontalalignment='right', x=1.05)\n",
|
|
" plt.ylabel('+y', verticalalignment='top', rotation=0, y=1.05)\n",
|
|
" plt.show()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 6,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"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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",
|
|
"text/plain": [
|
|
"<Figure size 640x480 with 1 Axes>"
|
|
]
|
|
},
|
|
"metadata": {},
|
|
"output_type": "display_data"
|
|
},
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"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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",
|
|
"text/plain": [
|
|
"<Figure size 640x480 with 1 Axes>"
|
|
]
|
|
},
|
|
"metadata": {},
|
|
"output_type": "display_data"
|
|
}
|
|
],
|
|
"source": [
|
|
"def draw_arm(x, y, z, details=False):\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.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)"
|
|
]
|
|
}
|
|
],
|
|
"metadata": {
|
|
"kernelspec": {
|
|
"display_name": "Python 3",
|
|
"language": "python",
|
|
"name": "python3"
|
|
},
|
|
"language_info": {
|
|
"codemirror_mode": {
|
|
"name": "ipython",
|
|
"version": 3
|
|
},
|
|
"file_extension": ".py",
|
|
"mimetype": "text/x-python",
|
|
"name": "python",
|
|
"nbconvert_exporter": "python",
|
|
"pygments_lexer": "ipython3",
|
|
"version": "3.10.4"
|
|
}
|
|
},
|
|
"nbformat": 4,
|
|
"nbformat_minor": 2
|
|
}
|