22110067_Assignment_2.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Question 3"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### RRP SCARA"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Homogeneous Transformations"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "#Values for USER-INPUT\n",
    "th1=30\n",
    "th2=20\n",
    "k3=0.5\n",
    "\n",
    "#Pre Defined Values\n",
    "r1=2;r2=1;r3=3  #Fixed Length between the joints"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Defining the Rotation Matrices\n",
    "R0_1=np.array(([np.cos(th1),-np.sin(th1),0],\n",
    "               [np.sin(th1),np.cos(th1),0],\n",
    "               [0,0,1]))\n",
    "\n",
    "R1_2=np.array(([np.cos(th2),-np.sin(th2),0],\n",
    "               [np.sin(th2),np.cos(th2),0],\n",
    "               [0,0,1]))\n",
    "\n",
    "R2_3=np.array(([1,0,0],\n",
    "               [0,1,0],\n",
    "               [0,0,1]))\n",
    "\n",
    "RL=[R0_1,R1_2,R2_3]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Defining the translation matrices\n",
    "d0_1=np.array(([0],\n",
    "               [0],\n",
    "               [0]))\n",
    "\n",
    "d1_2=np.array(([0],\n",
    "               [r2],\n",
    "               [r1]))\n",
    "\n",
    "d2_3=np.array(([0],\n",
    "               [r3],\n",
    "               [0]))\n",
    "\n",
    "TL=[d0_1,d1_2,d2_3]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "P3=np.array(([0],\n",
    "             [0],\n",
    "             [-k3],\n",
    "             [1]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "#The below matrix creates a Homogeneos Transformation Matrix by taking into input the Rotation and Transllation Matices\n",
    "def htrans(Rotation_Matrix,Translation_Matrix):\n",
    "    htrans_mat=np.zeros((4,4))  #Initializing the Matrix as a Zero 4x4 Matrix\n",
    "    htrans_mat[3,3]=1   #Updating the Scaling Factor as \n",
    "    \n",
    "    #Augmenting a Matrix in Python -> https://chat.openai.com/share/0e9d93f2-7631-4313-85b1-2c922ffc1414\n",
    "    htrans_mat[:3,:3]=Rotation_Matrix\n",
    "    htrans_mat[:3,3:4]=Translation_Matrix\n",
    "\n",
    "    return htrans_mat"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "Output=P3\n",
    "for i in range(-1,-(len(RL)),-1):\n",
    "    Output=np.dot(htrans(RL[i],TL[i]),Output)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[-2.73883575],\n",
       "       [ 2.22424619],\n",
       "       [ 1.5       ],\n",
       "       [ 1.        ]])"
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Output"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Question 8"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### RRP SCARA"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Manipulator Jacobian"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "num_links=3\n",
    "\n",
    "Output1=np.identity(4)\n",
    "for i in range(-1,-(len(RL)),-1):\n",
    "    Output1=np.dot(htrans(RL[i],TL[i]),Output1)\n",
    "\n",
    "d3_0=Output1[0:3,3]\n",
    "d3_0=d3_0.reshape(-1,1)\n",
    "\n",
    "def mani_jaco(Rot_List,Trans_List,num_links):\n",
    "    manipulator_jacobian=np.zeros((6,num_links))\n",
    "    Output=np.identity(4)\n",
    "    Output=np.dot(htrans(Rot_List[0],TL[0]),Output)\n",
    "    d1_0=Output[0:3,3]\n",
    "    K1=d3_0-d1_0\n",
    "    MJ=np.dot(Rot_List[0],np.array(([0],[0],[1]))).reshape(-1,1)\n",
    "    manipulator_jacobian[0:3,0]=np.cross(MJ[:, 0],K1[:, 0])\n",
    "    manipulator_jacobian[3:6, 0] = np.dot(Rot_List[0], np.array(([[0], [0], [1]]))).flatten()\n",
    "\n",
    "    R2_0=np.dot(RL[1],RL[0])\n",
    "    R2_0m=np.dot(R2_0,np.array(([0],[0],[1])))\n",
    "    for i in range(-2,-(len(Rot_List)),-1):\n",
    "        Output=np.dot(htrans(Rot_List[i],TL[i]),Output)\n",
    "    d2_0=Output[0:3,3]\n",
    "    d2_0=d2_0.reshape(-1,1)\n",
    "    K1=d3_0-d2_0\n",
    "    manipulator_jacobian[0:3,1]=np.cross(R2_0m[:, 0],K1[:, 0])\n",
    "    manipulator_jacobian[3:6,1] = np.dot(R2_0, np.array(([[0], [0], [1]]))).flatten()\n",
    "\n",
    "    R3_0=np.dot(RL[2],R2_0)\n",
    "    manipulator_jacobian[0:3,2] = np.dot(R3_0, np.array(([[0], [0], [1]]))).flatten()\n",
    "    \n",
    "    print(manipulator_jacobian)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[-2.22424619 -1.22424619  0.        ]\n",
      " [-2.73883575 -2.73883575  0.        ]\n",
      " [ 0.          0.          1.        ]\n",
      " [ 0.          0.          0.        ]\n",
      " [ 0.          0.          0.        ]\n",
      " [ 1.          1.          0.        ]]\n"
     ]
    }
   ],
   "source": [
    "mani_jaco(RL,TL,num_links)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Question 4"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### RRP Standford"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Homogeneous Transformations"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "#Values for USER-INPUT\n",
    "th1=30\n",
    "th2=20\n",
    "k3=0.5\n",
    "\n",
    "#Pre Defined Values\n",
    "r1=2;r2=1;r3=3  #Fixed Length between the joints"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Defining the Rotation Matrices\n",
    "R0_1=np.array(([np.cos(th1),-np.sin(th1),0],\n",
    "               [np.sin(th1),np.cos(th1),0],\n",
    "               [0,0,1]))\n",
    "\n",
    "R1_2=np.array(([np.cos(th2),-np.sin(th2),0],\n",
    "               [0,0,1],\n",
    "               [-np.sin(2),-np.cos(th2),0],))\n",
    "\n",
    "R2_3=np.array(([1,0,0],\n",
    "               [0,0,-1],\n",
    "               [0,1,0]))\n",
    "\n",
    "RL=[R0_1,R1_2,R2_3]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Defining the translation matrices\n",
    "d0_1=np.array(([0],\n",
    "               [0],\n",
    "               [0]))\n",
    "\n",
    "d1_2=np.array(([0],\n",
    "               [r2],\n",
    "               [r1]))\n",
    "\n",
    "d2_3=np.array(([0],\n",
    "               [r3],\n",
    "               [0]))\n",
    "\n",
    "TL=[d0_1,d1_2,d2_3]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "P3=np.array(([0],\n",
    "             [0],\n",
    "             [-k3],\n",
    "             [1]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [],
   "source": [
    "Output=P3\n",
    "for i in range(-1,-(len(RL)),-1):\n",
    "    Output=np.dot(htrans(RL[i],TL[i]),Output)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[-3.19530838],\n",
       "       [ 1.        ],\n",
       "       [ 0.57171278],\n",
       "       [ 1.        ]])"
      ]
     },
     "execution_count": 16,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Output"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Question 10"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### RRR"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "#Values for USER-INPUT\n",
    "th1=30\n",
    "th2=20\n",
    "th3=50\n",
    "\n",
    "#Pre Defined Values\n",
    "r1=2;r2=1;r3=3;r4=2;r5=1  #Fixed Length between the joints"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "R0_1=np.array(([np.cos(th1),-np.sin(th1),0],\n",
    "               [np.sin(th1),np.cos(th1),0],\n",
    "               [0,0,1]))\n",
    "\n",
    "R1_2=np.array(([np.cos(th2),-np.sin(th2),0],\n",
    "               [np.sin(th2),np.cos(th2),0],\n",
    "               [0,0,1]))\n",
    "\n",
    "R2_3=np.array(([np.cos(th3),-np.sin(th3),0],\n",
    "               [np.sin(th3),np.cos(th3),0],\n",
    "               [0,0,1]))\n",
    "\n",
    "RL=[R0_1,R1_2,R2_3]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [],
   "source": [
    "d0_1=np.array(([0],\n",
    "               [0],\n",
    "               [0]))\n",
    "\n",
    "d1_2=np.array(([0],\n",
    "               [r2],\n",
    "               [r1]))\n",
    "\n",
    "d2_3=np.array(([0],\n",
    "               [r3],\n",
    "               [r4]))\n",
    "\n",
    "TL=[d0_1,d1_2,d2_3]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [],
   "source": [
    "P3=np.array(([0],\n",
    "             [0],\n",
    "             [r5]))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Manipulator Jacobian"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [],
   "source": [
    "num_links=3\n",
    "\n",
    "Output1=np.identity(4)\n",
    "for i in range(-1,-(len(RL)),-1):\n",
    "    Output1=np.dot(htrans(RL[i],TL[i]),Output1)\n",
    "\n",
    "d3_0=Output1[0:3,3]\n",
    "d3_0=d3_0.reshape(-1,1)\n",
    "\n",
    "def mani_jaco(Rot_List,Trans_List,num_links):\n",
    "    manipulator_jacobian=np.zeros((6,num_links))\n",
    "    Output=np.identity(4)\n",
    "    Output=np.dot(htrans(Rot_List[0],TL[0]),Output)\n",
    "    d1_0=Output[0:3,3]\n",
    "    K1=d3_0-d1_0\n",
    "    MJ=np.dot(Rot_List[0],np.array(([0],[0],[1]))).reshape(-1,1)\n",
    "    manipulator_jacobian[0:3,0]=np.cross(MJ[:, 0],K1[:, 0])\n",
    "    manipulator_jacobian[3:6, 0] = np.dot(Rot_List[0], np.array(([[0], [0], [1]]))).flatten()\n",
    "\n",
    "    R2_0=np.dot(RL[1],RL[0])\n",
    "    R2_0m=np.dot(R2_0,np.array(([0],[0],[1])))\n",
    "    for i in range(-2,-(len(Rot_List)),-1):\n",
    "        Output=np.dot(htrans(Rot_List[i],TL[i]),Output)\n",
    "    d2_0=Output[0:3,3]\n",
    "    d2_0=d2_0.reshape(-1,1)\n",
    "    K1=d3_0-d2_0\n",
    "    manipulator_jacobian[0:3,1]=np.cross(R2_0m[:, 0],K1[:, 0])\n",
    "    manipulator_jacobian[3:6,1] = np.dot(R2_0, np.array(([[0], [0], [1]]))).flatten()\n",
    "\n",
    "    R3_0=np.dot(RL[2],R2_0)\n",
    "    R3_0m=np.dot(R3_0,np.array(([0],[0],[1])))\n",
    "    for i in range(-1,-(len(Rot_List)),-1):\n",
    "        Output=np.dot(htrans(Rot_List[i],TL[i]),Output)\n",
    "    d1_0=Output[0:3,3]\n",
    "    d1_0=d1_0.reshape(-1,1)\n",
    "    K1=d3_0-P3\n",
    "    manipulator_jacobian[0:3,2]=np.cross(R3_0m[:, 0],K1[:, 0])\n",
    "    manipulator_jacobian[3:6,2] = np.dot(R3_0, np.array(([[0], [0], [1]]))).flatten()\n",
    "\n",
    "    print(manipulator_jacobian)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[-2.22424619 -1.22424619 -2.22424619]\n",
      " [-2.73883575 -2.73883575 -2.73883575]\n",
      " [ 0.          0.          0.        ]\n",
      " [ 0.          0.          0.        ]\n",
      " [ 0.          0.          0.        ]\n",
      " [ 1.          1.          1.        ]]\n"
     ]
    }
   ],
   "source": [
    "mani_jaco(RL,TL,num_links)"
   ]
  }
 ],
 "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.10.12"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}