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": [
+ ""
+ ]
+ },
+ {
+ "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": [
+ ""
+ ]
+ },
+ {
+ "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": [
+ ""
+ ]
+ },
+ {
+ "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": [
+ ""
+ ]
+ },
+ {
+ "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": [
+ ""
+ ]
+ },
+ {
+ "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