{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "22855c48",
   "metadata": {},
   "source": [
    "# Flexible Two-Link Pendulum from FEMBridge H5 File\n",
    "This notebook walks you through creating a simple two-link pendulum, where both links in the pendulum are flexible bodies."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ec2eee2a",
   "metadata": {},
   "source": [
    "Complete the [basic two-link pendulum](../example_2_link_pendulum/notebook.ipynb) before working through this example. We use the same simulation setup, but replace both rigid links with flexible beams imported from a FEMBridge H5 file.\n",
    "\n",
    "![](../resources/nb_images/flexible_double_pendulum_fembridge_h5.png)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "34fa4f1c",
   "metadata": {},
   "outputs": [],
   "source": [
    "import atexit\n",
    "from pathlib import Path\n",
    "\n",
    "import numpy as np\n",
    "import Karana.Dynamics as kd\n",
    "import Karana.Dynamics.SOAFlexDyn_types as kdft\n",
    "import Karana.Integrators as ki\n",
    "import Karana.Models as kmdl\n",
    "from Karana.KUtils.Sim import Sim\n",
    "from Karana.Math.Kquantities import ureg\n",
    "\n",
    "beam_file = Path(\"..\") / \"resources\" / \"2_link_pendulum\" / \"flexible_beam.h5\""
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0277d99d",
   "metadata": {},
   "source": [
    "## Create the Simulation\n",
    "\n",
    "As in the [basic example](../example_2_link_pendulum/notebook.ipynb), use {py:class}`Karana.KUtils.Sim.Sim` to manage the {py:class}`multibody <Karana.Dynamics.Multibody>`, {py:class}`state propagator <Karana.Dynamics.StatePropagator>`, and {py:class}`frame container <Karana.Frame.FrameContainer>`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "7b4af108",
   "metadata": {},
   "outputs": [],
   "source": [
    "sim: Sim = Sim()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a185a591",
   "metadata": {},
   "source": [
    "## Read the FEMBridge Beam\n",
    "\n",
    "{py:class}`Karana.Dynamics.SOAFlexDyn_types.ReadFEMBridgePhysicalModalBody` imports flexible-body data from a FEMBridge H5 file. The original FEM analysis supplies the beam's mass properties, modal frequencies, and mode shapes. We reuse this model for both links.\n",
    "\n",
    "The beam used in this simulation is extremely flexible. This is to help make the deformation easily visible. \n",
    "\n",
    "The {py:class}`Karana.Dynamics.SOAFlexDyn_types.FEMBridgePhysicalModalBodyDS` configuration specifies the file's mass and length units, pnode grid point, damping, and sensor nodes. `p_node_grid=1` selects FEM grid node 1 for the pnode; this is the node on `link_1` used for the hinge that attaches it to its parent body. The imported definition places this joint node at location of grid point 1 in the original FEM file and uses the corresponding translational and rotational nodal matrices, which gives the joint the coupling needed to produce the correct flexible dynamics.\n",
    "\n",
    "`damping=0.05` specifies a 5% modal damping ratio. The sensor-node mapping creates nodes along the beam that can be used to visualize its flexible deformation."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "944c917f",
   "metadata": {},
   "outputs": [],
   "source": [
    "beam_reader = kdft.ReadFEMBridgePhysicalModalBody(\n",
    "    kdft.FEMBridgePhysicalModalBodyDS(\n",
    "        filename=beam_file,\n",
    "        mass_unit=ureg.kg,\n",
    "        length_unit=ureg.m,\n",
    "        p_node_grid=1,\n",
    "        damping=0.05,\n",
    "        sensor_nodes={node: f\"sn_{node}\" for node in range(1, 52, 3)},\n",
    "    )\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b4ce7a33",
   "metadata": {},
   "source": [
    "## Create the First Link\n",
    "\n",
    "Create a revolute hinge about y-axis, as in the [basic example](../example_2_link_pendulum/notebook.ipynb). {py:meth}`getModalBodyDS() <Karana.Dynamics.SOAFlexDyn_types.ReadFEMBridgePhysicalModalBody.getModalBodyDS>` combines the {py:class}`PhysicalHingeParams <Karana.Dynamics.PhysicalHingeParams>` with the imported beam model to create a {py:class}`ModalBodyDS <Karana.Dynamics.SOAFlexDyn_types.ModalBodyDS>`: this now contains a complete flexible-body definition, including all the information need to connect it to its parent body. The {py:meth}`toBody() <Karana.Dynamics.SOAFlexDyn_types.ModalBodyDS.toBody>` method creates the actual flexible-body and attaches it to its parent (the virtual root in this case)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "66cb4905",
   "metadata": {},
   "outputs": [],
   "source": [
    "link_1_ds = beam_reader.getModalBodyDS(\n",
    "    \"link_1\",\n",
    "    kd.PhysicalHingeParams(\n",
    "        hinge_type=kd.HingeType.REVOLUTE,\n",
    "        subhinge_params=[kd.PinSubhingeParams(unit_axis=np.array([0.0, 1.0, 0.0]))],\n",
    "    ),\n",
    ")\n",
    "link_1: kd.PhysicalModalBody = link_1_ds.toBody(sim.mb.virtualRoot())"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "274883c9",
   "metadata": {},
   "source": [
    "## Attach the Second Link\n",
    "\n",
    "Deep-copy the first link's definition to reuse its data. Here, we change the name, and modify the coupling to the parent body, since the second link is connected to the first link, which itself is a flexible body. Whenever one is attaching a body of any type (rigid or flex) to a a flexible body, meaning the flexible body is the parent, they need to specify the translational and rotational nodal matrix of the onode. The onode is the node on the parent body in a hinge that connects a parent body to a child body. In this case, the hinge will connect that pnode on `link_2` to an onode on `link_1` at the beam's far end, FEM grid node 51. \n",
    "\n",
    "In general, the {py:class}`ParentPhysicalModalBodyAttachmentParams <Karana.Dynamics.ParentPhysicalModalBodyAttachmentParams>` can be used to specify the `onode` attachment information for a flexible body. Here, {py:meth}`addInbNodeToDS() <Karana.Dynamics.SOAFlexDyn_types.ReadFEMBridgePhysicalModalBody.addInbNodeToDS>` provides a convenient helper method to add this information."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "5fd14459",
   "metadata": {},
   "outputs": [],
   "source": [
    "link_2_ds = link_1_ds.model_copy(deep=True)\n",
    "link_2_ds.name = \"link_2\"\n",
    "beam_reader.addInbNodeToDS(link_2_ds, 51)\n",
    "link_2 = link_2_ds.toBody(link_1)\n",
    "del link_1_ds, link_2_ds"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d0d18090",
   "metadata": {},
   "source": [
    "## Add Gravity\n",
    "\n",
    "Add gravity just as was done in the [basic example](../example_2_link_pendulum/notebook.ipynb)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "093f1cd3",
   "metadata": {},
   "outputs": [],
   "source": [
    "ug = kmdl.Gravity(\"grav_model\", sim.sp, kmdl.UniformGravity(\"uniform_gravity\"), sim.mb)\n",
    "ug.getGravityInterface().setGravity(np.array([0, 0, -9.81]), 0.0, kmdl.OutputUpdateType.PRE_HOP)\n",
    "del ug"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2f9d9682",
   "metadata": {},
   "source": [
    "## Set the Initial State\n",
    "\n",
    "Initialize the {py:class}`multibody <Karana.Dynamics.Multibody>` as in the basic example, then set the hinge angles to **110°** and **15°**. Both links start at rest with zero modal coordinates, so the release produces swinging motion and flexible-body deformation."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "9ee5b73c",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "True"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "sim.mb.ensureHealthy()\n",
    "sim.mb.resetData()\n",
    "link_1.parentHinge().subhinge(0).setQ(110.0 * ureg.deg)\n",
    "link_2.parentHinge().subhinge(0).setQ(15.0 * ureg.deg)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f42d276c",
   "metadata": {},
   "source": [
    "## Configure the Integrator\n",
    "Here, we configure the integrator. The performance of flexible-body problems in particular is sensitive to the integrator chosen. kdFlex offers a large variety of integrators and options to meet simulations' needs. Here, SUNDIALS's ARKODE's implicit ESDIRK45 method does well."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "5949ccac",
   "metadata": {},
   "outputs": [],
   "source": [
    "integrator_options = ki.ArkImplicitIntegratorOptions()\n",
    "integrator_options.rtol = 1e-5\n",
    "integrator_options.atol = 3e-8\n",
    "integrator_options.max_nl_iters = 5\n",
    "sim.sp.setIntegrator(ki.IntegratorType.ARKIMPLICIT_ESDIRK45, integrator_options)\n",
    "\n",
    "sim.sp.ensureHealthy()\n",
    "sim.sp.hardReset()\n",
    "sim.sp.setState(sim.sp.assembleState())\n",
    "\n",
    "# Here, a maximum hop size is added so we get visualization updates every 1/60 seconds\n",
    "sim.sp.setMaxHopSize(1.0 / 60.0)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "60ea2735",
   "metadata": {},
   "source": [
    "## Open the kdFlex Viewer\n",
    "\n",
    "Open up the GUI to view the model, and set the camera position."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "f3f67e8f",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[WebUI] Karana Viewer is running on port 32891\n",
      "Open this URL to connect:\n",
      "\t\u001b[1mhttp://newton:32891\u001b[0m\n"
     ]
    },
    {
     "data": {
      "text/html": [
       "\n",
       "        <a target=\"_blank\" rel=\"noopener noreferrer\" href=\"http://newton:32891\">\n",
       "            <b>Click to open Karana Viewer in a new tab</b>\n",
       "        </a>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Set the stick parts to be a bit larger so they are easier to see\n",
    "sim.mb.createStickParts()\n",
    "sim.mb.scaleStickParts(5.0)\n",
    "\n",
    "# Create the GUI and set the camera location\n",
    "sim.setupGui(port=0)\n",
    "target = sim.mb.virtualRoot().frameToFrame(link_2).relTransform().getTranslation().m\n",
    "sim.mb.getScene().viewAroundFrame(\n",
    "    sim.mb.virtualRoot(),\n",
    "    target + np.array([0.0, 6.0, 0.0]),\n",
    "    target,\n",
    "    np.array([0.0, 0.0, 1.0]),\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "96392047",
   "metadata": {},
   "source": [
    "## Run the Simulation\n",
    "Advance to 10 seconds."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "id": "53a4b843",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "<SpStatusEnum.REACHED_END_TIME: 1>"
      ]
     },
     "execution_count": 10,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "sim.sp.advanceTo(10.0)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e594747d",
   "metadata": {},
   "source": [
    "## Clean Up the Simulation\n",
    "Below, we cleanup our simulation."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "id": "06edf35f",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "<function __main__.cleanup() -> None>"
      ]
     },
     "execution_count": 11,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "def cleanup() -> None:\n",
    "    \"\"\"Cleanup the simulation.\"\"\"\n",
    "    global link_1, link_2, sim\n",
    "    del link_1, link_2, sim\n",
    "\n",
    "\n",
    "atexit.register(cleanup)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e53e2035",
   "metadata": {},
   "source": [
    "## Further Reading\n",
    "\n",
    "Next, work through the [Flexible Two-Link Pendulum from Explicit Modal Data](../example_flexible_2_link_pendulum/notebook.ipynb) notebook. It builds the same pendulum using explicit modal data, without an external file, so you can see how to supply the flexible-body parameters directly."
   ]
  }
 ],
 "metadata": {
  "docs": {
   "tags": [
    "dynamics",
    "time domain",
    "pendulum",
    "flexible"
   ]
  },
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "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.12.3"
  },
  "name": "notebook.ipynb"
 },
 "nbformat": 4,
 "nbformat_minor": 5
}