{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "0e827775",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "# Import/Export Multibodies\n",
    "\n",
    "In this notebook we demonstrate how to export and import **kdflex** multibodies to various formats. This notebook follows almost the same procedure as the $n$-body pendulum notebook, but differs in that it demonstrates how to export a multibody after it has been created as well as how to import one. Everything included after the import cell in this notebook has already been demonstrated in other notebooks, and has only been included to prove that the imported multibody can now be ran. As such feel free to skip the rest of the notebook after that section.\n",
    "\n",
    "Requirements:\n",
    "- [2-link Pendulum](../example_2_link_pendulum/notebook.ipynb)\n",
    "\n",
    "In this tutorial we will:\n",
    "- Create the multibody\n",
    "- Export/Import the multibody\n",
    "- Setup the **kdFlex** Scene\n",
    "- Add a root visual geometry\n",
    "- Set up the state propagator and models\n",
    "- Set initial state\n",
    "- Register a timed event\n",
    "- Run the simulation\n",
    "- Clean up the simulation\n",
    "\n",
    "![](../resources/nb_images/importexport.png)\n",
    "\n",
    "For a more in-depth descriptions of **kdflex** concepts see [usage](usage_page).\n",
    " "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b7e2551c",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import atexit\n",
    "from typing import cast\n",
    "from math import pi\n",
    "\n",
    "import Karana.Core as kc\n",
    "import Karana.Math as km\n",
    "import Karana.Integrators as ki\n",
    "import Karana.Frame as kf\n",
    "import Karana.Dynamics as kd\n",
    "import Karana.Dynamics.SOADyn_types as kdt\n",
    "import Karana.Scene as ks\n",
    "import Karana.Models as kmdl"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9bde08ea",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "## Create the Multibody\n",
    "\n",
    "Below we create the 2-link pendulum that we will export in different formats. We are using the procedural approach since it is simpler and we can build the multibody and add visual parts all at once. If you would like to learn more about the procedural approach, check out the [n-link Pendulum example](../example_n_link_pendulum/notebook.ipynb).\n",
    "\n",
    "See [Multibody](treembody_sec) or [Frames](frames_layer_sec) for more information relating to this step."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "a5b75454",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "fc = kf.FrameContainer(\"root\")\n",
    "\n",
    "\n",
    "def createMbody(n_links: int):\n",
    "    \"\"\"Create the multibody.\n",
    "\n",
    "    Parameters\n",
    "    ----------\n",
    "    n_links : int\n",
    "        The number of pendulum links to use.\n",
    "    \"\"\"\n",
    "    mb = kd.Multibody(\"mb\", fc)\n",
    "\n",
    "    # create visual materials to color the bodies\n",
    "    mat_info = ks.PhysicalMaterialInfo()\n",
    "    mat_info.color = ks.Color.FIREBRICK\n",
    "    brown = ks.PhysicalMaterial(mat_info)\n",
    "\n",
    "    # using scene part spec to define visual geometry for each procedural body\n",
    "    sp_body = ks.ScenePartSpec()\n",
    "    sp_body.name = \"sp0\"\n",
    "    sp_body.geometry = ks.BoxGeometry(0.05, 0.05, 1)\n",
    "    sp_body.material = brown\n",
    "    sp_body.transform = km.HomTran([0.0, 0.0, 0.0])\n",
    "    sp_body.scale = [1, 1, 1]\n",
    "\n",
    "    sp_pivot = ks.ScenePartSpec()\n",
    "    sp_pivot.name = \"sp1\"\n",
    "    sp_pivot.geometry = ks.CylinderGeometry(0.1, 0.1)\n",
    "    sp_pivot.material = brown\n",
    "    sp_pivot.transform = km.HomTran([0.0, 0.0, -0.5])\n",
    "    sp_pivot.scale = [1, 1, 1]\n",
    "\n",
    "    # Here, we use the addSerialChain method to add multiple instances of the same body\n",
    "    # connected in a chain. We add n instances of the pendulum link with the following parameters:\n",
    "    params = kd.PhysicalBodyParams(\n",
    "        spI=km.SpatialInertia(2.0, np.zeros(3), np.diag([3, 2, 1])),\n",
    "        axes=[np.array([0.0, 1.0, 0.0])],\n",
    "        body_to_joint_transform=km.HomTran(np.array([0, 0, 0.5])),\n",
    "        inb_to_joint_transform=km.HomTran(np.array([0, 0, -0.5])),\n",
    "        scene_part_specs=[sp_body, sp_pivot],\n",
    "    )\n",
    "    kd.PhysicalBody.addSerialChain(\n",
    "        \"link\",\n",
    "        n_links,\n",
    "        cast(kd.PhysicalBody, mb.virtualRoot()),\n",
    "        htype=kd.HingeType.REVOLUTE,\n",
    "        params=params,\n",
    "    )\n",
    "\n",
    "    # finalize and verify the multibody\n",
    "    mb.ensureHealthy()\n",
    "    mb.resetData()\n",
    "    assert kc.allReady()\n",
    "\n",
    "    return mb"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "28b778b9",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "We then create the multibody by calling the createMbody method we defined in the previous cell."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "d4a29bd7",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "# initialization\n",
    "n_links = 2\n",
    "mb = createMbody(n_links)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "bab31ad2",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "## Export/Import the Multibody\n",
    "\n",
    "Now that we have the multibody we convert it from a {py:class}`Karana.Dynamics.Multibody` to a intermediate {py:class}`Karana.Dynamics.SOADyn_types.SubGraphDS` DataStruct type, which we then export to various file formats.\n",
    "\n",
    "See [Model Data Files](importing_model_data_files_sec) for more info."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "e244a8a9",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "mb_DS_to_export = mb.toDS()\n",
    "\n",
    "# delete old multibody\n",
    "kc.discard(mb)\n",
    "\n",
    "# export to JSON\n",
    "mb_DS_to_export.toFile(\"2_link_multibody.json\")\n",
    "\n",
    "# export multibody to yaml\n",
    "mb_DS_to_export.toFile(\"2_link_multibody.yaml\")\n",
    "\n",
    "# export multibody to hdf5\n",
    "mb_DS_to_export.toFile(\"2_link_multibody.hdf5\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "268602d9",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "We now import the exported {py:class}`Karana.Dynamics.SOADyn_types.SubGraphDS` Multibody DataStruct. After importing we convert it back to a {py:class}`Karana.Dynamics.Multibody`. We have to run the {py:meth}`Karana.Core.LockingBase.ensureHealthy` and {py:meth}`Karana.Dynamics.SubTree.resetData` methods after we import a model even if the initial model we exported had these methods ran. After this point, the multibody should be functionally identical to the original multibody we defined procedurally with the createMBody() method above. Since the multibody is identical, the notebook from this point is the same as the $n$-link pendulum, since it is the same exact process to run the simulation. You can run the rest of the notebook if you would like, but there will be no new concepts introduced after the following cell."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "0887c28e",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "LEGEND: [<hinge type> <prescribed subhinges>] <body name>[/num embedded bodies > 0] <flex dofs > 0>\n",
      "|-mb_MBVROOT_\n",
      "   |-[REVOLUTE] link_0\n",
      "      |-[REVOLUTE] link_1\n",
      "\n"
     ]
    }
   ],
   "source": [
    "imported_ds = kdt.SubGraphDS.fromFile(\"2_link_multibody.json\")\n",
    "# imported_ds = SubGraphDS.fromFile(\"2_link_multibody.yaml\")\n",
    "# imported_ds = SubGraphDS.fromFile(\"2_link_multibody.hdf5\")\n",
    "\n",
    "mb_new = imported_ds.toMultibody(fc)\n",
    "\n",
    "# Even if a multibody is reimported we have to use ensureHealthy() to make sure\n",
    "# the multibody is healthy. We then also can use resetData() to initialize the\n",
    "# state and set coordinates at zero.\n",
    "mb_new.ensureHealthy()\n",
    "mb_new.resetData()\n",
    "\n",
    "# verify that the imported multibody is ready to go\n",
    "assert kc.allReady()\n",
    "\n",
    "mb_new.dumpTree()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8761b66b",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "## Setup the kdFlex Scene\n",
    "\n",
    "Next we setup **kdflex**'s graphics by calling the setupGraphics helper method on the multibody. This method takes care of setting up the graphics environment. \n",
    "\n",
    "See [Visualization](visualization_sec) and [Scene Layer](scene_layer_sec) for more information relating to this section."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "b5b4a228",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "        <div style=\"height:300px; resize:vertical; overflow: auto;\">\n",
       "          <iframe src=\"http://localhost:38417\" style=\"width:100%; height:100%; border:0; display:block;\"></iframe>\n",
       "        </div>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "cleanup_graphics, web_scene = mb_new.setupGraphics(port=0, axes=0.0)\n",
    "\n",
    "# position the viewpoint camera of the visualization\n",
    "web_scene.defaultCamera().pointCameraAt([0.0, 5.0, -1.0], [0, 0, -1], [0, 0, 1])\n",
    "\n",
    "# Get the scene for later use\n",
    "proxy_scene = mb_new.getScene()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e2ffa804",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "## Setup State Propagator and Models\n",
    "\n",
    "Now, we setup the {py:class}`Karana.Dynamics.StatePropagator` and register our models as well. \n",
    "\n",
    "See [Models](system_level_models_sec) for more concepts and information."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b9270355",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "# Set up state propagator and select integrator type: rk4 or cvode\n",
    "sp = kd.StatePropagator(mb_new, integrator_type=ki.IntegratorType.RK4)\n",
    "integrator = sp.getIntegrator()\n",
    "\n",
    "# add a gravitational model to the state propagator\n",
    "ug = kmdl.Gravity(\"grav_model\", sp, kmdl.UniformGravity(\"uniform_gravity\"), mb_new)\n",
    "ug.getGravityInterface().setGravity(np.array([0, 0, -9.81]), 0.0, kmdl.OutputUpdateType.PRE_HOP)\n",
    "del ug\n",
    "\n",
    "# Makes sure the visualization scene is updated after each state change.\n",
    "kmdl.UpdateProxyScene(\"update_proxy_scene\", sp, proxy_scene)\n",
    "\n",
    "# sync the simulation time with real-time.\n",
    "kmdl.SyncRealTime(\"sync_real_time\", sp, 1.0)\n",
    "\n",
    "\n",
    "# Register a post step function to log state information.\n",
    "def post_step_fn(t, x):\n",
    "    \"\"\"Print out the time and state.\n",
    "\n",
    "    Parameters\n",
    "    ----------\n",
    "    t : float\n",
    "        The current time.\n",
    "    x : NDArray[np.float64]\n",
    "       The current state.\n",
    "    \"\"\"\n",
    "    print(f\"t = {float(integrator.getTime()) / 1e9}s; x = {integrator.getX()}\")\n",
    "\n",
    "\n",
    "sp.fns.post_hop_fns[\"update_and_info\"] = post_step_fn"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "32c8a66d",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "## Set Initial State\n",
    "\n",
    "Before we begin our simulation, we set the initial state for our multibody and statepropagator. \n",
    "\n",
    "When accessing or modifying [generalized coordinates](coords_sec) for a subhinge, it is recommended to directly set the subhinge's values rather than for the entire multibody in order to avoid ambiguity."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "d6e32415",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "# Initialize the multibody state.\n",
    "# Here we will set the first pendulum position to pi/8 radians and its velocity to 0.0\n",
    "bd1 = mb_new.getBody(\"link_0\")\n",
    "bd1.parentHinge().subhinge(0).setQ(pi / 4)\n",
    "bd1.parentHinge().subhinge(0).setU(0.0)\n",
    "\n",
    "# Initialize the integrator state\n",
    "t_init = np.timedelta64(0, \"ns\")\n",
    "x_init = sp.assembleState()\n",
    "sp.setTime(t_init)\n",
    "sp.setState(x_init)\n",
    "\n",
    "# Syncs graphics\n",
    "proxy_scene.update()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b35e1ef0",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "## Register a Timed Event\n",
    "\n",
    "To terminate a simulation step every 0.1 seconds, we create a timed event and register it to our state propagator here. See [timed events](timed_events_sec) for more information."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "d5e4c74d",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "h = np.timedelta64(int(1e8), \"ns\")\n",
    "t = kd.TimedEvent(\"hop_size\", h, lambda _: None, False)\n",
    "t.period = h\n",
    "\n",
    "# register the timed event\n",
    "sp.registerTimedEvent(t)\n",
    "del t"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "06e5af80",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "## Run the Simulation\n",
    "\n",
    "Now, run the simulation for 10 seconds."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "id": "f92e5b77",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "t = 0.0s; x = [0.78539816 0.         0.         0.        ]\n",
      "t = 0.1s; x = [ 0.76349445  0.01680313 -0.43624439  0.33371087]\n",
      "t = 0.2s; x = [ 0.69888834  0.06577862 -0.85022684  0.63827478]\n",
      "t = 0.3s; x = [ 0.59499828  0.14234329 -1.21770063  0.87926231]\n",
      "t = 0.4s; x = [ 0.45777804  0.23818444 -1.51246493  1.01726067]\n",
      "t = 0.5s; x = [ 0.29578038  0.34113369 -1.70949023  1.01662179]\n",
      "t = 0.6s; x = [ 0.11978697  0.43621738 -1.79010604  0.85901723]\n",
      "t = 0.7s; x = [-0.05806892  0.50791391 -1.74649608  0.5524414 ]\n",
      "t = 0.8s; x = [-0.22550436  0.54277735 -1.58356094  0.1292049 ]\n",
      "t = 0.9s; x = [-0.37134283  0.53138927 -1.31825157 -0.36444363]\n",
      "t = 1.0s; x = [-0.48660819  0.46916797 -0.97709001 -0.87959775]\n",
      "t = 1.1s; x = [-0.56530097  0.35624875 -0.59239322 -1.37145346]\n",
      "t = 1.2s; x = [-0.60476258  0.19697353 -0.19794044 -1.80056533]\n",
      "t = 1.3s; x = [-6.05605290e-01 -5.91349616e-04  1.75352580e-01 -2.13186132e+00]\n",
      "t = 1.4s; x = [-0.5712026  -0.22516493  0.503928   -2.33699369]\n",
      "t = 1.5s; x = [-0.50674337 -0.46326313  0.77544607 -2.40147497]\n",
      "t = 1.6s; x = [-0.41807229 -0.70085943  0.98857346 -2.32882141]\n",
      "t = 1.7s; x = [-0.31085454 -0.9249631   1.14692464 -2.13456555]\n",
      "t = 1.8s; x = [-0.19047512 -1.12428471  1.2514652  -1.8356949 ]\n",
      "t = 1.9s; x = [-0.06252719 -1.28904021  1.29693848 -1.44499766]\n",
      "t = 2.0s; x = [ 0.06661033 -1.41052612  1.27347608 -0.97170898]\n",
      "t = 2.1s; x = [ 0.18952507 -1.48090589  1.17108931 -0.42437044]\n",
      "t = 2.2s; x = [ 0.29799175 -1.49324731  0.98411517  0.18731851]\n",
      "t = 2.3s; x = [ 0.38355233 -1.44165895  0.7138203   0.85230024]\n",
      "t = 2.4s; x = [ 0.4382625  -1.32140652  0.36930346  1.5584436 ]\n",
      "t = 2.5s; x = [ 0.45551223 -1.12909132 -0.03154281  2.29026962]\n",
      "t = 2.6s; x = [ 0.43105868 -0.86344568 -0.45819652  3.01739087]\n",
      "t = 2.7s; x = [ 0.3647267  -0.52815106 -0.85781785  3.66491659]\n",
      "t = 2.8s; x = [ 0.2631527  -0.13805693 -1.14795373  4.08412802]\n",
      "t = 2.9s; x = [ 0.14148259  0.27561166 -1.25245558  4.12092901]\n",
      "t = 3.0s; x = [ 0.01876834  0.67347749 -1.17801964  3.78700418]\n",
      "t = 3.1s; x = [-0.09076816  1.02611299 -1.00267344  3.24520398]\n",
      "t = 3.2s; x = [-0.18070413  1.32041526 -0.79393106  2.63699632]\n",
      "t = 3.3s; x = [-0.24948606  1.55350894 -0.58228958  2.02686807]\n",
      "t = 3.4s; x = [-0.29735189  1.72635962 -0.3760869   1.43321122]\n",
      "t = 3.5s; x = [-0.32491508  1.84073783 -0.17623962  0.8569881 ]\n",
      "t = 3.6s; x = [-0.33282077  1.89820988  0.01691665  0.29432755]\n",
      "t = 3.7s; x = [-0.3218028   1.89989899  0.20192701 -0.25945707]\n",
      "t = 3.8s; x = [-0.29276814  1.84645748  0.37702139 -0.80907413]\n",
      "t = 3.9s; x = [-0.24674969  1.73806518  0.54170318 -1.35924757]\n",
      "t = 4.0s; x = [-0.18471938  1.57444845  0.69770882 -1.91407647]\n",
      "t = 4.1s; x = [-0.10740371  1.35506757  0.84768945 -2.47387501]\n",
      "t = 4.2s; x = [-0.01543271  1.07989354  0.98947492 -3.02570296]\n",
      "t = 4.3s; x = [ 0.08962705  0.7516833   1.10403725 -3.5222507 ]\n",
      "t = 4.4s; x = [ 0.20284524  0.38081495  1.14188599 -3.8559983 ]\n",
      "t = 4.5s; x = [ 0.31310333 -0.00917991  1.03517073 -3.88477861]\n",
      "t = 4.6s; x = [ 0.40406363 -0.38397644  0.75852058 -3.55757752]\n",
      "t = 4.7s; x = [ 0.46077951 -0.71213102  0.36268605 -2.97570687]\n",
      "t = 4.8s; x = [ 0.47522711 -0.97530943 -0.07501427 -2.2773575 ]\n",
      "t = 4.9s; x = [ 0.44640263 -1.166667   -0.49479514 -1.54918461]\n",
      "t = 5.0s; x = [ 0.37817678 -1.28559619 -0.85799031 -0.83384591]\n",
      "t = 5.1s; x = [ 0.27751426 -1.33470554 -1.14051932 -0.15638146]\n",
      "t = 5.2s; x = [ 0.15321121 -1.31882116 -1.32952956  0.46279   ]\n",
      "t = 5.3s; x = [ 0.0148421  -1.24472448 -1.42210371  1.00491175]\n",
      "t = 5.4s; x = [-0.12815369 -1.12097104 -1.42341636  1.45340278]\n",
      "t = 5.5s; x = [-0.26711511 -0.95763683 -1.34314207  1.79447311]\n",
      "t = 5.6s; x = [-0.39438612 -0.76604025 -1.19103905  2.0170366 ]\n",
      "t = 5.7s; x = [-0.50316135 -0.55846602 -0.97417133  2.11305241]\n",
      "t = 5.8s; x = [-0.58722342 -0.34776126 -0.69765199  2.07986976]\n",
      "t = 5.9s; x = [-0.64091309 -0.1466203  -0.3681918   1.92363902]\n",
      "t = 6.0s; x = [-0.65947136  0.03336685  0.00253218  1.66003575]\n",
      "t = 6.1s; x = [-0.63960824  0.18252551  0.39682845  1.31100668]\n",
      "t = 6.2s; x = [-0.58007869  0.29354182  0.79165304  0.90157381]\n",
      "t = 6.3s; x = [-0.48216446  0.361769    1.15973107  0.46047252]\n",
      "t = 6.4s; x = [-0.35001818  0.385752    1.47131517  0.02327514]\n",
      "t = 6.5s; x = [-0.19076578  0.36800047  1.69733992 -0.36654687]\n",
      "t = 6.6s; x = [-0.01422113  0.31559233  1.81392059 -0.66249057]\n",
      "t = 6.7s; x = [ 0.16787659  0.23995321  1.80717068 -0.82630933]\n",
      "t = 6.8s; x = [ 0.34305552  0.15536257  1.6766659  -0.84096689]\n",
      "t = 6.9s; x = [ 0.49949913  0.07644337  1.43561452 -0.71662844]\n",
      "t = 7.0s; x = [ 0.62724411  0.01565407  1.10696882 -0.48454749]\n",
      "t = 7.1s; x = [ 0.71885836 -0.01816522  0.717433   -0.18357692]\n",
      "t = 7.2s; x = [ 0.76955857 -0.01995339  0.29274385  0.15072578]\n",
      "t = 7.3s; x = [ 0.77700883  0.01210616 -0.14388367  0.48880448]\n",
      "t = 7.4s; x = [ 0.74111058  0.07701767 -0.57058868  0.80297933]\n",
      "t = 7.5s; x = [ 0.66397465  0.17085427 -0.9647299   1.06150285]\n",
      "t = 7.6s; x = [ 0.55006771  0.28619662 -1.30185487  1.22654793]\n",
      "t = 7.7s; x = [ 0.40633936  0.41175208 -1.55723601  1.2601306 ]\n",
      "t = 7.8s; x = [ 0.24207356  0.53292702 -1.70959971  1.13636458]\n",
      "t = 7.9s; x = [ 0.06834581  0.63361776 -1.7449035   0.85204611]\n",
      "t = 8.0s; x = [-0.10282791  0.69864429 -1.65865107  0.42830333]\n",
      "t = 8.1s; x = [-0.25951365  0.71589169 -1.45713524 -0.09621135]\n",
      "t = 8.2s; x = [-0.39097389  0.67758522 -1.15798998 -0.67481945]\n",
      "t = 8.3s; x = [-0.4887746   0.58070844 -0.78935903 -1.25980126]\n",
      "t = 8.4s; x = [-0.54771921  0.42696819 -0.38728864 -1.80429435]\n",
      "t = 8.5s; x = [-0.5664483   0.22280248  0.00833045 -2.26015922]\n",
      "t = 8.6s; x = [-0.54753787 -0.02047007  0.36000742 -2.57897001]\n",
      "t = 8.7s; x = [-0.49679459 -0.28718633  0.64238126 -2.72551941]\n",
      "t = 8.8s; x = [-0.42157046 -0.55966837  0.8504973  -2.69686178]\n",
      "t = 8.9s; x = [-0.32878829 -0.82165355  0.99610191 -2.52181486]\n",
      "t = 9.0s; x = [-0.22394984 -1.06042353  1.09343767 -2.23829524]\n",
      "t = 9.1s; x = [-0.11150521 -1.26664066  1.14828597 -1.87418954]\n",
      "t = 9.2s; x = [ 0.00415919 -1.43306968  1.15654672 -1.44412297]\n",
      "t = 9.3s; x = [ 0.11793123 -1.55348667  1.10874978 -0.95472389]\n",
      "t = 9.4s; x = [ 0.22372184 -1.62216616  0.99566363 -0.41002101]\n",
      "t = 9.5s; x = [ 0.31470488 -1.63378167  0.8122178   0.1857907 ]\n",
      "t = 9.6s; x = [ 0.38382486 -1.5834643   0.5589675   0.8277682 ]\n",
      "t = 9.7s; x = [ 0.42436188 -1.46686629  0.2420341   1.51042091]\n",
      "t = 9.8s; x = [ 0.43049312 -1.28025655 -0.1265392   2.22629459]\n",
      "t = 9.9s; x = [ 0.39801312 -1.02104997 -0.52545576  2.95735211]\n",
      "t = 10.0s; x = [ 0.3256916  -0.68999641 -0.91411491  3.6485481 ]\n"
     ]
    }
   ],
   "source": [
    "# run the simulation\n",
    "print(f\"t = {float(integrator.getTime()) / 1e9}s; x = {integrator.getX()}\")\n",
    "\n",
    "# run the simulation\n",
    "sp.advanceTo(10.0)\n",
    "\n",
    "# dump the state propagator info\n",
    "sp.dump(\"sp\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a4d77758",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "## Clean Up the Simulation\n",
    "\n",
    "Below, we cleanup our simulation. We first delete local variables, cleanup our visualizer, discard remaining Karana objects, and optionally verify using allDestroyed."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "id": "efb64e64",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "<function __main__.cleanup()>"
      ]
     },
     "execution_count": 11,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Cleanup\n",
    "def cleanup():\n",
    "    \"\"\"Cleanup the simulation.\"\"\"\n",
    "    global integrator, mb_DS_to_export, proxy_scene, web_scene, imported_ds, bd1\n",
    "    del integrator, mb_DS_to_export, proxy_scene, web_scene, imported_ds, bd1\n",
    "    kc.discard(sp)\n",
    "    cleanup_graphics()\n",
    "    kc.discard(mb_new)\n",
    "    kc.discard(fc)\n",
    "\n",
    "\n",
    "atexit.register(cleanup)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b3f4f872",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "## Summary\n",
    "\n",
    "By converting between {py:class}`Karana.Dynamics.SOADyn_types.SubGraphDS` and {py:class}`Karana.Dynamics.Multibody` for both import and export, you are able to load multibodies from files and save them into files. This opens up many exciting possibilities!\n",
    "\n",
    "## Further Readings\n",
    "[Load a mars 2020 rover urdf](../example_m2020/notebook.ipynb)  \n",
    "[Load a robotic arm urdf](../example_urdf/notebook.ipynb)  \n",
    "[Enforce loop constraints in a double-wishbone model](../example_double_wishbone/notebook.ipynb)  \n",
    "[Create and use constraints in a slider-crank model](../example_slider_crank/notebook.ipynb)  \n",
    "[Drive an ATRVjr rover](../example_atrvjr_drive/notebook.ipynb)"
   ]
  }
 ],
 "metadata": {
  "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
}