Shaandili

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": [
+        "<a href=\"https://colab.research.google.com/github/Shaandili/robotics-me639-2022/blob/Shaandili/ME639_Assignment_4%265_Q1%2C2%2C3%2C6.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
+      ]
+    },
+    {
+      "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": []
+    }
+  ]
+}

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": [
+        "<a href=\"https://colab.research.google.com/github/Shaandili/robotics-me639-2022/blob/Shaandili/ME_639_Assignment_3_Q11.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
+      ]
+    },
+    {
+      "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": []
+    }
+  ]
+}