usage/kinematics_example.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "%run base.ipynb"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0.943714, 0.127679, -0.144878, 0.268536]\n",
      "[0.174579, -0.34912, 0.523523]\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "0"
      ]
     },
     "execution_count": 2,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# unit quaternion from euler angles\n",
    "\n",
    "euler2qf = Function('euler2q', [eul], [euler2q(eul)])\n",
    "phi_f = 10*(pi/180)\n",
    "psi_f = 30*(pi/180)\n",
    "theta_f = -20*(pi/180)\n",
    "print(euler2qf([phi_f, theta_f, psi_f]))\n",
    "\n",
    "\n",
    "\n",
    "# euler angles from a unit quaternion\n",
    "\n",
    "q2eulerf = Function('q2euler', [uq], [q2euler(uq)])\n",
    "q_f = np.array([0.9437, 0.1277, -0.1449, 0.2685])\n",
    "print(q2eulerf(q_f))\n",
    "\n",
    "\n",
    "# c , cpp or matlab code generation for forward dynamics\n",
    "euler2qf.generate(\"euler2qf.c\")\n",
    "os.system(f\"gcc -fPIC -shared euler2qf.c -o libeuler2qf.so\")\n",
    "\n",
    "q2eulerf.generate(\"q2eulerf.c\")\n",
    "os.system(f\"gcc -fPIC -shared q2eulerf.c -o libq2eulerf.so\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<h1>Kinematics ned Transformation Matrix</h1>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "@1=0, \n",
      "[[(cos(psi)*cos(thet)), (sin(psi)*cos(thet)), (-sin(thet)), @1, @1, @1], \n",
      " [(((cos(psi)*sin(thet))*sin(phi))-(sin(psi)*cos(phi))), ((cos(psi)*cos(phi))+((sin(phi)*sin(thet))*sin(psi))), (cos(thet)*sin(phi)), @1, @1, @1], \n",
      " [((sin(psi)*sin(phi))+((cos(psi)*cos(phi))*sin(thet))), (((sin(thet)*sin(psi))*cos(phi))-(cos(psi)*sin(phi))), (cos(thet)*cos(phi)), @1, @1, @1], \n",
      " [@1, @1, @1, 1, 00, (-sin(thet))], \n",
      " [@1, @1, @1, 00, cos(phi), (cos(thet)*sin(phi))], \n",
      " [@1, @1, @1, 00, (-sin(phi)), (cos(thet)*cos(phi))]]\n"
     ]
    }
   ],
   "source": [
    "Kinematics = kin()\n",
    "Jk = Kinematics.J\n",
    "J_inv = Kinematics.J_INV\n",
    "print(J_inv)\n",
    "J_inv_func = Function('J_inv', [eul], [J_inv]) # for numerical & symbolic use\n",
    "\n",
    "dJk = Kinematics.J_dot\n",
    "dJk_func = Function('dJk', [eul, deul, x_nb], [dJk]) # for numerical & symbolic use\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 0.98066095,  0.09839429, -0.16918235,  0.        ,  0.        ,\n",
       "         0.        ],\n",
       "       [-0.07716877,  0.98879774,  0.12776544,  0.        ,  0.        ,\n",
       "         0.        ],\n",
       "       [ 0.17985851, -0.11223898,  0.9772683 ,  0.        ,  0.        ,\n",
       "         0.        ],\n",
       "       [ 0.        ,  0.        ,  0.        ,  1.        ,  0.        ,\n",
       "        -0.16918235],\n",
       "       [ 0.        ,  0.        ,  0.        ,  0.        ,  0.99156189,\n",
       "         0.12776544],\n",
       "       [ 0.        ,  0.        ,  0.        ,  0.        , -0.12963414,\n",
       "         0.9772683 ]])"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# example usage of J mat\n",
    "jinv = np.array(J_inv_func([0.13,0.17,0.1]))\n",
    "jinv"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<h1>state discretization</h1>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "euler response [0.374894, 0.172823, 0.179304]\n",
      "quaternion response [0.97633, 0.177319, 0.101042, 0.0716117]\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "DM(\n",
       "[[1, 3.91182e-19, 2.28221e-18], \n",
       " [3.91182e-19, 1, 1.2201e-17], \n",
       " [2.28221e-18, 1.2201e-17, 1]])"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# state transformations\n",
    "J_ = Kinematics.J\n",
    "Jq_ = Kinematics.Jq\n",
    "\n",
    "J_func = Function('J_', [eul], [J_])    \n",
    "Jq_func = Function('Jq_', [uq], [Jq_])\n",
    "\n",
    "vr_v = np.array([0, 0.2, 0.0, 0.5, 0, 0.1]) # linear and angular velocity\n",
    "r = R.from_euler(\"ZYX\", (10, 10, 20), degrees=True)     # rotation matrix\n",
    "eul_v = r.as_euler(\"ZYX\")   # euler angles\n",
    "uq_v = r.as_quat()        # unit quaternion\n",
    "\n",
    "# reverse scipy convention\n",
    "def q_reverse(q):\n",
    "    x, y, z, w = q\n",
    "    return np.array([w, x, y, z])\n",
    "\n",
    "def eul_reverse(eul):\n",
    "    phi, thet, psi = eul\n",
    "    return np.array([psi, thet, phi])\n",
    "\n",
    "\n",
    "Jo = J_func(eul_reverse(eul_v))\n",
    "Jqo = Jq_func(q_reverse(uq_v))  \n",
    "\n",
    "To = Jo[3:,3:]\n",
    "Tq = Jqo[3:,3:]\n",
    "\n",
    "# state discrete time propagation in euler\n",
    "h = 0.05\n",
    "wk_next = eul_reverse(eul_v) + h*To@vr_v[3:]\n",
    "\n",
    "# state discrete time propagation and normalization in unit quaternion\n",
    "qk_next = q_reverse(uq_v)+ h*Tq@vr_v[3:]\n",
    "norm_qk_next = qk_next/sqrt(qk_next.T@qk_next)\n",
    "\n",
    "\n",
    "# response\n",
    "print(f'euler response {wk_next}')\n",
    "print(f'quaternion response {norm_qk_next}')\n",
    "\n",
    "\n",
    "#checking for property 8.17 --> eye(3)\n",
    "4*(Tq.T@Tq) # should be identity matrix"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(array([0.97632633, 0.17731887, 0.10107375, 0.0716171 ]),\n",
       " array([0.97633009, 0.17731853, 0.1010419 , 0.07161171]))"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# compare euler and quaternion results\n",
    "rr = R.from_euler(\"ZYX\", eul_reverse(wk_next.full().flatten()), degrees=False)\n",
    "q_reverse(rr.as_quat()) , norm_qk_next.full().flatten()\n",
    "\n",
    "# as expected both quaternion and euler produce same response"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# # c , cpp or matlab code generation for forward dynamics\n",
    "# J_func.generate(\"J_func.c\")\n",
    "# os.system(f\"gcc -fPIC -shared J_func.c -o libJk.so\")"
   ]
  },
  {
   "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.9.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}