{
"cells": [
{
"cell_type": "markdown",
"id": "0e827775",
"metadata": {
"collapsed": false,
"jupyter": {
"outputs_hidden": false
}
},
"source": [
"# Slider crank example\n",
"This notebook is an example of a simple slider crank. This demonstrates how to do the following:\n",
"\n",
"Requirements:\n",
"- [Import/export](../example_import_export/notebook.ipynb)\n",
"\n",
"In this tutorial we will:\n",
"- Create a Multibody using DataStruct.DataStruct \n",
"- Use ConstraintKinematicsSolver solve a system\n",
"- Setup the simulation\n",
"- Run the simulation with constraints\n",
"\n",
"\n",
"\n",
"For a more in-depth descriptions of **kdflex** concepts see [usage](usage_page)."
]
},
{
"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 copy import deepcopy\n",
"from typing import cast\n",
"from Karana.Math.Kquantities import ureg\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": "96d2534e",
"metadata": {
"collapsed": false,
"jupyter": {
"outputs_hidden": false
}
},
"source": [
"# Create a Multibody Using DataStruct\n",
"\n",
"Let's create the slider-crank multibody. Here, we choose to do so via DataStruct.DataStruct s. We define a DataStruct for each body, and then combine them together into a SOADyn_types.SubGraphDS.\n",
"\n",
"DataStructs have support for units. The units will be automatically converted to the simulation's units system, which is SI units by default. The link_1 BodyDS has an example of this, where the mass units are in lbs."
]
},
{
"cell_type": "code",
"execution_count": 2,
"id": "34372ede",
"metadata": {
"collapsed": false,
"jupyter": {
"outputs_hidden": false
}
},
"outputs": [],
"source": [
"fc = kf.FrameContainer(\"root\")\n",
"\n",
"# Create a BodyDS for the first link. This can be used to specify all the parameters for the body, and the way\n",
"# the body attaches to it's parent, i.e., the hinge.\n",
"link_1 = kdt.BodyDS(\n",
" name=\"link_1\",\n",
" mass=1.0 * ureg.lb,\n",
" body_to_cm=np.zeros(3),\n",
" inertia=np.eye(3),\n",
" hinge=kdt.HingeDS(\n",
" hinge_type=kd.HingeType.REVOLUTE,\n",
" subhinges=[kdt.PinSubhingeDS(prescribed=False, unit_axis=np.array([0.0, 1.0, 0.0]))], # pyright: ignore - Types are okay\n",
" ),\n",
" body_to_joint=np.array([-0.25, 0.0, 0.0]),\n",
")\n",
"\n",
"# Let's create the second link of our slider crank. Here, we will simply copy the first and modify the parameters\n",
"# that should be changed.\n",
"link_2 = deepcopy(link_1)\n",
"link_2.name = \"link_2\"\n",
"link_2.mass = 2.0 * ureg.kg\n",
"link_2.body_to_joint = np.array([0.75, 0.0, 0.0])\n",
"link_2.inb_to_joint = np.array([0.25, 0.0, 0.0])\n",
"\n",
"# We create a block body in a similar fashion. The block is the body that will have the slider constraint,\n",
"# so we also add a constraint node via a NodeDS.\n",
"block = deepcopy(link_1)\n",
"block.name = \"block\"\n",
"block.mass = 1.0\n",
"block.body_to_joint = np.zeros(3)\n",
"block.constraint_nodes = [\n",
" kdt.NodeDS(\n",
" name=\"cn\",\n",
" constraint_node=True,\n",
" force_node=True,\n",
" )\n",
"]\n",
"\n",
"# Now, we combine all the bodies together to form a SubGraphDS. In addition, we add our LoopConstraint\n",
"# and a constraint frame for the slider constraint.\n",
"mbody_ds = kdt.SubGraphDS(\n",
" name=\"mb\",\n",
" base_bodies=[\n",
" kdt.BodyWithContextDS(\n",
" body=link_1,\n",
" children=[\n",
" kdt.BodyWithContextDS(\n",
" body=link_2,\n",
" children=[\n",
" kdt.BodyWithContextDS(body=block, children=[]),\n",
" ],\n",
" ),\n",
" ],\n",
" )\n",
" ],\n",
" loop_constraints=[\n",
" kdt.LoopConstraintCutJointDS(\n",
" name=\"block_constraint\",\n",
" hinge=kdt.HingeDS(\n",
" hinge_type=kd.HingeType.SLIDER,\n",
" subhinges=[\n",
" kdt.LinearSubhingeDS(prescribed=False, unit_axis=np.array([1.0, 0.0, 0.0]))\n",
" ],\n",
" ),\n",
" get_oframe={\"frame\": \"constraint_frame\"},\n",
" get_pframe={\"body\": \"block\", \"node\": \"cn\"},\n",
" )\n",
" ],\n",
" constraint_frames=[\n",
" kdt.ConstraintFrameDS(\n",
" name=\"constraint_frame\",\n",
" translation=np.zeros(3),\n",
" unit_quaternion=km.UnitQuaternion(0, 0, 0, 1),\n",
" parent=\"newtonian\",\n",
" )\n",
" ],\n",
")"
]
},