Task3_4_5_6_Assignment3.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "a52a3096",
   "metadata": {},
   "source": [
    "# Task 3"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "id": "77a021a1",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      " Enter the number of links in your robot: 2\n",
      "Enter the value of a_i for link 1: 1\n",
      "Enter the value of alpha_i for link 1: 0\n",
      "Enter the value of d_i for link 1: 0\n",
      "Enter the value of theta_i for link 1: 90\n",
      "Enter the value of a_i for link 2: 1\n",
      "Enter the value of alpha_i for link 2: 0\n",
      "Enter the value of d_i for link 2: 0\n",
      "Enter the value of theta_i for link 2: 0\n",
      "Enter 'yes' if you want to provide the type of each joints and enter 'no' for all revolute joints as default: no\n",
      "DH Parameters provided are:\n",
      " [[ 1  0  0 90]\n",
      " [ 1  0  0  0]] \n",
      "\n",
      "T2_0 is given by \n",
      " [[ 6.1232340e-17 -1.0000000e+00  0.0000000e+00  1.2246468e-16]\n",
      " [ 1.0000000e+00  6.1232340e-17  0.0000000e+00  2.0000000e+00]\n",
      " [ 0.0000000e+00  0.0000000e+00  1.0000000e+00  0.0000000e+00]\n",
      " [ 0.0000000e+00  0.0000000e+00  0.0000000e+00  1.0000000e+00]] \n",
      "\n",
      "\n",
      " end-effector position is \n",
      " [1.2246467991473532e-16, 2.0, 0.0] \n",
      "\n",
      "Manipulator Jacobian is \n",
      " [[-2.0000000e+00 -1.0000000e+00]\n",
      " [ 1.2246468e-16  6.1232340e-17]\n",
      " [ 0.0000000e+00  0.0000000e+00]\n",
      " [ 0.0000000e+00  0.0000000e+00]\n",
      " [ 0.0000000e+00  0.0000000e+00]\n",
      " [ 1.0000000e+00  1.0000000e+00]]\n"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "import math\n",
    "\n",
    "n = int(input(\" Enter the number of links in your robot: \"))\n",
    "D_H = []\n",
    "joints = [] \n",
    "for i in range(n):          \n",
    "    a_i = int(input(f\"Enter the value of a_i for link {i+1}: \"))\n",
    "    alpha_i = int(input(f\"Enter the value of alpha_i for link {i+1}: \"))\n",
    "    d_i = int(input(f\"Enter the value of d_i for link {i+1}: \"))\n",
    "    theta_i = int(input(f\"Enter the value of theta_i for link {i+1}: \"))\n",
    "    D_H.append([a_i,alpha_i,d_i,theta_i])\n",
    "    \n",
    "k = input(\"Enter 'yes' if you want to provide the type of each joints and enter 'no' for all revolute joints as default: \")\n",
    "if k=='yes':\n",
    "    for i in range(n):  \n",
    "        joint = input(\"Enter 'p' if joint is prismatic else enter 'r' if joint is revolute: \")\n",
    "        joints.append(joint)\n",
    "else:\n",
    "     for i in range(n):  \n",
    "        joint = 'r'\n",
    "        joints.append(joint)\n",
    "        \n",
    "DH_par = np.array(D_H)\n",
    "print(f'DH Parameters provided are:\\n {DH_par} \\n')\n",
    "\n",
    "A = []\n",
    "\n",
    "################ Homogenous transformation ###############\n",
    "for i in range(n):\n",
    "    r1 = [np.cos(DH_par[i,3]*math.pi/180),-1*np.sin(DH_par[i,3]*math.pi/180)*np.cos(DH_par[i,1]*math.pi/180),np.sin(DH_par[i,3]*math.pi/180)*np.sin(DH_par[i,1]*math.pi/180),DH_par[i,0]*np.cos(DH_par[i,3]*math.pi/180)]\n",
    "    r2 = [np.sin(DH_par[i,3]*math.pi/180),np.cos(DH_par[i,3]*math.pi/180)*np.cos(DH_par[i,1]*math.pi/180),-1*np.cos(DH_par[i,3]*math.pi/180)*np.sin(DH_par[i,1]*math.pi/180),DH_par[i,0]*np.sin(DH_par[i,3]*math.pi/180)]\n",
    "    r3 = [0,np.sin(DH_par[i,1]*math.pi/180),np.cos(DH_par[i,1]*math.pi/180),DH_par[i,2]]\n",
    "    r4 = [0,0,0,1]\n",
    "    A_i =np.array([r1,r2,r3,r4])\n",
    "    A.append(A_i)\n",
    "    \n",
    "################ transformation for end- effector #############\n",
    "T = []   # T is the list containing the value of A1,A1A2,A1A2A3,............., A1A2....An\n",
    "Ti = np.identity(4)\n",
    "for i in range(n):\n",
    "    Ti = Ti@A[i]\n",
    "    T.append(Ti)    \n",
    "print(f'T{n}_0 is given by \\n {T[n-1]} \\n')\n",
    "\n",
    "################ end- effector position #############\n",
    "end_effector_pos = [T[n-1][0,3],T[n-1][1,3],T[n-1][2,3]]\n",
    "print(f'\\n end-effector position is \\n {end_effector_pos} \\n' )\n",
    "\n",
    "O_n = np.array(end_effector_pos) # distance of end-effector from O_0\n",
    "O_0 = np.array([0,0,0])\n",
    "O = []                 # list contains the position of all origins\n",
    "O.append(O_0)\n",
    "j_v = []\n",
    "j_w = []\n",
    "\n",
    "for i in range(n):\n",
    "    O_i = np.array([T[i][0,3],T[i][1,3],T[i][2,3]])\n",
    "    O.append(O_i)\n",
    "    \n",
    "    Z_i = np.array([T[i][0,2],T[i][1,2],T[i][2,2]])   # Z_i is R_i_0 * k_i_0\n",
    "    \n",
    "    rv = np.cross(Z_i,(O_n - O[i]))             # rv denotes revolute joint value of element of velocity Jacobian\n",
    "    pv = Z_i                                    # pv denotes prismatic joint value of element of velocity Jacobian\n",
    "    if joints[i]=='p':\n",
    "        j_v.append(pv)\n",
    "        j_w.append(O_0)\n",
    "    else:\n",
    "        j_v.append(rv)\n",
    "        j_w.append(pv)\n",
    "        \n",
    "Jv = np.transpose(np.array(j_v))    # velocity Jacobian\n",
    "Jw = np.transpose(np.array(j_w))    # Angular velocity Jacobian\n",
    "J = np.append(Jv,Jw,axis=0)         # Manipulator Jacobian\n",
    "print(f\"Manipulator Jacobian is \\n {J}\")\n",
    "\n",
    "\n",
    "\n",
    "        \n",
    "        \n",
    "    \n",
    "        \n",
    "    \n",
    "    "
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8a00d447",
   "metadata": {},
   "source": [
    "# Task 4\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "id": "1266d3c1",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Enter the excel file name 'scara.xlsx' for scara or 'stanford.xlsx' for stanford: scara.xlsx\n",
      "DH Parameter table is: \n",
      "[[  3   0   0  30]\n",
      " [  4 180   0  45]\n",
      " [  0   0   2   0]\n",
      " [  0   0   3  60]]\n",
      "\n",
      "T4_0 is given by \n",
      " [[ 9.65925826e-01  2.58819045e-01  1.18291797e-16  3.63335239e+00]\n",
      " [ 2.58819045e-01 -9.65925826e-01 -3.16961915e-17  5.36370331e+00]\n",
      " [ 1.06057524e-16  6.12323400e-17 -1.00000000e+00 -5.00000000e+00]\n",
      " [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]] \n",
      "\n"
     ]
    }
   ],
   "source": [
    "\n",
    "\n",
    "# For Stanford type stanford.xlsx\n",
    "# For Scara type scara.xlsx\n",
    "\n",
    "import numpy as np\n",
    "import pandas as pd\n",
    "import math\n",
    "\n",
    "user = input(\"Enter the excel file name 'scara.xlsx' for scara or 'stanford.xlsx' for stanford: \")\n",
    "data = pd.read_excel(user)\n",
    "DH_par = data.to_numpy() # converting the dataframe into numpy array\n",
    "n = DH_par.shape[0]\n",
    "print(f\"DH Parameter table is: \\n{DH_par}\\n\")\n",
    "A = []\n",
    "\n",
    "################ Homogenous transformation ###############\n",
    "for i in range(n):\n",
    "    r1 = [np.cos(DH_par[i,3]*math.pi/180),-1*np.sin(DH_par[i,3]*math.pi/180)*np.cos(DH_par[i,1]*math.pi/180),np.sin(DH_par[i,3]*math.pi/180)*np.sin(DH_par[i,1]*math.pi/180),DH_par[i,0]*np.cos(DH_par[i,3]*math.pi/180)]\n",
    "    r2 = [np.sin(DH_par[i,3]*math.pi/180),np.cos(DH_par[i,3]*math.pi/180)*np.cos(DH_par[i,1]*math.pi/180),-1*np.cos(DH_par[i,3]*math.pi/180)*np.sin(DH_par[i,1]*math.pi/180),DH_par[i,0]*np.sin(DH_par[i,3]*math.pi/180)]\n",
    "    r3 = [0,np.sin(DH_par[i,1]*math.pi/180),np.cos(DH_par[i,1]*math.pi/180),DH_par[i,2]]\n",
    "    r4 = [0,0,0,1]\n",
    "    A_i =np.array([r1,r2,r3,r4])\n",
    "    A.append(A_i)\n",
    "    \n",
    "################ transformation for end- effector #############\n",
    "T = []         # T is the list containing the value of A1,A1A2,A1A2A3,............., A1A2....An\n",
    "Ti = np.identity(4)\n",
    "for i in range(n):\n",
    "    Ti = Ti@A[i]\n",
    "    T.append(Ti)    \n",
    "print(f'T{n}_0 is given by \\n {T[n-1]} \\n')\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c14ec06b",
   "metadata": {},
   "source": [
    "# Task5"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "id": "1fedb27e",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "DH Parameter table is: \n",
      "[[  0 -90   2   0]\n",
      " [  0 -90   3 -90]\n",
      " [  0   0   4   0]]\n",
      "\n",
      "T3_0 is given by \n",
      " [[ 6.123234e-17  6.123234e-17  1.000000e+00  4.000000e+00]\n",
      " [-6.123234e-17 -1.000000e+00  6.123234e-17  3.000000e+00]\n",
      " [ 1.000000e+00 -6.123234e-17 -6.123234e-17  2.000000e+00]\n",
      " [ 0.000000e+00  0.000000e+00  0.000000e+00  1.000000e+00]] \n",
      "\n"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "import math\n",
    "import pandas as pd\n",
    "\n",
    "user = 'task5.xlsx'\n",
    "data = pd.read_excel(user)\n",
    "DH_par = data.to_numpy() # converting the dataframe into numpy array\n",
    "n = DH_par.shape[0]\n",
    "print(f\"DH Parameter table is: \\n{DH_par}\\n\")\n",
    "A = []\n",
    "################ Homogenous transformation ###############\n",
    "for i in range(n):\n",
    "    r1 = [np.cos(DH_par[i,3]*math.pi/180),-1*np.sin(DH_par[i,3]*math.pi/180)*np.cos(DH_par[i,1]*math.pi/180),np.sin(DH_par[i,3]*math.pi/180)*np.sin(DH_par[i,1]*math.pi/180),DH_par[i,0]*np.cos(DH_par[i,3]*math.pi/180)]\n",
    "    r2 = [np.sin(DH_par[i,3]*math.pi/180),np.cos(DH_par[i,3]*math.pi/180)*np.cos(DH_par[i,1]*math.pi/180),-1*np.cos(DH_par[i,3]*math.pi/180)*np.sin(DH_par[i,1]*math.pi/180),DH_par[i,0]*np.sin(DH_par[i,3]*math.pi/180)]\n",
    "    r3 = [0,np.sin(DH_par[i,1]*math.pi/180),np.cos(DH_par[i,1]*math.pi/180),DH_par[i,2]]\n",
    "    r4 = [0,0,0,1]\n",
    "    A_i =np.array([r1,r2,r3,r4])\n",
    "    A.append(A_i)\n",
    "    \n",
    "################ transformation for end- effector #############\n",
    "T = []   # T is the list containing the value of A1,A1A2,A1A2A3,............., A1A2....An\n",
    "Ti = np.identity(4)\n",
    "for i in range(n):\n",
    "    Ti = Ti@A[i]\n",
    "    T.append(Ti)    \n",
    "print(f'T{n}_0 is given by \\n {T[n-1]} \\n')\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "440a5bac",
   "metadata": {},
   "source": [
    "# Task6\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "id": "fa56b24c",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "DH Parameter table is: \n",
      "[[  0  90   2  30]\n",
      " [  2   0   0  60]\n",
      " [  2   0   0  45]\n",
      " [  0 -90   0  30]\n",
      " [  0 -90   0  60]\n",
      " [  0   0   4  90]]\n",
      "\n",
      "T6_0 is given by \n",
      " [[ 0.61237244  0.73919892  0.28033009  1.53905801]\n",
      " [ 0.35355339 -0.5732233   0.73919892  3.19797663]\n",
      " [ 0.70710678 -0.35355339 -0.61237244  3.21441272]\n",
      " [ 0.          0.          0.          1.        ]] \n",
      "\n"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "import math\n",
    "import pandas as pd\n",
    "\n",
    "user = 'task6.xlsx'\n",
    "data = pd.read_excel(user)\n",
    "DH_par = data.to_numpy() # converting the dataframe into numpy array\n",
    "n = DH_par.shape[0]\n",
    "print(f\"DH Parameter table is: \\n{DH_par}\\n\")\n",
    "A = []\n",
    "################ Homogenous transformation ###############\n",
    "for i in range(n):\n",
    "    r1 = [np.cos(DH_par[i,3]*math.pi/180),-1*np.sin(DH_par[i,3]*math.pi/180)*np.cos(DH_par[i,1]*math.pi/180),np.sin(DH_par[i,3]*math.pi/180)*np.sin(DH_par[i,1]*math.pi/180),DH_par[i,0]*np.cos(DH_par[i,3]*math.pi/180)]\n",
    "    r2 = [np.sin(DH_par[i,3]*math.pi/180),np.cos(DH_par[i,3]*math.pi/180)*np.cos(DH_par[i,1]*math.pi/180),-1*np.cos(DH_par[i,3]*math.pi/180)*np.sin(DH_par[i,1]*math.pi/180),DH_par[i,0]*np.sin(DH_par[i,3]*math.pi/180)]\n",
    "    r3 = [0,np.sin(DH_par[i,1]*math.pi/180),np.cos(DH_par[i,1]*math.pi/180),DH_par[i,2]]\n",
    "    r4 = [0,0,0,1]\n",
    "    A_i =np.array([r1,r2,r3,r4])\n",
    "    A.append(A_i)\n",
    "    \n",
    "################ transformation for end- effector #############\n",
    "T = []   # T is the list containing the value of A1,A1A2,A1A2A3,............., A1A2....An\n",
    "Ti = np.identity(4)\n",
    "for i in range(n):\n",
    "    Ti = Ti@A[i]\n",
    "    T.append(Ti)    \n",
    "print(f'T{n}_0 is given by \\n {T[n-1]} \\n')\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "eba6cc10",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "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.9.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}