ITR_Assignment2_Vignesh_Ramakrishnan_21310037.ipynb

{
  "nbformat": 4,
  "nbformat_minor": 0,
  "metadata": {
    "colab": {
      "name": "ITR_Assignment2_Vignesh_Ramakrishnan_21310037.ipynb",
      "provenance": [],
      "collapsed_sections": []
    },
    "kernelspec": {
      "name": "python3",
      "display_name": "Python 3"
    },
    "language_info": {
      "name": "python"
    }
  },
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "cPceD7rLVWsf"
      },
      "source": [
        "### Assignment 2 ITR Vignesh Ramakrishnan 21310037"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "sTjvMOJ-Lz_9"
      },
      "source": [
        "#Vignesh Ramakrishnan\n",
        "#21310037\n",
        "#Importing libraries\n",
        "%matplotlib inline\n",
        "import numpy as np\n",
        "import matplotlib.pyplot as plt\n",
        "import scipy.integrate as integrate \n",
        "import matplotlib.animation as animation\n",
        "from matplotlib.animation import FuncAnimation\n",
        "from matplotlib import rc\n",
        "from IPython.display import HTML\n",
        "from numpy import sin, cos,arctan,arccos\n",
        "from collections import deque"
      ],
      "execution_count": 9,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "QIJo1iAhMbgR"
      },
      "source": [
        "#Some fixed values for our system\n",
        "\n",
        "#lengths\n",
        "l1 = 0.5\n",
        "l2 = 0.5\n",
        "l3 = 0.5\n",
        "#jointangles\n",
        "theta1 = 0\n",
        "theta2 = 0\n",
        "\n",
        "#Prismatic extension\n",
        "dz3 = 0\n",
        "\n",
        "#masses\n",
        "m1 = 1\n",
        "m2 = 2\n",
        "m3 = 3\n",
        "#acceleration due to gravity\n",
        "g  = 9.8\n",
        "\n"
      ],
      "execution_count": 10,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "2s--HESbLLt0"
      },
      "source": [
        "\n",
        "#Rotational matrix along x\n",
        "def RotX(theta):\n",
        "  return np.array([[1,0,0,0][0,np.cos(theta),-np.sin(theta),0],[0,np.sin(theta),np.cos(theta),0],[0,0,0,1]])\n",
        "\n",
        "\n",
        "#Rotational matrix along y\n",
        "def RotY(theta):\n",
        "  return np.array([[np.cos(theta),0,-np.sin(theta),0],[0,1,0,0],[np.sin(theta),0,np.cos(theta),0],[0,0,0,1]])\n",
        "\n",
        "\n",
        "#Rotational matrix along z\n",
        "def RotZ(theta):\n",
        "  return np.array([[np.cos(theta),-np.sin(theta),0,0],[np.sin(theta),np.cos(theta),0,0],[0,0,1,0],[0,0,0,1]])\n",
        "\n",
        "\n",
        "#Translational matrix along x\n",
        "def TransX(dist):\n",
        "  return np.array([[1,0,0,dist],[0,1,0,0],[0,0,1,0],[0,0,0,1]])\n",
        "\n",
        "#Translational matrix along y\n",
        "def TransY(dist):\n",
        "  return np.array([[1,0,0,0],[0,1,0,dist],[0,0,1,0],[0,0,0,1]])\n",
        "\n",
        "#Translational matrix along z\n",
        "def TransZ(dist):\n",
        "  return np.array([[1,0,0,0],[0,1,0,0],[0,0,1,dist],[0,0,0,1]])"
      ],
      "execution_count": 11,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "a_cZpHMbKnj3"
      },
      "source": [
        "## Task 7: Position of end effector for RRP SCARA manipulator"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "D94HB9nDPQ0t"
      },
      "source": [
        "def RRP_SCARA_EE_Positioner(theta1,theta2,dz3):\n",
        "  P3mat = np.transpose(np.array([0,0,-dz3,1]))\n",
        "  P2mat=np.mult(np.add(RotZ(theta2),TransX(l3)),P3mat)\n",
        "  P1mat=np.mult(np.add(RotZ(np.pi),TransX(l2)),P2mat)\n",
        "  P0mat=np.mult(np.add(RotZ(theta1),TransZ(l1)),P1mat)\n",
        "  return P0mat"
      ],
      "execution_count": 12,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "Z5xTksV4X5pf"
      },
      "source": [
        "## Task 8: Position of end effector for RRP Stanford manipulator"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "cG-ZvOWQYLOl"
      },
      "source": [
        "def RRP_Stanford_EE_Positioner(theta1,theta2,dy2):\n",
        "  P2mat = np.transpose(np.array([0,dy2,0,1]))\n",
        "  P1mat=np.mult(np.add(RotX(theta2),TransX(-l2)),P2mat)\n",
        "  P0mat=np.mult(np.add(RotZ(theta1),TransZ(l1)),P1mat)\n",
        "  return P0mat"
      ],
      "execution_count": 2,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "4aIliG4FKCoK"
      },
      "source": [
        "## Task 12: Jacobian for RRP SCARA manipulator"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "zs0RLaE6KKqv"
      },
      "source": [
        "def RRP_SCARA_Jacobian(theta1,theta2):\n",
        "  thetas=theta1+theta2\n",
        "  return np.array([-l2*np.sin(theta1)-l3*np.sin(thetas),-l3*np.sin(thetas),0,0],[l2*np.cos(theta1)+l3*np.cos(thetas),l3*np.cos(thetas),0,0],[0,0,-1,0],[0,0,0,0],[0,0,0,0],[1,1,0,-1])\n"
      ],
      "execution_count": 1,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "ufLtrpaKNPah"
      },
      "source": [
        "## Task 14: Jacobian for RRR manipulator"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "BHxen5JyNZgJ"
      },
      "source": [
        "def RRR_Jacobian(theta1,theta2,theta3):\n",
        "  thetas1=theta1+theta2\n",
        "  thetas2=theta1+theta2+theta3\n",
        "  return np.array([-l1*np.sin(theta1),-l2*np.sin(thetas1),-l3*np.sin(thetas2),0],[l1*np.cos(theta1),l2*np.cos(thetas2),l3*np.cos(thetas3),0,0],[0,0,0,0],[0,0,0,0],[0,0,0,0],[1,1,1,1])\n"
      ],
      "execution_count": 3,
      "outputs": []
    }
  ]
}