
{
"cell_type": "markdown",
"id": "31f715b9",
"metadata": {
"collapsed": false,
"jupyter": {
"outputs_hidden": false
}
},
"source": [
"DataStructs can be saved to a variety of formats, for example YAML and HDF5. This makes it easy to re-create the same DataStruct in different simulations.\n",
"\n",
"Here, we don't need to save the DataStruct for later, we simply use it to create the Multibody."
]
},
{
"cell_type": "code",
"execution_count": 3,
"id": "88e5b2dc",
"metadata": {
"collapsed": false,
"jupyter": {
"outputs_hidden": false
}
},
"outputs": [],
"source": [
"# Create the Multibody from the DataStruct\n",
"mb = mbody_ds.toMultibody(fc)\n",
"del mbody_ds, link_2, block\n",
"\n",
"# Once all bodies are created, we make the Multibody healthy. This sets up internal\n",
"# variables for use in dynamics computations.\n",
"mb.ensureHealthy()"
]
},
{
"cell_type": "markdown",
"id": "56a8ac71",
"metadata": {
"collapsed": false,
"jupyter": {
"outputs_hidden": false
}
},
"source": [
"As mentioned previously, values are automatically converted to the correct units system."
]
},
{
"cell_type": "code",
"execution_count": 4,
"id": "964bcb11",
"metadata": {
"collapsed": false,
"jupyter": {
"outputs_hidden": false
}
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"DataStruct value for link_1 mass: 1.0 lb\n",
"Mass for link_1 in the multibody: 0.45359237 kg\n"
]
}
],
"source": [
"print(f\"DataStruct value for link_1 mass: {link_1.mass}\")\n",
"link_1_bd = cast(kd.PhysicalBody, mb.getBody(\"link_1\"))\n",
"print(f\"Mass for link_1 in the multibody: {link_1_bd.getSpatialInertia().mass()}\")\n",
"del link_1_bd, link_1"
]
},
{
"cell_type": "markdown",
"id": "fe1a17b2",
"metadata": {
"collapsed": false,
"jupyter": {
"outputs_hidden": false
}
},
"source": [
"## Use ConstraintKinematicsSolver to Solve the System\n",
"\n",
"Now, we initialize the state with the {py:class}`Karana.Dynamics.ConstraintKinematicsSolver`"
]
},