{ "cells": [ { "cell_type": "markdown", "id": "fcc18f04-afc9-40ef-a01d-c480628004f3", "metadata": {}, "source": [ "# Cut-joint loop constraint\n", "This notebook illustrates the use of a cu-joint loop constraint." ] }, { "cell_type": "markdown", "id": "2e253605", "metadata": {}, "source": [ "This notebook builds upon the system-level simulation showcased in the [system level notebook](../example_system_level/notebook.ipynb). If you are not already familiar with the concepts described in that notebook, please go through it before this one.\n", "\n", "" ] }, { "cell_type": "code", "execution_count": 1, "id": "22ae75ea", "metadata": {}, "outputs": [], "source": [ "import numpy as np\n", "import Karana.Dynamics as kd\n", "import Karana.Dynamics.SOADyn_types as kdt\n", "from Karana.KUtils.Sim import SimDS\n", "import Karana.Models as kmdl\n", "import Karana.Integrators as ki\n", "import Karana.KUtils as ku\n", "import Karana.Core as kc\n", "import atexit\n", "import Karana.Math as km\n", "from Karana.Math.Kquantities import ureg\n", "from pathlib import Path\n", "import runpy\n", "from tempfile import NamedTemporaryFile" ] }, { "cell_type": "markdown", "id": "dfd3d9dc", "metadata": {}, "source": [ "Create the same serial chain simulation and populate the multibody using a DataStruct (DS) file just as before." ] }, { "cell_type": "code", "execution_count": 2, "id": "68fd102c", "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "[WebUI] Web server is running on port 33889\n", "You may be able to connect in your browser at:\n", "\t\u001b[1mhttp://newton:33889\u001b[0m\n", "[WebUI] Karana Viewer is running on port 29534\n", "Open this URL to connect:\n", "\t\u001b[1mhttp://newton:29534\u001b[0m\n" ] }, { "data": { "text/html": [ "\n", " \n", " Click to open Karana Viewer in a new tab\n", " \n", " " ], "text/plain": [ "" ] }, "metadata": {}, "output_type": "display_data" } ], "source": [ "sim_ds_path = Path(\"..\") / \"resources\" / \"2_link_pendulum\" / \"2_link_pendulum_cut_joint.h5\"\n", "sim = SimDS.fromFile(sim_ds_path).toSim()\n", "sim.mb.resetData()\n", "sim.mb.getScene().viewAroundFrame(\n", " sim.mb.virtualRoot(), np.array([0.0, 5.0, 0.0]), np.zeros(3), np.array([0.0, 0.0, 1.0])\n", ")\n", "sim.mb.getScene().update()" ] }, { "cell_type": "markdown", "id": "e83f05af", "metadata": {}, "source": [ "## Set initial position\n", "As before, we'll use the Python API to set the joint values to 0.5 and 1 radians. However, here, we need to set the constraint's hinge to 1 radian rather than the hinge of the body. In addition, we need to use the `ConstraintKinematicsSolver` to remove any constraint error after setting the pose. Then, we need to update the `Integrator` by calling `sim.sp.setState`." ] }, { "cell_type": "code", "execution_count": 3, "id": "112e755e", "metadata": {}, "outputs": [], "source": [ "# Set the first body's hinge to 0.5 radians\n", "bd1 = sim.mb.getBody(\"bd1\")\n", "bd1.parentHinge().subhinge(0).setQ(0.5)\n", "\n", "# Set the second body's constraint hinge to 1.0 radians\n", "constraint = sim.mb.enabledConstraints()[0]\n", "constraint.hinge().subhinge(0).setQ(1.0)\n", "\n", "# Use the ConstraintKinematicsSolver to eliminate any constraint error\n", "assert abs(sim.mb.cks().solveQ()) < 1e-8\n", "assert abs(sim.mb.cks().solveU()) < 1e-8\n", "\n", "# Update the Integrator with these values\n", "sim.sp.ensureHealthy()\n", "sim.sp.hardReset()\n", "sim.sp.setState(sim.sp.assembleState())" ] }, { "cell_type": "markdown", "id": "3629de3c-c5d1-451e-a20c-89f51ed71342", "metadata": {}, "source": [ "## Analyze the multibody\n", "In this version of the double pendulum, the second hinge has been swapped for a cut-joint constraint. This means the second body is attached to the inertial frame using a 6 DoF joint, but also has a hinge-like constraint attaching it to the first pendulum. Conceptually this pendulum model represents the same system as the 2-link serial chain pendulum system in the earlier notebooks. However the models representations are different. The earlier serial-chain system had only 2 degrees of freedom, and no constraints. This new model on the other hand has 7 degrees of freedom with 5 internal cut-joint bilateral constraints. While having the same dynamics, the different models require different methods for solving the equations of motion. While **kdFlex** supports both of these modeling choices, it is advantageous to use minimal coordinates and minimize the number of explicit bilateral constraints. The gui, as well as the following command line methods illustrate the difference in this model with cut-joint constraint." ] }, { "cell_type": "markdown", "id": "b6fb4d40", "metadata": {}, "source": [ "We use the `displayModel` and the `dumpTree` methods to explore the model next." ] }, { "cell_type": "code", "execution_count": 4, "id": "ffc530f0-9de9-4d21-a40a-5b6e0ff4f44d", "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "\n", "LEGEND: [prescribed subhinges] [flex dofs/offset]\n", "\n", " Body Parent Hinge Dofs \n", " ____ ______ _____ ____ \n", " 1. bd1 multibody_MBVROOT_ REVOLUTE 1/0 \n", " 2. bd2 multibody_MBVROOT_ FULL6DOF 6/1 \n", " ____ \n", " 7 \n", "\n", " Nodes\n", " ---\n", " bd1_bd2_loop_constraint_source body=bd1\n", " bd1_bd2_loop_constraint_target body=bd2\n", "\n", " Enabled cutjoint loop constraints <1, residuals=5, error=6>\n", " ---------------\n", " bd1_bd2_loop_constraint source=bd1/bd1_bd2_loop_constraint_source target=bd2/bd1_bd2_loop_constraint_target\n", "\n" ] } ], "source": [ "sim.mb.displayModel()" ] }, { "cell_type": "code", "execution_count": 5, "id": "5560f05d", "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "LEGEND: [ ] [/num embedded bodies > 0] 0>\n", "|-multibody_MBVROOT_\n", " |-[REVOLUTE] bd1 [---> lc/REVOLUTE ---> bd2]\n", " |-[FULL6DOF] bd2 [<--- lc/REVOLUTE <--- bd1]\n", "\n" ] } ], "source": [ "sim.mb.dumpTree()" ] }, { "cell_type": "markdown", "id": "8f99f7e2", "metadata": {}, "source": [ "## Register a Timed Event\n", "\n", "As before, to stop the simulation and print information every 0.1 seconds, we create a timed event and register it with our state propagator here. See [timed events](timed_events_sec) for more information." ] }, { "cell_type": "code", "execution_count": 6, "id": "ee0a5819", "metadata": {}, "outputs": [], "source": [ "sp = sim.sp\n", "\n", "\n", "def fn(t):\n", " \"\"\"Print out the time and state.\n", "\n", " Parameters\n", " ----------\n", " t : float\n", " The current time.\n", " \"\"\"\n", " print(f\"t = {float(sp.getTime()) / 1e9}s; x = {np.round(sp.getIntegrator().getX(), 6)}\")\n", "\n", "\n", "h = np.timedelta64(100, \"ms\")\n", "t = kd.TimedEvent(\"hop_size\", h, fn, False)\n", "t.period = h\n", "\n", "# register the timed event\n", "sim.sp.registerTimedEvent(t)\n", "del t" ] }, { "cell_type": "markdown", "id": "e3133f7d", "metadata": {}, "source": [ "## Add plots\n", "Use the `addPlot` helper method on the `sim` to add some plots to the GUI." ] }, { "cell_type": "code", "execution_count": 7, "id": "ce5fb62f", "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "Dash app started at http://newton:32865\n" ] } ], "source": [ "import platform\n", "\n", "if platform.system() != \"Darwin\":\n", " # In the Jupyter notebook, the live plotting does not work on macOS due to multiprocessing issues.\n", " # This works just fine from a regular Python script if guarded by if __name__ == \"__main__\"\n", " sim.addPlots(\n", " [\n", " ku.SinglePlotData(\n", " \"Pendulum Angles\",\n", " sim.sp.getVars().time,\n", " [\n", " kc.VarVec(\n", " \"p1_angle\", bd1.parentHinge().subhinge(0).getVars().Q, \"pendulum 1 angle\"\n", " ),\n", " kc.VarVec(\n", " \"p2_angle\", constraint.hinge().subhinge(0).getVars().Q, \"pendulum 2 angle\"\n", " ),\n", " ],\n", " ),\n", " ku.SinglePlotData(\n", " \"Energy\",\n", " sim.sp.getVars().time,\n", " [\n", " (\n", " \"Energy\",\n", " lambda: np.array(\n", " [\n", " kd.Algorithms.evalKineticEnergy(sim.mb)\n", " + kd.Algorithms.evalGravitationalPotentialEnergy(sim.mb)\n", " ]\n", " ),\n", " 1,\n", " )\n", " ],\n", " ),\n", " ],\n", " 0.1,\n", " title=\"Time domain plots\",\n", " )\n", "\n", "# Set the gravitational values for each body so the potential energy starts\n", "# off with the correct value\n", "sim.sp.getRegisteredModel(\"grav_model\").preDeriv(0.0, np.zeros(1))" ] }, { "cell_type": "markdown", "id": "2dc7f01d", "metadata": {}, "source": [ "## Run the simulation\n", "Now, let's run the simulation. We'll advance a total of 10 seconds just as in the time-domain notebook." ] }, { "cell_type": "code", "execution_count": 8, "id": "de32a7c2", "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "t = 0.1s; x = [ 0.481173 -0.471094 -0. -1.886418 -0. 0.008276 0.\n", " -0.472898 -0.374745 0.167232 0. -0.172096 0. 0.164958\n", " -0. 0.539703]\n", "t = 0.2s; x = [ 0.425787 -0.44576 0. -1.910175 -0. 0.032734 0.\n", " -0.393053 -0.727128 0.340713 0. -0.289844 0. 0.32165\n", " -0. 1.048778]\n", "t = 0.3s; x = [ 0.337429 -0.402725 0. -1.941027 -0. 0.071735 0.\n", " -0.265694 -1.028733 0.520385 0. -0.308263 0. 0.451477\n", " -0. 1.48021 ]\n", "t = 0.4s; x = [ 0.22296 -0.341844 0. -1.967909 -0. 0.121035 0.\n", " -0.101924 -1.242987 0.694606 0. -0.21192 0. 0.521403\n", " -0. 1.76439 ]\n", "t = 0.5s; x = [ 0.092946 -0.264745 0. -1.98076 -0. 0.172801 0.\n", " 0.079855 -1.335314 0.840544 0. -0.038562 0. 0.496384\n", " 0. 1.831698]\n", "t = 0.6s; x = [-0.039589 -0.175457 0. -1.975793 -0. 0.216731 0.\n", " 0.256319 -1.294247 0.935647 -0. 0.130033 0. 0.366161\n", " 0. 1.660408]\n", "t = 0.7s; x = [-0.162043 -0.079594 -0. -1.95742 0. 0.243323 0.\n", " 0.405366 -1.138447 0.972336 -0. 0.221298 0. 0.155802\n", " 0. 1.294249]\n", "t = 0.8s; x = [-0.264496 0.017346 -0. -1.934959 0. 0.24657 0.\n", " 0.511065 -0.899306 0.959465 -0. 0.212174 0. -0.09428\n", " 0. 0.805027]\n", "t = 0.9s; x = [-0.340041 0.11114 -0. -1.917675 0. 0.224268 0.\n", " 0.564308 -0.604204 0.911654 -0. 0.123565 0. -0.350841\n", " 0. 0.253363]\n", "t = 1.0s; x = [-3.841700e-01 1.988640e-01 -0.000000e+00 -1.911482e+00 0.000000e+00\n", " 1.768560e-01 0.000000e+00 5.610260e-01 -2.742190e-01 8.390810e-01\n", " -0.000000e+00 -1.704000e-03 0.000000e+00 -5.941240e-01 0.000000e+00\n", " -3.199050e-01]\n", "t = 1.1s; x = [-0.394377 0.278188 -0. -1.917559 0. 0.106256 0.\n", " 0.500633 0.070653 0.743182 -0. -0.11335 0. -0.812987\n", " 0. -0.88364 ]\n", "t = 1.2s; x = [-0.370368 0.346517 -0. -1.932029 0. 0.015452 0.\n", " 0.38582 0.405163 0.617785 -0. -0.162043 0. -0.995556\n", " 0. -1.400719]\n", "t = 1.3s; x = [-0.314867 0.400487 -0. -1.946649 0. -0.090914 0.\n", " 0.223954 0.69365 0.455176 -0. -0.113219 0. -1.119276\n", " 0. -1.812927]\n", "t = 1.4s; x = [-0.234647 0.436336 -0. -1.951531 0. -0.205267 0.\n", " 0.02938 0.89262 0.257138 -0. 0.026743 -0. -1.149337\n", " 0. -2.041957]\n", "t = 1.5s; x = [-0.140445 0.4514 -0. -1.940355 0. -0.316668 0.\n", " -0.176223 0.97071 0.044355 -0. 0.193655 -0. -1.058047\n", " 0. -2.028757]\n", "t = 1.6s; x = [-0.044464 0.445805 -0. -1.914873 0. -0.412969 0.\n", " -0.368506 0.931872 -0.151023 0. 0.300385 -0. -0.85147\n", " 0. -1.783341]\n", "t = 1.7s; x = [ 0.043078 0.422561 -0. -1.884005 0. -0.484303 0.\n", " -0.527381 0.807809 -0.306515 0. 0.298184 -0. -0.565553\n", " 0. -1.373362]\n", "t = 1.8s; x = [ 0.115333 0.385954 -0. -1.858741 0. -0.524745 0.\n", " -0.640078 0.630413 -0.419168 0. 0.192457 -0. -0.239205\n", " 0. -0.869618]\n", "t = 1.9s; x = [ 0.168024 0.339816 -0. -1.847788 -0. -0.53171 0.\n", " -0.699734 0.418651 -0.499402 0. 0.019086 -0. 0.100575\n", " 0. -0.318077]\n", "t = 2.0s; x = [ 0.198228 0.286673 -0. -1.855651 -0. -0.504719 0.\n", " -0.702947 0.181873 -0.561566 0. -0.175871 -0. 0.437897\n", " 0. 0.256024]\n", "t = 2.1s; x = [ 0.203805 0.227708 -0. -1.882019 -0. -0.44456 0.\n", " -0.648365 -0.07236 -0.617287 0. -0.342413 -0. 0.762271\n", " 0. 0.834632]\n", "t = 2.2s; x = [ 0.183607 0.163259 0. -1.921407 -0. -0.353081 0.\n", " -0.536689 -0.33041 -0.671316 0. -0.427397 -0. 1.061667\n", " 0. 1.392077]\n", "t = 2.3s; x = [ 0.138499 0.093647 0. -1.963106 -0. -0.233783 0.\n", " -0.372282 -0.564351 -0.718886 0. -0.382172 -0. 1.313522\n", " 0. 1.877873]\n", "t = 2.4s; x = [ 0.073096 0.020166 0. -1.992841 -0. -0.093289 0.\n", " -0.166385 -0.727458 -0.7459 0. -0.190735 -0. 1.477748\n", " -0. 2.205206]\n", "t = 2.5s; x = [-0.002895 -0.054312 0. -1.998198 -0. 0.057277 0.\n", " 0.060173 -0.769802 -0.737043 0. 0.088753 -0. 1.509208\n", " -0. 2.279009]\n", "t = 2.6s; x = [-0.076287 -0.125968 0. -1.976272 -0. 0.203619 0.\n", " 0.279906 -0.677891 -0.690837 0. 0.334048 -0. 1.395512\n", " -0. 2.073402]\n", "t = 2.7s; x = [-0.135185 -0.191745 0. -1.935898 -0. 0.332646 0.\n", " 0.467831 -0.488388 -0.622666 0. 0.448309 0. 1.170741\n", " -0. 1.659128]\n", "t = 2.8s; x = [-0.172448 -0.250373 0. -1.891602 -0. 0.43564 0.\n", " 0.608088 -0.252851 -0.550162 0. 0.415574 0. 0.8816\n", " -0. 1.13445 ]\n", "t = 2.9s; x = [-0.185451 -0.301926 0. -1.856447 -0. 0.507895 0.\n", " 0.693346 -0.007536 -0.481639 0. 0.273742 0. 0.559729\n", " -0. 0.567265]\n", "t = 3.0s; x = [-0.174253 -0.34677 0. -1.838732 0. 0.547048 0.\n", " 0.7213 0.229214 -0.41486 0. 0.075597 0. 0.221509\n", " -0. -0.007704]\n", "t = 3.1s; x = [-0.140171 -0.38463 0. -1.841478 0. 0.551984 0.\n", " 0.692155 0.449125 -0.339878 0. -0.127158 0. -0.123005\n", " -0. -0.57213 ]\n", "t = 3.2s; x = [-0.085191 -0.414004 0. -1.862683 0. 0.522605 0.\n", " 0.607796 0.64589 -0.24272 0. -0.285699 0. -0.462402\n", " -0. -1.108291]\n", "t = 3.3s; x = [-0.012158 -0.431987 0. -1.895622 0. 0.460294 0.\n", " 0.472452 0.80717 -0.110361 0. -0.355159 0. -0.777477\n", " -0. -1.584648]\n", "t = 3.4s; x = [ 0.07422 -0.434725 0. -1.9297 0. 0.36898 0.\n", " 0.29476 0.907315 0.061685 0. -0.306387 0. -1.035991\n", " -0. -1.943306]\n", "t = 3.5s; x = [ 0.166004 -0.418721 0. -1.953309 0. 0.256398 0.\n", " 0.090394 0.909229 0.260699 0. -0.153138 0. -1.196263\n", " -0. -2.105492]\n", "t = 3.6s; x = [ 0.25185 -0.382669 0. -1.959233 0. 0.134017 0.\n", " -0.117833 0.787029 0.457195 -0. 0.031805 0. -1.230168\n", " -0. -2.017197]\n", "t = 3.7s; x = [ 0.319593 -0.328339 0. -1.949016 0. 0.01432 0.\n", " -0.305273 0.552272 0.622895 -0. 0.157192 -0. -1.147119\n", " -0. -1.699391]\n", "t = 3.8s; x = [ 0.359918 -0.25947 0. -1.931389 0. -0.092688 0.\n", " -0.452606 0.246287 0.747957 -0. 0.177821 -0. -0.982556\n", " -0. -1.228843]\n", "t = 3.9s; x = [ 0.367922 -0.179921 0. -1.916567 0. -0.180557 0.\n", " -0.548479 -0.087912 0.83801 -0. 0.106545 -0. -0.768435\n", " -0. -0.680523]\n", "t = 4.0s; x = [ 0.342452 -0.092742 -0. -1.91172 0. -0.24533 0.\n", " -0.587782 -0.418923 0.901641 -0. -0.013499 -0. -0.522756\n", " -0. -0.103833]\n", "t = 4.1s; x = [ 2.849720e-01 -3.680000e-04 -0.000000e+00 -1.919201e+00 0.000000e+00\n", " -2.844100e-01 0.000000e+00 -5.693820e-01 -7.249830e-01 9.416500e-01\n", " -0.000000e+00 -1.316380e-01 -0.000000e+00 -2.563700e-01 -0.000000e+00\n", " 4.686130e-01]\n", "t = 4.2s; x = [ 0.198894 0.094607 -0. -1.936365 -0. -0.296357 0.\n", " -0.49525 -0.987784 0.951919 -0. -0.199838 -0. 0.016853\n", " -0. 1.004637]\n", "t = 4.3s; x = [ 0.089561 0.188585 -0. -1.956237 -0. -0.281611 0.\n", " -0.371172 -1.18597 0.91929 -0. -0.181501 -0. 0.272267\n", " -0. 1.458236]\n", "t = 4.4s; x = [-0.035195 0.276593 -0. -1.969422 -0. -0.243735 0.\n", " -0.20854 -1.291149 0.831136 -0. -0.068441 -0. 0.4727\n", " -0. 1.763849]\n", "t = 4.5s; x = [-0.164566 0.352915 -0. -1.968025 -0. -0.190216 0.\n", " -0.025651 -1.274579 0.687082 -0. 0.099287 -0. 0.580206\n", " -0. 1.854785]\n", "t = 4.6s; x = [-0.285679 0.41273 -0. -1.950422 -0. -0.131333 0.\n", " 0.154346 -1.12713 0.505162 0. 0.241755 -0. 0.580775\n", " -0. 1.707904]\n", "t = 4.7s; x = [-0.386308 0.453583 -0. -1.922907 -0. -0.076965 0.\n", " 0.309343 -0.870428 0.311945 0. 0.290029 0. 0.495375\n", " -0. 1.365803]\n", "t = 4.8s; x = [-0.457373 0.47539 -0. -1.896196 -0. -0.033913 0.\n", " 0.423461 -0.542126 0.126182 0. 0.227348 0. 0.360123\n", " -0. 0.902249]\n", "t = 4.9s; x = [-0.493501 0.479251 -0. -1.880207 -0. -0.005668 0.\n", " 0.487833 -0.176637 -0.047085 0. 0.0827 0. 0.202498\n", " -0. 0.379136]\n" ] }, { "name": "stdout", "output_type": "stream", "text": [ "t = 5.0s; x = [-0.492329 0.466247 -0. -1.880739 -0. 0.006299 -0.\n", " 0.498628 0.200063 -0.212253 0. -0.094139 0. 0.035985\n", " -0. -0.164078]\n", "t = 5.1s; x = [-4.538330e-01 4.368320e-01 0.000000e+00 -1.898273e+00 -0.000000e+00\n", " 1.458000e-03 -0.000000e+00 4.552910e-01 5.662780e-01 -3.763170e-01\n", " 0.000000e+00 -2.482340e-01 0.000000e+00 -1.324050e-01 -0.000000e+00\n", " -6.986830e-01]\n", "t = 5.2s; x = [-0.380202 0.390898 0. -1.927862 -0. -0.019887 -0.\n", " 0.360315 0.898304 -0.542501 0. -0.327352 0. -0.291271\n", " -0. -1.189575]\n", "t = 5.3s; x = [-0.27634 0.328437 0. -1.959946 0. -0.055678 -0.\n", " 0.220662 1.165052 -0.704908 0. -0.294531 0. -0.41595\n", " -0. -1.581002]\n", "t = 5.4s; x = [-0.150638 0.250606 0. -1.983014 0. -0.100687 -0.\n", " 0.04995 1.329073 -0.846293 0. -0.15214 0. -0.469327\n", " -0. -1.7984 ]\n", "t = 5.5s; x = [-0.014967 0.16065 0. -1.988623 0. -0.146097 -0.\n", " -0.13113 1.361738 -0.94398 -0. 0.041029 0. -0.421271\n", " -0. -1.783009]\n", "t = 5.6s; x = [ 0.117268 0.063765 0. -1.976088 0. -0.181569 -0.\n", " -0.298836 1.262988 -0.983963 -0. 0.197288 0. -0.274087\n", " -0. -1.537075]\n", "t = 5.7s; x = [ 0.234106 -0.034305 0. -1.952453 0. -0.198708 -0.\n", " -0.432813 1.059191 -0.969466 -0. 0.257812 0. -0.061374\n", " -0. -1.120564]\n", "t = 5.8s; x = [ 0.326684 -0.128784 -0. -1.927965 0. -0.192994 -0.\n", " -0.519678 0.78274 -0.914701 -0. 0.217205 -0. 0.17718\n", " -0. -0.60556 ]\n", "t = 5.9s; x = [ 0.389108 -0.216364 -0. -1.911353 0. -0.163362 -0.\n", " -0.55247 0.459886 -0.83318 -0. 0.107269 -0. 0.413543\n", " -0. -0.046343]\n", "t = 6.0s; x = [ 0.417814 -0.294734 -0. -1.907245 0. -0.11087 -0.\n", " -0.528684 0.111771 -0.730702 -0. -0.024481 -0. 0.632597\n", " -0. 0.520826]\n", "t = 6.1s; x = [ 0.411335 -0.361717 -0. -1.915256 0. -0.037732 -0.\n", " -0.449067 -0.23979 -0.604438 -0. -0.126716 -0. 0.824755\n", " -0. 1.064544]\n", "t = 6.2s; x = [ 0.370677 -0.414568 -0. -1.930029 0. 0.052726 -0.\n", " -0.317951 -0.56625 -0.446813 -0. -0.153327 0. 0.975656\n", " -0. 1.541906]\n", "t = 6.3s; x = [ 0.300207 -0.449879 -0. -1.942546 -0. 0.155128 -0.\n", " -0.14508 -0.829003 -0.253973 -0. -0.081217 0. 1.05806\n", " -0. 1.887063]\n", "t = 6.4s; x = [ 0.208396 -0.464521 -0. -1.943751 -0. 0.260867 -0.\n", " 0.05247 -0.987473 -0.03689 -0. 0.063765 0. 1.03738\n", " -0. 2.024853]\n", "t = 6.5s; x = [ 0.1069 -0.457392 -0. -1.929877 -0. 0.358537 -0.\n", " 0.251637 -1.022525 0.176033 0. 0.206125 0. 0.896911\n", " -0. 1.919436]\n", "t = 6.6s; x = [ 0.007524 -0.430479 -0. -1.905205 -0. 0.436842 -0.\n", " 0.429318 -0.949811 0.354961 0. 0.270662 -0. 0.655635\n", " -0. 1.605447]\n", "t = 6.7s; x = [-0.080552 -0.387997 -0. -1.879329 -0. 0.487605 -0.\n", " 0.568157 -0.801767 0.486942 0. 0.230068 -0. 0.352642\n", " -0. 1.154409]\n", "t = 6.8s; x = [-0.1512 -0.334502 -0. -1.862132 -0. 0.506493 -0.\n", " 0.657693 -0.604536 0.577054 0. 0.102447 -0. 0.022903\n", " -0. 0.627438]\n", "t = 6.9s; x = [-0.200309 -0.273532 -0. -1.860431 -0. 0.492064 -0.\n", " 0.692373 -0.372715 0.638854 0. -0.07245 -0. -0.310854\n", " -0. 0.061861]\n", "t = 7.0s; x = [-0.224882 -0.207229 -0. -1.876625 -0. 0.444645 -0.\n", " 0.669527 -0.115262 0.685501 0. -0.24721 -0. -0.635052\n", " -0. -0.519789]\n", "t = 7.1s; x = [-0.222861 -0.136682 -0. -1.908148 -0. 0.365717 -0.\n", " 0.588578 0.156714 0.724266 0. -0.370232 -0. -0.939105\n", " -0. -1.095819]\n", "t = 7.2s; x = [-0.193733 -0.062679 0. -1.947172 -0. 0.257996 -0.\n", " 0.451729 0.4221 0.75355 0. -0.389071 -0. -1.207318\n", " -0. -1.629418]\n", "t = 7.3s; x = [-0.139918 0.013356 0. -1.981181 -0. 0.126434 -0.\n", " 0.266351 0.64256 0.762527 0. -0.267128 -0. -1.409386\n", " -0. -2.051947]\n", "t = 7.4s; x = [-0.068493 0.088647 0. -1.99635 -0. -0.020148 -0.\n", " 0.048345 0.765826 0.736672 0. -0.021991 -0. -1.500156\n", " -0. -2.265982]\n", "t = 7.5s; x = [ 0.008699 0.159346 0. -1.984645 -0. -0.168702 -0.\n", " -0.177401 0.755625 0.671517 0. 0.249719 -0. -1.446806\n", " -0. -2.202431]\n", "t = 7.6s; x = [ 0.078556 0.222101 0. -1.94962 -0. -0.305066 -0.\n", " -0.383623 0.625343 0.581226 0. 0.428416 -0. -1.262222\n", " -0. -1.887566]\n", "t = 7.7s; x = [ 0.131377 0.275477 0. -1.904039 -0. -0.418286 -0.\n", " -0.549662 0.423381 0.48704 0. 0.458552 0. -0.991793\n", " 0. -1.415174]\n", "t = 7.8s; x = [ 0.162341 0.319825 0. -1.862351 0. -0.501947 -0.\n", " -0.664289 0.193979 0.401735 0. 0.357031 0. -0.676348\n", " 0. -0.870327]\n", "t = 7.9s; x = [ 0.170113 0.356156 0. -1.835421 0. -0.552852 -0.\n", " -0.722965 -0.037711 0.325978 0. 0.172153 0. -0.339378\n", " 0. -0.301667]\n", "t = 8.0s; x = [ 0.155097 0.385071 0. -1.828947 0. -0.569518 -0.\n", " -0.724614 -0.260663 0.25152 0. -0.043556 0. 0.006813\n", " 0. 0.267476]\n", "t = 8.1s; x = [ 0.118447 0.406065 0. -1.843484 0. -0.55149 -0.\n", " -0.669937 -0.469503 0.165193 0. -0.239899 0. 0.352773\n", " 0. 0.822276]\n", "t = 8.2s; x = [ 0.061906 0.417241 0. -1.874652 0. -0.4994 -0.\n", " -0.561306 -0.656601 0.053059 0. -0.368324 0. 0.68514\n", " 0. 1.341742]\n", "t = 8.3s; x = [-0.011567 0.415496 0. -1.913419 0. -0.415687 -0.\n", " -0.40412 -0.803866 -0.093879 0. -0.386133 0. 0.980086\n", " 0. 1.783953]\n", "t = 8.4s; x = [-0.096416 0.397405 0. -1.947532 0. -0.305896 -0.\n", " -0.20948 -0.877477 -0.271683 0. -0.276507 0. 1.199523\n", " 0. 2.077 ]\n", "t = 8.5s; x = [-0.183251 0.360854 0. -1.965717 0. -0.179728 -0.\n", " 0.003523 -0.838361 -0.458152 -0. -0.079803 0. 1.302194\n", " 0. 2.140555]\n", "t = 8.6s; x = [-0.259797 0.306536 0. -1.963754 0. -0.049919 -0.\n", " 0.209879 -0.673132 -0.622409 -0. 0.109588 0. 1.273489\n", " 0. 1.946621]\n", "t = 8.7s; x = [-0.314573 0.237758 0. -1.946933 0. 0.071384 -0.\n", " 0.385957 -0.410066 -0.746069 -0. 0.208315 -0. 1.138228\n", " 0. 1.548294]\n", "t = 8.8s; x = [-0.340116 0.158654 -0. -1.925894 0. 0.175469 -0.\n", " 0.515584 -0.096022 -0.830209 -0. 0.19556 -0. 0.934852\n", " 0. 1.030874]\n", "t = 8.9s; x = [-0.333406 0.072709 -0. -1.910588 0. 0.257002 -0.\n", " 0.590408 0.229606 -0.884455 -0. 0.10078 -0. 0.690428\n", " 0. 0.460822]\n", "t = 9.0s; x = [-0.294696 -0.017472 -0. -1.906859 0. 0.312667 -0.\n", " 0.607363 0.54046 -0.915408 -0. -0.02749 -0. 0.419341\n", " 0. -0.12112 ]\n", "t = 9.1s; x = [-0.226417 -0.109549 -0. -1.915515 -0. 0.340343 -0.\n", " 0.56676 0.818244 -0.921298 -0. -0.138859 -0. 0.132807\n", " 0. -0.685438]\n", "t = 9.2s; x = [-0.132709 -0.200559 -0. -1.932551 -0. 0.339268 -0.\n", " 0.471977 1.045974 -0.891848 -0. -0.188573 -0. -0.151899\n", " 0. -1.197873]\n", "t = 9.3s; x = [-0.019621 -0.286265 -0. -1.950145 -0. 0.31098 -0.\n", " 0.330601 1.201514 -0.813125 -0. -0.14752 -0. -0.405589\n", " 0. -1.607103]\n", "t = 9.4s; x = [ 0.104202 -0.36126 -0. -1.959122 -0. 0.260506 -0.\n", " 0.156304 1.255756 -0.677617 -0. -0.021197 -0. -0.589022\n", " 0. -1.844778]\n", "t = 9.5s; x = [ 0.227285 -0.420186 -0. -1.953311 -0. 0.196661 -0.\n", " -0.030624 1.18436 -0.494978 0. 0.135727 -0. -0.669744\n", " 0. -1.854104]\n", "t = 9.6s; x = [ 0.336861 -0.459542 -0. -1.933693 -0. 0.130117 -0.\n", " -0.206744 0.98852 -0.290994 0. 0.242626 -0. -0.645665\n", " 0. -1.634185]\n", "t = 9.7s; x = [ 0.4218 -0.478554 -0. -1.908291 -0. 0.07009 -0.\n", " -0.35171 0.697825 -0.091407 0. 0.247278 0. -0.545308\n", " 0. -1.243133]\n", "t = 9.8s; x = [ 0.474546 -0.4784 -0. -1.88766 -0. 0.022446 -0.\n", " -0.4521 0.350659 0.091531 0. 0.15121 0. -0.402803\n", " 0. -0.753462]\n", "t = 9.9s; x = [ 0.491116 -0.460785 -0. -1.880154 -0. -0.009805 -0.\n", " -0.500921 -0.021159 0.25867 0. -0.007265 0. -0.239872\n", " 0. -0.218713]\n" ] }, { "name": "stdout", "output_type": "stream", "text": [ "t = 10.0s; x = [ 0.470296 -0.426967 -0. -1.889457 -0. -0.02518 -0.\n", " -0.495476 -0.393519 0.416739 0. -0.175933 0. -0.066447\n", " 0. 0.327072]\n" ] }, { "data": { "text/plain": [ "" ] }, "execution_count": 8, "metadata": {}, "output_type": "execute_result" } ], "source": [ "sim.sp.advanceTo(10)" ] }, { "cell_type": "markdown", "id": "80b28389", "metadata": {}, "source": [ "## Python script\n", "Let's use the simulation we already have to create a Python script. This will dump just the simulation information." ] }, { "cell_type": "code", "execution_count": 9, "id": "0e6f29af", "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "[warning] Model auto_time_display (TimeDisplay) does not have a \"toDS\" method, so it cannot be added to the StatePropagatorDS.\n", "[warning] Model data_plotter (DataPlotter) does not have a \"toDS\" method, so it cannot be added to the StatePropagatorDS.\n", "1 file reformatted\n" ] } ], "source": [ "tf = NamedTemporaryFile(suffix=\".py\")\n", "sim.toDS().toFile(tf.name)" ] }, { "cell_type": "markdown", "id": "1ad920f2", "metadata": {}, "source": [ "This call will show the file in the Jupyter notebook" ] }, { "cell_type": "code", "execution_count": 10, "id": "5df98bbd", "metadata": {}, "outputs": [ { "data": { "text/markdown": [ "```python\n", "# This is an auto-generated file for the Sim\n", "# This file was generated on 2026-06-30 10:03:45.715437\n", "import numpy as np\n", "import Karana.Dynamics as kd\n", "import Karana.Scene as ks\n", "import Karana.Models as kmdl\n", "from typing import cast\n", "import Karana.Integrators as ki\n", "import Karana.Models as kmo\n", "import atexit\n", "import Karana.Math as km\n", "from Karana.KUtils.Sim import Sim\n", "from Karana.Math.Kquantities import ureg\n", "\n", "\n", "def populateMultibodyAndStatePropagator(mb: kd.Multibody, sp: kd.StatePropagator):\n", " \"\"\"Populate and configure the provided Multibody and StatePropagator.\n", "\n", " Parameters\n", " ----------\n", " mb : kd.Multibody\n", " The Multibody to populate with physical bodies.\n", " sp : kd.StatePropagator\n", " The StatePropagator to populate and configure.\n", " \"\"\"\n", " # This creates, adds, and connects physical bodies via hinges,\n", " # also referred to as joints, to the provided Multibody instance.\n", " # Often, the input `Multibody' instance is empty and contains just a\n", " # virtual root body.\n", " #\n", " # The code below consists of repeated blocks - one per body. Each block\n", " # defines the mass properties for the body, the parent hinge type and\n", " # location on the body and its parent body, and the mesh geometries attached.\n", " # ----------------------------------------------------------------------------------------------\n", " # Look up the unique virtual root body for the multibody system that\n", " # will serve as an anchor for the new physical bodies.\n", " vroot = mb.virtualRoot()\n", "\n", " # ----------------------------------------------------------------------------------------------\n", " # Create 'bd1' body\n", " # ----------------------------------------------------------------------------------------------\n", "\n", " # ----------------------------------------------------------------------------------------------\n", " # Start defining meshes for 'bd1' body\"\n", "\n", " # Create one or more meshes attached to the 'bd1' body. Each mesh\n", " # consists of the shape and color/texture definition for its visual appearance.\n", " # The shapes can be parameterized shapes (e.g., spheres, boxes, etc., or be\n", " # defined in external files such as '.glb', '.obj', etc. formats.\n", "\n", " # ----------------------------------------------------------------------------------------------\n", " # Define the 'bd1_body' mesh parameters for 'bd1' body\n", " # Define the part shape (geometry)\n", " box = ks.BoxGeometry(0.05000000074505806, 0.05000000074505806, 1.0)\n", "\n", " # Define the part color/texture material\n", " physical_material_info = ks.PhysicalMaterialInfo()\n", " physical_material_info.color = ks.Color.fromRGBA(\n", " 0.6980392336845398, 0.13333334028720856, 0.13333334028720856, 1.0\n", " )\n", " physical_material_info.roughness = 1.0\n", " physical_material = ks.PhysicalMaterial(physical_material_info)\n", " bd1_body = ks.ScenePartSpec(\n", " name=\"bd1_body\",\n", " transform=km.HomTran(\n", " km.UnitQuaternion(np.array([0.0, 0.0, 0.0, 1.0])),\n", " np.array([0.0, 0.0, 0.0]) * ureg.meter,\n", " ),\n", " geometry=box,\n", " material=physical_material,\n", " scale=np.array([1.0, 1.0, 1.0]),\n", " layers=419430400,\n", " )\n", "\n", " # ----------------------------------------------------------------------------------------------\n", " # Define the 'bd1_pivot' mesh parameters for 'bd1' body\n", " # Define the part shape (geometry)\n", " cylinder = ks.CylinderGeometry(0.10000000149011612, 0.10000000149011612)\n", "\n", " # Define the part color/texture material\n", " physical_material_1_info = ks.PhysicalMaterialInfo()\n", " physical_material_1_info.color = ks.Color.fromRGBA(\n", " 0.6980392336845398, 0.13333334028720856, 0.13333334028720856, 1.0\n", " )\n", " physical_material_1_info.roughness = 1.0\n", " physical_material_1 = ks.PhysicalMaterial(physical_material_1_info)\n", " bd1_pivot = ks.ScenePartSpec(\n", " name=\"bd1_pivot\",\n", " transform=km.HomTran(\n", " km.UnitQuaternion(np.array([0.0, 0.0, 0.0, 1.0])),\n", " np.array([0.0, 0.0, -0.5]) * ureg.meter,\n", " ),\n", " geometry=cylinder,\n", " material=physical_material_1,\n", " scale=np.array([1.0, 1.0, 1.0]),\n", " layers=419430400,\n", " )\n", "\n", " # End defining meshes for 'bd1' body\n", " # ----------------------------------------------------------------------------------------------\n", "\n", " # Assemble overall parameters needed to define the 'bd1' body\n", " # including mass and kinematics properties.\n", " bd1_params = kd.PhysicalBodyParams(\n", " # 'bd1' mass properties\n", " spI=km.SpatialInertia(\n", " 2.0 * ureg.kilogram,\n", " np.array([0.0, 0.0, 0.0]) * ureg.meter,\n", " np.array([[3.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 1.0]])\n", " * ureg.kilogram\n", " * ureg.meter**2,\n", " ),\n", " # Define the location and orientation of the 'bd1' parent hinge\n", " # relative to the 'bd1' body frame\n", " body_to_joint_transform=km.HomTran(\n", " km.UnitQuaternion(np.array([0.0, 0.0, 0.0, 1.0])),\n", " np.array([-0.0, -0.0, 0.5]) * ureg.meter,\n", " ),\n", " # Define the location and orientation of the 'bd1' parent hinge\n", " # relative to the parent body's frame\n", " inb_to_joint_transform=km.HomTran(\n", " km.UnitQuaternion(np.array([0.0, 0.0, 0.0, 1.0])),\n", " np.array([0.0, 0.0, 0.0]) * ureg.meter,\n", " ),\n", " # define the 'bd1' parent hinge type and parameters\n", " hinge_params=kd.PhysicalHingeParams(\n", " hinge_type=kd.HingeType.REVOLUTE,\n", " subhinge_params=[\n", " kd.PinSubhingeParams(\n", " unit_axis=np.array([0.0, 1.0, 0.0]),\n", " prescribed=False,\n", " ),\n", " ],\n", " ),\n", " # Add 'bd1' scene parts defined above\n", " scene_part_specs=[bd1_body, bd1_pivot],\n", " )\n", "\n", " # Create the 'bd1' body\n", " bd1 = kd.PhysicalBody(\"bd1\", mb)\n", "\n", " # Connect the 'bd1' body to its parent via a hinge\n", " kd.PhysicalHinge(vroot, bd1, kd.HingeType.REVOLUTE)\n", "\n", " # Set the parameters for the 'bd1' body\n", " bd1.setParams(bd1_params)\n", "\n", " # Constraint node bd1_bd2_loop_constraint_source\n", " bd1_bd2_loop_constraint_source = kd.ConstraintNode.lookupOrCreate(\n", " \"bd1_bd2_loop_constraint_source\", bd1\n", " )\n", " bd1_bd2_loop_constraint_source.setBodyToNodeTransform(\n", " km.HomTran(\n", " km.UnitQuaternion(np.array([0.0, 0.0, 0.0, 1.0])),\n", " np.array([0.0, 0.0, -0.5]) * ureg.meter,\n", " )\n", " )\n", "\n", " # ----------------------------------------------------------------------------------------------\n", " # Create 'bd2' body\n", " # ----------------------------------------------------------------------------------------------\n", "\n", " # ----------------------------------------------------------------------------------------------\n", " # Start defining meshes for 'bd2' body\"\n", "\n", " # Create one or more meshes attached to the 'bd2' body. Each mesh\n", " # consists of the shape and color/texture definition for its visual appearance.\n", " # The shapes can be parameterized shapes (e.g., spheres, boxes, etc., or be\n", " # defined in external files such as '.glb', '.obj', etc. formats.\n", "\n", " # ----------------------------------------------------------------------------------------------\n", " # Define the 'bd2_body' mesh parameters for 'bd2' body\n", " # Define the part shape (geometry)\n", " box_1 = ks.BoxGeometry(0.05000000074505806, 0.05000000074505806, 1.0)\n", "\n", " # Define the part color/texture material\n", " physical_material_2_info = ks.PhysicalMaterialInfo()\n", " physical_material_2_info.color = ks.Color.fromRGBA(\n", " 0.6980392336845398, 0.13333334028720856, 0.13333334028720856, 1.0\n", " )\n", " physical_material_2_info.roughness = 1.0\n", " physical_material_2 = ks.PhysicalMaterial(physical_material_2_info)\n", " bd2_body = ks.ScenePartSpec(\n", " name=\"bd2_body\",\n", " transform=km.HomTran(\n", " km.UnitQuaternion(np.array([0.0, 0.0, 0.0, 1.0])),\n", " np.array([0.0, 0.0, 0.5]) * ureg.meter,\n", " ),\n", " geometry=box_1,\n", " material=physical_material_2,\n", " scale=np.array([1.0, 1.0, 1.0]),\n", " layers=419430400,\n", " )\n", "\n", " # ----------------------------------------------------------------------------------------------\n", " # Define the 'bd2_end' mesh parameters for 'bd2' body\n", " # Define the part shape (geometry)\n", " cylinder_1 = ks.CylinderGeometry(0.10000000149011612, 0.10000000149011612)\n", "\n", " # Define the part color/texture material\n", " physical_material_3_info = ks.PhysicalMaterialInfo()\n", " physical_material_3_info.color = ks.Color.fromRGBA(1.0, 0.843137264251709, 0.0, 1.0)\n", " physical_material_3_info.roughness = 1.0\n", " physical_material_3 = ks.PhysicalMaterial(physical_material_3_info)\n", " bd2_end = ks.ScenePartSpec(\n", " name=\"bd2_end\",\n", " transform=km.HomTran(\n", " km.UnitQuaternion(np.array([0.0, 0.0, 0.0, 1.0])),\n", " np.array([0.0, 0.0, 0.0]) * ureg.meter,\n", " ),\n", " geometry=cylinder_1,\n", " material=physical_material_3,\n", " scale=np.array([2.0, 2.0, 2.0]),\n", " layers=419430400,\n", " )\n", "\n", " # End defining meshes for 'bd2' body\n", " # ----------------------------------------------------------------------------------------------\n", "\n", " # Assemble overall parameters needed to define the 'bd2' body\n", " # including mass and kinematics properties.\n", " bd2_params = kd.PhysicalBodyParams(\n", " # 'bd2' mass properties\n", " spI=km.SpatialInertia(\n", " 2.0 * ureg.kilogram,\n", " np.array([0.0, 0.0, 0.0]) * ureg.meter,\n", " np.array([[3.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 1.0]])\n", " * ureg.kilogram\n", " * ureg.meter**2,\n", " ),\n", " # Define the location and orientation of the 'bd2' parent hinge\n", " # relative to the 'bd2' body frame\n", " body_to_joint_transform=km.HomTran(\n", " km.UnitQuaternion(np.array([0.0, 0.0, 0.0, 1.0])),\n", " np.array([-0.0, -0.0, -0.0]) * ureg.meter,\n", " ),\n", " # Define the location and orientation of the 'bd2' parent hinge\n", " # relative to the parent body's frame\n", " inb_to_joint_transform=km.HomTran(\n", " km.UnitQuaternion(np.array([0.0, 0.0, 0.0, 1.0])),\n", " np.array([0.0, 0.0, 0.0]) * ureg.meter,\n", " ),\n", " # define the 'bd2' parent hinge type and parameters\n", " hinge_params=kd.PhysicalHingeParams(\n", " hinge_type=kd.HingeType.FULL6DOF,\n", " subhinge_params=[\n", " kd.Linear3SubhingeParams(prescribed=False),\n", " kd.SphericalSubhingeParams(prescribed=False),\n", " ],\n", " ),\n", " # Add 'bd2' scene parts defined above\n", " scene_part_specs=[bd2_body, bd2_end],\n", " )\n", "\n", " # Create the 'bd2' body\n", " bd2 = kd.PhysicalBody(\"bd2\", mb)\n", "\n", " # Connect the 'bd2' body to its parent via a hinge\n", " kd.PhysicalHinge(vroot, bd2, kd.HingeType.FULL6DOF)\n", "\n", " # Set the parameters for the 'bd2' body\n", " bd2.setParams(bd2_params)\n", "\n", " # Constraint node bd1_bd2_loop_constraint_target\n", " bd1_bd2_loop_constraint_target = kd.ConstraintNode.lookupOrCreate(\n", " \"bd1_bd2_loop_constraint_target\", bd2\n", " )\n", " bd1_bd2_loop_constraint_target.setBodyToNodeTransform(\n", " km.HomTran(\n", " km.UnitQuaternion(np.array([0.0, 0.0, 0.0, 1.0])),\n", " np.array([-0.0, -0.0, 1.0]) * ureg.meter,\n", " )\n", " )\n", "\n", " # ----------------------------------------------------------------------------------------------\n", " # Start defining parts attached to the virtual root\n", " # ----------------------------------------------------------------------------------------------\n", " # Define the 'obstacle_part' mesh parameters for virtual root\n", " # Define the part shape (geometry)\n", " box_2 = ks.BoxGeometry(0.25, 0.25, 0.25)\n", "\n", " # Define the part color/texture material\n", " physical_material_4_info = ks.PhysicalMaterialInfo()\n", " physical_material_4_info.color = ks.Color.fromRGBA(\n", " 0.49803921580314636, 1.0, 0.8313725590705872, 1.0\n", " )\n", " physical_material_4_info.roughness = 1.0\n", " physical_material_4 = ks.PhysicalMaterial(physical_material_4_info)\n", " obstacle_part = ks.ScenePartSpec(\n", " name=\"obstacle_part\",\n", " transform=km.HomTran(\n", " km.UnitQuaternion(np.array([0.0, 0.0, 0.0, 1.0])),\n", " np.array([0.0, 0.0, 0.0]) * ureg.meter,\n", " ),\n", " geometry=box_2,\n", " material=physical_material_4,\n", " scale=np.array([1.0, 1.0, 1.0]),\n", " layers=419430400,\n", " )\n", "\n", " vroot.addScenePartSpec(obstacle_part)\n", "\n", " # End defining parts attached to the virtual root\n", " # ----------------------------------------------------------------------------------------------\n", "\n", " # bd1_bd2_loop_constraint kd.LoopConstraintCutJoint\n", " lc = kd.LoopConstraintCutJoint(\n", " \"bd1_bd2_loop_constraint\",\n", " mb,\n", " bd1_bd2_loop_constraint_source.frameToFrame(bd1_bd2_loop_constraint_target),\n", " kd.HingeType.REVOLUTE,\n", " )\n", " sh = cast(kd.PinSubhinge, lc.hinge().subhinge(0))\n", " sh.setUnitAxis(np.array([0.0, 1.0, 0.0]))\n", " # ----------------------------------------------------------------------------------------------\n", " # Finished physical body additions\n", " # ----------------------------------------------------------------------------------------------\n", "\n", " # Finalize the multibody for the added bodies\n", " mb.ensureHealthy()\n", "\n", " # ----------------------------------------------------------------------------------------------\n", " # Populate and configure the StatePropagator. This will involve\n", " # defining the type of solver to use for the multibody equations of\n", " # motion, adding an integrator to use for state propagation,\n", " # addition of KModels for interacting with the multibody dynamics,\n", " # and timed events for execution during time advancement.\n", "\n", " # ----------------------------------------------------------------------------------------------\n", " # Set the StatePropagator's solver type. 'FORWARD_DYNAMICS' is the\n", " # typical choice for setting up the propagator to solve the\n", " # equations of motion for time domain simulations\n", " sp.setSolverType(kd.MMSolverType.FORWARD_DYNAMICS)\n", "\n", " # Set up the integrator\n", " sp.setIntegrator(ki.IntegratorType.RK4)\n", "\n", " # ----------------------------------------------------------------------------------------------\n", " # Set the StatePropagator options\n", "\n", " # Call with True to make an extra dynamics derivative call at the\n", " # end of a simulation step to update the acceleration values.\n", " sp.setUpdateStateDerivativesHopEnd(False)\n", "\n", " # Set a limit on the maximum integration time step size.\n", " # A value of 0.0 means there is no maximum limit.\n", " sp.setMaxStepSize(0.0)\n", "\n", " # ----------------------------------------------------------------------------------------------\n", " # Add KModels. KModels are parameterized models for\n", " # component actuator, sensors, environmental, etc. that interact\n", " # with the dynamics.\n", " # ----------------------------------------------------------------------------------------------\n", "\n", " # ----------------------------------------------------------------------------------------------\n", " # Add the update_proxy_scene KModel\n", " multibody_scene = mb.getScene()\n", " if multibody_scene is not None:\n", " update_proxy_scene = kmdl.UpdateProxyScene(\"update_proxy_scene\", sp, multibody_scene)\n", "\n", " # ----------------------------------------------------------------------------------------------\n", " # Add the gil_release KModel\n", " gil_release = kmdl.GilRelease(\"gil_release\", sp)\n", "\n", " # ----------------------------------------------------------------------------------------------\n", " # Add the sync_real_time KModel\n", " sync_real_time = kmdl.SyncRealTime(\"sync_real_time\", sp, 1.0)\n", "\n", " # ----------------------------------------------------------------------------------------------\n", " # Add the grav_model KModel\n", " uniform_gravity = kmo.UniformGravity(\"uniform_gravity\")\n", " uniform_gravity.setGravity(\n", " np.array([0.0, 0.0, -9.81]) * ureg.meter / ureg.second**2,\n", " 10.0,\n", " kmo.OutputUpdateType.PRE_DERIV,\n", " )\n", "\n", " grav_model = kmdl.Gravity(\"grav_model\", sp, uniform_gravity, sp.getSubTree())\n", "\n", " # Finalize the StatePropagator for the updated multibody system, the\n", " # KModels and the integrator\n", " sp.hardReset()\n", " sp.ensureHealthy()\n", "\n", " # End setting up StatePropagator and KModels\n", " # ----------------------------------------------------------------------------------------------\n", "\n", "\n", "if __name__ == \"__main__\":\n", " # Create a simulation\n", " sim = Sim()\n", "\n", " # Populate the Multibody and StatePropagator for the Sim\n", " populateMultibodyAndStatePropagator(sim.mb, sim.sp)\n", "\n", " # Code to cleanup upon exit\n", " def cleanup():\n", " global sim\n", " del sim\n", "\n", " atexit.register(cleanup)\n", "\n", "```" ], "text/plain": [ "" ] }, "execution_count": 10, "metadata": {}, "output_type": "execute_result" } ], "source": [ "from IPython.display import Markdown\n", "from pathlib import Path\n", "\n", "Markdown(f\"```python\\n{Path(tf.name).read_text()}\\n```\")" ] }, { "cell_type": "markdown", "id": "073230ce", "metadata": {}, "source": [ "Above, you'll see the multibody code similar to the [system level notebook](../example_system_level/notebook.ipynb), but with the addition of cut-joint constraint between the first and second pendulums." ] }, { "cell_type": "code", "execution_count": 11, "id": "efbf99cc", "metadata": {}, "outputs": [], "source": [ "del sim\n", "with open(tf.name, \"r\") as f:\n", " exec(f.read())" ] }, { "cell_type": "markdown", "id": "4b60265b", "metadata": {}, "source": [ "Let's bring up the GUI again just to show we captured everything" ] }, { "cell_type": "code", "execution_count": 12, "id": "0c014a29", "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "[WebUI] Web server is running on port 43743\n", "You may be able to connect in your browser at:\n", "\t\u001b[1mhttp://newton:43743\u001b[0m\n", "[WebUI] Karana Viewer is running on port 29534\n", "Open this URL to connect:\n", "\t\u001b[1mhttp://newton:29534\u001b[0m\n" ] }, { "data": { "text/html": [ "\n", " \n", " Click to open Karana Viewer in a new tab\n", " \n", " " ], "text/plain": [ "" ] }, "metadata": {}, "output_type": "display_data" } ], "source": [ "# Create the GUI\n", "sim.setupGui()\n", "\n", "# Update the camera view\n", "sim.mb.resetData()\n", "sim.mb.cks().solveQ()\n", "sim.mb.getScene().viewAroundFrame(\n", " sim.mb.virtualRoot(), np.array([0.0, 5.0, 0.0]), np.zeros(3), np.array([0.0, 0.0, 1.0])\n", ")\n", "\n", "# Redraw the scene\n", "sim.mb.getScene().update()" ] }, { "cell_type": "markdown", "id": "550b3b2e", "metadata": {}, "source": [ "## Cleanup" ] }, { "cell_type": "code", "execution_count": 13, "id": "dbf76e3d", "metadata": {}, "outputs": [ { "data": { "text/plain": [ "" ] }, "execution_count": 13, "metadata": {}, "output_type": "execute_result" } ], "source": [ "def cleanup():\n", " \"\"\"Clean up the simulation.\"\"\"\n", " global bd1, sim, sp, constraint\n", " del bd1, sim, sp, constraint\n", "\n", "\n", "atexit.register(cleanup)" ] } ], "metadata": { "docs": { "tags": [ "dynamics", "constraints", "cutjoint", "TA model", "time domain" ] }, "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" } }, "nbformat": 4, "nbformat_minor": 5 }