Equations.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "import math"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here I put our constants"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "thetaMB = 0\n",
    "thetaME = 70\n",
    "thetaMS = 70\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "ratio = 4.09 #45/21\n",
    "Al = 182\n",
    "Au = 188\n",
    "L2 = 73 \n",
    "Lo = 47\n",
    "L1o = 40\n",
    "Zo = -70\n",
    "SHOULDEROFFSET = 0 #142.0\n",
    "ELBOWOFFSET = 0 #45.0"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "FWD Kinematics"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "def FW(thetaMB, thetaME, thetaMS):\n",
    "    theta1 = thetaMB * (1/ratio)\n",
    "    theta2 = (thetaME - thetaMS) * (1/ratio)\n",
    "    theta3 = thetaMS * (1/ratio)\n",
    "\n",
    "    theta1 = theta1 #* 3.1415 / 180.0# To radians\n",
    "    theta2 = theta2 #* 3.1415 / 180.0# To radians\n",
    "    theta3 = theta3 #* 3.1415 / 180.0# To radians\n",
    "\n",
    "    z=L2 + Al*math.sin(theta2) - Au*math.cos(theta3 - (90 - theta2)) + Zo\n",
    "    k1 = (math.sin(theta3 - (90 - theta2)))*Au + Al*math.cos(theta2)+Lo + L1o\n",
    "    x=math.cos(theta1)*k1\n",
    "    y=math.sin(theta1)*k1\n",
    "    print(x,y,z, theta1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "def INV(x,y,z, theta1):\n",
    "    x = x - math.cos(theta1)*L1o\n",
    "    y = y - math.sin(theta1)*L1o\n",
    "    z = z - Zo - L2\n",
    "    theta1 = math.atan2(y,x)\n",
    "    L1 = math.sqrt(x*x + y*y) - Lo\n",
    "    L7 = math.sqrt(L1*L1 + z*z)\n",
    "    a = z/L7\n",
    "    b = (L7*L7 + Al*Al - Au*Au)/(2*L7*Al)\n",
    "    c = (Al*Al + Au*Au - L7*L7)/(2*Al*Au)\n",
    "    print(c)\n",
    "    theta2 = math.atan2(a,math.sqrt(1-a*a)) + math.atan2(math.sqrt(1-a*a),b)\n",
    "    theta3 = math.atan2(math.sqrt(1-c*c),c)\n",
    "    theta1 = theta1 #*  180.0/  3.1415; # To degree\n",
    "    theta2 = (theta2-ELBOWOFFSET) #*  180.0/  3.1415; # To degree\n",
    "    theta3 = (theta3 - ELBOWOFFSET) #*  180.0/  3.1415; # To degree\n",
    "\n",
    "    thetaMB = theta1 * ratio\n",
    "    thetaME = (theta2 +theta3) * ratio\n",
    "    thetaMS = theta3 * ratio\n",
    "    print(thetaMB, thetaME, thetaMS)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "100.92862724309512 0.0 87.23783983228398 0.0\n"
     ]
    }
   ],
   "source": [
    "FW(0,0,0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "0.9910647591769933\n",
      "0.0 12.259603422922726 0.5471612651906901\n"
     ]
    }
   ],
   "source": [
    "INV(100.92,0,24.3, 0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 37,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "0.10554332769111344\n",
      "346.22103489331784 -10673.508959894269 -5345.233106663916\n"
     ]
    }
   ],
   "source": [
    "INV(10,10,250 , 0)"
   ]
  }
 ],
 "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.9.13 (tags/v3.9.13:6de2ca5, May 17 2022, 16:36:42) [MSC v.1929 64 bit (AMD64)]"
  },
  "orig_nbformat": 4,
  "vscode": {
   "interpreter": {
    "hash": "e83f520a7929674eccf0ac68acce5e3c32e93178a10045b372d8f5e66f90ae45"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}