diff --git a/ME 639 Assignment 1 20110186.pdf b/ME 639 Assignment 1 20110186.pdf new file mode 100644 index 0000000..dd19686 Binary files /dev/null and b/ME 639 Assignment 1 20110186.pdf differ diff --git a/ME 639 Assignment 3.pdf b/ME 639 Assignment 3.pdf new file mode 100644 index 0000000..a099f58 Binary files /dev/null and b/ME 639 Assignment 3.pdf differ diff --git a/ME 639 Mini Project Task 0.pdf b/ME 639 Mini Project Task 0.pdf new file mode 100644 index 0000000..984a263 Binary files /dev/null and b/ME 639 Mini Project Task 0.pdf differ diff --git a/ME 639 mexam.zip b/ME 639 mexam.zip new file mode 100644 index 0000000..c2e56bb Binary files /dev/null and b/ME 639 mexam.zip differ diff --git a/ME639_Assignment_4&5_Q1,2,3,6.ipynb b/ME639_Assignment_4&5_Q1,2,3,6.ipynb new file mode 100644 index 0000000..1c55eb7 --- /dev/null +++ b/ME639_Assignment_4&5_Q1,2,3,6.ipynb @@ -0,0 +1,237 @@ +{ + "nbformat": 4, + "nbformat_minor": 0, + "metadata": { + "colab": { + "provenance": [], + "authorship_tag": "ABX9TyNfecWN0TPP7rcnZuCCnTYf", + "include_colab_link": true + }, + "kernelspec": { + "name": "python3", + "display_name": "Python 3" + }, + "language_info": { + "name": "python" + } + }, + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "view-in-github", + "colab_type": "text" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "markdown", + "source": [ + "README:\n", + "Run each code block sequentially - each assignment question is in a separate code block (except for q3, which is in a couple). The question that the code block corresponds to is mentioned at the top of each block. To test the code for questions 1 and 2 run the first block of question 3 and the block before question 2 to print the verified end effector position." + ], + "metadata": { + "id": "51aPiIjpFbz7" + } + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "maUkNAFo7EZf" + }, + "outputs": [], + "source": [ + "#question 1\n", + "#input = [px,py,pz]\n", + "#output = dh parameters for stanford manipulator\n", + "#d1 and a2 for stanford manipulator are 0, and the following formulae have been written accordingly\n", + "#also, only the first solution has been taken into account\n", + "import math as m\n", + "p = list(map(float,input(\"end effector coordinates: \").strip().split()))\n", + "theta1 = m.atan(p[1]/p[0])\n", + "r = m.sqrt(p[0]**2 + p[1]**2)\n", + "s = p[2]\n", + "theta2 = m.atan(s/r) \n", + "d3 = m.sqrt(r**2 + s**2)\n", + "\n", + "#for testing: substituting above values into dh parameter matrix for stanford manipulator, then using the code given below for calculating\n", + "#transformation matrices and manipulator jacobian" + ] + }, + { + "cell_type": "code", + "source": [ + "#question 3\n", + "#the entire process for calculating the jacobian\n", + "#inputs = no. of links, dh parameters, revolute or prismatic?\n", + "#output = jacobian\n", + "import math as m\n", + "import numpy as np\n", + "links = int(input()) #no. of links (if spherical - include as 3 revolute joints)\n", + "dh = np.zeros((links,4)) #matrix with denavit hartenburg parameters\n", + "print(\"enter dh parameters for each link in the order d,theta,r,alpha\")\n", + "for i in range(links-1):\n", + " a = list(map(float,input().strip().split()))[:4]\n", + " dh[i] = a\n", + "#calculation of transformation matrices:\n", + "H = [0]*(links) #array with link to link transformation matrices\n", + "H[0] = np.identity(4)\n", + "H0 = np.identity(4) #overall transformation matrix\n", + "for i in range(links-1):\n", + " d = dh[i][0]\n", + " theta = dh[i][1]\n", + " r = dh[i][2]\n", + " alpha = dh[i][3]\n", + " Z = [[m.cos(theta), -1*m.sin(theta), 0, 0], #transforation matrix, Z operations\n", + " [m.sin(theta), m.cos(theta), 0, 0],\n", + " [0, 0, 1, d],\n", + " [0, 0, 0, 1]]\n", + " X = [[1, 0, 0, r], #transforation matrix, X operations\n", + " [0, m.cos(alpha), -1*m.sin(alpha), 0],\n", + " [0, m.sin(alpha), m.cos(alpha), 0],\n", + " [0, 0, 0, 1]]\n", + " H[i+1] = np.matmul(Z,X) #adding current link to link transformation to H matrix\n", + " H0 = H0@H[i+1] #mutliplying link to link transformations to get overall transformation\n", + "on0 = [H0[0][3],H0[1][3],H0[2][3]] #position of last origin (end effector) wrt base\n", + "o = np.zeros(3) #for storing position of end effector wrt current frame\n", + "J = np.zeros((6,links)) #jacobian (initialized)\n", + "rorp = input(\"linkwise, enter type of joint (include end effector too): \").strip().split() #revolute or prismatic joint?\n", + "h = np.identity(4) #matrix representing transformation from base to current frame\n", + "for i in range(links):\n", + " h = h@H[i]\n", + " z = [h[0][2], h[1][2], h[2][2]] #z -> last column of rotation\n", + " if rorp[i] == \"prismatic\": #assigning of values to jacobian column in case of prismatic joint\n", + " J[0][i] = z[0]\n", + " J[1][i] = z[1]\n", + " J[2][i] = z[2]\n", + " J[3][i] = 0\n", + " J[4][i] = 0\n", + " J[5][i] = 0\n", + " else: #assigning of values to jacobian column in case of revolute joint\n", + " oi0 = [h[0][3], h[1][3], h[2][3]] #position of current frame origin wrt base origin\n", + " #calculation of o:\n", + " o[0] = on0[0] - oi0[0] \n", + " o[1] = on0[1] - oi0[1]\n", + " o[2] = on0[2] - oi0[2]\n", + " zxoi = (z[1]*o[2]) - (o[1]*z[2]) \n", + " zxoj = (o[0]*z[2]) - (z[0]*o[2])\n", + " zxok = (z[0]*o[1]) - (o[0]*z[1])\n", + " zxo = [zxoi, zxoj, zxok] #cross product of z and o\n", + " J[0][i] = zxo[0]\n", + " J[1][i] = zxo[1]\n", + " J[2][i] = zxo[2]\n", + " J[3][i] = z[0]\n", + " J[4][i] = z[1]\n", + " J[5][i] = z[2]\n", + "\n", + "print(J) #complete manipulator jacobian" + ], + "metadata": { + "id": "UeLgitUmh3OR" + }, + "execution_count": null, + "outputs": [] + }, + { + "cell_type": "code", + "source": [ + "# rate of change of cartesian variables aka end effector velocities\n", + "xdot = list(map(float,input(\"xdot values: \").strip().split()))" + ], + "metadata": { + "id": "HXad4nuciVfD" + }, + "execution_count": null, + "outputs": [] + }, + { + "cell_type": "code", + "source": [ + "#finding pseudoinverse of jacobian:\n", + "Jtrans = np.transpose(J)\n", + "Jcross = J@Jtrans\n", + "Jcross = np.linalg.inv(Jcross)\n", + "Jcross = Jtrans@Jcross" + ], + "metadata": { + "id": "rkIWwP8Pih9f" + }, + "execution_count": null, + "outputs": [] + }, + { + "cell_type": "code", + "source": [ + "qdot = Jcross@xdot\n", + "print(qdot) #joint velocities - rate of change of each joint variable" + ], + "metadata": { + "id": "cGG0VFkxjUwZ" + }, + "execution_count": null, + "outputs": [] + }, + { + "cell_type": "code", + "source": [ + "print(on0) #for testing out the end effector positions for questions 1 and 2" + ], + "metadata": { + "id": "bCe-ez2BFztc" + }, + "execution_count": null, + "outputs": [] + }, + { + "cell_type": "code", + "source": [ + "#question 2\n", + "#input = [px,py,pz], l1, l2, and d4 (the lengths of the robot links), and final transformation matrix H (0-4) (or, just H(0-4) as it contains [px,py,pz])\n", + "#taking H values from code for jacobian given above (when input is for a scara manipulator specifically)\n", + "#output = joint var for scara manipulator\n", + "import math as m\n", + "#taking in values for link lengths\n", + "l1 = float(input()) \n", + "l2 = float(input())\n", + "d4 = float(input())\n", + "#calculation of joint variables\n", + "alpha = m.atan(H[0][0]/H[0][1]) #theta1 + theta2 - theta4 = alpha\n", + "r = m.sqrt((H[0][3]**2 + H[1][3]**2 - l1**2 - l2**2)/(2*l1*l2))\n", + "theta2 = m.atan(r/m.sqrt(1-r**2)) #only picking the first solution\n", + "theta1 = m.atan(H[1][3]/H[0][3]) - m.atan(l2*m.sin(theta2)/(l1 + l2*m.cos(theta2)))\n", + "theta4 = theta1 + theta2 - alpha\n", + "d3 = H[2][3] + d4\n", + "\n", + "#for testing: substituting above values into dh parameter matrix for scara manipulator, then using the code given above for calculating\n", + "#transformation matrices and manipulator jacobian" + ], + "metadata": { + "id": "8hBMrQqv2rT-" + }, + "execution_count": null, + "outputs": [] + }, + { + "cell_type": "code", + "source": [ + "#question 6\n", + "#for spherical manipulator - it's about finding the euler angles phi, theta, psi\n", + "#just taking the first solution where theta = Atan(u33,sqrt(1-u33**2)), not theta = Atan(u33,-1*sqrt(1-u33**2))\n", + "#given transformation matrix U = np.transpose(R(0 -> 3))@R or U = H[-3]@H[-2]@H[-1]\n", + "U = H[-2]@H[-1]\n", + "U = H[-3]@U\n", + "theta5 = m.atan(m.sqrt(1 - U[2][2]**2)/U[2][2]) #theta\n", + "theta4 = m.atan(U[1][2]/U[0][2]) #phi\n", + "theta6 = m.atan(U[2][1],(-1*U[2][1])) #psi" + ], + "metadata": { + "id": "tFyuW-ZUk__s" + }, + "execution_count": null, + "outputs": [] + } + ] +} \ No newline at end of file diff --git a/ME_639_Assignment_3_Q11.ipynb b/ME_639_Assignment_3_Q11.ipynb new file mode 100644 index 0000000..2f9afe5 --- /dev/null +++ b/ME_639_Assignment_3_Q11.ipynb @@ -0,0 +1,116 @@ +{ + "nbformat": 4, + "nbformat_minor": 0, + "metadata": { + "colab": { + "provenance": [], + "authorship_tag": "ABX9TyPOaPUVkrfvSAGKUpcVzS/z", + "include_colab_link": true + }, + "kernelspec": { + "name": "python3", + "display_name": "Python 3" + }, + "language_info": { + "name": "python" + } + }, + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "view-in-github", + "colab_type": "text" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "gC8i1Hsvt_aW" + }, + "outputs": [], + "source": [ + "import sympy as sym\n", + "import numpy as np\n", + "n = int(input()) #no. of dof\n", + "g = [] #dV/dqk matrix\n", + "V = sym.sympify(input()) #V(q) input\n", + "for i in range(n):\n", + " g.append(sym.diff(V,sym.symbols(\"q\"+str(i+1)))) #calculating values of g" + ] + }, + { + "cell_type": "code", + "source": [ + "d = [] #D(q)\n", + "for i in range(n):\n", + " drow = []\n", + " for j in range(n):\n", + " drow.append(sym.sympify(input()))\n", + " d.append(drow) #D(q) input" + ], + "metadata": { + "id": "OVs5kzjdAJ7o" + }, + "execution_count": null, + "outputs": [] + }, + { + "cell_type": "code", + "source": [ + "c = [] #christoffel's symbols of the first kind\n", + "for k in range(n):\n", + " crow = []\n", + " for i in range(n):\n", + " for j in range(n):\n", + " A = sym.diff(d[k][j],sym.symbols(\"q\"+str(i+1)))\n", + " B = sym.diff(d[k][i],sym.symbols(\"q\"+str(j+1)))\n", + " C = sym.diff(d[i][j],sym.symbols(\"q\"+str(k+1)))\n", + " crow.append(0.5*(A+B-C)) #calculation of cijk\n", + " c.append(crow)\n", + "c" + ], + "metadata": { + "id": "jvPZ2ipUR-xc" + }, + "execution_count": null, + "outputs": [] + }, + { + "cell_type": "code", + "source": [ + "#creating new symbols for first and second order derivatives of q\n", + "qdubdot = []\n", + "qdot = []\n", + "for k in range(n):\n", + " qdubdot.append(sym.symbols(\"q\"+str(k+1)+\"dubdot\"))\n", + " qdot.append(sym.symbols(\"q\"+str(k+1)+\"dot\"))\n", + "#finding torque, i.e. dynamic equations\n", + "tau = []\n", + "for k in range(n):\n", + " D = 0\n", + " for i in range(n):\n", + " D = D+d[k][i]*qdubdot[i]\n", + " tau.append(D+g[k]) #tau = D(q)q\"+ g(q)\n", + "for k in range(n):\n", + " C = 0\n", + " p = 0\n", + " for i in range(n):\n", + " for j in range(n):\n", + " C = C + c[k][p]*qdot[i]*qdot[j]\n", + " p = p+1\n", + " tau[k] = tau[k]+C #tau = D(q)q\" + C(q,q')q' + g(q)\n", + "tau #matrix for system's dynamic equations, where tau[k] = torque at a particular joint" + ], + "metadata": { + "id": "DLddyCvsASTG" + }, + "execution_count": null, + "outputs": [] + } + ] +} \ No newline at end of file diff --git a/ME_639_Assignment_3_Q3.ipynb b/ME_639_Assignment_3_Q3.ipynb new file mode 100644 index 0000000..224d983 --- /dev/null +++ b/ME_639_Assignment_3_Q3.ipynb @@ -0,0 +1,156 @@ +{ + "nbformat": 4, + "nbformat_minor": 0, + "metadata": { + "colab": { + "provenance": [], + "include_colab_link": true + }, + "kernelspec": { + "name": "python3", + "display_name": "Python 3" + }, + "language_info": { + "name": "python" + } + }, + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "view-in-github", + "colab_type": "text" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "Kn6-O0KEUk8I" + }, + "outputs": [], + "source": [ + "#inputs = no. of links, dh parameters, revolute or prismatic?\n", + "#output = jacobian\n", + "import math as m\n", + "import numpy as np\n", + "links = int(input()) #no. of links (if spherical - include as 3 revolute joints)\n", + "dh = np.zeros((links,4)) #matrix with denavit hartenburg parameters\n", + "print(\"enter dh parameters for each link in the order d,theta,r,alpha\")\n", + "for i in range(links-1):\n", + " a = list(map(float,input().strip().split()))[:4]\n", + " dh[i] = a\n", + "#calculation of transformation matrices:\n", + "H = [0]*(links) #array with link to link transformation matrices\n", + "H[0] = np.identity(4)\n", + "H0 = np.identity(4) #overall transformation matrix\n", + "for i in range(links-1):\n", + " d = dh[i][0]\n", + " theta = dh[i][1]\n", + " r = dh[i][2]\n", + " alpha = dh[i][3]\n", + " Z = [[m.cos(theta), -1*m.sin(theta), 0, 0], #transforation matrix, Z operations\n", + " [m.sin(theta), m.cos(theta), 0, 0],\n", + " [0, 0, 1, d],\n", + " [0, 0, 0, 1]]\n", + " X = [[1, 0, 0, r], #transforation matrix, X operations\n", + " [0, m.cos(alpha), -1*m.sin(alpha), 0],\n", + " [0, m.sin(alpha), m.cos(alpha), 0],\n", + " [0, 0, 0, 1]]\n", + " H[i+1] = np.matmul(Z,X) #adding current link to link transformation to H matrix\n", + " H0 = H0@H[i+1] #mutliplying link to link transformations to get overall transformation" + ] + }, + { + "cell_type": "code", + "source": [ + "on0 = [H0[0][3],H0[1][3],H0[2][3]] #position of last origin (end effector) wrt base\n", + "o = np.zeros(3) #for storing position of end effector wrt current frame\n", + "J = np.zeros((6,links)) #jacobian (initialized)\n", + "rorp = input(\"linkwise, enter type of joint (include end effector too): \").strip().split() #revolute or prismatic joint?" + ], + "metadata": { + "id": "a6UL2DV-GLtW" + }, + "execution_count": null, + "outputs": [] + }, + { + "cell_type": "code", + "source": [ + "h = np.identity(4) #matrix representing transformation from base to current frame\n", + "for i in range(links):\n", + " h = h@H[i]\n", + " z = [h[0][2], h[1][2], h[2][2]] #z -> last column of rotation\n", + " if rorp[i] == \"prismatic\": #assigning of values to jacobian column in case of prismatic joint\n", + " J[0][i] = z[0]\n", + " J[1][i] = z[1]\n", + " J[2][i] = z[2]\n", + " J[3][i] = 0\n", + " J[4][i] = 0\n", + " J[5][i] = 0\n", + " else: #assigning of values to jacobian column in case of revolute joint\n", + " oi0 = [h[0][3], h[1][3], h[2][3]] #position of current frame origin wrt base origin\n", + " #calculation of o:\n", + " o[0] = on0[0] - oi0[0] \n", + " o[1] = on0[1] - oi0[1]\n", + " o[2] = on0[2] - oi0[2]\n", + " zxoi = (z[1]*o[2]) - (o[1]*z[2]) \n", + " zxoj = (o[0]*z[2]) - (z[0]*o[2])\n", + " zxok = (z[0]*o[1]) - (o[0]*z[1])\n", + " zxo = [zxoi, zxoj, zxok] #cross product of z and o\n", + " J[0][i] = zxo[0]\n", + " J[1][i] = zxo[1]\n", + " J[2][i] = zxo[2]\n", + " J[3][i] = z[0]\n", + " J[4][i] = z[1]\n", + " J[5][i] = z[2]\n", + "\n", + "print(J) #complete manipulator jacobian" + ], + "metadata": { + "id": "nfcU-VdQ45lo" + }, + "execution_count": null, + "outputs": [] + }, + { + "cell_type": "code", + "source": [ + "print(on0) #end effector position" + ], + "metadata": { + "id": "sXfnvL07-6Bt" + }, + "execution_count": null, + "outputs": [] + }, + { + "cell_type": "code", + "source": [ + "qdot = list(map(float,input(\"qdot values: \").strip().split())) # rate of change of joint variables" + ], + "metadata": { + "id": "5NF_5Lfl_DMh" + }, + "execution_count": null, + "outputs": [] + }, + { + "cell_type": "code", + "source": [ + "xdot = J@qdot\n", + "print(\"linear velocity: \", xdot[0], xdot[1], xdot[2],\"\\n\")\n", + "print(\"angular velocity: \", xdot[3], xdot[4], xdot[5])" + ], + "metadata": { + "id": "QnCzrFbjANvs" + }, + "execution_count": null, + "outputs": [] + } + ] +} \ No newline at end of file diff --git a/ME_639_Assignment_6_Q1.ipynb b/ME_639_Assignment_6_Q1.ipynb new file mode 100644 index 0000000..d11a78e --- /dev/null +++ b/ME_639_Assignment_6_Q1.ipynb @@ -0,0 +1,189 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "view-in-github", + "colab_type": "text" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "MtaH-g0Tg375" + }, + "outputs": [], + "source": [ + "#note: the following code has been written specifically for an R3 planar robot with link lengths = 1m\n", + "import math as m\n", + "import numpy as np\n", + "import matplotlib.pyplot as plt\n", + "from scipy.integrate import odeint\n", + "#setting desired trajectory\n", + "r = np.linspace(-3, 3, 100) #extent is 3, since the maximum extent of the robot is also 3 m from the base\n", + "t = np.linspace(0,2*m.pi,100) #for a whole circle\n", + "#trajectory for circle with radius 1.5 centered at base - what we desire\n", + "xd = 1.5*np.cos(t)\n", + "yd = 1.5*np.sin(t)\n", + "#matrix for desired trajectory points\n", + "Xd = [xd, yd]\n", + "#for desired velocity:\n", + "xdot = - 1.5*np.sin(t)\n", + "ydot = 1.5*np.cos(t)\n", + "Xdot = [xdot,ydot]\n", + "#array for actual end effector position\n", + "X = [[0]*len(t),[0]*len(t)]" + ] + }, + { + "cell_type": "code", + "source": [ + "#user defined function for finding the pseudoinverse of the velocity jacobian\n", + "def jvplus (q):\n", + " dh = [[0, q[0], 1, 0],[0, q[1], 1, 0],[0, q[2], 1, 0]] #dh parameters with joint variables for 3R manipulator\n", + " H = [0]*4 #array with link to link transformation matrices\n", + " H[0] = np.identity(4)\n", + " H0 = np.identity(4) #overall transformation matrix\n", + " for i in range(3):\n", + " d = dh[i][0]\n", + " theta = dh[i][1]\n", + " r = dh[i][2]\n", + " alpha = dh[i][3]\n", + " Z = [[m.cos(theta), -1*m.sin(theta), 0, 0], #transforation matrix, Z operations\n", + " [m.sin(theta), m.cos(theta), 0, 0],\n", + " [0, 0, 1, d],\n", + " [0, 0, 0, 1]]\n", + " X = [[1, 0, 0, r], #transforation matrix, X operations\n", + " [0, m.cos(alpha), -1*m.sin(alpha), 0],\n", + " [0, m.sin(alpha), m.cos(alpha), 0],\n", + " [0, 0, 0, 1]]\n", + " H[i+1] = np.matmul(Z,X) #adding current link to link transformation to H matrix\n", + " H0 = H0@H[i+1] #mutliplying link to link transformations to get overall transformation\n", + " on0 = [H0[0][3],H0[1][3],H0[2][3]] #position of last origin (end effector) wrt base\n", + " o = np.zeros(3) #for storing position of end effector wrt current frame\n", + " Jv = np.zeros((2,3)) #jacobian (initialized) (only for lin velocities, not angular)\n", + " h = np.identity(4) #matrix representing transformation from base to current frame\n", + " for i in range(3):\n", + " h = h@H[i]\n", + " z = [h[0][2], h[1][2], h[2][2]] #z -> last column of rotation\n", + " oi0 = [h[0][3], h[1][3], h[2][3]] #position of current frame origin wrt base origin\n", + " #calculation of o:\n", + " o[0] = on0[0] - oi0[0] \n", + " o[1] = on0[1] - oi0[1]\n", + " o[2] = on0[2] - oi0[2]\n", + " zxoi = (z[1]*o[2]) - (o[1]*z[2]) \n", + " zxoj = (o[0]*z[2]) - (z[0]*o[2])\n", + " zxok = (z[0]*o[1]) - (o[0]*z[1])\n", + " zxo = [zxoi, zxoj, zxok] #cross product of z and o\n", + " Jv[0][i] = zxo[0]\n", + " Jv[1][i] = zxo[1]\n", + " Jvt = np.transpose(Jv)\n", + " Jvplus = Jvt@np.linalg.inv(Jv@Jvt)\n", + " #returns the pseudoinverse after calculating it\n", + " return Jvplus" + ], + "metadata": { + "id": "xREiFtblvGih" + }, + "execution_count": null, + "outputs": [] + }, + { + "cell_type": "code", + "source": [ + "#input initial conditions:\n", + "x0 = float(input())\n", + "y0 = float(input())\n", + "#add to matrix of actual values\n", + "X[0][0] = x0\n", + "X[1][0] = y0\n", + "#assuming orientation of initial position is 2*PI\n", + "#inverse kinematics for initial joint conditions:\n", + "x2 = x0 - 1\n", + "y2 = y0\n", + "q20 = m.acos((x2**2 + y2**2 - 2)/2)\n", + "q10 = m.atan(y2/x2) - m.atan(m.sin(q20)/(1+m.cos(q20)))\n", + "q30 = 2*m.pi - (q10 + q20)\n", + "q0 = [q10,q20,q30]\n", + "#matrix with all actual q values\n", + "q = [0]*len(t)\n", + "q[0] = q0" + ], + "metadata": { + "id": "BQA38d4noJwl" + }, + "execution_count": null, + "outputs": [] + }, + { + "cell_type": "code", + "source": [ + "kp = 4 #stiffness constant for error" + ], + "metadata": { + "id": "PNJ-ZXKNAT2D" + }, + "execution_count": null, + "outputs": [] + }, + { + "cell_type": "code", + "source": [ + "for k in range(len(t)):\n", + " #calculating current x,y values from current q values\n", + " X[0][k] = m.cos(q[k][0]+q[k][1]+q[k][2]) + m.cos(q[k][0]+q[k][1]) + m.cos(q[k][0])\n", + " X[1][k] = m.sin(q[k][0]+q[k][1]+q[k][2]) + m.sin(q[k][0]+q[k][1]) + m.sin(q[k][0])\n", + " #finding error in position\n", + " ex = [Xd[0][k] - X[0][k], Xd[1][k] - X[1][k]]\n", + " xdot = [Xdot[0][k],Xdot[1][k]]\n", + " #finding pseudoinverse of velocity jacobian for current q values\n", + " Jvp = jvplus(q[k])\n", + " term1 = Jvp@xdot\n", + " term2 = Jvp@ex\n", + " delq = term1 + kp*term2\n", + " #finding the next position of the robot\n", + " q[k+1] = q[k]+ delq" + ], + "metadata": { + "id": "2eyuGWHcJUGX" + }, + "execution_count": null, + "outputs": [] + }, + { + "cell_type": "code", + "source": [ + "plt.plot(X[0],X[1], color = 'red',label = 'actual') #actual path \n", + "plt.plot(xd,yd, color = 'blue', label = 'desired') #desired path\n", + "plt.xlim([-2,2])\n", + "plt.ylim([-2,2])\n", + "plt.show()" + ], + "metadata": { + "id": "sFSfIV7WOyom" + }, + "execution_count": null, + "outputs": [] + } + ], + "metadata": { + "colab": { + "provenance": [], + "authorship_tag": "ABX9TyNWWsYmII+Bjm9h4kuyk06V", + "include_colab_link": true + }, + "kernelspec": { + "display_name": "Python 3", + "name": "python3" + }, + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} \ No newline at end of file diff --git a/ME_639_Mini_Project.ipynb b/ME_639_Mini_Project.ipynb new file mode 100644 index 0000000..5a2b8a9 --- /dev/null +++ b/ME_639_Mini_Project.ipynb @@ -0,0 +1,616 @@ +{ + "nbformat": 4, + "nbformat_minor": 0, + "metadata": { + "colab": { + "provenance": [], + "authorship_tag": "ABX9TyNBHtQ5lvBtxuqD8clz7RBr", + "include_colab_link": true + }, + "kernelspec": { + "name": "python3", + "display_name": "Python 3" + }, + "language_info": { + "name": "python" + } + }, + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "view-in-github", + "colab_type": "text" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": 12, + "metadata": { + "id": "23VLZY9JY_sU" + }, + "outputs": [], + "source": [ + "#if there are any issues while viewing animation, please skip the FuncAnimation method\n", + "#instead, please copy-paste the for loop towards the end of task 1 part 1 (right before anim has been defined)\n", + "#it is in its own code cell, and can be used to view individual frames/ a crude animation\n", + "#task 1 part 1\n", + "from matplotlib import rc\n", + "rc('animation',html = 'jshtml') #to view the animation object in colab\n", + "import math as m \n", + "import numpy as np\n", + "import matplotlib.pyplot as plt\n", + "import matplotlib.animation as animation #to create animation objects\n", + "import sys\n", + "import seaborn as sns\n", + "from scipy.integrate import odeint #to solve ordinary differential equations" + ] + }, + { + "cell_type": "code", + "source": [ + "PI = 3.14159\n", + "m1 = int(input())\n", + "m2 = int(input())\n", + "l1 = int(input())\n", + "l2 = int(input())" + ], + "metadata": { + "id": "8JuGpbwuIOCX" + }, + "execution_count": null, + "outputs": [] + }, + { + "cell_type": "code", + "source": [ + "Rmax = l1 + l2 #maximum extent of workspace\n", + "Rmin = abs(l1 - l2) #minimum extent of workspace" + ], + "metadata": { + "id": "3cxSqDJZKiDN" + }, + "execution_count": 5, + "outputs": [] + }, + { + "cell_type": "code", + "source": [ + "#function to plot axes:\n", + "def initialize():\n", + " ax = plt.axes(xlim = (-Rmax-0.1,Rmax+0.1), ylim = (-Rmax-0.1,Rmax+0.1))\n", + "#creating a figure object, specifying size\n", + "fig = plt.figure(figsize = (5,5))" + ], + "metadata": { + "id": "u4Dyf4uYGjNC" + }, + "execution_count": null, + "outputs": [] + }, + { + "cell_type": "code", + "source": [ + "#checks if a point is within the workspace (assuming both arms can move by 360 degrees)\n", + "def isworkspace(x,y):\n", + " dist = m.sqrt(x**2 + y**2) #finds point's distance from base\n", + " if dist>Rmax or dist=x2:\n", + " x = np.linspace(x2,x1,100)\n", + "else:\n", + " x = np.linspace(x1,x2,100)\n", + "if y1>=y2:\n", + " y = np.linspace(y2,y1,100)\n", + "else:\n", + " y = np.linspace(y1,y2,100)" + ], + "metadata": { + "id": "NYB6Rt_65Xsy" + }, + "execution_count": null, + "outputs": [] + }, + { + "cell_type": "code", + "source": [ + "#external additional driving forces\n", + "FX = int(input())\n", + "FY = int(input())\n", + "#defining the spring forces, adding with external driving forces\n", + "Fx = k*(x-x0) + FX\n", + "Fy = k*(y-y0) + FY\n", + "#defining array of joint variables that we will go through for each end effector point\n", + "theta = np.arccos((x**2 + y**2 - l1**2 - l2**2)/(2*l1*l2))\n", + "q1 = np.arctan(y/x) - np.arctan(l2*np.sin(theta)/(l1 + l2*np.cos(theta)))\n", + "q2 = q1 + theta" + ], + "metadata": { + "id": "8hXe8fqS5btD" + }, + "execution_count": null, + "outputs": [] + }, + { + "cell_type": "code", + "source": [ + "#defining array of torques we will go through\n", + "#reason: spring action can only be simulated accurately if we consider the varying speeds along our trajectory\n", + "# - otherwise it's just oscillation around a point\n", + "cartspace2 = [[-1*l1*m.sin(q1), l1*m.cos(q2)],[-1*l2*m.sin(q2) l2*m.cos(q2)]]\n", + "F = [Fx, Fy]\n", + "tau = np.matmul(cartspace2,F)\n", + "\n", + "tau1 = tau[0]\n", + "tau2 = tau[1]\n", + "\n", + "#from here onwards, same logic as in task 1 part b\n", + "#inital conditions of q and x are changed as they are defined by the user here\n", + "t = np.linspace(0,100,100)\n", + "\n", + "#setting initial conditions\n", + "XX0 = x1\n", + "YY0 = y1\n", + "XX = [XX0, YY0]\n", + "theta = m.acos((x1**2 + y1**2 - l1**2 - l2**2)/(2*l1*l2))\n", + "q10 = m.atan(y1/x1) - m.atan(l2*m.sin(theta)/(l1 + l2*m.cos(theta)))\n", + "q20 = q1 + theta\n", + "q0 = [q10,q20]\n", + "Q10 = 0\n", + "Q20 = 0\n", + "Q0 = [Q10, Q20]" + ], + "metadata": { + "id": "bOZzJD5G55A7" + }, + "execution_count": null, + "outputs": [] + }, + { + "cell_type": "code", + "source": [ + "Q = odeint(ode2,Q0,t) #solution for angular velocities\n", + "q = odeint(ode1,q0,t) #solution for joint variables as a function of time\n", + "X = odeint(odexy,XX,t) #solution for x and y as functions of time\n", + "x = X[0]\n", + "y = X[1]" + ], + "metadata": { + "id": "m5AQGTBvg5AQ" + }, + "execution_count": null, + "outputs": [] + }, + { + "cell_type": "code", + "source": [ + "spring = animation.FuncAnimation(fig,frame, frames = 100, blit = True) #animation for spring" + ], + "metadata": { + "id": "7b2Cqw3ohkOr" + }, + "execution_count": null, + "outputs": [] + }, + { + "cell_type": "code", + "source": [ + "spring #viewing animation\n", + "#loop option already provided in output - is able to simulated repeated oscillations over time" + ], + "metadata": { + "id": "O0-98HPch2sr" + }, + "execution_count": null, + "outputs": [] + }, + { + "cell_type": "code", + "source": [ + "# task 4\n", + "#initializing joint 1 variable to 35 degrees\n", + "q1 = 35*PI/180\n", + "#creating empty x and y arrays to store end effector values\n", + "X = []\n", + "Y = []\n", + "#moving joint 1 by 1 degree, and each time moving joint 2 from 35 to 145 degrees (again, 1 degree at a time)\n", + "#at each configuration of the robot's arms, we store the end effector coordinates in X,Y\n", + "for i in range(111):\n", + " q2 = 35*PI/180\n", + " for j in range(111):\n", + " q2 = q2 + PI/180\n", + " X.append(l1*m.cos(q1) + l2*m.cos(q2))\n", + " Y.append(l1*m.sin(q1) + l2*m.sin(q2))\n", + " q1 = q1 + PI/180\n", + "#setting limits for axes so that it's easier to understand the graph\n", + "plt.xlim(-(l1 + l2),(l1 + l2))\n", + "plt.ylim(0,l1 + 1.1*l2)\n", + "#creating a scatterplot of end effector points obtained by above process\n", + "#this will give us a \"shaded\" region that is our workspace\n", + "sns.scatterplot(X,Y)\n", + "plt.show()" + ], + "metadata": { + "id": "UI6mw5W8ujqY" + }, + "execution_count": null, + "outputs": [] + } + ] +} \ No newline at end of file