{ "cells": [ { "cell_type": "code", "execution_count": 3, "metadata": { "scrolled": true }, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ " X Y theta1_1 theta1_2 theta1_3 clank_x clank_y clank_z\n", "0 -1.0000 1.0 2.103778 0.025131 4.154653 0.600506 1.608398 -2.184785\n", "1 -1.0000 -1.0 2.128565 -0.024874 4.179879 2.186893 -1.591932 -0.570349\n", "2 -1.0000 -1.0 2.128565 -0.024874 4.179879 2.186893 -1.591932 -0.570349\n", "3 -1.0000 1.0 2.103778 0.025131 4.154653 0.600506 1.608398 -2.184785\n", "4 -0.9975 1.0 2.103723 0.025131 4.154707 0.597014 1.608358 -2.181312\n", "... ... ... ... ... ... ... ... ...\n", "3199 -1.0000 1.0 2.103778 0.025131 4.154653 0.600506 1.608398 -2.184785\n", "3200 1.0000 1.0 2.060255 0.025138 4.198179 -2.184964 1.608802 0.600883\n", "3201 1.0000 -1.0 2.085475 -0.024868 4.222972 -0.570868 -1.591535 2.187618\n", "3202 -1.0000 -1.0 2.128565 -0.024874 4.179879 2.186893 -1.591932 -0.570349\n", "3203 -1.0000 1.0 2.103778 0.025131 4.154653 0.600506 1.608398 -2.184785\n", "\n", "[3204 rows x 8 columns]\n" ] } ], "source": [ "# inverse kinematics\n", "# zach fredin, 2021\n", "\n", "# desired coordinates (x,y) of stage centroid are stored in dataframe df.\n", "# after IK calculation, actuator rotation values are stored as three \n", "# additional columns in df (theta1_1, theta1_2, theta1_3). angles are in \n", "# radians, dimensions are in millimeters, origin is at nominal stage \n", "# centroid as oriented as in the CAD model: for someone standing in front \n", "# of the optical table in 023, X+ is right, Y+ is forward, and Z+ is up.\n", "\n", "import pandas as pd\n", "import matplotlib.pyplot as plt\n", "import numpy as np\n", "\n", "# first pass: geometric, a la Williams and Shelley (ASME 1997)\n", "\n", "# constants from CAD model\n", "angle_max = np.radians(15) # maximum joint angle in radians\n", "length_L1 = 40 # linkage lengths (nominal, from CAD, in mm)\n", "length_L2 = 100\n", "length_L3 = 33.6953\n", "location_A1_x = 124.9124 # mm from stage centroid (nominal,from CAD, in mm)\n", "location_A1_y = 43.6453\n", "location_A2_x = -24.6582\n", "location_A2_y = -130.0003\n", "location_A3_x = -100.2545\n", "location_A3_y = 86.3546\n", "angle_psi_1 = np.radians(57.0849) # final joint vs centroid X-axis\n", "angle_psi_2 = np.radians(297.0849)\n", "angle_psi_3 = np.radians(177.0849)\n", "angle_theta1_1_nominal = np.radians(120) # nominal actuator angles\n", "angle_theta1_2_nominal = np.radians(0) # nominal actuator angles\n", "angle_theta1_3_nominal = np.radians(240) # nominal actuator angles\n", "\n", "# input values\n", "input_phi = 0\n", "\n", "# clank scaling\n", "clank_scaling = 64\n", "\n", "def ik(L1, L2, L3, Ax, Ay, psi, phi, x, y, angle_theta_nominal, angle_max):\n", " Cx = x + L3 * np.cos(phi + psi) # stage corner locations (eqn 3)\n", " Cy = y + L3 * np.sin(phi + psi)\n", " \n", " E = 2 * (Cx - Ax) * L1 # intermediate values (eqn 5)\n", " F = 2 * (Cy - Ay) * L1\n", " G = L2**2 - L1**2 - (Cx - Ax)**2 - (Cy - Ay)**2 \n", " \n", " theta1_a = 2 * np.arctan((-F + np.sqrt(E**2 + F**2 - G**2)) / (G - E)) # angle theta1_x (eqn 6)\n", " theta1_b = 2 * np.arctan((-F - np.sqrt(E**2 + F**2 - G**2)) / (G - E))\n", " \n", " # conditional statements require theta1_x[0] so pandas knows we're looking at one element\n", " if np.absolute(theta1_a[0] - angle_theta_nominal) < angle_max: \n", " return(theta1_a)\n", " elif np.absolute(theta1_b[0] - angle_theta_nominal) < angle_max:\n", " return(theta1_b)\n", " elif np.absolute(theta1_a[0] + 2*np.pi - angle_theta_nominal) < angle_max: # go around again!\n", " return(theta1_a + 2 * np.pi)\n", " elif np.absolute(theta1_b[0] + 2*np.pi - angle_theta_nominal) < angle_max:\n", " return(theta1_b + 2 * np.pi) \n", "\n", "#df = pd.DataFrame( # some test data: draw a zig-zag boxy line.\n", "# {\n", "# 'X': [0,0,0,0,0,0,\n", "# 1,1,1,1,1,1,\n", "# 2,2,2,2,2,2,\n", "# 3,3,3,3,3,3,\n", "# 4,4,4,4,4,4,\n", "# 5,5,5,5,5,5],\n", "# 'Y': [0,1,2,3,4,5,\n", "# 5,4,3,2,1,0,\n", "# 0,1,2,3,4,5,\n", "# 5,4,3,2,1,0,\n", "# 0,1,2,3,4,5,\n", "# 5,4,3,2,1,0]\n", "# }\n", "#)\n", "\n", "#df = pd.DataFrame( # more test data: a spiral, starting at 0,0\n", "# {\n", "# 'X': [0,1,1,-1,-1,2,2,-2,-2,3,3,-3,-3,4,4,-4,-4,5,5,-5,-5,6,6,-6,-6,7,7,-7,-7,8,8,-8,-8,9,9,-9,-9,10,10,-10,-10,11,11,-11,-11],\n", "# 'Y': [0,0,1,1,-1,-1,2,2,-2,-2,3,3,-3,-3,4,4,-4,-4,5,5,-5,-5,6,6,-6,-6,7,7,-7,-7,8,8,-8,-8,9,9,-9,-9,10,10,-10,-10,11,11,-11]\n", "# }\n", "#)\n", "\n", "#df = pd.DataFrame( # test corner positions at a given Clank scaling\n", "# {\n", "# 'X': [0,-1,-1,1,1,0],\n", "# 'Y': [0,1,-1,-1,1,0]\n", "# }\n", "#)\n", "\n", "spacing = 0.0025\n", "start_x = -1\n", "start_y = 1\n", "stop_y = -1\n", "\n", "x_val = np.zeros(int((2/spacing + 1)*4))\n", "y_val = np.zeros(int((2/spacing + 1)*4))\n", "index = 0\n", "stroke = 0\n", "cycle = 0\n", "\n", "for val in x_val:\n", " if cycle == 0:\n", " x_val[index] = start_x + stroke\n", " y_val[index] = start_y\n", " cycle = 1\n", " elif cycle == 1:\n", " x_val[index] = start_x + stroke\n", " y_val[index] = stop_y\n", " cycle = 2\n", " elif cycle == 2:\n", " x_val[index] = start_x\n", " y_val[index] = stop_y\n", " cycle = 3\n", " else:\n", " x_val[index] = start_x\n", " y_val[index] = start_y\n", " cycle = 0\n", " stroke = stroke + spacing\n", " index = index + 1\n", "\n", "df = pd.DataFrame( # test corner positions at a given Clank scaling\n", " {\n", " 'X': x_val,\n", " 'Y': y_val\n", " }\n", ")\n", "\n", "\n", "df.insert(2,'theta1_1','')\n", "df.insert(3,'theta1_2','')\n", "df.insert(4,'theta1_3','')\n", "\n", "df['theta1_1'] = ik(length_L1, \n", " length_L2, \n", " length_L3, \n", " location_A1_x, \n", " location_A1_y, \n", " angle_psi_1, \n", " input_phi, \n", " df['X'], \n", " df['Y'], \n", " angle_theta1_1_nominal, \n", " angle_max)\n", "df['theta1_2'] = ik(length_L1, \n", " length_L2, \n", " length_L3, \n", " location_A2_x, \n", " location_A2_y, \n", " angle_psi_2, \n", " input_phi, \n", " df['X'], \n", " df['Y'], \n", " angle_theta1_2_nominal, \n", " angle_max)\n", "df['theta1_3'] = ik(length_L1,\n", " length_L2,\n", " length_L3,\n", " location_A3_x,\n", " location_A3_y,\n", " angle_psi_3,\n", " input_phi,\n", " df['X'],\n", " df['Y'],\n", " angle_theta1_3_nominal,\n", " angle_max)\n", "\n", "df.insert(5,'clank_x','')\n", "df.insert(6,'clank_y','')\n", "df.insert(7,'clank_z','')\n", "\n", "df['clank_x'] = (df['theta1_1'] - angle_theta1_1_nominal) * clank_scaling\n", "df['clank_y'] = (df['theta1_2'] - angle_theta1_2_nominal) * clank_scaling\n", "df['clank_z'] = (df['theta1_3'] - angle_theta1_3_nominal) * clank_scaling\n", "\n", "gcode_intro = [\"G20\",\"G90\",\"G94\",\"F5\"]\n", "\n", "#f = open(\"test.gcode\", \"x\")\n", "#for listitem in gcode_intro:\n", "# f.write('%s\\n' % listitem)\n", "#for listitem in df['clank_x'].tolist(): # haha wow slow, why are we using pandas again?\n", "# f.write('G01 ')\n", "# f.write('X%.4f ' % listitem)\n", "# f.write('\\n')\n", "#f.close()\n", "\n", "f = open(\"test.gcode\", \"x\")\n", "for listitem in gcode_intro:\n", " f.write('%s\\n' % listitem)\n", "for index, row in df.iterrows():\n", " f.write('G01 ')\n", " f.write('X%.4f' % row['clank_x'])\n", " f.write('Y%.4f' % row['clank_y'])\n", " f.write('Z%.4f' % row['clank_z'])\n", " f.write('\\n')\n", "f.close()\n", "\n", "print(df)" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [] } ], "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.8.5" } }, "nbformat": 4, "nbformat_minor": 4 }