usage/dynamic_matrices_example.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "%run base.ipynb"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<h1>System Inertia Matrix example</h1>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "inertia_mat = uv_body.body_inertia_matrix()\n",
    "M_func = Function('M_b', [m, I_o, z_g, decoupled_added_m, coupled_added_m], [inertia_mat]) # for numerical & symbolic use\n",
    "\n",
    "# M_func.save('M.casadi')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[17.  ,  0.  ,  0.  ,  0.  ,  0.23,  0.  ],\n",
       "       [ 0.  , 24.2 ,  0.  , -0.23,  0.  ,  0.  ],\n",
       "       [ 0.  ,  0.  , 26.07,  0.  ,  0.  ,  0.  ],\n",
       "       [ 0.  , -0.23,  0.  ,  0.28,  0.  ,  0.  ],\n",
       "       [ 0.23,  0.  ,  0.  ,  0.  ,  0.28,  0.  ],\n",
       "       [ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.28]])"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# example usage of inertia mat\n",
    "M_mat = np.array(M_func(blue.m, blue.Io, blue.rg[2], blue.added_m, blue.coupl_added_m))\n",
    "M_mat"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ True,  True,  True,  True,  True,  True],\n",
       "       [ True,  True,  True,  True,  True,  True],\n",
       "       [ True,  True,  True,  True,  True,  True],\n",
       "       [ True,  True,  True,  True,  True,  True],\n",
       "       [ True,  True,  True,  True,  True,  True],\n",
       "       [ True,  True,  True,  True,  True,  True]])"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "M_mat.T == M_mat"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "inertia_ned_mat = uv_ned.ned_euler_inertia_matrix()\n",
    "Onb = vertcat(thet, phi, psi)\n",
    "M_ned_func = Function('M_ned', [m, I_o, z_g, Onb, decoupled_added_m, coupled_added_m], [inertia_ned_mat]) # for numerical & symbolic use"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 1.72303360e+01, -7.45090577e-01,  1.18405662e+00,\n",
       "         1.76115723e-02,  2.23648786e-01,  3.57838329e-02],\n",
       "       [-7.45090577e-01,  2.41747103e+01, -1.91929773e-01,\n",
       "        -2.26055609e-01,  2.24397276e-02,  3.31239544e-02],\n",
       "       [ 1.18405662e+00, -1.91929773e-01,  2.58649536e+01,\n",
       "        -3.85835972e-02, -2.93860503e-02,  0.00000000e+00],\n",
       "       [ 1.76115723e-02, -2.26055609e-01, -3.85835972e-02,\n",
       "         2.80000000e-01,  0.00000000e+00, -3.62975599e-02],\n",
       "       [ 2.23648786e-01,  2.24397276e-02, -2.93860503e-02,\n",
       "         0.00000000e+00,  2.80000000e-01,  0.00000000e+00],\n",
       "       [ 3.57838329e-02,  3.31239544e-02,  0.00000000e+00,\n",
       "        -3.62975599e-02,  0.00000000e+00,  2.80000000e-01]])"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# example usage of inertia mat in ned\n",
    "Onb_v = np.array([0.13,0.17,0.1])\n",
    "np.array(M_ned_func(blue.m, blue.Io, blue.rg[2], Onb_v,  blue.added_m, blue.coupl_added_m))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<h1>Coriolis Matrix example</h1>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "coriolis_mat = uv_body.body_coriolis_centripetal_matrix()\n",
    "C_func = Function('C_b', [m, I_o, z_g, decoupled_added_m, coupled_added_m, x_nb, v_c], [coriolis_mat]) # for numerical & symbolic use"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "C = np.array(C_func(blue.m, blue.Io, blue.rg[2], blue.added_m, blue.coupl_added_m, np.array([0.1 ,0.2,0.1,0.1,0.1,0.5]), \n",
    "                    np.array([0.0 ,0.0,0.0,0.0,0.0,0.0])))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ True,  True,  True,  True,  True,  True],\n",
       "       [ True,  True,  True,  True,  True,  True],\n",
       "       [ True,  True,  True,  True,  True,  True],\n",
       "       [ True,  True,  True,  True,  True,  True],\n",
       "       [ True,  True,  True,  True,  True,  True],\n",
       "       [ True,  True,  True,  True,  True,  True]])"
      ]
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "-C.T == C"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<h1>Restoring Forces</h1>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "res_G = uv_body.body_restoring_vector()\n",
    "res_G_func = Function('G_n', [z, eul, W, B, r_g, r_b], [res_G]) # for numerical & symbolic use"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "# # c , cpp or matlab code generation for forward dynamics\n",
    "# res_G_func.generate(\"res_G_func.c\")\n",
    "# os.system(f\"gcc -fPIC -shared res_G_func.c -o libg.so\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "0"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "bias_force = uv_body.get_bias()\n",
    "bias_force_func = Function('Bias_b', [z, eul, x_nb ,sim_p, f_ext], [bias_force])\n",
    "# bias_force_func.save('C.casadi')\n",
    "\n",
    "# # c , cpp or matlab code generation for dynamics instance\n",
    "# bias_force_func.generate(\"C_uv_vec.c\")\n",
    "# os.system(f\"gcc -fPIC -shared C_uv_vec.c -o libBias_vehicle.so\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "0"
      ]
     },
     "execution_count": 13,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "M_mat_func = Function('M_b', [sim_p], [inertia_mat]) # for numerical & symbolic use\n",
    "# M_mat_func.save('M.casadi')\n",
    "\n",
    "# # c , cpp or matlab code generation for dynamics instance\n",
    "# M_mat_func.generate(\"M_uv_matrix.c\")\n",
    "# os.system(f\"gcc -fPIC -shared M_uv_matrix.c -o libMass_vehicle.so\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "\n",
    "\n",
    "\n"
   ]
  }
 ],
 "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.10.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}