
{
"cell_type": "code",
"execution_count": 5,
"id": "264550a3",
"metadata": {
"collapsed": false,
"jupyter": {
"outputs_hidden": false
}
},
"outputs": [],
"source": [
"# Now, we need to initialize our state. Our state has constraints, so we will use the\n",
"# ConstraintKinematicsSolver to get a valid state that satisfies the constraints.\n",
"# First, we set everything to zero as our initial guess, except the velocity of the\n",
"# first link. We want this to have an initial velocity so that our slider-crank moves.\n",
"cd = mb.subhingeCoordData()\n",
"cd.setQ(0.0)\n",
"cd.setU(0.0)\n",
"u = cd.getU()\n",
"u[0] = 1.0\n",
"cd.setU(u)\n",
"cd.setUdot(0.0)\n",
"cd.setT(0.0)\n",
"\n",
"# In addition to the subhinge data, we also need to set the constraint data. We do so here.\n",
"cdc = mb.cutjointCoordData()\n",
"cdc.setQ(0.0)\n",
"cdc.setU(0.0)\n",
"cdc.setUdot(0.0)\n",
"cdc.setT(0.0)\n",
"del cdc"
]
},
{
"cell_type": "markdown",
"id": "0c700305",
"metadata": {
"collapsed": false,
"jupyter": {
"outputs_hidden": false
}
},
"source": [
"The {py:class}`Karana.Dynamics.ConstraintKinematicsSolver` has solveQ and solveU methods we will use. These methods return the error after solving. We will ensure this is less than 1e-6 before moving forward."
]
},
{
"cell_type": "code",
"execution_count": 6,
"id": "d045db55",
"metadata": {
"collapsed": false,
"jupyter": {
"outputs_hidden": false
}
},
"outputs": [],
"source": [
"# Now, we create a ConstraintKinematicsSolver and solve for the initial state.\n",
"cks = mb.cks()\n",
"errQ = cks.solveQ()\n",
"errU = cks.solveU()\n",
"\n",
"# Make sure we were able to solve for Q and U\n",
"assert abs(errQ) < 1e-6\n",
"assert abs(errU) < 1e-6\n",
"del cks"
]
},
{
"cell_type": "code",
"execution_count": 7,
"id": "5048ec72",
"metadata": {
"collapsed": false,
"jupyter": {
"outputs_hidden": false
}
},
"outputs": [],
"source": [
"# Before we finalize everything (see below), we need to set the gravity. Ultimately, we will\n",
"# be setting this with a model (see further below). For now, we will just set this to zero\n",
"# so we can do the readiness check.\n",
"mb.setUniformGravAccel(np.zeros(3))\n",
"\n",
"# verify the multibody and its state\n",
"assert kc.allReady()"
]
},
{
"cell_type": "markdown",
"id": "7065d1b2",
"metadata": {
"collapsed": false,
"jupyter": {
"outputs_hidden": false
}
},
"source": [
"## Setup the Simulation\n",
"Start 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": 8,
"id": "d4a29bd7",
"metadata": {
"collapsed": false,
"jupyter": {
"outputs_hidden": false
}
},
"outputs": [
{
"data": {
"text/html": [
"\n",
" \n",
" \n",
"
\n",
" "
],
"text/plain": [
""
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"# setup graphics helper function\n",
"cleanup_graphics, web_scene = mb.setupGraphics(port=0, axes=0.5)\n",
"mb.createStickParts()\n",
"\n",
"# position the viewpoint camera of the visualization\n",
"web_scene.defaultCamera().pointCameraAt([0, 3, 0], [0, 0, 0], [0, 0, 1])\n",
"del web_scene"
]
},