{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 326,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Clears the variables\n",
    "from IPython import get_ipython\n",
    "get_ipython().magic('reset -sf')\n",
    "\n",
    "import math as mt\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Transfromation matrix from real sensors to virtual sensors\n",
    "The real sensors are the LVDTs H1, H2 and H3 and the virtual ones measure along are the L, T and Y coordinates."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 327,
   "metadata": {},
   "outputs": [],
   "source": [
    "to_rad = mt.pi / 180\n",
    "\n",
    "######################################\n",
    "# The values of alpha1 and r immediately below are Mark's values,\n",
    "# written here for comparison of the result.\n",
    "# I don't know where he got them.\n",
    "########################################\n",
    "# alpha1 = 22.9 * to_rad  # In radians. \n",
    "# r = 599.26e3 # In micrometers.\n",
    "\n",
    "######################################\n",
    "# The values of alpha1 and r immediately below were read from Hirata-san's drawing.\n",
    "########################################\n",
    "r = 601.31e3 # In micrometers.\n",
    "alpha1 = 21.05 * to_rad        # In radians\n",
    "alpha2 = alpha1 + 120 * to_rad # alpha1 is already in radians\n",
    "alpha3 = alpha1 + 240 * to_rad # alpha1 is already in radians\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 328,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "sen_V2R =\n",
      " [[ 9.33267336e-01  3.59182516e-01  6.01310000e+05]\n",
      " [-7.77694851e-01  6.28641964e-01  6.01310000e+05]\n",
      " [-1.55572485e-01 -9.87824479e-01  6.01310000e+05]]\n"
     ]
    }
   ],
   "source": [
    "# Transformation matrix from virtual sensors (L,T,Y) to real sensors (H1,H2,H3).\n",
    "# Units: H1, H2, H3, L and T are in micrometers and Y is in radians.\n",
    "sen_V2R = np.array( [ [ mt.cos(alpha1), mt.sin(alpha1) , r ] , \n",
    "                  [ mt.cos(alpha2), mt.sin(alpha2) , r ] ,\n",
    "                  [ mt.cos(alpha3), mt.sin(alpha3) , r ] ] )\n",
    "print( 'sen_V2R =\\n' , sen_V2R )"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 329,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "sen_R2V =\n",
      " [[ 6.22178224e-01 -5.18463234e-01 -1.03714990e-01]\n",
      " [ 2.39455010e-01  4.19094642e-01 -6.58549653e-01]\n",
      " [ 5.54345235e-07  5.54345235e-07  5.54345235e-07]] \n",
      "\n",
      "This should be the unity matrix:\n",
      " [[ 1.00000000e+00 -4.43959282e-17 -3.52837759e-11]\n",
      " [ 1.10622849e-16  1.00000000e+00 -3.49218432e-11]\n",
      " [-1.06379511e-22  5.07979643e-23  1.00000000e+00]]\n"
     ]
    }
   ],
   "source": [
    "# The inverse is calculated\n",
    "sen_R2V = np.linalg.inv( sen_V2R ) \n",
    "\n",
    "print( 'sen_R2V =\\n' ,  sen_R2V , '\\n') \n",
    "\n",
    "print( 'This should be the unity matrix:\\n', np.dot( sen_R2V , sen_V2R ) )"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 330,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Yrad2urad = \n",
      " [[1.e+00 0.e+00 0.e+00]\n",
      " [0.e+00 1.e+00 0.e+00]\n",
      " [0.e+00 0.e+00 1.e+06]] \n",
      "\n",
      "\n",
      "The transformation matrix to write in the medm screen is:\n",
      "\n",
      "LVDT2EUL = sen_R2V_medm = \n",
      " [[ 0.62217822 -0.51846323 -0.10371499]\n",
      " [ 0.23945501  0.41909464 -0.65854965]\n",
      " [ 0.55434524  0.55434524  0.55434524]] \n",
      "\n"
     ]
    }
   ],
   "source": [
    "# Yaw has the units of radians but we want to use microradians, therefore,\n",
    "# an additional matrix has to be used for this unit conversion.\n",
    "\n",
    "Yrad2urad = np.array([ [1,0,0] , [0,1,0] , [0,0,1e6] ])\n",
    "print( 'Yrad2urad = \\n' , Yrad2urad , '\\n')\n",
    "\n",
    "print('\\nThe transformation matrix to write in the medm screen is:\\n')\n",
    "sen_R2V_medm = np.dot( Yrad2urad , sen_R2V ) ;\n",
    "print( 'LVDT2EUL = sen_R2V_medm = \\n' , sen_R2V_medm , '\\n' )"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Transfromation matrix from virtual actuators to real actuators\n",
    "The real actuators are the coil-magnet actuators incorporated into the same units as the LVDTs and they are also referred to as H1, H2 and H3. The virtual actuators actuate along the L, T and Y coordinates."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 331,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Variable names are cleared in order to avoid errors in copy-paste-change-names procedure\n",
    "get_ipython().magic('reset -sf')\n",
    "import math as mt\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 332,
   "metadata": {},
   "outputs": [],
   "source": [
    "to_rad = mt.pi / 180\n",
    "\n",
    "######################################\n",
    "# The values of alpha1 and r immediately below are Mark's values,\n",
    "# written here for comparison of the result.\n",
    "# I don't know where he got them.\n",
    "########################################\n",
    "# R = 597.31e3 # In micrometers.\n",
    "# beta1 = 32.94 * to_rad       # In radians\n",
    "\n",
    "######################################\n",
    "# The values of alpha1 and r immediately below were read from Hirata-san's drawing.\n",
    "########################################\n",
    "R = 594.23e3 # In micrometers.\n",
    "beta1 = 31.59 * to_rad       # In radians\n",
    "beta2 = beta1 + 120 * to_rad # beta1 is already in radians\n",
    "beta3 = beta1 + 240 * to_rad # beta1 is already in radians\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 333,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "act_V2R =\n",
      " [[ 8.51818374e-01  5.23837244e-01  5.94230000e+05]\n",
      " [-8.79565547e-01  4.75777729e-01  5.94230000e+05]\n",
      " [ 2.77471734e-02 -9.99614973e-01  5.94230000e+05]]\n"
     ]
    }
   ],
   "source": [
    "# Transformation matrix from virtual sensors (L,T,Y) to real sensors (H1,H2,H3).\n",
    "# Units: H1, H2, H3, L and T are in micrometers and Y is in radians.\n",
    "act_V2R = np.array( [ [ mt.cos(beta1), mt.sin(beta1) , R ] , \n",
    "                      [ mt.cos(beta2), mt.sin(beta2) , R ] ,\n",
    "                      [ mt.cos(beta3), mt.sin(beta3) , R ] ] )\n",
    "print( 'act_V2R =\\n' , act_V2R )"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 334,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Yurad2rad = \n",
      " [[1.e+00 0.e+00 0.e+00]\n",
      " [0.e+00 1.e+00 0.e+00]\n",
      " [0.e+00 0.e+00 1.e-06]] \n",
      "\n",
      "\n",
      "The transformation matrix to write in the medm screen is:\n",
      "\n",
      "EUL2COIL = act_V2R_medm = \n",
      " [[ 0.85181837  0.52383724  0.59423   ]\n",
      " [-0.87956555  0.47577773  0.59423   ]\n",
      " [ 0.02774717 -0.99961497  0.59423   ]] \n",
      "\n"
     ]
    }
   ],
   "source": [
    "Yurad2rad = np.array([ [1,0,0] , [0,1,0] , [0,0,1e-6] ])\n",
    "print( 'Yurad2rad = \\n' , Yurad2rad , '\\n')\n",
    "\n",
    "print('\\nThe transformation matrix to write in the medm screen is:\\n')\n",
    "act_V2R_medm = np.dot( act_V2R , Yurad2rad) ;\n",
    "print( 'EUL2COIL = act_V2R_medm = \\n' , act_V2R_medm , '\\n' )"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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.7.7"
  },
  "toc": {
   "base_numbering": 1,
   "nav_menu": {},
   "number_sections": true,
   "sideBar": true,
   "skip_h1_title": false,
   "title_cell": "Table of Contents",
   "title_sidebar": "Contents",
   "toc_cell": false,
   "toc_position": {},
   "toc_section_display": true,
   "toc_window_display": false
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}