Assign_2.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Position :[10.  0. -3.  1.]\n",
      "Jacobian :([[0.0, 10.0, 0.0], [0.0, 5.0, 0.0], [0, 0, 1]], [[0, 0, 1], [0, 0, 1], [0, 0, 1]])\n"
     ]
    }
   ],
   "source": [
    "### Code for SCARA both Position and Jacobian\n",
    "import numpy as np\n",
    "from numpy import pi, sin, cos, sqrt, absolute, arccos, arctan, sign\n",
    "import matplotlib.pyplot as plt\n",
    "a1,a2 = 5,5\n",
    "q = input(\"Enter the joint angles in degrees q1,q2 :\")\n",
    "d = float(input(\"Enter extension of end-effector :\"))\n",
    "ang1,ang2 = float(q.split(\",\")[0]) , float(q.split(\",\")[1])\n",
    "q1,q2 = ang1*3.14/180, ang2*3.14/180\n",
    "end_pos = [0,0,0,1]\n",
    "theta,alpha,dis,a = 0,0,0,0\n",
    "dh = [[cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), cos(theta)*a],\n",
    "[sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), sin(theta)*a],\n",
    "[0, sin(alpha), cos(alpha),d],\n",
    "[0,0,0,1]]\n",
    "t = [[cos(q1+q2),sin(q1+q2),0,a1*cos(q1)+a2*cos(q1+q2)],\n",
    "[sin(q1+q2),-cos(q1+q2),0,a1*sin(q1)+a2*sin(q1+q2)],\n",
    "[0,0,-1,-d],\n",
    "[0,0,0,1]]\n",
    "res = np.dot(t,end_pos)\n",
    "print(\"Position :\" +str(res) )\n",
    "z0,z1,z2 =np.array([0,0,1]),np.array([0,0,1]),np.array([0,0,1])\n",
    "o,o1,o2,o3 = np.array([0,0,0]),np.array([a1*cos(q1),a1*sin(q1),0]),np.array([a1*cos(q1)+a2*cos(q1+q2),a1*sin(q1)+ a2*sin(q1+q2),0]),np.array([a1*cos(q1)+a2*cos(q1+q2),a1*sin(q1)+ a2*sin(q1+q2),d])\n",
    "jacob = [np.transpose(np.cross(z0,o3-o)).tolist(),np.transpose(np.cross(z1,o3-o1)).tolist(),np.transpose(z2).tolist()],[np.transpose(z0).tolist(),np.transpose(z1).tolist(),np.transpose(z2).tolist()]\n",
    "print(\"Jacobian :\"+ str(jacob))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Code to get position of Stanford Manipulator"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0. 0. 0.]\n",
      "[0. 0. 0.]\n",
      "[0.00000000e+00 2.82887083e-20 3.00000000e+00]\n",
      "[0.00000000e+00 2.82887083e-20 3.00000000e+00 1.00000000e+00]\n"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "from numpy import pi, sin, cos, sqrt, absolute, arccos, arctan, sign\n",
    "import matplotlib.pyplot as plt\n",
    "a1,a2 = 5,5\n",
    "q = input(\"Enter the joint angles in degrees q1,q2 :\")\n",
    "d = float(input(\"Enter extension of end-effector :\"))\n",
    "ang1,ang2 = float(q.split(\",\")[0]) , float(q.split(\",\")[1])\n",
    "q1,q2 = ang1*3.14/180, ang2*3.14/180\n",
    "end_pos = [0,0,0,1]\n",
    "#theta,alpha,dis,a = 0,0,0,0\n",
    "def get_dh(theta,alpha,d,a):\n",
    "    dh = [[cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), cos(theta)*a],\n",
    "    [sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), sin(theta)*a],\n",
    "    [0, sin(alpha), cos(alpha),d],\n",
    "    [0,0,0,1]]\n",
    "    return dh\n",
    "qs = [q1,q2,0]\n",
    "ds = [0,0,d]\n",
    "aas = [0,0,0]\n",
    "alphas = [-3.14/2,3.14/2,0]\n",
    "t = [[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]]\n",
    "z0,z1,z2 =np.array([0,0,1]),np.array([0,0,1]),np.array([0,0,1])\n",
    "o = [[],[],[]]\n",
    "#t = get_dh(qs[0],alphas[0],ds[0],aas[0])\n",
    "#print(t)\n",
    "for i in range(0,3):\n",
    "    #t2 = get_dh(qs[i],alphas[i],ds[i],aas[i])\n",
    "    t = np.dot(t,get_dh(qs[i],alphas[i],ds[i],aas[i]))\n",
    "    o[i] = np.dot(t,[0,0,0,1])[0:3]\n",
    "    #print(o[i])\n",
    "    #print(t)\n",
    "res = np.dot(t,end_pos)\n",
    "print(res)\n",
    "# print([-cos(q1)*sin(q2)*d, sin(q1)*sin(q2)*d, -cos(q2)*d,1])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Code to get the jacobian of # link planar manipulator"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Jacob :([[-11.156448908138957, 9.335387362109374, 0.0], [-8.657598394923445, 5.004596890082059, 0.0], [-4.328799197461723, 2.5022984450410295, 0.0]], [[0, 0, 1], [0, 0, 1], [0, 0, 1]])\n"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "from numpy import pi, sin, cos, sqrt, absolute, arccos, arctan, sign\n",
    "\n",
    "a1,a2,a3 = 5,5,5\n",
    "q = input(\"Enter the joint angles in degrees q1,q2,q3 :\")\n",
    "\n",
    "ang1,ang2,ang3 = float(q.split(\",\")[0]) , float(q.split(\",\")[1]),float(q.split(\",\")[2])\n",
    "q1,q2,q3 = ang1*3.14/180, ang2*3.14/180,ang3*3.14/180\n",
    "end_pos = [0,0,0,1]\n",
    "#theta,alpha,dis,a = 0,0,0,0\n",
    "def get_dh(theta,alpha,d,a):\n",
    "    dh = [[cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), cos(theta)*a],\n",
    "    [sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), sin(theta)*a],\n",
    "    [0, sin(alpha), cos(alpha),d],\n",
    "    [0,0,0,1]]\n",
    "    return dh\n",
    "qs = [q1,q2,q3]\n",
    "ds = [0,0,0]\n",
    "aas = [a1,a2,a3]\n",
    "alphas = [0,0,0]\n",
    "t = [[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]]\n",
    "z0,z1,z2 =np.array([0,0,1]),np.array([0,0,1]),np.array([0,0,1])\n",
    "o = [[],[],[]]\n",
    "o0 = [0,0,0]\n",
    "for i in range(0,3):\n",
    "    t = np.dot(t,get_dh(qs[i],alphas[i],ds[i],aas[i]))\n",
    "    o[i] = np.dot(t,[0,0,0,1])[0:3]\n",
    "jacob = [np.transpose(np.cross(z0,o[2]-o0)).tolist(),np.transpose(np.cross(z1,o[2]-o[0])).tolist(),np.transpose(np.cross(z1,o[2]-o[1])).tolist()],[np.transpose(z0).tolist(),np.transpose(z1).tolist(),np.transpose(z2).tolist()]\n",
    "print(\"Jacob :\" + str(jacob))"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.9.1 64-bit",
   "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.1"
  },
  "orig_nbformat": 4,
  "vscode": {
   "interpreter": {
    "hash": "f5b775166e527c2c4c8c7a313eadde439897f6893f91e429bbc029ec9a09e8d5"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}