
{
"cell_type": "markdown",
"id": "e2ffa804",
"metadata": {
"collapsed": false,
"jupyter": {
"outputs_hidden": false
}
},
"source": [
"Setup the {py:class}`Karana.Dynamics.StatePropagator` and integrator."
]
},
{
"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(\n",
" mb,\n",
" integrator_type=ki.IntegratorType.RK4,\n",
" integ_opts=None,\n",
" solver_type=kd.MMSolverType.TREE_AUGMENTED_DYNAMICS,\n",
")\n",
"integrator = sp.getIntegrator()\n",
"\n",
"# Makes sure the visualization scene is updated after each state change.\n",
"kmdl.UpdateProxyScene(\"update_proxy_scene\", sp, mb.getScene())\n",
"\n",
"# sync the simulation time with real-time.\n",
"kmdl.SyncRealTime(\"sync_real_time\", sp, 1.0)\n",
"\n",
"# register callback to print the state at each step\n",
"f2f = fc.root().frameToFrame(mb.getBody(\"block\"))\n",
"\n",
"\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(\n",
" f\"t = {float(integrator.getTime()) / 1e9}s; block_pos = {np.round(f2f.relTransform().getTranslation(), 10)}\"\n",
" )\n",
"\n",
"\n",
"sp.fns.post_hop_fns[\"update_and_info\"] = post_step_fn"
]
},
{
"cell_type": "markdown",
"id": "42b741a2",
"metadata": {
"collapsed": false,
"jupyter": {
"outputs_hidden": false
}
},
"source": [
"Initialize integrator state and add timed event"
]
},
{
"cell_type": "code",
"execution_count": 10,
"id": "d5e4c74d",
"metadata": {
"collapsed": false,
"jupyter": {
"outputs_hidden": false
}
},
"outputs": [],
"source": [
"# Initialize integrator state\n",
"t_init = np.timedelta64(0, \"ns\")\n",
"sp.setTime(t_init)\n",
"sp.setState(sp.assembleState())\n",
"\n",
"# Syncs up graphics\n",
"mb.getScene().update()\n",
"\n",
"# register the timed event\n",
"h = np.timedelta64(int(1e8), \"ns\")\n",
"t = kd.TimedEvent(\"hop_size\", h, lambda _: None, False)\n",
"t.period = h\n",
"sp.registerTimedEvent(t)\n",
"del t"
]
},
{
"cell_type": "markdown",
"id": "06e5af80",
"metadata": {
"collapsed": false,
"jupyter": {
"outputs_hidden": false
}
},
"source": [
"## Run the Simulation With Constraints\n",
"\n",
"Now, run the simulation for 10 seconds. This will print the time and location of the block. The block should oscillate back and forth along the x-axis. The y- and z-components should be near zero, but may not be identically zero, because the slider is enforced via a constraint."
]
},