{ "cells": [ { "cell_type": "markdown", "id": "5cf5ee11", "metadata": { "jupyter": { "outputs_hidden": false } }, "source": [ "# Double-Wishbone\n", "\n", "This notebook walks through loading a DARTS Model file of a Double Wishbone multibody. Additionally, it demonstrates how to enforce loop constraints in the graph multibody.\n", "\n", "Requirements:\n", "- [Import/export](../example_import_export/notebook.ipynb)\n", "\n", "In this tutorial we will:\n", "- Import the multibody\n", "- Setup constraint kinematics\n", "- Setup the **kdFlex** Scene\n", "- Run the simulation with loop constraints\n", "- Run the simulation without constraints\n", "- Clean up the simulation\n", "\n", "![](../resources/nb_images/double_wishbone.PNG)\n", "\n", "For a more in-depth descriptions of **kdflex** concepts see [usage](usage_page)." ] }, { "cell_type": "code", "execution_count": 1, "id": "a91efbe2", "metadata": { "jupyter": { "outputs_hidden": false } }, "outputs": [], "source": [ "import atexit\n", "import Karana.Core as kc\n", "import Karana.Frame as kf\n", "import Karana.Dynamics as kd\n", "import Karana.Dynamics.SOADyn_types as kdt\n", "from Karana.KUtils.Sim import Sim" ] }, { "cell_type": "markdown", "id": "7173fd44", "metadata": { "jupyter": { "outputs_hidden": false } }, "source": [ "## Import the Multibody\n", "\n", "The double wishbone model has already been prepared as a SOADyn_types.SubGraphDS and saved to an H5 file. We can use this file to populate our Multibody. We then perform basic initialization and log the model to verify." ] }, { "cell_type": "code", "execution_count": 2, "id": "35d3d296", "metadata": { "jupyter": { "outputs_hidden": false } }, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "LEGEND: [ ] [/num embedded bodies > 0] 0>\n", "|-mb_MBVROOT_\n", " |-[LOCKED] chassis [<--- lc/BALL <--- upper_control_arm], [---> lc/BALL ---> tie_rod], [---> lc/REVOLUTE ---> shock_absorber_upper]\n", " |-[REVOLUTE (P)] lower_control_arm\n", " |-[REVOLUTE] shock_absorber_lower\n", " |-[SLIDER] shock_absorber_upper [<--- lc/REVOLUTE <--- chassis]\n", " |-[REVOLUTE] spindle\n", " |-[REVOLUTE] spindleAlt\n", " |-[UJOINT] tie_rod [<--- lc/BALL <--- chassis]\n", " |-[REVOLUTE] wheel\n", " |-[BALL] upper_control_arm [---> lc/BALL ---> chassis]\n", "\n" ] } ], "source": [ "# load our wishbone model into a simulation\n", "mbody_ds = kdt.SubGraphDS.fromFile(\"../resources/double_wishbone/rigid_double_wishbone.h5\")\n", "sim = Sim(mbody_ds, None)\n", "del mbody_ds\n", "\n", "# finalize and verify our multibody\n", "sim.mb.ensureHealthy()\n", "sim.mb.resetData()\n", "assert kc.allReady()\n", "\n", "# dump tree structure\n", "sim.mb.dumpTree()" ] }, { "cell_type": "code", "execution_count": 3, "id": "80a05431", "metadata": { "jupyter": { "outputs_hidden": false } }, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "\n", "LEGEND: [prescribed subhinges] [flex dofs/offset]\n", "\n", " Body Parent Hinge Dofs \n", " ____ ______ _____ ____ \n", " 1. chassis mb_MBVROOT_ LOCKED 0/0 \n", " 2. lower_control_arm chassis REVOLUTE 1/0 P \n", " 3. shock_absorber_lower lower_control_arm REVOLUTE 1/1 \n", " 4. shock_absorber_upper shock_absorber_lower SLIDER 1/2 \n", " 5. spindle lower_control_arm REVOLUTE 1/3 \n", " 6. spindleAlt spindle REVOLUTE 1/4 \n", " 7. tie_rod spindleAlt UJOINT 2/5 \n", " 8. wheel spindleAlt REVOLUTE 1/7 \n", " 9. upper_control_arm spindle BALL 3/8 \n", " ____ \n", " 11 \n", "\n", " Nodes\n", " ---\n", " spindleAlt_ik_ref_node body=spindleAlt\n", " WheelContactNode body=wheel force=true\n", " chassis_shock_absorber_constraint_node body=chassis\n", " chassis_tie_rod_constraint_node body=chassis\n", " chassis_upper_control_arm_constraint_node body=chassis\n", " shock_absorber_chassis_constraint_node body=shock_absorber_upper\n", " tie_rod_chassis_constraint_node body=tie_rod\n", " upper_control_arm_chassis_constraint_node body=upper_control_arm\n", "\n", " Enabled cutjoint loop constraints <3, residuals=11, error=18>\n", " ---------------\n", " chassis_shock_absorber_constraint source=chassis/chassis_shock_absorber_constraint_node target=shock_absorber_upper/shock_absorber_chassis_constraint_node\n", " chassis_tie_rod_constraint source=chassis/chassis_tie_rod_constraint_node target=tie_rod/tie_rod_chassis_constraint_node\n", " chassis_upper_control_arm_constraint source=upper_control_arm/upper_control_arm_chassis_constraint_node target=chassis/chassis_upper_control_arm_constraint_node\n", "\n" ] } ], "source": [ "# show model details\n", "sim.mb.displayModel()" ] }, { "cell_type": "markdown", "id": "db9d2ea2", "metadata": { "jupyter": { "outputs_hidden": false } }, "source": [ "## Setup Constraint Kinematics\n", "\n", "To use [constraint kinematics](constraint_kinematics_sec), we use {py:meth}`Karana.Dynamics.Multibody.cks` to get an instance of {py:class}`Karana.Dynamics.ConstraintKinematicsSolver`. Then, {py:meth}`Karana.Dynamics.ConstraintKinematicsSolver.solveQ` can solve for the coordinates to satisfy loop constraints and return the residual error." ] }, { "cell_type": "code", "execution_count": 4, "id": "930bbd02", "metadata": { "jupyter": { "outputs_hidden": false } }, "outputs": [], "source": [ "solver = sim.mb.cks()\n", "error = solver.solveQ()\n", "assert error < 1e-10" ] }, { "cell_type": "markdown", "id": "40536c10", "metadata": { "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. We use {py:meth}`Karana.Dynamics.Multibody.createStickParts` to add automatically add visual geometries.\n", "\n", "See [Visualization](visualization_sec) and [Scene Layer](scene_layer_sec) for more information relating to this section." ] }, { "cell_type": "code", "execution_count": 5, "id": "5dc21e28", "metadata": { "jupyter": { "outputs_hidden": false } }, "outputs": [ { "data": { "text/html": [ "\n", "

\n", " \n", "

\n", " " ], "text/plain": [ "" ] }, "metadata": {}, "output_type": "display_data" } ], "source": [ "# setup graphics and add visual stick parts\n", "_, web_scene = sim.setupGraphics(port=0, axes=0.5)\n", "sim.mb.createStickParts()" ] }, { "cell_type": "markdown", "id": "f3f73926", "metadata": { "jupyter": { "outputs_hidden": false } }, "source": [ "## Run the Simulation With Loop Constraints\n", "\n", "We can now articulate through the lower control arm's subhinge and demonstrate how loop constraints are enforced on the subhinge." ] }, { "cell_type": "code", "execution_count": 6, "id": "9fbcf010", "metadata": { "jupyter": { "outputs_hidden": false } }, "outputs": [], "source": [ "# grab the lower arm subhinge and enforce the loop constraints in it\n", "lca = sim.mb.getBody(\"lower_control_arm\")\n", "lca_shg = lca.parentHinge().subhinge(0)\n", "sim.mb.articulateSubhinge(lca_shg, 0)" ] }, { "cell_type": "markdown", "id": "a0deda0b", "metadata": { "jupyter": { "outputs_hidden": false } }, "source": [ "## Run the Simulation Without Constraints\n", "\n", "Compare this to when we articulate the bodies without enforcing loop constraints." ] }, { "cell_type": "code", "execution_count": 7, "id": "a7043957", "metadata": { "jupyter": { "outputs_hidden": false } }, "outputs": [], "source": [ "# this will not enforce the loop constraints\n", "sim.mb.articulateBodies()" ] }, { "cell_type": "markdown", "id": "e5a9b641", "metadata": { "jupyter": { "outputs_hidden": false } }, "source": [ "## Clean Up the Simulation\n", "\n", "Finally, we cleanup by deleting local variables, discarding our containers, multibodies, and cleaning up the scene." ] }, { "cell_type": "code", "execution_count": 8, "id": "35e5bd1e", "metadata": { "jupyter": { "outputs_hidden": false } }, "outputs": [ { "data": { "text/plain": [ "" ] }, "execution_count": 8, "metadata": {}, "output_type": "execute_result" } ], "source": [ "# Cleanup\n", "def cleanup():\n", " \"\"\"Cleanup the simulation.\"\"\"\n", " import gc\n", "\n", " global web_scene, solver, lca, lca_shg, sim\n", " del solver, lca, lca_shg, web_scene, sim\n", "\n", "\n", "atexit.register(cleanup)" ] }, { "cell_type": "markdown", "id": "0545232b", "metadata": { "jupyter": { "outputs_hidden": false } }, "source": [ "## Summary\n", "By importing a model with built-in constraints, we can setup a ConstraintKinematicsSolver to enforce these constraints. Then, we compare the outputs from running the simulation with and without constraints.\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", "[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 }