
{
"cell_type": "code",
"execution_count": 11,
"id": "f92e5b77",
"metadata": {
"collapsed": false,
"jupyter": {
"outputs_hidden": false
}
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"t = 0.0s; x = [ 0. 0. 0. -0.25 1. -0.33333333\n",
" -0.66666667 0. ]\n",
"t = 0.1s; block_pos = [-2.50835263e-01 0.00000000e+00 2.66120017e-08]\n",
"t = 0.2s; block_pos = [-2.53364350e-01 0.00000000e+00 5.41549326e-08]\n",
"t = 0.3s; block_pos = [-2.57658048e-01 0.00000000e+00 8.36652793e-08]\n",
"t = 0.4s; block_pos = [-2.63837196e-01 0.00000000e+00 1.15819022e-07]\n",
"t = 0.5s; block_pos = [-2.72076493e-01 0.00000000e+00 1.50486238e-07]\n",
"t = 0.6s; block_pos = [-2.82608535e-01 0.00000000e+00 1.85912595e-07]\n",
"t = 0.7s; block_pos = [-2.95725945e-01 0.00000000e+00 2.17486330e-07]\n",
"t = 0.8s; block_pos = [-3.11777550e-01 0.00000000e+00 2.36557962e-07]\n",
"t = 0.9s; block_pos = [-3.31151907e-01 0.00000000e+00 2.31129365e-07]\n",
"t = 1.0s; block_pos = [-3.54239340e-01 0.00000000e+00 1.92102924e-07]\n",
"t = 1.1s; block_pos = [-3.81365729e-01 0.00000000e+00 1.27370642e-07]\n",
"t = 1.2s; block_pos = [-4.12703553e-01 0.00000000e+00 7.38739071e-08]\n",
"t = 1.3s; block_pos = [-4.48188762e-01 0.00000000e+00 8.23730407e-08]\n",
"t = 1.4s; block_pos = [-4.87488836e-01 0.00000000e+00 1.72212803e-07]\n",
"t = 1.5s; block_pos = [-5.30049758e-01 0.00000000e+00 3.09150500e-07]\n",
"t = 1.6s; block_pos = [-5.75200703e-01 0.00000000e+00 4.39428533e-07]\n",
"t = 1.7s; block_pos = [-6.22262221e-01 0.00000000e+00 5.32065287e-07]\n",
"t = 1.8s; block_pos = [-6.70616564e-01 0.00000000e+00 5.85742556e-07]\n",
"t = 1.9s; block_pos = [-7.19733069e-01 0.00000000e+00 6.13435375e-07]\n",
"t = 2.0s; block_pos = [-7.69162683e-01 0.00000000e+00 6.28946852e-07]\n",
"t = 2.1s; block_pos = [-8.18518032e-01 0.00000000e+00 6.41799976e-07]\n",
"t = 2.2s; block_pos = [-8.67449397e-01 0.00000000e+00 6.57142545e-07]\n",
"t = 2.3s; block_pos = [-9.15620994e-01 0.00000000e+00 6.77132508e-07]\n",
"t = 2.4s; block_pos = [-9.62688431e-01 0.00000000e+00 7.02232464e-07]\n",
"t = 2.5s; block_pos = [-1.00827661e+00 0.00000000e+00 7.32054536e-07]\n",
"t = 2.6s; block_pos = [-1.05195695e+00 0.00000000e+00 7.65833100e-07]\n",
"t = 2.7s; block_pos = [-1.09322319e+00 0.00000000e+00 8.02703910e-07]\n",
"t = 2.8s; block_pos = [-1.13146638e+00 0.00000000e+00 8.42000555e-07]\n",
"t = 2.9s; block_pos = [-1.16595277e+00 0.00000000e+00 8.83831970e-07]\n",
"t = 3.0s; block_pos = [-1.19581384e+00 0.00000000e+00 9.30178900e-07]\n",
"t = 3.1s; block_pos = [-1.22006652e+00 0.00000000e+00 9.86196835e-07]\n",
"t = 3.2s; block_pos = [-1.23768881e+00 0.00000000e+00 1.05974510e-06]\n",
"t = 3.3s; block_pos = [-1.24776842e+00 0.00000000e+00 1.15584883e-06]\n",
"t = 3.4s; block_pos = [-1.24970291e+00 0.00000000e+00 1.26785947e-06]\n",
"t = 3.5s; block_pos = [-1.24336897e+00 0.00000000e+00 1.37749156e-06]\n",
"t = 3.6s; block_pos = [-1.22916088e+00 0.00000000e+00 1.46895060e-06]\n",
"t = 3.7s; block_pos = [-1.20787213e+00 0.00000000e+00 1.53971250e-06]\n",
"t = 3.8s; block_pos = [-1.18049646e+00 0.00000000e+00 1.59631597e-06]\n",
"t = 3.9s; block_pos = [-1.14804876e+00 0.00000000e+00 1.64571041e-06]\n",
"t = 4.0s; block_pos = [-1.11145619e+00 0.00000000e+00 1.69167256e-06]\n",
"t = 4.1s; block_pos = [-1.07151534e+00 0.00000000e+00 1.73531760e-06]\n",
"t = 4.2s; block_pos = [-1.02889097e+00 0.00000000e+00 1.77644273e-06]\n",
"t = 4.3s; block_pos = [-9.84134123e-01 0.00000000e+00 1.81440708e-06]\n",
"t = 4.4s; block_pos = [-9.37706458e-01 0.00000000e+00 1.84854887e-06]\n",
"t = 4.5s; block_pos = [-8.90005078e-01 0.00000000e+00 1.87842145e-06]\n",
"t = 4.6s; block_pos = [-8.41385914e-01 0.00000000e+00 1.90408227e-06]\n",
"t = 4.7s; block_pos = [-7.92186007e-01 0.00000000e+00 1.92661065e-06]\n",
"t = 4.8s; block_pos = [-7.42745712e-01 0.00000000e+00 1.94898329e-06]\n",
"t = 4.9s; block_pos = [-6.93431844e-01 0.00000000e+00 1.97726863e-06]\n",
"t = 5.0s; block_pos = [-6.44661829e-01 0.00000000e+00 2.02151789e-06]\n",
"t = 5.1s; block_pos = [-5.96926428e-01 0.00000000e+00 2.09438169e-06]\n",
"t = 5.2s; block_pos = [-5.50803674e-01 0.00000000e+00 2.20393968e-06]\n",
"t = 5.3s; block_pos = [-5.06950102e-01 0.00000000e+00 2.33951657e-06]\n",
"t = 5.4s; block_pos = [-4.66052276e-01 0.00000000e+00 2.46182169e-06]\n",
"t = 5.5s; block_pos = [-4.28733184e-01 0.00000000e+00 2.52148124e-06]\n",
"t = 5.6s; block_pos = [-3.95438954e-01 0.00000000e+00 2.50352447e-06]\n",
"t = 5.7s; block_pos = [-3.66359362e-01 0.00000000e+00 2.44386597e-06]\n",
"t = 5.8s; block_pos = [-3.41423812e-01 0.00000000e+00 2.39251089e-06]\n",
"t = 5.9s; block_pos = [-3.20366214e-01 0.00000000e+00 2.37500228e-06]\n",
"t = 6.0s; block_pos = [-3.02816295e-01 0.00000000e+00 2.38976466e-06]\n",
"t = 6.1s; block_pos = [-2.88378318e-01 0.00000000e+00 2.42412620e-06]\n",
"t = 6.2s; block_pos = [-2.76681813e-01 0.00000000e+00 2.46629842e-06]\n",
"t = 6.3s; block_pos = [-2.67406922e-01 0.00000000e+00 2.50905808e-06]\n",
"t = 6.4s; block_pos = [-2.60293115e-01 0.00000000e+00 2.54915934e-06]\n",
"t = 6.5s; block_pos = [-2.55139177e-01 0.00000000e+00 2.58580872e-06]\n",
"t = 6.6s; block_pos = [-2.51799701e-01 0.00000000e+00 2.61946151e-06]\n",
"t = 6.7s; block_pos = [-2.50180994e-01 0.00000000e+00 2.65111438e-06]\n",
"t = 6.8s; block_pos = [-2.50237855e-01 0.00000000e+00 2.68195172e-06]\n",
"t = 6.9s; block_pos = [-2.51971872e-01 0.00000000e+00 2.71317988e-06]\n",
"t = 7.0s; block_pos = [-2.55431489e-01 0.00000000e+00 2.74592070e-06]\n",
"t = 7.1s; block_pos = [-2.60713815e-01 0.00000000e+00 2.78105624e-06]\n",
"t = 7.2s; block_pos = [-2.67967937e-01 0.00000000e+00 2.81890799e-06]\n",
"t = 7.3s; block_pos = [-2.77399033e-01 0.00000000e+00 2.85860861e-06]\n",
"t = 7.4s; block_pos = [-2.89271757e-01 0.00000000e+00 2.89704924e-06]\n",
"t = 7.5s; block_pos = [-3.03909846e-01 0.00000000e+00 2.92755471e-06]\n",
"t = 7.6s; block_pos = [-3.21686555e-01 0.00000000e+00 2.93934978e-06]\n",
"t = 7.7s; block_pos = [-3.42997834e-01 0.00000000e+00 2.92068698e-06]\n",
"t = 7.8s; block_pos = [-3.68209642e-01 0.00000000e+00 2.86945263e-06]\n",
"t = 7.9s; block_pos = [-3.97577588e-01 0.00000000e+00 2.80866181e-06]\n",
"t = 8.0s; block_pos = [-4.31155962e-01 0.00000000e+00 2.78709020e-06]\n",
"t = 8.1s; block_pos = [-4.68736392e-01 0.00000000e+00 2.84451672e-06]\n",
"t = 8.2s; block_pos = [-5.09857422e-01 0.00000000e+00 2.97052659e-06]\n",
"t = 8.3s; block_pos = [-5.53888065e-01 0.00000000e+00 3.11402288e-06]\n",
"t = 8.4s; block_pos = [-6.00141274e-01 0.00000000e+00 3.23016801e-06]\n",
"t = 8.5s; block_pos = [-6.47964860e-01 0.00000000e+00 3.30509799e-06]\n",
"t = 8.6s; block_pos = [-6.9678682e-01 0.0000000e+00 3.3472802e-06]\n",
"t = 8.7s; block_pos = [-7.46122007e-01 0.00000000e+00 3.37124283e-06]\n",
"t = 8.8s; block_pos = [-7.95557218e-01 0.00000000e+00 3.38871488e-06]\n",
"t = 8.9s; block_pos = [-8.44728210e-01 0.00000000e+00 3.40674236e-06]\n",
"t = 9.0s; block_pos = [-8.93295579e-01 0.00000000e+00 3.42869198e-06]\n",
"t = 9.1s; block_pos = [-9.40921696e-01 0.00000000e+00 3.45568294e-06]\n",
"t = 9.2s; block_pos = [-9.87248538e-01 0.00000000e+00 3.48764580e-06]\n",
"t = 9.3s; block_pos = [-1.03187536e+00 0.00000000e+00 3.52394889e-06]\n",
"t = 9.4s; block_pos = [-1.07433518e+00 0.00000000e+00 3.56374347e-06]\n",
"t = 9.5s; block_pos = [-1.11406996e+00 0.00000000e+00 3.60622304e-06]\n",
"t = 9.6s; block_pos = [-1.15040639e+00 0.00000000e+00 3.65103377e-06]\n",
"t = 9.7s; block_pos = [-1.18253856e+00 0.00000000e+00 3.69911405e-06]\n",
"t = 9.8s; block_pos = [-1.20953137e+00 0.00000000e+00 3.75401755e-06]\n",
"t = 9.9s; block_pos = [-1.23036702e+00 0.00000000e+00 3.82265815e-06]\n",
"t = 10.0s; block_pos = [-1.24405911e+00 0.00000000e+00 3.91245475e-06]\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": [
"Below, we cleanup everything whenever the script exits. This requires us to delete local variables and discard our containers and scene."
]
},
{
"cell_type": "code",
"execution_count": 12,
"id": "efb64e64",
"metadata": {
"collapsed": false,
"jupyter": {
"outputs_hidden": false
}
},
"outputs": [
{
"data": {
"text/plain": [
""
]
},
"execution_count": 12,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"def cleanup():\n",
" \"\"\"Cleanup the simulation.\"\"\"\n",
" global integrator, cd, f2f\n",
" del integrator, cd, f2f\n",
"\n",
" kc.discard(sp)\n",
" cleanup_graphics()\n",
"\n",
" mb.ensureNotHealthy()\n",
" kc.discard(mb.enabledConstraints())\n",
" kc.discard(fc.lookupFrame(\"constraint_frame\"))\n",
" kc.discard(mb)\n",
" kc.discard(fc)\n",
"\n",
"\n",
"atexit.register(cleanup)"
]
},
{
"cell_type": "markdown",
"id": "f3f7397e",
"metadata": {
"collapsed": false,
"jupyter": {
"outputs_hidden": false
}
},
"source": [
"## Summary\n",
"You are now familiar with creating a Multibody using DataStruct and setting up constraints for it using ConstraintKinematicsSolver. These constraints are automatically enforced when the simulation is run.\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",
"[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
}